From b46d05835b9ff55c1f21d37483202888eeea1656 Mon Sep 17 00:00:00 2001 From: px4dev Date: Fri, 25 Jan 2013 22:58:33 -0800 Subject: Implement settable failsafe values for PWM outputs. By default in failsafe mode, PWM output pulses are not generated. --- apps/px4io/i2c.c | 2 +- apps/px4io/mixer.cpp | 17 +++++++++++++---- apps/px4io/protocol.h | 5 ++++- apps/px4io/px4io.h | 1 + apps/px4io/registers.c | 29 +++++++++++++++++++++++++---- 5 files changed, 44 insertions(+), 10 deletions(-) diff --git a/apps/px4io/i2c.c b/apps/px4io/i2c.c index dca5ceb1d..bbbbb6e46 100644 --- a/apps/px4io/i2c.c +++ b/apps/px4io/i2c.c @@ -82,7 +82,7 @@ static unsigned rx_len; static const uint8_t junk_buf[] = { 0xff, 0xff, 0xff, 0xff }; -static uint8_t *tx_buf = junk_buf; +static const uint8_t *tx_buf = junk_buf; static unsigned tx_len = sizeof(junk_buf); static uint8_t selected_page; diff --git a/apps/px4io/mixer.cpp b/apps/px4io/mixer.cpp index 1c7a05343..91a914575 100644 --- a/apps/px4io/mixer.cpp +++ b/apps/px4io/mixer.cpp @@ -92,7 +92,7 @@ mixer_tick(void) /* too long without FMU input, time to go to failsafe */ r_status_flags |= PX4IO_P_STATUS_FLAGS_OVERRIDE; - r_status_flags &= ~(PX4IO_P_STATUS_FLAGS_FMU_OK | PX4IO_P_STATUS_FLAGS_RAW_PPM); + r_status_flags &= ~(PX4IO_P_STATUS_FLAGS_FMU_OK | PX4IO_P_STATUS_FLAGS_RAW_PWM); r_status_alarms |= PX4IO_P_STATUS_ALARMS_FMU_LOST; debug("AP RX timeout"); } @@ -102,7 +102,7 @@ mixer_tick(void) /* * Decide which set of controls we're using. */ - if (r_status_flags & PX4IO_P_STATUS_FLAGS_RAW_PPM) { + if (r_status_flags & PX4IO_P_STATUS_FLAGS_RAW_PWM) { /* don't actually mix anything - we already have raw PWM values */ source = MIX_NONE; @@ -127,7 +127,13 @@ mixer_tick(void) /* * Run the mixers. */ - if (source != MIX_NONE) { + if (source == MIX_FAILSAFE) { + + /* copy failsafe values to the servo outputs */ + for (unsigned i = 0; i < IO_SERVO_COUNT; i++) + r_page_servos[i] = r_page_servo_failsafe[i]; + + } else if (source != MIX_NONE) { float outputs[IO_SERVO_COUNT]; unsigned mixed; @@ -225,9 +231,12 @@ mixer_tick(void) * * We must be armed, and we must have a PWM source; either raw from * FMU or from the mixer. + * + * XXX correct behaviour for failsafe may require an additional case + * here. */ bool should_arm = ((r_status_flags & PX4IO_P_STATUS_FLAGS_ARMED) && - (r_status_flags & (PX4IO_P_STATUS_FLAGS_RAW_PPM | PX4IO_P_STATUS_FLAGS_MIXER_OK))); + (r_status_flags & (PX4IO_P_STATUS_FLAGS_RAW_PWM | PX4IO_P_STATUS_FLAGS_MIXER_OK))); if (should_arm && !mixer_servos_armed) { /* need to arm, but not armed */ diff --git a/apps/px4io/protocol.h b/apps/px4io/protocol.h index 39ee4c0b1..6dbe16229 100644 --- a/apps/px4io/protocol.h +++ b/apps/px4io/protocol.h @@ -100,7 +100,7 @@ #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_FLAGS_FMU_OK (1 << 6) /* controls from FMU are valid */ -#define PX4IO_P_STATUS_FLAGS_RAW_PPM (1 << 7) /* raw PPM from FMU is bypassing the mixer */ +#define PX4IO_P_STATUS_FLAGS_RAW_PWM (1 << 7) /* raw PWM from FMU is bypassing the mixer */ #define PX4IO_P_STATUS_FLAGS_MIXER_OK (1 << 8) /* mixer is OK */ #define PX4IO_P_STATUS_ALARMS 3 /* alarm flags - alarms latch, write 1 to a bit to clear it */ @@ -172,6 +172,9 @@ /* PWM output - overrides mixer */ #define PX4IO_PAGE_DIRECT_PWM 104 /* 0..CONFIG_ACTUATOR_COUNT-1 */ +/* PWM failsafe values - zero disables the output */ +#define PX4IO_PAGE_FAILSAFE_PWM 105 /* 0..CONFIG_ACTUATOR_COUNT-1 */ + /** * As-needed mixer data upload. * diff --git a/apps/px4io/px4io.h b/apps/px4io/px4io.h index d8cdafb4b..c90db5d78 100644 --- a/apps/px4io/px4io.h +++ b/apps/px4io/px4io.h @@ -76,6 +76,7 @@ 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 */ extern uint16_t r_page_rc_input_config[]; /* PX4IO_PAGE_RC_INPUT_CONFIG */ +extern uint16_t r_page_servo_failsafe[]; /* PX4IO_PAGE_FAILSAFE_PWM */ /* * Register aliases. diff --git a/apps/px4io/registers.c b/apps/px4io/registers.c index a4d81fd6e..22ed1de70 100644 --- a/apps/px4io/registers.c +++ b/apps/px4io/registers.c @@ -118,6 +118,11 @@ uint16_t r_page_actuators[IO_SERVO_COUNT]; */ uint16_t r_page_servos[IO_SERVO_COUNT]; +/** + * Servo PWM values + */ +uint16_t r_page_servo_failsafe[IO_SERVO_COUNT]; + /** * Scaled/routed RC input */ @@ -166,7 +171,7 @@ registers_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num /* XXX we should cause a mixer tick ASAP */ system_state.fmu_data_received_time = hrt_absolute_time(); r_status_flags |= PX4IO_P_STATUS_FLAGS_FMU_OK; - r_status_flags &= ~PX4IO_P_STATUS_FLAGS_RAW_PPM; + r_status_flags &= ~PX4IO_P_STATUS_FLAGS_RAW_PWM; break; /* handle raw PWM input */ @@ -183,9 +188,24 @@ registers_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num values++; } - /* XXX need to force these values to the servos */ + /* XXX we should cause a mixer tick ASAP */ system_state.fmu_data_received_time = hrt_absolute_time(); - r_status_flags |= PX4IO_P_STATUS_FLAGS_FMU_OK | PX4IO_P_STATUS_FLAGS_RAW_PPM; + r_status_flags |= PX4IO_P_STATUS_FLAGS_FMU_OK | PX4IO_P_STATUS_FLAGS_RAW_PWM; + break; + + /* handle setup for servo failsafe values */ + case PX4IO_PAGE_FAILSAFE_PWM: + + /* copy channel data */ + while ((offset < PX4IO_CONTROL_CHANNELS) && (num_values > 0)) { + + /* XXX range-check value? */ + r_page_servo_failsafe[offset] = *values; + + offset++; + num_values--; + values++; + } break; /* handle text going to the mixer parser */ @@ -353,7 +373,7 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value) int registers_get(uint8_t page, uint8_t offset, uint16_t **values, unsigned *num_values) { -#define SELECT_PAGE(_page_name) { *values = _page_name; *num_values = sizeof(_page_name) / sizeof(_page_name[0]); } +#define SELECT_PAGE(_page_name) { *values = (uint16_t *)_page_name; *num_values = sizeof(_page_name) / sizeof(_page_name[0]); } switch (page) { @@ -441,6 +461,7 @@ registers_get(uint8_t page, uint8_t offset, uint16_t **values, unsigned *num_val COPY_PAGE(PX4IO_PAGE_SETUP, r_page_setup); COPY_PAGE(PX4IO_PAGE_CONTROLS, r_page_controls); COPY_PAGE(PX4IO_PAGE_RC_CONFIG, r_page_rc_input_config); + COPY_PAGE(PX4IO_PAGE_FAILSAFE_PWM, r_page_servo_failsafe); default: return -1; -- cgit v1.2.3