Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-22 08:38:36 -04:00
Code Issues Packages Projects Releases Wiki Activity

Copter: rewrite gcs-failsafe check for clarity

Browse Source
This commit is contained in:
Peter Barker 2018-06-22 14:21:39 +10:00 committed by Randy Mackay
parent 6cb8fcc466
commit 567ba799f1
1 changed files with 15 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

21
ArduCopter/events.cpp
View File

@ -87,17 +87,26 @@ void Copter::handle_battery_failsafe(const char *type_str, const int8_t action)
// failsafe_gcs_check - check for ground station failsafe
void Copter::failsafe_gcs_check()
{
uint32_t last_gcs_update_ms;
// return immediately if gcs failsafe is disabled, gcs has never been connected or we are not overriding rc controls from the gcs and we are not in guided mode
// this also checks to see if we have a GCS failsafe active, if we do, then must continue to process the logic for recovery from this state.
if ((!failsafe.gcs)&&(g.failsafe_gcs == FS_GCS_DISABLED || failsafe.last_heartbeat_ms == 0 || (!RC_Channels::has_active_overrides() && control_mode != GUIDED))) {
if (failsafe.gcs) {
// we must run the failsafe checks if we are in failsafe -
// otherwise we will never leave failsafe
} else if (g.failsafe_gcs == FS_GCS_DISABLED) {
// simply disabled
return;
} else if (failsafe.last_heartbeat_ms == 0) {
// GCS has never connected
return;
} else if (RC_Channels::has_active_overrides()) {
// GCS is currently telling us what to do!
} else if (control_mode == GUIDED) {
// GCS is currently telling us what to do!
} else {
return;
}
// calc time since last gcs update
// note: this only looks at the heartbeat from the device id set by g.sysid_my_gcs
last_gcs_update_ms = millis() - failsafe.last_heartbeat_ms;
const uint32_t last_gcs_update_ms = millis() - failsafe.last_heartbeat_ms;
// check if all is well
if (last_gcs_update_ms < FS_GCS_TIMEOUT_MS) {