From 6e288551abc4422b8223ac8e5cb146e1c926bfa6 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sat, 20 Jul 2013 17:08:58 +1000 Subject: [PATCH] Plane: fixed reversion to pre-failsafe mode if AUTO was entered using GCS, we need to switch back to the old mode, not control channel --- ArduPlane/ArduPlane.pde | 16 +++++++++++----- ArduPlane/Parameters.pde | 2 +- ArduPlane/events.pde | 10 +++++++--- 3 files changed, 19 insertions(+), 9 deletions(-) diff --git a/ArduPlane/ArduPlane.pde b/ArduPlane/ArduPlane.pde index 7f89e9ebf8..49716297e2 100644 --- a/ArduPlane/ArduPlane.pde +++ b/ArduPlane/ArduPlane.pde @@ -348,16 +348,22 @@ static struct { //////////////////////////////////////////////////////////////////////////////// static struct { // A flag if GCS joystick control is in use - bool rc_override_active; + uint8_t rc_override_active:1; + + // Used to track if the value on channel 3 (throtttle) has fallen below the failsafe threshold + // RC receiver should be set up to output a low throttle value when signal is lost + uint8_t ch3_failsafe:1; + + // has the saved mode for failsafe been set? + uint8_t saved_mode_set:1; + + // saved flight mode + enum FlightMode saved_mode; // A tracking variable for type of failsafe active // Used for failsafe based on loss of RC signal or GCS signal int16_t state; - // Used to track if the value on channel 3 (throtttle) has fallen below the failsafe threshold - // RC receiver should be set up to output a low throttle value when signal is lost - bool ch3_failsafe; - // number of low ch3 values uint8_t ch3_counter; diff --git a/ArduPlane/Parameters.pde b/ArduPlane/Parameters.pde index 1f6d3db067..da901a1d5d 100644 --- a/ArduPlane/Parameters.pde +++ b/ArduPlane/Parameters.pde @@ -360,7 +360,7 @@ const AP_Param::Info var_info[] PROGMEM = { // @Param: FS_SHORT_ACTN // @DisplayName: Short failsafe action - // @Description: The action to take on a short (FS_SHORT_TIMEOUT) failsafe event in AUTO, GUIDED or LOITER modes. A short failsafe event in stabilization modes will always cause an immediate change to CIRCLE mode. In AUTO mode you can choose whether it will RTL (ReturnToLaunch) or continue with the mission. If FS_SHORT_ACTN is 0 then it will continue with the mission, if it is 1 then it will enter CIRCLE mode, and then enter RTL if the failsafe condition persists for FS_LONG_TIMEOUT seconds. + // @Description: The action to take on a short (FS_SHORT_TIMEOUT) failsafe event in AUTO, GUIDED or LOITER modes. A short failsafe event in stabilization modes will always cause an immediate change to CIRCLE mode. In AUTO mode you can choose whether it will enter CIRCLE mode or continue with the mission. If FS_SHORT_ACTN is 0 then it will continue with the mission, if it is 1 then it will enter CIRCLE mode, and then enter RTL if the failsafe condition persists for FS_LONG_TIMEOUT seconds. // @Values: 0:Continue,1:Circle/ReturnToLaunch // @User: Standard GSCALAR(short_fs_action, "FS_SHORT_ACTN", SHORT_FAILSAFE_ACTION), diff --git a/ArduPlane/events.pde b/ArduPlane/events.pde index 5a1f08f5cc..7cf2dc9540 100644 --- a/ArduPlane/events.pde +++ b/ArduPlane/events.pde @@ -16,6 +16,8 @@ static void failsafe_short_on_event(enum failsafe_state fstype) case FLY_BY_WIRE_B: case CRUISE: case TRAINING: + failsafe.saved_mode = control_mode; + failsafe.saved_mode_set = 1; set_mode(CIRCLE); break; @@ -23,6 +25,8 @@ static void failsafe_short_on_event(enum failsafe_state fstype) case GUIDED: case LOITER: if(g.short_fs_action == 1) { + failsafe.saved_mode = control_mode; + failsafe.saved_mode_set = 1; set_mode(CIRCLE); } break; @@ -78,9 +82,9 @@ static void failsafe_short_off_event() // re-read the switch so we can return to our preferred mode // -------------------------------------------------------- - if (control_mode == CIRCLE || - (g.short_fs_action == 1 && control_mode == RTL)) { - reset_control_switch(); + if (control_mode == CIRCLE && failsafe.saved_mode_set) { + failsafe.saved_mode_set = 0; + set_mode(failsafe.saved_mode); } }