Rover: add continue-in-auto option to failsafe

APMrover2/Parameters.cpp

@@ -144,9 +144,9 @@ const AP_Param::Info Rover::var_info[] = {
     // @Param: FS_THR_ENABLE
     // @DisplayName: Throttle Failsafe Enable
     // @Description: The throttle failsafe allows you to configure a software failsafe activated by a setting on the throttle input channel to a low value. This can be used to detect the RC transmitter going out of range. Failsafe will be triggered when the throttle channel goes below the FS_THR_VALUE for FS_TIMEOUT seconds.
-    // @Values: 0:Disabled,1:Enabled
+    // @Values: 0:Disabled,1:Enabled,2:Enabled Continue with Mission in Auto
     // @User: Standard
-    GSCALAR(fs_throttle_enabled,    "FS_THR_ENABLE",     1),
+    GSCALAR(fs_throttle_enabled,    "FS_THR_ENABLE",     FS_THR_ENABLED),
 
     // @Param: FS_THR_VALUE
     // @DisplayName: Throttle Failsafe Value
@@ -159,9 +159,9 @@ const AP_Param::Info Rover::var_info[] = {
     // @Param: FS_GCS_ENABLE
     // @DisplayName: GCS failsafe enable
     // @Description: Enable ground control station telemetry failsafe. When enabled the Rover will execute the FS_ACTION when it fails to receive MAVLink heartbeat packets for FS_TIMEOUT seconds.
-    // @Values: 0:Disabled,1:Enabled
+    // @Values: 0:Disabled,1:Enabled,2:Enabled Continue with Mission in Auto
     // @User: Standard
-    GSCALAR(fs_gcs_enabled, "FS_GCS_ENABLE",   0),
+    GSCALAR(fs_gcs_enabled, "FS_GCS_ENABLE",   FS_GCS_DISABLED),
 
     // @Param: FS_CRASH_CHECK
     // @DisplayName: Crash check action

APMrover2/defines.h

@@ -77,6 +77,20 @@
 // subsystem specific error codes -- crash checker
 #define ERROR_CODE_CRASH_CHECK_CRASH 1
 
+// radio failsafe enum (FS_THR_ENABLE parameter)
+enum fs_thr_enable {
+    FS_THR_DISABLED = 0,
+    FS_THR_ENABLED,
+    FS_THR_ENABLED_CONTINUE_MISSION,
+};
+
+// gcs failsafe enum (FS_GCS_ENABLE parameter)
+enum fs_gcs_enable {
+    FS_GCS_DISABLED = 0,
+    FS_GCS_ENABLED,
+    FS_GCS_ENABLED_CONTINUE_MISSION,
+};
+
 enum fs_crash_action {
   FS_CRASH_DISABLE = 0,
   FS_CRASH_HOLD = 1,

APMrover2/failsafe.cpp

@@ -73,7 +73,12 @@ void Rover::failsafe_trigger(uint8_t failsafe_type, bool on)
         // clear rc overrides
         RC_Channels::clear_overrides();
 
-        switch (g.fs_action) {
+        if ((control_mode == &mode_auto) &&
+            ((failsafe_type == FAILSAFE_EVENT_THROTTLE && g.fs_throttle_enabled == FS_THR_ENABLED_CONTINUE_MISSION) ||
+             (failsafe_type == FAILSAFE_EVENT_GCS && g.fs_gcs_enabled == FS_GCS_ENABLED_CONTINUE_MISSION))) {
+            // continue with mission in auto mode
+        } else {
+            switch (g.fs_action) {
             case Failsafe_Action_None:
                 break;
             case Failsafe_Action_RTL:
@@ -96,6 +101,7 @@ void Rover::failsafe_trigger(uint8_t failsafe_type, bool on)
                     set_mode(mode_hold, MODE_REASON_FAILSAFE);
                 }
                 break;
+            }
         }
     }
 }