From 9e0fd1c9d5929d23e1eca0bafa60319399c2d8c1 Mon Sep 17 00:00:00 2001
From: Willian Galvani <williangalvani@gmail.com>
Date: Tue, 9 Jul 2024 19:40:58 -0300
Subject: [PATCH] Sub: make gcs failsafe timeout a parameter

---
 ArduSub/Parameters.cpp | 8 ++++++++
 ArduSub/Parameters.h   | 2 ++
 ArduSub/defines.h      | 4 ++--
 ArduSub/failsafe.cpp   | 3 ++-
 4 files changed, 14 insertions(+), 3 deletions(-)

diff --git a/ArduSub/Parameters.cpp b/ArduSub/Parameters.cpp
index b2e319088a77d..d76a3521446be 100644
--- a/ArduSub/Parameters.cpp
+++ b/ArduSub/Parameters.cpp
@@ -78,6 +78,14 @@ const AP_Param::Info Sub::var_info[] = {
     // @User: Standard
     GSCALAR(failsafe_gcs, "FS_GCS_ENABLE", FS_GCS_DISARM),
 
+    // @Param: FS_GCS_TIMEOUT
+    // @DisplayName: GCS failsafe timeout
+    // @Description: Timeout before triggering the GCS failsafe
+    // @Units: s
+    // @Increment: 1
+    // @User: Standard
+    GSCALAR(failsafe_gcs_timeout, "FS_GCS_TIMEOUT", FS_GCS_TIMEOUT_S),
+
     // @Param: FS_LEAK_ENABLE
     // @DisplayName: Leak Failsafe Enable
     // @Description: Controls what action to take if a leak is detected.
diff --git a/ArduSub/Parameters.h b/ArduSub/Parameters.h
index d40aa4a6170c4..fe176e400e6c9 100644
--- a/ArduSub/Parameters.h
+++ b/ArduSub/Parameters.h
@@ -185,6 +185,7 @@ class Parameters {
         k_param_fs_batt_voltage,          // unused - moved to AP_BattMonitor
         k_param_failsafe_pilot_input,
         k_param_failsafe_pilot_input_timeout,
+        k_param_failsafe_gcs_timeout,
 
 
         // Misc Sub settings
@@ -257,6 +258,7 @@ class Parameters {
     AP_Int8         failsafe_terrain;
     AP_Int8         failsafe_pilot_input;       // pilot input failsafe behavior
     AP_Float        failsafe_pilot_input_timeout;
+    AP_Float        failsafe_gcs_timeout;       // ground station failsafe timeout (seconds)
 
     AP_Int8         xtrack_angle_limit;
 
diff --git a/ArduSub/defines.h b/ArduSub/defines.h
index 87e45820bc982..49dcde32fd828 100644
--- a/ArduSub/defines.h
+++ b/ArduSub/defines.h
@@ -79,8 +79,8 @@ enum LoggingParameters {
 #ifndef FS_GCS
 # define FS_GCS                        DISABLED
 #endif
-#ifndef FS_GCS_TIMEOUT_MS
-# define FS_GCS_TIMEOUT_MS             2500    // gcs failsafe triggers after this number of milliseconds with no GCS heartbeat
+#ifndef FS_GCS_TIMEOUT_S
+# define FS_GCS_TIMEOUT_S             5.0    // gcs failsafe triggers after this number of seconds with no GCS heartbeat
 #endif
 
 // missing terrain data failsafe
diff --git a/ArduSub/failsafe.cpp b/ArduSub/failsafe.cpp
index 80c74ed45829d..7807daed146be 100644
--- a/ArduSub/failsafe.cpp
+++ b/ArduSub/failsafe.cpp
@@ -321,7 +321,8 @@ void Sub::failsafe_gcs_check()
     uint32_t tnow = AP_HAL::millis();
 
     // Check if we have gotten a GCS heartbeat recently (GCS sysid must match SYSID_MYGCS parameter)
-    if (tnow - gcs_last_seen_ms < FS_GCS_TIMEOUT_MS) {
+    const uint32_t gcs_timeout_ms = uint32_t(constrain_float(g.failsafe_gcs_timeout * 1000.0f, 0.0f, UINT32_MAX));
+    if (tnow - gcs_last_seen_ms < gcs_timeout_ms) {
         // Log event if we are recovering from previous gcs failsafe
         if (failsafe.gcs) {
             LOGGER_WRITE_ERROR(LogErrorSubsystem::FAILSAFE_GCS, LogErrorCode::FAILSAFE_RESOLVED);