From 006004be13e52dcc79df76072629dae79ff31a41 Mon Sep 17 00:00:00 2001
From: Henry Wurzburg <hwurzburg@yahoo.com>
Date: Sun, 29 Sep 2019 21:32:07 -0500
Subject: [PATCH] Rover: FS_OPTION allows failsafe from hold mode

---
 APMrover2/Parameters.cpp | 8 ++++++++
 APMrover2/Parameters.h   | 3 +++
 APMrover2/Rover.h        | 4 ++++
 APMrover2/failsafe.cpp   | 2 +-
 4 files changed, 16 insertions(+), 1 deletion(-)

diff --git a/APMrover2/Parameters.cpp b/APMrover2/Parameters.cpp
index adba118219..27c18aadf5 100644
--- a/APMrover2/Parameters.cpp
+++ b/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
 };
 
diff --git a/APMrover2/Parameters.h b/APMrover2/Parameters.h
index 9ca93f4be1..3bb03178c2 100644
--- a/APMrover2/Parameters.h
+++ b/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[];
diff --git a/APMrover2/Rover.h b/APMrover2/Rover.h
index baf4e59f57..61851aad76 100644
--- a/APMrover2/Rover.h
+++ b/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,
diff --git a/APMrover2/failsafe.cpp b/APMrover2/failsafe.cpp
index 3a7903d6ee..b43853ae82 100644
--- a/APMrover2/failsafe.cpp
+++ b/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);