zrzk
/
px4_autopilot
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

Checkpoint - I2C protocol register decode

sbg
px4dev 12 years ago
parent
b0fb86a693
commit
8ebe21b27b
4 changed files with 369 additions and 22 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Unified View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 1
      Firmware.sublime-project
  2. 20
      apps/px4io/protocol.h
  3. 36
      apps/px4io/px4io.h
  4. 334
      apps/px4io/registers.c

1
Firmware.sublime-project vendored
Unescape View File

@ -7,6 +7,7 @@
[ [
"*.o", "*.o",
"*.a", "*.a",
"*.d",
".built", ".built",
".context", ".context",
".depend", ".depend",

20
apps/px4io/protocol.h
Unescape View File

@ -62,6 +62,11 @@
* packed. * packed.
*/ */
#define PX4IO_CONTROL_CHANNELS 8
#define PX4IO_INPUT_CHANNELS 12
#define PX4IO_RELAY_CHANNELS 4
/* static configuration page */ /* static configuration page */
#define PX4IO_PAGE_CONFIG 0 #define PX4IO_PAGE_CONFIG 0
#define PX4IO_P_CONFIG_PROTOCOL_VERSION 0 /* magic numbers TBD */ #define PX4IO_P_CONFIG_PROTOCOL_VERSION 0 /* magic numbers TBD */
@ -73,7 +78,6 @@
#define PX4IO_P_CONFIG_RC_INPUT_COUNT 6 /* hardcoded max R/C input count supported */ #define PX4IO_P_CONFIG_RC_INPUT_COUNT 6 /* hardcoded max R/C input count supported */
#define PX4IO_P_CONFIG_ADC_INPUT_COUNT 7 /* hardcoded max ADC inputs */ #define PX4IO_P_CONFIG_ADC_INPUT_COUNT 7 /* hardcoded max ADC inputs */
#define PX4IO_P_CONFIG_RELAY_COUNT 8 /* harcoded # of relay outputs */ #define PX4IO_P_CONFIG_RELAY_COUNT 8 /* harcoded # of relay outputs */
#define PX4IO_P_CONFIG_POWERSW_COUNT 9 /* harcoded # of switched power outputs */
/* dynamic status page */ /* dynamic status page */
#define PX4IO_PAGE_STATUS 1 #define PX4IO_PAGE_STATUS 1
@ -83,9 +87,10 @@
#define PX4IO_P_STATUS_FLAGS 2 /* monitoring flags */ #define PX4IO_P_STATUS_FLAGS 2 /* monitoring flags */
#define PX4IO_P_STATUS_FLAGS_ARMED (1 << 0) /* arm-ok and locally armed */ #define PX4IO_P_STATUS_FLAGS_ARMED (1 << 0) /* arm-ok and locally armed */
#define PX4IO_P_STATUS_FLAGS_OVERRIDE (1 << 1) /* in manual override */ #define PX4IO_P_STATUS_FLAGS_OVERRIDE (1 << 1) /* in manual override */
#define PX4IO_P_STATUS_FLAGS_RC_PPM (1 << 2) /* PPM input is valid */ #define PX4IO_P_STATUS_FLAGS_RC_OK (1 << 2) /* RC input is valid */
#define PX4IO_P_STATUS_FLAGS_RC_DSM (1 << 3) /* DSM input is valid */ #define PX4IO_P_STATUS_FLAGS_RC_PPM (1 << 3) /* PPM input is valid */
#define PX4IO_P_STATUS_FLAGS_RC_SBUS (1 << 4) /* SBUS input is valid */ #define PX4IO_P_STATUS_FLAGS_RC_DSM (1 << 4) /* DSM input is valid */
#define PX4IO_P_STATUS_FLAGS_RC_SBUS (1 << 5) /* SBUS input is valid */
#define PX4IO_P_STATUS_ALARMS 3 /* alarm flags - alarms latch, write 1 to a bit to clear it */ #define PX4IO_P_STATUS_ALARMS 3 /* alarm flags - alarms latch, write 1 to a bit to clear it */
#define PX4IO_P_STATUS_ALARMS_VBATT_LOW (1 << 0) /* VBatt is very close to regulator dropout */ #define PX4IO_P_STATUS_ALARMS_VBATT_LOW (1 << 0) /* VBatt is very close to regulator dropout */
@ -107,7 +112,7 @@
#define PX4IO_PAGE_RAW_RC_INPUT 4 /* 0..CONFIG_RC_INPUT_COUNT-1 */ #define PX4IO_PAGE_RAW_RC_INPUT 4 /* 0..CONFIG_RC_INPUT_COUNT-1 */
/* array of scaled RC input values, -10000..10000 */ /* array of scaled RC input values, -10000..10000 */
#define PX4IO_PAGE_RAW_RC_INPUT 5 /* 0..CONFIG_RC_INPUT_COUNT-1 */ #define PX4IO_PAGE_RC_INPUT 5 /* 0..CONFIG_RC_INPUT_COUNT-1 */
/* array of raw ADC values */ /* array of raw ADC values */
#define PX4IO_PAGE_RAW_ADC_INPUT 6 /* 0..CONFIG_ADC_INPUT_COUNT-1 */ #define PX4IO_PAGE_RAW_ADC_INPUT 6 /* 0..CONFIG_ADC_INPUT_COUNT-1 */
@ -123,8 +128,7 @@
#define PX4IO_P_SETUP_PWM_LOWRATE 3 /* 'low' PWM frame output rate in Hz */ #define PX4IO_P_SETUP_PWM_LOWRATE 3 /* 'low' PWM frame output rate in Hz */
#define PX4IO_P_SETUP_PWM_HIGHRATE 4 /* 'high' PWM frame output rate in Hz */ #define PX4IO_P_SETUP_PWM_HIGHRATE 4 /* 'high' PWM frame output rate in Hz */
#define PX4IO_P_SETUP_RELAYS 5 /* bitmask of relay outputs, 0 = off, 1 = on */ #define PX4IO_P_SETUP_RELAYS 5 /* bitmask of relay/switch outputs, 0 = off, 1 = on */
#define PX4IO_P_SETUP_POWERSW 6 /* bitmask of switched power outputs, 0 = off, 1 = on */
/* autopilot control values, -10000..10000 */ /* autopilot control values, -10000..10000 */
#define PX4IO_PAGE_CONTROLS 101 /* 0..STATUS_CONTROL_COUNT */ #define PX4IO_PAGE_CONTROLS 101 /* 0..STATUS_CONTROL_COUNT */
@ -143,8 +147,6 @@
*/ */
#define PX4IO_CONTROL_CHANNELS 8
#define PX4IO_INPUT_CHANNELS 12
#define PX4IO_RELAY_CHANNELS 4 #define PX4IO_RELAY_CHANNELS 4
#pragma pack(push, 1) #pragma pack(push, 1)

36
apps/px4io/px4io.h
Unescape View File

@ -63,8 +63,24 @@
# define debug(fmt, args...) do {} while(0) # define debug(fmt, args...) do {} while(0)
#endif #endif
/*
* Registers.
*/
extern uint16_t r_page_status[]; /* PX4IO_PAGE_STATUS */
extern uint16_t r_page_actuators[]; /* PX4IO_PAGE_ACTUATORS */
extern uint16_t r_page_servos[]; /* PX4IO_PAGE_SERVOS */
extern uint16_t r_page_raw_rc_input[]; /* PX4IO_PAGE_RAW_RC_INPUT */
extern uint16_t r_page_rc_input[]; /* PX4IO_PAGE_RC_INPUT */
extern uint16_t r_page_adc[]; /* PX4IO_PAGE_RAW_ADC_INPUT */
extern volatile uint16_t r_page_setup[]; /* PX4IO_PAGE_SETUP */
extern volatile uint16_t r_page_controls[]; /* PX4IO_PAGE_CONTROLS */
/* /*
* System state structure. * System state structure.
*
* XXX note that many fields here are deprecated and replaced by
* registers.
*/ */
struct sys_state_s { struct sys_state_s {
@ -128,12 +144,6 @@ struct sys_state_s {
*/ */
uint64_t fmu_data_received_time; uint64_t fmu_data_received_time;
/**
* Current serial interface mode, per the serial_rx_mode parameter
* in the config packet.
*/
uint8_t serial_rx_mode;
/** /**
* If true, the RC signal has been lost for more than a timeout interval * If true, the RC signal has been lost for more than a timeout interval
*/ */
@ -206,6 +216,7 @@ extern volatile int timers[TIMER_NUM_TIMERS];
#define ADC_VBATT 4 #define ADC_VBATT 4
#define ADC_IN5 5 #define ADC_IN5 5
#define ADC_CHANNEL_COUNT 2
/* /*
* Mixer * Mixer
@ -224,6 +235,12 @@ extern void safety_init(void);
extern void comms_main(void) __attribute__((noreturn)); extern void comms_main(void) __attribute__((noreturn));
extern void i2c_init(void); extern void i2c_init(void);
/*
* Register space
*/
extern void registers_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num_values);
extern int registers_get(uint8_t page, uint8_t offset, uint16_t **values, unsigned *num_values);
/* /*
* Sensors/misc inputs * Sensors/misc inputs
*/ */
@ -238,10 +255,3 @@ extern int dsm_init(const char *device);
extern bool dsm_input(void); extern bool dsm_input(void);
extern int sbus_init(const char *device); extern int sbus_init(const char *device);
extern bool sbus_input(void); extern bool sbus_input(void);
/*
* Assertion codes
*/
#define A_GPIO_OPEN_FAIL 100
#define A_SERVO_OPEN_FAIL 101
#define A_INPUTQ_OPEN_FAIL 102

334
apps/px4io/registers.c
Unescape View File

@ -0,0 +1,334 @@
/****************************************************************************
*
* Copyright (C) 2012 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file registers.c
*
* Implementation of the PX4IO register space.
*/
#include "px4io.h"
#include "protocol.h"
static int registers_set_one(uint8_t page, uint8_t offset, uint16_t value);
/**
* Setup registers
*/
uint16_t r_page_setup[] =
{
[PX4IO_P_SETUP_ARMING] = 0,
[PX4IO_P_SETUP_PWM_RATES] = 0,
[PX4IO_P_SETUP_PWM_LOWRATE] = 50,
[PX4IO_P_SETUP_PWM_HIGHRATE] = 200,
[PX4IO_P_SETUP_RELAYS] = 0,
};
#define PX4IO_P_SETUP_ARMING_VALID (PX4IO_P_SETUP_ARMING_ARM_OK | PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE)
#define PX4IO_P_SETUP_RATES_VALID ((1 << IO_SERVO_COUNT) - 1)
#define PX4IO_P_SETUP_RELAYS_VALID ((1 << PXIO_RELAY_CHANNELS) - 1)
/**
* Control values from the FMU.
*/
uint16_t r_page_controls[PX4IO_CONTROL_CHANNELS];
/**
* Static configuration parameters.
*/
static const uint16_t r_page_config[] = {
[PX4IO_P_CONFIG_PROTOCOL_VERSION] = 0,
[PX4IO_P_CONFIG_SOFTWARE_VERSION] = 0,
[PX4IO_P_CONFIG_BOOTLOADER_VERSION] = 0,
[PX4IO_P_CONFIG_MAX_TRANSFER] = 64,
[PX4IO_P_CONFIG_CONTROL_COUNT] = PX4IO_CONTROL_CHANNELS,
[PX4IO_P_CONFIG_ACTUATOR_COUNT] = IO_SERVO_COUNT,
[PX4IO_P_CONFIG_RC_INPUT_COUNT] = MAX_CONTROL_CHANNELS,
[PX4IO_P_CONFIG_ADC_INPUT_COUNT] = ADC_CHANNEL_COUNT,
[PX4IO_P_CONFIG_RELAY_COUNT] = PX4IO_RELAY_CHANNELS,
};
/**
* Status values.
*/
uint16_t r_page_status[] = {
[PX4IO_P_STATUS_FREEMEM] = 0,
[PX4IO_P_STATUS_CPULOAD] = 0,
[PX4IO_P_STATUS_FLAGS] = 0,
[PX4IO_P_STATUS_ALARMS] = 0,
[PX4IO_P_STATUS_VBATT] = 0,
[PX4IO_P_STATUS_TEMPERATURE] = 0
};
/**
* ADC input buffer.
*/
uint16_t r_page_adc[ADC_CHANNEL_COUNT];
/**
* Post-mixed actuator values.
*/
uint16_t r_page_actuators[IO_SERVO_COUNT];
/**
* Servo PWM values
*/
uint16_t r_page_servos[IO_SERVO_COUNT];
/**
* Scaled/routed RC input
*/
uint16_t r_page_rc_input[MAX_CONTROL_CHANNELS];
/**
* Raw RC input
*/
uint16_t r_page_raw_rc_input[MAX_CONTROL_CHANNELS];
void
registers_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num_values)
{
switch (page) {
/* handle bulk controls input */
case PX4IO_PAGE_CONTROLS:
/* copy channel data */
while ((offset < PX4IO_CONTROL_CHANNELS) && (num_values > 0)) {
/* XXX scaling - should be -10000..10000 */
r_page_controls[offset] = *values;
offset++;
num_values--;
values++;
}
/* XXX we should cause a mixer tick ASAP */
system_state.mixer_fmu_available = true;
break;
/* handle text going to the mixer parser */
case PX4IO_PAGE_MIXERLOAD:
mixer_handle_text(values, num_values * sizeof(*values));
break;
default:
/* avoid offset wrap */
if ((offset + num_values) > 255)
num_values = 255 - offset;
/* iterate individual registers, set each in turn */
while (num_values--) {
if (registers_set_one(page, offset, *values))
break;
offset++;
values++;
}
}
}
static int
registers_set_one(uint8_t page, uint8_t offset, uint16_t value)
{
switch (page) {
case PX4IO_PAGE_STATUS:
switch (offset) {
case PX4IO_P_STATUS_ALARMS:
/* clear bits being written */
r_page_status[PX4IO_P_STATUS_ALARMS] &= ~value;
break;
default:
/* just ignore writes to other registers in this page */
break;
}
break;
case PX4IO_PAGE_SETUP:
switch (offset) {
case PX4IO_P_SETUP_ARMING:
value &= PX4IO_P_SETUP_ARMING_VALID;
r_page_setup[PX4IO_P_SETUP_ARMING] = value;
/* update arming state - disarm if no longer OK */
if (system_state.armed && !(value & PX4IO_P_SETUP_ARMING_ARM_OK))
system_state.armed = false;
break;
case PX4IO_P_SETUP_PWM_RATES:
value &= PX4IO_P_SETUP_RATES_VALID;
r_page_setup[PX4IO_P_SETUP_PWM_RATES] = value;
/* XXX re-configure timers */
break;
case PX4IO_P_SETUP_PWM_LOWRATE:
if (value < 50)
value = 50;
if (value > 400)
value = 400;
r_page_setup[PX4IO_P_SETUP_PWM_LOWRATE] = value;
/* XXX re-configure timers */
break;
case PX4IO_P_SETUP_PWM_HIGHRATE:
if (value < 50)
value = 50;
if (value > 400)
value = 400;
r_page_setup[PX4IO_P_SETUP_PWM_HIGHRATE] = value;
/* XXX re-configure timers */
break;
case PX4IO_P_SETUP_RELAYS:
value &= PX4IO_P_SETUP_RELAYS_VALID;
r_page_setup[PX4IO_P_SETUP_RELAYS] = value;
POWER_RELAY1(value & (1 << 0) ? 1 : 0);
POWER_RELAY2(value & (1 << 1) ? 1 : 0);
POWER_ACC1(value & (1 << 2) ? 1 : 0);
POWER_ACC2(value & (1 << 3) ? 1 : 0);
break;
default:
return -1;
}
break;
default:
return -1;
}
return 0;
}
int
registers_get(uint8_t page, uint8_t offset, uint16_t **values, unsigned *num_values)
{
switch (page) {
case PX4IO_PAGE_CONFIG:
*values = r_page_config;
*num_values = sizeof(r_page_config) / sizeof(r_page_config[0]);
break;
case PX4IO_PAGE_STATUS:
{
struct mallinfo minfo = mallinfo();
r_page_status[PX4IO_P_STATUS_FREEMEM] = minfo.fordblks;
}
/* XXX PX4IO_P_STATUS_CPULOAD */
r_page_status[PX4IO_P_STATUS_FLAGS] =
(system_state.armed ? PX4IO_P_STATUS_FLAGS_ARMED : 0) |
(system_state.manual_override_ok ? PX4IO_P_STATUS_FLAGS_OVERRIDE : 0) |
((system_state.rc_channels > 0) ? PX4IO_P_STATUS_FLAGS_RC_OK : 0))
/* XXX specific receiver status */
/* XXX PX4IO_P_STATUS_ALARMS] */
{
/*
* Coefficients here derived by measurement of the 5-16V
* range on one unit:
*
* V counts
* 5 1001
* 6 1219
* 7 1436
* 8 1653
* 9 1870
* 10 2086
* 11 2303
* 12 2522
* 13 2738
* 14 2956
* 15 3172
* 16 3389
*
* slope = 0.0046067
* intercept = 0.3863
*
* Intercept corrected for best results @ 12V.
*/
unsigned counts = adc_measure(ADC_VBATT);
r_page_status[PX4IO_P_STATUS_VBATT] = (4150 + (counts * 46)) / 10;
}
/* XXX PX4IO_P_STATUS_TEMPERATURE */
*values = r_page_status;
*num_values = sizeof(r_page_status) / sizeof(r_page_status[0]);
break;
case PX4IO_PAGE_ACTUATORS:
*values = r_page_actuators;
*num_values = sizeof(r_page_actuators) / sizeof(r_page_actuators[0]);
break;
case PX4IO_PAGE_SERVOS:
*values = system_state.servos;
*num_values = IO_SERVO_COUNT;
break;
case PX4IO_PAGE_RAW_RC_INPUT:
*values = r_page_raw_rc_input;
*num_values = sizeof(r_page_raw_rc_input) / sizeof(r_page_raw_rc_input[0]);
break;
case PX4IO_PAGE_RC_INPUT:
*values = system_state.rc_channel_data;
*num_values = system_state.rc_channels;
return -1;
case PX4IO_PAGE_RAW_ADC_INPUT:
r_page_adc[0] = adc_measure(ADC_VBATT);
r_page_adc[1] = adc_measure(ADC_IN5);
*values = r_page_adc;
*num_values = ADC_CHANNEL_COUNT;
break;
default:
return -1;
}
/* if the offset is beyond the end of the page, we have no data */
if (*num_values <= offset)
return -1;
/* adjust value count and pointer for the page offset */
*num_values -= offset;
*values += offset;
return 0;
}
Write Preview
Loading…
Cancel
Save