From 837ebd4491cea1109d6f202eee7dd9f48e045ac8 Mon Sep 17 00:00:00 2001
From: Pierre Kancir <pierre.kancir.emn@gmail.com>
Date: Sun, 29 Jan 2023 23:49:35 +0100
Subject: [PATCH] Rover: add FS_GCS_TIMEOUT

---
 Rover/Parameters.cpp |  9 +++++++++
 Rover/Parameters.h   |  3 +++
 Rover/Rover.cpp      | 16 +++++++++-------
 Rover/failsafe.cpp   |  1 +
 4 files changed, 22 insertions(+), 7 deletions(-)

diff --git a/Rover/Parameters.cpp b/Rover/Parameters.cpp
index 0c9777ee1f..2b2863c2db 100644
--- a/Rover/Parameters.cpp
+++ b/Rover/Parameters.cpp
@@ -683,6 +683,15 @@ const AP_Param::GroupInfo ParametersG2::var_info[] = {
     // @User: Advanced
     AP_GROUPINFO("MANUAL_STR_EXPO", 55, ParametersG2, manual_steering_expo, 0),
 
+    // @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", 56, ParametersG2, fs_gcs_timeout, 5),
+
     AP_GROUPEND
 };
 
diff --git a/Rover/Parameters.h b/Rover/Parameters.h
index 74593e1f72..83f718b91e 100644
--- a/Rover/Parameters.h
+++ b/Rover/Parameters.h
@@ -433,6 +433,9 @@ public:
 
     // manual mode steering expo
     AP_Float manual_steering_expo;
+
+    // FS GCS timeout trigger time
+    AP_Float fs_gcs_timeout;
 };
 
 extern const AP_Param::Info var_info[];
diff --git a/Rover/Rover.cpp b/Rover/Rover.cpp
index 398ae33c57..f8a3ef7800 100644
--- a/Rover/Rover.cpp
+++ b/Rover/Rover.cpp
@@ -344,22 +344,24 @@ void Rover::ahrs_update()
  */
 void Rover::gcs_failsafe_check(void)
 {
-    if (!g.fs_gcs_enabled) {
+    if (g.fs_gcs_enabled == FS_GCS_DISABLED) {
         // gcs failsafe disabled
         return;
     }
 
-    // check for updates from GCS within 2 seconds
     const uint32_t gcs_last_seen_ms = gcs().sysid_myggcs_last_seen_time_ms();
-    bool do_failsafe = true;
     if (gcs_last_seen_ms == 0) {
         // we've never seen the GCS, so we never failsafe for not seeing it
-        do_failsafe = false;
-    } else if (millis() - gcs_last_seen_ms <= 2000) {
-        // we've never seen the GCS in the last couple of seconds, so all good
-        do_failsafe = false;
+        return;
     }
 
+    // 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() - gcs_last_seen_ms;
+    const uint32_t gcs_timeout_ms = uint32_t(constrain_float(g2.fs_gcs_timeout * 1000.0f, 0.0f, UINT32_MAX));
+
+    const bool do_failsafe = last_gcs_update_ms >= gcs_timeout_ms ? true : false;
+
     failsafe_trigger(FAILSAFE_EVENT_GCS, "GCS", do_failsafe);
 }
 
diff --git a/Rover/failsafe.cpp b/Rover/failsafe.cpp
index e4d91e95d9..2520c73f66 100644
--- a/Rover/failsafe.cpp
+++ b/Rover/failsafe.cpp
@@ -78,6 +78,7 @@ void Rover::failsafe_trigger(uint8_t failsafe_type, const char* type_str, bool o
             ((failsafe_type == FAILSAFE_EVENT_THROTTLE && g.fs_throttle_enabled == FS_THR_ENABLED_CONTINUE_MISSION) ||
              (failsafe_type == FAILSAFE_EVENT_GCS && g.fs_gcs_enabled == FS_GCS_ENABLED_CONTINUE_MISSION))) {
             // continue with mission in auto mode
+            gcs().send_text(MAV_SEVERITY_WARNING, "Failsafe - Continuing Auto Mode");
         } else {
             switch (g.fs_action) {
             case Failsafe_Action_None: