5
0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-05 15:38:29 -04:00

Copter: Allow the user to specify the GCS failsafe timeout

This commit is contained in:
Michael du Breuil 2020-11-11 17:46:06 -07:00 committed by Randy Mackay
parent 5e8e088978
commit 5e2093b105
4 changed files with 16 additions and 7 deletions

View File

@ -994,6 +994,15 @@ const AP_Param::GroupInfo ParametersG2::var_info[] = {
AP_GROUPINFO("GUID_OPTIONS", 41, ParametersG2, guided_options, 0),
#endif
// @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", 42, ParametersG2, fs_gcs_timeout, 5),
AP_GROUPEND
};

View File

@ -627,6 +627,8 @@ public:
AP_Int32 guided_options;
#endif
AP_Float fs_gcs_timeout;
};
extern const AP_Param::Info var_info[];

View File

@ -148,9 +148,6 @@
#ifndef FS_GCS
# define FS_GCS DISABLED
#endif
#ifndef FS_GCS_TIMEOUT_MS
# define FS_GCS_TIMEOUT_MS 5000 // gcs failsafe triggers after 5 seconds with no GCS heartbeat
#endif
// Radio failsafe while using RC_override
#ifndef FS_RADIO_RC_OVERRIDE_TIMEOUT_MS

View File

@ -119,20 +119,21 @@ void Copter::failsafe_gcs_check()
// 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() - failsafe.last_heartbeat_ms;
const uint32_t gcs_timeout_ms = uint32_t(constrain_float(g2.fs_gcs_timeout * 1000.0f, 0.0f, UINT32_MAX));
// Determine which event to trigger
if (last_gcs_update_ms < FS_GCS_TIMEOUT_MS && failsafe.gcs) {
if (last_gcs_update_ms < gcs_timeout_ms && failsafe.gcs) {
// Recovery from a GCS failsafe
set_failsafe_gcs(false);
failsafe_gcs_off_event();
} else if (last_gcs_update_ms < FS_GCS_TIMEOUT_MS && !failsafe.gcs) {
} else if (last_gcs_update_ms < gcs_timeout_ms && !failsafe.gcs) {
// No problem, do nothing
} else if (last_gcs_update_ms > FS_GCS_TIMEOUT_MS && failsafe.gcs) {
} else if (last_gcs_update_ms > gcs_timeout_ms && failsafe.gcs) {
// Already in failsafe, do nothing
} else if (last_gcs_update_ms > FS_GCS_TIMEOUT_MS && !failsafe.gcs) {
} else if (last_gcs_update_ms > gcs_timeout_ms && !failsafe.gcs) {
// New GCS failsafe event, trigger events
set_failsafe_gcs(true);
failsafe_gcs_on_event();