diff --git a/Firmware.sublime-project b/Firmware.sublime-project index a316ae2fa7..72bacee9fd 100644 --- a/Firmware.sublime-project +++ b/Firmware.sublime-project @@ -7,6 +7,7 @@ [ "*.o", "*.a", + "*.d", ".built", ".context", ".depend", diff --git a/apps/px4io/protocol.h b/apps/px4io/protocol.h index 218ab90308..aabc476dd7 100644 --- a/apps/px4io/protocol.h +++ b/apps/px4io/protocol.h @@ -62,6 +62,11 @@ * packed. */ +#define PX4IO_CONTROL_CHANNELS 8 +#define PX4IO_INPUT_CHANNELS 12 +#define PX4IO_RELAY_CHANNELS 4 + + /* static configuration page */ #define PX4IO_PAGE_CONFIG 0 #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_ADC_INPUT_COUNT 7 /* hardcoded max ADC inputs */ #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 */ #define PX4IO_PAGE_STATUS 1 @@ -83,9 +87,10 @@ #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_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_DSM (1 << 3) /* DSM input is valid */ -#define PX4IO_P_STATUS_FLAGS_RC_SBUS (1 << 4) /* SBUS input is valid */ +#define PX4IO_P_STATUS_FLAGS_RC_OK (1 << 2) /* RC input is valid */ +#define PX4IO_P_STATUS_FLAGS_RC_PPM (1 << 3) /* PPM 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_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 */ /* 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 */ #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_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_POWERSW 6 /* bitmask of switched power outputs, 0 = off, 1 = on */ +#define PX4IO_P_SETUP_RELAYS 5 /* bitmask of relay/switch outputs, 0 = off, 1 = on */ /* autopilot control values, -10000..10000 */ #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 #pragma pack(push, 1) diff --git a/apps/px4io/px4io.h b/apps/px4io/px4io.h index afbaa78dcc..fb06596023 100644 --- a/apps/px4io/px4io.h +++ b/apps/px4io/px4io.h @@ -63,8 +63,24 @@ # define debug(fmt, args...) do {} while(0) #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. + * + * XXX note that many fields here are deprecated and replaced by + * registers. */ struct sys_state_s { @@ -128,12 +144,6 @@ struct sys_state_s { */ 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 */ @@ -206,6 +216,7 @@ extern volatile int timers[TIMER_NUM_TIMERS]; #define ADC_VBATT 4 #define ADC_IN5 5 +#define ADC_CHANNEL_COUNT 2 /* * Mixer @@ -224,6 +235,12 @@ extern void safety_init(void); extern void comms_main(void) __attribute__((noreturn)); 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 */ @@ -238,10 +255,3 @@ extern int dsm_init(const char *device); extern bool dsm_input(void); extern int sbus_init(const char *device); 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 diff --git a/apps/px4io/registers.c b/apps/px4io/registers.c new file mode 100644 index 0000000000..1397dd2a44 --- /dev/null +++ b/apps/px4io/registers.c @@ -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; +}