From 567ba799f107417f6c64baeeced17e4b75fba180 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Fri, 22 Jun 2018 14:21:39 +1000 Subject: [PATCH] Copter: rewrite gcs-failsafe check for clarity --- ArduCopter/events.cpp | 21 +++++++++++++++------ 1 file changed, 15 insertions(+), 6 deletions(-) diff --git a/ArduCopter/events.cpp b/ArduCopter/events.cpp index af856c8a7c..ef5dcb87ee 100644 --- a/ArduCopter/events.cpp +++ b/ArduCopter/events.cpp @@ -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) {