From ae98836db8948edbcf59333627b25f69df4127d4 Mon Sep 17 00:00:00 2001 From: Simon Wilks Date: Wed, 6 Mar 2013 20:37:01 +0100 Subject: Correct RC config sanity checking and report back when RC config errors occur. --- apps/px4io/mixer.cpp | 8 ++++--- apps/px4io/protocol.h | 1 + apps/px4io/px4io.c | 3 +++ apps/px4io/registers.c | 57 +++++++++++++++++++++++++++++++++++--------------- apps/px4io/safety.c | 7 ++++++- 5 files changed, 55 insertions(+), 21 deletions(-) (limited to 'apps') diff --git a/apps/px4io/mixer.cpp b/apps/px4io/mixer.cpp index ed74cb3d3..0fba2cbe5 100644 --- a/apps/px4io/mixer.cpp +++ b/apps/px4io/mixer.cpp @@ -170,9 +170,11 @@ mixer_tick(void) * XXX correct behaviour for failsafe may require an additional case * here. */ - bool should_arm = (/* FMU is armed */ (r_setup_arming & PX4IO_P_SETUP_ARMING_ARM_OK) && - /* IO is armed */ (r_status_flags & PX4IO_P_STATUS_FLAGS_ARMED) && - /* there is valid input */ (r_status_flags & (PX4IO_P_STATUS_FLAGS_RAW_PWM | PX4IO_P_STATUS_FLAGS_MIXER_OK))); + bool should_arm = ( + /* FMU is armed */ (r_setup_arming & PX4IO_P_SETUP_ARMING_ARM_OK) && + /* IO is armed */ (r_status_flags & PX4IO_P_STATUS_FLAGS_ARMED) && + /* there is valid input */ (r_status_flags & (PX4IO_P_STATUS_FLAGS_RAW_PWM | PX4IO_P_STATUS_FLAGS_MIXER_OK)) && + /* IO initialised without error */ (r_status_flags & PX4IO_P_STATUS_FLAGS_INIT_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 0ee6d2c4b..14d221b5b 100644 --- a/apps/px4io/protocol.h +++ b/apps/px4io/protocol.h @@ -103,6 +103,7 @@ #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_FLAGS_ARM_SYNC (1 << 9) /* the arming state between IO and FMU is in sync */ +#define PX4IO_P_STATUS_FLAGS_INIT_OK (1 << 10) /* initialisation of the IO completed without error */ #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 */ diff --git a/apps/px4io/px4io.c b/apps/px4io/px4io.c index 589264661..a0e0002a6 100644 --- a/apps/px4io/px4io.c +++ b/apps/px4io/px4io.c @@ -153,6 +153,9 @@ user_start(int argc, char *argv[]) /* configure the first 8 PWM outputs (i.e. all of them) */ up_pwm_servo_init(0xff); + /* initialise the registry space */ + registers_init(); + /* initialise the control inputs */ controls_init(); diff --git a/apps/px4io/registers.c b/apps/px4io/registers.c index 5ec2f7258..dac09021d 100644 --- a/apps/px4io/registers.c +++ b/apps/px4io/registers.c @@ -179,6 +179,12 @@ uint16_t r_page_rc_input_config[MAX_CONTROL_CHANNELS * PX4IO_P_RC_CONFIG_STRIDE */ uint16_t r_page_servo_failsafe[IO_SERVO_COUNT]; +void +registers_init(void) +{ + r_status_flags |= PX4IO_P_STATUS_FLAGS_INIT_OK; +} + void registers_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num_values) { @@ -390,27 +396,44 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value) /* should the channel be enabled? */ /* this option is normally set last */ if (value & PX4IO_P_RC_CONFIG_OPTIONS_ENABLED) { + uint8_t count = 0; + /* assert min..center..max ordering */ - if (conf[PX4IO_P_RC_CONFIG_MIN] < 500) - break; - if (conf[PX4IO_P_RC_CONFIG_MAX] > 2500) - break; - if (conf[PX4IO_P_RC_CONFIG_CENTER] < conf[PX4IO_P_RC_CONFIG_MIN]) - break; - if (conf[PX4IO_P_RC_CONFIG_CENTER] > conf[PX4IO_P_RC_CONFIG_MAX]) - break; + if (conf[PX4IO_P_RC_CONFIG_MIN] < 500) { + count++; + } + if (conf[PX4IO_P_RC_CONFIG_MAX] > 2500) { + count++; + } + if (conf[PX4IO_P_RC_CONFIG_CENTER] < conf[PX4IO_P_RC_CONFIG_MIN]) { + count++; + } + if (conf[PX4IO_P_RC_CONFIG_CENTER] > conf[PX4IO_P_RC_CONFIG_MAX]) { + count++; + } + /* assert deadzone is sane */ - if (conf[PX4IO_P_RC_CONFIG_DEADZONE] > 500) - break; - if (conf[PX4IO_P_RC_CONFIG_MIN] > (conf[PX4IO_P_RC_CONFIG_CENTER] - conf[PX4IO_P_RC_CONFIG_DEADZONE])) - break; - if (conf[PX4IO_P_RC_CONFIG_MAX] < (conf[PX4IO_P_RC_CONFIG_CENTER] + conf[PX4IO_P_RC_CONFIG_DEADZONE])) - break; - if (conf[PX4IO_P_RC_CONFIG_ASSIGNMENT] >= MAX_CONTROL_CHANNELS) - break; + if (conf[PX4IO_P_RC_CONFIG_DEADZONE] > 500) { + count++; + } + // The following check isn't needed as constraint checks in controls.c will catch this. + //if (conf[PX4IO_P_RC_CONFIG_MIN] > (conf[PX4IO_P_RC_CONFIG_CENTER] - conf[PX4IO_P_RC_CONFIG_DEADZONE])) { + // count++; + //} + //if (conf[PX4IO_P_RC_CONFIG_MAX] < (conf[PX4IO_P_RC_CONFIG_CENTER] + conf[PX4IO_P_RC_CONFIG_DEADZONE])) { + // count++; + //} + if (conf[PX4IO_P_RC_CONFIG_ASSIGNMENT] >= MAX_CONTROL_CHANNELS) { + count++; + } /* sanity checks pass, enable channel */ - conf[index] |= PX4IO_P_RC_CONFIG_OPTIONS_ENABLED; + if (count) { + isr_debug(0, "ERROR: %d config error(s) for RC%d.\n", count, (channel + 1)); + r_status_flags &= ~PX4IO_P_STATUS_FLAGS_INIT_OK; + } else { + conf[index] |= PX4IO_P_RC_CONFIG_OPTIONS_ENABLED; + } } break; /* inner switch: case PX4IO_P_RC_CONFIG_OPTIONS */ diff --git a/apps/px4io/safety.c b/apps/px4io/safety.c index 540636e19..5495d5ae1 100644 --- a/apps/px4io/safety.c +++ b/apps/px4io/safety.c @@ -176,12 +176,17 @@ heartbeat_blink(void *arg) static void failsafe_blink(void *arg) { + /* indicate that a serious initialisation error occured */ + if (!(r_status_flags & PX4IO_P_STATUS_FLAGS_INIT_OK)) { + LED_AMBER(true); + return; + } + static bool failsafe = false; /* blink the failsafe LED if we don't have FMU input */ if (!(r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK)) { failsafe = !failsafe; - } else { failsafe = false; } -- cgit v1.2.3