Rover: FS_OPTION allows failsafe from hold mode

APMrover2/Parameters.cpp

@@ -620,6 +620,14 @@ const AP_Param::GroupInfo ParametersG2::var_info[] = {
     // @User: Advanced
     AP_GROUPINFO("LOIT_SPEED_GAIN", 47, ParametersG2, loiter_speed_gain, 0.5f),
 
+    // @Param: FS_OPTIONS
+    // @DisplayName: Rover Failsafe Options
+    // @Description: Bitmask to enable Rover failsafe options
+    // @Values: 0:None,1:Failsafe enabled in Hold mode
+    // @Bitmask: 0:Failsafe enabled in Hold mode
+    // @User: Advanced
+    AP_GROUPINFO("FS_OPTIONS", 48, ParametersG2, fs_options, 0),
+
     AP_GROUPEND
 };
 

APMrover2/Parameters.h

@@ -394,6 +394,9 @@ public:
 
     // gain for speed of correction in loiter
     AP_Float loiter_speed_gain;
+
+    // FS options
+    AP_Int32 fs_options;
 };
 
 extern const AP_Param::Info var_info[];

APMrover2/Rover.h

@@ -462,6 +462,10 @@ private:
         Failsafe_Action_Terminate     = 5
     };
 
+    enum class Failsafe_Options : uint32_t {
+        Failsafe_Option_Active_In_Hold = (1<<0)
+    };
+
     static constexpr int8_t _failsafe_priorities[] = {
                                                        Failsafe_Action_Terminate,
                                                        Failsafe_Action_Hold,

APMrover2/failsafe.cpp

@@ -68,7 +68,7 @@ void Rover::failsafe_trigger(uint8_t failsafe_type, bool on)
         failsafe.bits != 0 &&
         millis() - failsafe.start_time > g.fs_timeout * 1000 &&
         control_mode != &mode_rtl &&
-        control_mode != &mode_hold) {
+        ((control_mode != &mode_hold || (g2.fs_options & (uint32_t)Failsafe_Options::Failsafe_Option_Active_In_Hold)))) {
         failsafe.triggered = failsafe.bits;
         gcs().send_text(MAV_SEVERITY_WARNING, "Failsafe trigger 0x%x", (unsigned int)failsafe.triggered);