From ea64077f7780b3f8d5bd66740b495ab6b184a229 Mon Sep 17 00:00:00 2001 From: Michael du Breuil Date: Tue, 20 Aug 2019 09:24:01 -0700 Subject: [PATCH] Plane: Remove redundant protection --- ArduPlane/radio.cpp | 54 ++++++++++++++++++++++----------------------- 1 file changed, 26 insertions(+), 28 deletions(-) diff --git a/ArduPlane/radio.cpp b/ArduPlane/radio.cpp index 1d1e1d2899..5b18e2bca8 100644 --- a/ArduPlane/radio.cpp +++ b/ArduPlane/radio.cpp @@ -278,36 +278,34 @@ void Plane::control_failsafe() } } - if(g.throttle_fs_enabled == 0) + if(g.throttle_fs_enabled == 0) { return; + } - if (g.throttle_fs_enabled) { - if (rc_failsafe_active()) { - // we detect a failsafe from radio - // throttle has dropped below the mark - failsafe.throttle_counter++; - if (failsafe.throttle_counter == 10) { - gcs().send_text(MAV_SEVERITY_WARNING, "Throttle failsafe on"); - failsafe.rc_failsafe = true; - AP_Notify::flags.failsafe_radio = true; - } - if (failsafe.throttle_counter > 10) { - failsafe.throttle_counter = 10; - } - - }else if(failsafe.throttle_counter > 0) { - // we are no longer in failsafe condition - // but we need to recover quickly - failsafe.throttle_counter--; - if (failsafe.throttle_counter > 3) { - failsafe.throttle_counter = 3; - } - if (failsafe.throttle_counter == 1) { - gcs().send_text(MAV_SEVERITY_WARNING, "Throttle failsafe off"); - } else if(failsafe.throttle_counter == 0) { - failsafe.rc_failsafe = false; - AP_Notify::flags.failsafe_radio = false; - } + if (rc_failsafe_active()) { + // we detect a failsafe from radio + // throttle has dropped below the mark + failsafe.throttle_counter++; + if (failsafe.throttle_counter == 10) { + gcs().send_text(MAV_SEVERITY_WARNING, "Throttle failsafe on"); + failsafe.rc_failsafe = true; + AP_Notify::flags.failsafe_radio = true; + } + if (failsafe.throttle_counter > 10) { + failsafe.throttle_counter = 10; + } + } else if(failsafe.throttle_counter > 0) { + // we are no longer in failsafe condition + // but we need to recover quickly + failsafe.throttle_counter--; + if (failsafe.throttle_counter > 3) { + failsafe.throttle_counter = 3; + } + if (failsafe.throttle_counter == 1) { + gcs().send_text(MAV_SEVERITY_WARNING, "Throttle failsafe off"); + } else if(failsafe.throttle_counter == 0) { + failsafe.rc_failsafe = false; + AP_Notify::flags.failsafe_radio = false; } } }