From 5e2093b1051e1b48f48b025b27d1556a654bb7ed Mon Sep 17 00:00:00 2001
From: Michael du Breuil <wicked.shell.scripts@gmail.com>
Date: Wed, 11 Nov 2020 17:46:06 -0700
Subject: [PATCH] Copter: Allow the user to specify the GCS failsafe timeout

---
 ArduCopter/Parameters.cpp | 9 +++++++++
 ArduCopter/Parameters.h   | 2 ++
 ArduCopter/config.h       | 3 ---
 ArduCopter/events.cpp     | 9 +++++----
 4 files changed, 16 insertions(+), 7 deletions(-)

diff --git a/ArduCopter/Parameters.cpp b/ArduCopter/Parameters.cpp
index c518a0e4f9..e8df9c8195 100644
--- a/ArduCopter/Parameters.cpp
+++ b/ArduCopter/Parameters.cpp
@@ -994,6 +994,15 @@ const AP_Param::GroupInfo ParametersG2::var_info[] = {
     AP_GROUPINFO("GUID_OPTIONS", 41, ParametersG2, guided_options, 0),
 #endif
 
+    // @Param: FS_GCS_TIMEOUT
+    // @DisplayName: GCS failsafe timeout
+    // @Description: Timeout before triggering the GCS failsafe
+    // @Units: s
+    // @Range: 2 120
+    // @Increment: 1
+    // @User: Standard
+    AP_GROUPINFO("FS_GCS_TIMEOUT", 42, ParametersG2, fs_gcs_timeout, 5),
+
     AP_GROUPEND
 };
 
diff --git a/ArduCopter/Parameters.h b/ArduCopter/Parameters.h
index 9c8f67ecc2..24b3378eb9 100644
--- a/ArduCopter/Parameters.h
+++ b/ArduCopter/Parameters.h
@@ -627,6 +627,8 @@ public:
     AP_Int32 guided_options;
 #endif
 
+    AP_Float fs_gcs_timeout;
+
 };
 
 extern const AP_Param::Info        var_info[];
diff --git a/ArduCopter/config.h b/ArduCopter/config.h
index 503443235b..d4ab508115 100644
--- a/ArduCopter/config.h
+++ b/ArduCopter/config.h
@@ -148,9 +148,6 @@
 #ifndef FS_GCS
  # define FS_GCS                        DISABLED
 #endif
-#ifndef FS_GCS_TIMEOUT_MS
- # define FS_GCS_TIMEOUT_MS             5000    // gcs failsafe triggers after 5 seconds with no GCS heartbeat
-#endif
 
 // Radio failsafe while using RC_override
 #ifndef FS_RADIO_RC_OVERRIDE_TIMEOUT_MS
diff --git a/ArduCopter/events.cpp b/ArduCopter/events.cpp
index 554970ea84..2dfc68e787 100644
--- a/ArduCopter/events.cpp
+++ b/ArduCopter/events.cpp
@@ -119,20 +119,21 @@ void Copter::failsafe_gcs_check()
     // calc time since last gcs update
     // note: this only looks at the heartbeat from the device id set by g.sysid_my_gcs
     const uint32_t last_gcs_update_ms = millis() - failsafe.last_heartbeat_ms;
+    const uint32_t gcs_timeout_ms = uint32_t(constrain_float(g2.fs_gcs_timeout * 1000.0f, 0.0f, UINT32_MAX));
 
     // Determine which event to trigger
-    if (last_gcs_update_ms < FS_GCS_TIMEOUT_MS && failsafe.gcs) {
+    if (last_gcs_update_ms < gcs_timeout_ms && failsafe.gcs) {
         // Recovery from a GCS failsafe
         set_failsafe_gcs(false);
         failsafe_gcs_off_event();
 
-    } else if (last_gcs_update_ms < FS_GCS_TIMEOUT_MS && !failsafe.gcs) {
+    } else if (last_gcs_update_ms < gcs_timeout_ms && !failsafe.gcs) {
         // No problem, do nothing
 
-    } else if (last_gcs_update_ms > FS_GCS_TIMEOUT_MS && failsafe.gcs) {
+    } else if (last_gcs_update_ms > gcs_timeout_ms && failsafe.gcs) {
         // Already in failsafe, do nothing
 
-    } else if (last_gcs_update_ms > FS_GCS_TIMEOUT_MS && !failsafe.gcs) {
+    } else if (last_gcs_update_ms > gcs_timeout_ms && !failsafe.gcs) {
         // New GCS failsafe event, trigger events
         set_failsafe_gcs(true);
         failsafe_gcs_on_event();