Sub: make gcs failsafe timeout a parameter

Williangalvani · Jul 9, 2024 · 07c84e9 · 07c84e9
1 parent 38ca478
commit 07c84e9
Show file tree

Hide file tree

Showing 4 changed files with 13 additions and 2 deletions.
diff --git 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
@@ -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
@@ -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
@@ -321,7 +321,7 @@ 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);