From 79a1b84b09d5e731ea93129d84481e03325c8b08 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Wed, 13 Jul 2016 15:05:36 +0200 Subject: [PATCH] px4iofirmware: clean up override decision The override checking was scattered across two places and is now unified in controls_tick(). The part in mixer_tick only decides which mixer (or none) to use give the override flag. --- src/modules/px4iofirmware/controls.c | 8 +++++++- src/modules/px4iofirmware/mixer.cpp | 27 +++++++++------------------ 2 files changed, 16 insertions(+), 19 deletions(-) diff --git a/src/modules/px4iofirmware/controls.c b/src/modules/px4iofirmware/controls.c index 256e2d0902..17d18b66ae 100644 --- a/src/modules/px4iofirmware/controls.c +++ b/src/modules/px4iofirmware/controls.c @@ -486,6 +486,7 @@ controls_tick() if ((r_setup_arming & PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK) && (r_status_flags & PX4IO_P_STATUS_FLAGS_RC_OK) && !(r_raw_rc_flags & PX4IO_P_RAW_RC_FLAGS_FAILSAFE) && + !(r_setup_arming & PX4IO_P_SETUP_ARMING_RC_HANDLING_DISABLED) && (r_status_flags & PX4IO_P_STATUS_FLAGS_MIXER_OK)) { bool override = false; @@ -506,9 +507,14 @@ controls_tick() * If the FMU is dead then enable override if we have a mixer * and we want to immediately override (instead of using the RC channel * as in the case above. + * + * Also, do not enter manual override if we asked for termination + * failsafe and FMU is lost. */ if (!(r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK) && - (r_setup_arming & PX4IO_P_SETUP_ARMING_OVERRIDE_IMMEDIATE)) { + (r_setup_arming & PX4IO_P_SETUP_ARMING_OVERRIDE_IMMEDIATE) && + !(r_setup_arming & PX4IO_P_SETUP_ARMING_TERMINATION_FAILSAFE) + ) { override = true; } diff --git a/src/modules/px4iofirmware/mixer.cpp b/src/modules/px4iofirmware/mixer.cpp index 9c2dfdc5aa..5be37bba21 100644 --- a/src/modules/px4iofirmware/mixer.cpp +++ b/src/modules/px4iofirmware/mixer.cpp @@ -121,16 +121,10 @@ mixer_tick(void) * Decide which set of controls we're using. */ - bool override_enabled = ((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)); - - /* do not mix if RAW_PWM mode is on and FMU is good */ + /* Do not mix if we have raw PWM and FMU is ok. */ if ((r_status_flags & PX4IO_P_STATUS_FLAGS_RAW_PWM) && (r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK)) { - /* don't actually mix anything - we already have raw PWM values */ source = MIX_NONE; } else { @@ -143,19 +137,16 @@ mixer_tick(void) source = MIX_FMU; } - if (override_enabled && - !(r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK) && - /* do not enter manual override if we asked for termination failsafe and FMU is lost */ - !(r_setup_arming & PX4IO_P_SETUP_ARMING_TERMINATION_FAILSAFE)) { + else if (r_status_flags & PX4IO_P_STATUS_FLAGS_OVERRIDE) { - /* if allowed, mix from RC inputs directly */ - source = MIX_OVERRIDE; + if (r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK) { - } else if (override_enabled && - (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; + /* if allowed, mix from RC inputs directly up to available rc channels */ + source = MIX_OVERRIDE_FMU_OK; + } else { + /* if allowed, mix from RC inputs directly */ + source = MIX_OVERRIDE; + } } }