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

@@ -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;
 

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),

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);
     }
 }