diff options
Diffstat (limited to 'src/modules/px4iofirmware')
-rw-r--r-- | src/modules/px4iofirmware/dsm.c | 2 | ||||
-rw-r--r-- | src/modules/px4iofirmware/mixer.cpp | 33 | ||||
-rw-r--r-- | src/modules/px4iofirmware/protocol.h | 54 | ||||
-rw-r--r-- | src/modules/px4iofirmware/px4io.h | 6 | ||||
-rw-r--r-- | src/modules/px4iofirmware/registers.c | 18 | ||||
-rw-r--r-- | src/modules/px4iofirmware/sbus.c | 2 |
6 files changed, 77 insertions, 38 deletions
diff --git a/src/modules/px4iofirmware/dsm.c b/src/modules/px4iofirmware/dsm.c index fd3b72015..4d306d6d0 100644 --- a/src/modules/px4iofirmware/dsm.c +++ b/src/modules/px4iofirmware/dsm.c @@ -355,7 +355,7 @@ dsm_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values) continue; /* ignore channels out of range */ - if (channel >= PX4IO_INPUT_CHANNELS) + if (channel >= PX4IO_RC_INPUT_CHANNELS) continue; /* update the decoded channel count */ diff --git a/src/modules/px4iofirmware/mixer.cpp b/src/modules/px4iofirmware/mixer.cpp index 35ef5fcf6..e55ef784a 100644 --- a/src/modules/px4iofirmware/mixer.cpp +++ b/src/modules/px4iofirmware/mixer.cpp @@ -77,7 +77,8 @@ enum mixer_source { MIX_NONE, MIX_FMU, MIX_OVERRIDE, - MIX_FAILSAFE + MIX_FAILSAFE, + MIX_OVERRIDE_FMU_OK }; static mixer_source source; @@ -135,10 +136,19 @@ mixer_tick(void) if ( (r_status_flags & PX4IO_P_STATUS_FLAGS_OVERRIDE) && (r_status_flags & PX4IO_P_STATUS_FLAGS_RC_OK) && (r_status_flags & PX4IO_P_STATUS_FLAGS_MIXER_OK) && - !(r_setup_arming & PX4IO_P_SETUP_ARMING_RC_HANDLING_DISABLED)) { + !(r_setup_arming & PX4IO_P_SETUP_ARMING_RC_HANDLING_DISABLED) && + !(r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK)) { /* if allowed, mix from RC inputs directly */ source = MIX_OVERRIDE; + } else if ( (r_status_flags & PX4IO_P_STATUS_FLAGS_OVERRIDE) && + (r_status_flags & PX4IO_P_STATUS_FLAGS_RC_OK) && + (r_status_flags & PX4IO_P_STATUS_FLAGS_MIXER_OK) && + !(r_setup_arming & PX4IO_P_SETUP_ARMING_RC_HANDLING_DISABLED) && + (r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK)) { + + /* if allowed, mix from RC inputs directly up to available rc channels */ + source = MIX_OVERRIDE_FMU_OK; } } @@ -240,24 +250,35 @@ mixer_callback(uintptr_t handle, uint8_t control_index, float &control) { - if (control_group != 0) + if (control_group > 3) return -1; switch (source) { case MIX_FMU: - if (control_index < PX4IO_CONTROL_CHANNELS) { - control = REG_TO_FLOAT(r_page_controls[control_index]); + if (control_index < PX4IO_CONTROL_CHANNELS && control_group < PX4IO_CONTROL_GROUPS ) { + control = REG_TO_FLOAT(r_page_controls[CONTROL_PAGE_INDEX(control_group, control_index)]); break; } return -1; case MIX_OVERRIDE: - if (r_page_rc_input[PX4IO_P_RC_VALID] & (1 << control_index)) { + if (r_page_rc_input[PX4IO_P_RC_VALID] & (1 << CONTROL_PAGE_INDEX(control_group, control_index))) { control = REG_TO_FLOAT(r_page_rc_input[PX4IO_P_RC_BASE + control_index]); break; } return -1; + case MIX_OVERRIDE_FMU_OK: + /* FMU is ok but we are in override mode, use direct rc control for the available rc channels. The remaining channels are still controlled by the fmu */ + if (r_page_rc_input[PX4IO_P_RC_VALID] & (1 << CONTROL_PAGE_INDEX(control_group, control_index))) { + control = REG_TO_FLOAT(r_page_rc_input[PX4IO_P_RC_BASE + control_index]); + break; + } else if (control_index < PX4IO_CONTROL_CHANNELS && control_group < PX4IO_CONTROL_GROUPS) { + control = REG_TO_FLOAT(r_page_controls[CONTROL_PAGE_INDEX(control_group, control_index)]); + break; + } + return -1; + case MIX_FAILSAFE: case MIX_NONE: control = 0.0f; diff --git a/src/modules/px4iofirmware/protocol.h b/src/modules/px4iofirmware/protocol.h index 11e380397..c10f0167c 100644 --- a/src/modules/px4iofirmware/protocol.h +++ b/src/modules/px4iofirmware/protocol.h @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * Copyright (c) 2012, 2013 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 @@ -63,7 +63,7 @@ * readable pages to be densely packed. Page numbers do not need to be * packed. * - * Definitions marked 1 are only valid on PX4IOv1 boards. Likewise, + * Definitions marked [1] are only valid on PX4IOv1 boards. Likewise, * [2] denotes definitions specific to the PX4IOv2 board. */ @@ -76,6 +76,9 @@ #define PX4IO_PROTOCOL_VERSION 4 +/* maximum allowable sizes on this protocol version */ +#define PX4IO_PROTOCOL_MAX_CONTROL_COUNT 8 /**< The protocol does not support more than set here, individual units might support less - see PX4IO_P_CONFIG_CONTROL_COUNT */ + /* static configuration page */ #define PX4IO_PAGE_CONFIG 0 #define PX4IO_P_CONFIG_PROTOCOL_VERSION 0 /* PX4IO_PROTOCOL_VERSION */ @@ -87,6 +90,7 @@ #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 /* hardcoded # of relay outputs */ +#define PX4IO_P_CONFIG_CONTROL_GROUP_COUNT 8 /**< hardcoded # of control groups*/ /* dynamic status page */ #define PX4IO_PAGE_STATUS 1 @@ -185,7 +189,7 @@ enum { /* DSM bind states */ dsm_bind_reinit_uart }; /* 8 */ -#define PX4IO_P_SETUP_SET_DEBUG 9 /* debug level for IO board */ +#define PX4IO_P_SETUP_SET_DEBUG 9 /* debug level for IO board */ #define PX4IO_P_SETUP_REBOOT_BL 10 /* reboot IO into bootloader */ #define PX4IO_REBOOT_BL_MAGIC 14662 /* required argument for reboot (random) */ @@ -193,41 +197,51 @@ enum { /* DSM bind states */ #define PX4IO_P_SETUP_CRC 11 /* get CRC of IO firmware */ /* autopilot control values, -10000..10000 */ -#define PX4IO_PAGE_CONTROLS 51 /* 0..CONFIG_CONTROL_COUNT */ +#define PX4IO_PAGE_CONTROLS 51 /**< actuator control groups, one after the other, 8 wide */ +#define PX4IO_P_CONTROLS_GROUP_0 (PX4IO_PROTOCOL_MAX_CONTROL_COUNT * 0) /**< 0..PX4IO_PROTOCOL_MAX_CONTROL_COUNT - 1 */ +#define PX4IO_P_CONTROLS_GROUP_1 (PX4IO_PROTOCOL_MAX_CONTROL_COUNT * 1) /**< 0..PX4IO_PROTOCOL_MAX_CONTROL_COUNT - 1 */ +#define PX4IO_P_CONTROLS_GROUP_2 (PX4IO_PROTOCOL_MAX_CONTROL_COUNT * 2) /**< 0..PX4IO_PROTOCOL_MAX_CONTROL_COUNT - 1 */ +#define PX4IO_P_CONTROLS_GROUP_3 (PX4IO_PROTOCOL_MAX_CONTROL_COUNT * 3) /**< 0..PX4IO_PROTOCOL_MAX_CONTROL_COUNT - 1 */ + +#define PX4IO_P_CONTROLS_GROUP_VALID 64 +#define PX4IO_P_CONTROLS_GROUP_VALID_GROUP0 (1 << 0) /* group 0 is valid / received */ +#define PX4IO_P_CONTROLS_GROUP_VALID_GROUP1 (1 << 1) /* group 1 is valid / received */ +#define PX4IO_P_CONTROLS_GROUP_VALID_GROUP2 (1 << 2) /* group 2 is valid / received */ +#define PX4IO_P_CONTROLS_GROUP_VALID_GROUP3 (1 << 3) /* group 3 is valid / received */ /* raw text load to the mixer parser - ignores offset */ -#define PX4IO_PAGE_MIXERLOAD 52 +#define PX4IO_PAGE_MIXERLOAD 52 /* R/C channel config */ -#define PX4IO_PAGE_RC_CONFIG 53 /* R/C input configuration */ -#define PX4IO_P_RC_CONFIG_MIN 0 /* lowest input value */ -#define PX4IO_P_RC_CONFIG_CENTER 1 /* center input value */ -#define PX4IO_P_RC_CONFIG_MAX 2 /* highest input value */ -#define PX4IO_P_RC_CONFIG_DEADZONE 3 /* band around center that is ignored */ -#define PX4IO_P_RC_CONFIG_ASSIGNMENT 4 /* mapped input value */ -#define PX4IO_P_RC_CONFIG_OPTIONS 5 /* channel options bitmask */ +#define PX4IO_PAGE_RC_CONFIG 53 /**< R/C input configuration */ +#define PX4IO_P_RC_CONFIG_MIN 0 /**< lowest input value */ +#define PX4IO_P_RC_CONFIG_CENTER 1 /**< center input value */ +#define PX4IO_P_RC_CONFIG_MAX 2 /**< highest input value */ +#define PX4IO_P_RC_CONFIG_DEADZONE 3 /**< band around center that is ignored */ +#define PX4IO_P_RC_CONFIG_ASSIGNMENT 4 /**< mapped input value */ +#define PX4IO_P_RC_CONFIG_OPTIONS 5 /**< channel options bitmask */ #define PX4IO_P_RC_CONFIG_OPTIONS_ENABLED (1 << 0) #define PX4IO_P_RC_CONFIG_OPTIONS_REVERSE (1 << 1) -#define PX4IO_P_RC_CONFIG_STRIDE 6 /* spacing between channel config data */ +#define PX4IO_P_RC_CONFIG_STRIDE 6 /**< spacing between channel config data */ /* PWM output - overrides mixer */ -#define PX4IO_PAGE_DIRECT_PWM 54 /* 0..CONFIG_ACTUATOR_COUNT-1 */ +#define PX4IO_PAGE_DIRECT_PWM 54 /**< 0..CONFIG_ACTUATOR_COUNT-1 */ /* PWM failsafe values - zero disables the output */ -#define PX4IO_PAGE_FAILSAFE_PWM 55 /* 0..CONFIG_ACTUATOR_COUNT-1 */ +#define PX4IO_PAGE_FAILSAFE_PWM 55 /**< 0..CONFIG_ACTUATOR_COUNT-1 */ /* Debug and test page - not used in normal operation */ -#define PX4IO_PAGE_TEST 127 -#define PX4IO_P_TEST_LED 0 /* set the amber LED on/off */ +#define PX4IO_PAGE_TEST 127 +#define PX4IO_P_TEST_LED 0 /**< set the amber LED on/off */ /* PWM minimum values for certain ESCs */ -#define PX4IO_PAGE_CONTROL_MIN_PWM 106 /* 0..CONFIG_ACTUATOR_COUNT-1 */ +#define PX4IO_PAGE_CONTROL_MIN_PWM 106 /**< 0..CONFIG_ACTUATOR_COUNT-1 */ /* PWM maximum values for certain ESCs */ -#define PX4IO_PAGE_CONTROL_MAX_PWM 107 /* 0..CONFIG_ACTUATOR_COUNT-1 */ +#define PX4IO_PAGE_CONTROL_MAX_PWM 107 /**< 0..CONFIG_ACTUATOR_COUNT-1 */ /* PWM disarmed values that are active, even when SAFETY_SAFE */ -#define PX4IO_PAGE_DISARMED_PWM 108 /* 0..CONFIG_ACTUATOR_COUNT-1 */ +#define PX4IO_PAGE_DISARMED_PWM 108 /* 0..CONFIG_ACTUATOR_COUNT-1 */ /** * As-needed mixer data upload. diff --git a/src/modules/px4iofirmware/px4io.h b/src/modules/px4iofirmware/px4io.h index b9c9e0232..2145f23b9 100644 --- a/src/modules/px4iofirmware/px4io.h +++ b/src/modules/px4iofirmware/px4io.h @@ -53,7 +53,9 @@ */ #define PX4IO_SERVO_COUNT 8 #define PX4IO_CONTROL_CHANNELS 8 -#define PX4IO_INPUT_CHANNELS 8 // XXX this should be 18 channels +#define PX4IO_CONTROL_GROUPS 2 +#define PX4IO_RC_INPUT_CHANNELS 8 // XXX this should be 18 channels +#define PX4IO_RC_MAPPED_CONTROL_CHANNELS 8 /**< This is the maximum number of channels mapped/used */ /* * Debug logging @@ -169,6 +171,8 @@ extern pwm_limit_t pwm_limit; #define BUTTON_SAFETY stm32_gpioread(GPIO_BTN_SAFETY) +#define CONTROL_PAGE_INDEX(_group, _channel) (_group * PX4IO_CONTROL_CHANNELS + _channel) + /* * Mixer */ diff --git a/src/modules/px4iofirmware/registers.c b/src/modules/px4iofirmware/registers.c index 3f9e111ba..916b893c4 100644 --- a/src/modules/px4iofirmware/registers.c +++ b/src/modules/px4iofirmware/registers.c @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * Copyright (c) 2012, 2013 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 @@ -70,7 +70,7 @@ static const uint16_t r_page_config[] = { [PX4IO_P_CONFIG_MAX_TRANSFER] = 64, /* XXX hardcoded magic number */ [PX4IO_P_CONFIG_CONTROL_COUNT] = PX4IO_CONTROL_CHANNELS, [PX4IO_P_CONFIG_ACTUATOR_COUNT] = PX4IO_SERVO_COUNT, - [PX4IO_P_CONFIG_RC_INPUT_COUNT] = PX4IO_CONTROL_CHANNELS, + [PX4IO_P_CONFIG_RC_INPUT_COUNT] = PX4IO_RC_INPUT_CHANNELS, [PX4IO_P_CONFIG_ADC_INPUT_COUNT] = PX4IO_ADC_CHANNEL_COUNT, [PX4IO_P_CONFIG_RELAY_COUNT] = PX4IO_RELAY_CHANNELS, }; @@ -114,7 +114,7 @@ uint16_t r_page_servos[PX4IO_SERVO_COUNT]; uint16_t r_page_raw_rc_input[] = { [PX4IO_P_RAW_RC_COUNT] = 0, - [PX4IO_P_RAW_RC_BASE ... (PX4IO_P_RAW_RC_BASE + 24)] = 0 // XXX ensure we have enough space to decode beefy RX, will be replaced by patch soon + [PX4IO_P_RAW_RC_BASE ... (PX4IO_P_RAW_RC_BASE + PX4IO_RC_INPUT_CHANNELS)] = 0 }; /** @@ -124,7 +124,7 @@ uint16_t r_page_raw_rc_input[] = */ uint16_t r_page_rc_input[] = { [PX4IO_P_RC_VALID] = 0, - [PX4IO_P_RC_BASE ... (PX4IO_P_RC_BASE + PX4IO_CONTROL_CHANNELS)] = 0 + [PX4IO_P_RC_BASE ... (PX4IO_P_RC_BASE + PX4IO_RC_MAPPED_CONTROL_CHANNELS)] = 0 }; /** @@ -176,7 +176,7 @@ volatile uint16_t r_page_setup[] = * * Control values from the FMU. */ -volatile uint16_t r_page_controls[PX4IO_CONTROL_CHANNELS]; +volatile uint16_t r_page_controls[PX4IO_CONTROL_GROUPS * PX4IO_CONTROL_CHANNELS]; /* * PAGE 102 does not have a buffer. @@ -187,7 +187,7 @@ volatile uint16_t r_page_controls[PX4IO_CONTROL_CHANNELS]; * * R/C channel input configuration. */ -uint16_t r_page_rc_input_config[PX4IO_CONTROL_CHANNELS * PX4IO_P_RC_CONFIG_STRIDE]; +uint16_t r_page_rc_input_config[PX4IO_RC_INPUT_CHANNELS * PX4IO_P_RC_CONFIG_STRIDE]; /* valid options */ #define PX4IO_P_RC_CONFIG_OPTIONS_VALID (PX4IO_P_RC_CONFIG_OPTIONS_REVERSE | PX4IO_P_RC_CONFIG_OPTIONS_ENABLED) @@ -239,7 +239,7 @@ registers_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num case PX4IO_PAGE_CONTROLS: /* copy channel data */ - while ((offset < PX4IO_CONTROL_CHANNELS) && (num_values > 0)) { + while ((offset < PX4IO_CONTROL_GROUPS * PX4IO_CONTROL_CHANNELS) && (num_values > 0)) { /* XXX range-check value? */ r_page_controls[offset] = *values; @@ -552,7 +552,7 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value) unsigned index = offset - channel * PX4IO_P_RC_CONFIG_STRIDE; uint16_t *conf = &r_page_rc_input_config[channel * PX4IO_P_RC_CONFIG_STRIDE]; - if (channel >= PX4IO_CONTROL_CHANNELS) + if (channel >= PX4IO_RC_INPUT_CHANNELS) return -1; /* disable the channel until we have a chance to sanity-check it */ @@ -600,7 +600,7 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value) if (conf[PX4IO_P_RC_CONFIG_DEADZONE] > 500) { count++; } - if (conf[PX4IO_P_RC_CONFIG_ASSIGNMENT] >= PX4IO_CONTROL_CHANNELS) { + if (conf[PX4IO_P_RC_CONFIG_ASSIGNMENT] >= PX4IO_RC_MAPPED_CONTROL_CHANNELS) { count++; } diff --git a/src/modules/px4iofirmware/sbus.c b/src/modules/px4iofirmware/sbus.c index 388502b40..6aca2bd11 100644 --- a/src/modules/px4iofirmware/sbus.c +++ b/src/modules/px4iofirmware/sbus.c @@ -252,7 +252,7 @@ sbus_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values, uint } /* decode switch channels if data fields are wide enough */ - if (PX4IO_INPUT_CHANNELS > 17 && chancount > 15) { + if (PX4IO_RC_INPUT_CHANNELS > 17 && chancount > 15) { chancount = 18; /* channel 17 (index 16) */ |