Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Sub: make gcs failsafe timeout a parameter

This commit is contained in:
Willian Galvani 2024-07-09 19:40:58 -03:00 committed by Peter Barker
parent e4fc9b0b58
commit 9e0fd1c9d5
4 changed files with 14 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
ArduSub/Parameters.cpp
View File

@ -78,6 +78,14 @@ const AP_Param::Info Sub::var_info[] = {
// @User: Standard // @User: Standard
GSCALAR(failsafe_gcs, "FS_GCS_ENABLE", FS_GCS_DISARM), 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 // @Param: FS_LEAK_ENABLE
// @DisplayName: Leak Failsafe Enable // @DisplayName: Leak Failsafe Enable
// @Description: Controls what action to take if a leak is detected. // @Description: Controls what action to take if a leak is detected.

2
ArduSub/Parameters.h
View File

@ -185,6 +185,7 @@ public:
k_param_fs_batt_voltage, // unused - moved to AP_BattMonitor k_param_fs_batt_voltage, // unused - moved to AP_BattMonitor
k_param_failsafe_pilot_input, k_param_failsafe_pilot_input,
k_param_failsafe_pilot_input_timeout, k_param_failsafe_pilot_input_timeout,
k_param_failsafe_gcs_timeout,
// Misc Sub settings // Misc Sub settings
@ -257,6 +258,7 @@ public:
AP_Int8 failsafe_terrain; AP_Int8 failsafe_terrain;
AP_Int8 failsafe_pilot_input; // pilot input failsafe behavior AP_Int8 failsafe_pilot_input; // pilot input failsafe behavior
AP_Float failsafe_pilot_input_timeout; AP_Float failsafe_pilot_input_timeout;
AP_Float failsafe_gcs_timeout; // ground station failsafe timeout (seconds)
AP_Int8 xtrack_angle_limit; AP_Int8 xtrack_angle_limit;

4
ArduSub/defines.h
View File

@ -79,8 +79,8 @@ enum LoggingParameters {
#ifndef FS_GCS #ifndef FS_GCS
# define FS_GCS DISABLED # define FS_GCS DISABLED
#endif #endif
#ifndef FS_GCS_TIMEOUT_MS #ifndef FS_GCS_TIMEOUT_S
# define FS_GCS_TIMEOUT_MS 2500 // gcs failsafe triggers after this number of milliseconds with no GCS heartbeat # define FS_GCS_TIMEOUT_S 5.0 // gcs failsafe triggers after this number of seconds with no GCS heartbeat
#endif #endif
// missing terrain data failsafe // missing terrain data failsafe

3
ArduSub/failsafe.cpp
View File

@ -321,7 +321,8 @@ void Sub::failsafe_gcs_check()
uint32_t tnow = AP_HAL::millis(); uint32_t tnow = AP_HAL::millis();
// Check if we have gotten a GCS heartbeat recently (GCS sysid must match SYSID_MYGCS parameter) // 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 // Log event if we are recovering from previous gcs failsafe
if (failsafe.gcs) { if (failsafe.gcs) {
LOGGER_WRITE_ERROR(LogErrorSubsystem::FAILSAFE_GCS, LogErrorCode::FAILSAFE_RESOLVED); LOGGER_WRITE_ERROR(LogErrorSubsystem::FAILSAFE_GCS, LogErrorCode::FAILSAFE_RESOLVED);