From 018c32dc3fb6d1051209853f56759166c84bc1e0 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 a parameter

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

diff --git a/ArduSub/Parameters.cpp b/ArduSub/Parameters.cpp
index caebfe820a2373..6eb9f80601732a 100644
--- a/ArduSub/Parameters.cpp
+++ b/ArduSub/Parameters.cpp
@@ -78,6 +78,15 @@ 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
+    // @Range: 2 120
+    // @Increment: 1
+    // @User: Standard
+    GSCALAR(failsafe_gcs_timeout, "FS_GCS_TIMEOUT", FS_GCS_TIMEOUT_MS),
+
     // @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 6854c9332852fd..73ef8530cb8aee 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 87e45820bc9828..c8afaed606fdd4 100644
--- a/ArduSub/defines.h
+++ b/ArduSub/defines.h
@@ -80,7 +80,7 @@ enum LoggingParameters {
 # 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
+# define FS_GCS_TIMEOUT_MS             5.0    // gcs failsafe triggers after this number of milliseconds with no GCS heartbeat
 #endif
 
 // missing terrain data failsafe
diff --git a/ArduSub/failsafe.cpp b/ArduSub/failsafe.cpp
index 80c74ed45829d1..7d29410c462f65 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) {
+
+    if (tnow - gcs_last_seen_ms < (g.failsafe_gcs_timeout * 1000)) {
         // Log event if we are recovering from previous gcs failsafe
         if (failsafe.gcs) {
             LOGGER_WRITE_ERROR(LogErrorSubsystem::FAILSAFE_GCS, LogErrorCode::FAILSAFE_RESOLVED);