From c604f98731bc2ee9bf69c181d797d0f130984f60 Mon Sep 17 00:00:00 2001 From: Willian Galvani 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);