Rover: add FS_GCS_TIMEOUT

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
 };
 

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[];

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;
+        return;
-    } 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;
     }
 
+    // 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);
 }
 

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: