From acb70abe86e59238e9abb854d226f513d7e9eec3 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Thu, 27 May 2021 07:50:18 +1000 Subject: [PATCH] Copter: adjust yaw imbalance check to only check imax rather than i Existing code was causing way too many false-positives --- ArduCopter/crash_check.cpp | 4 +--- 1 file changed, 1 insertion(+), 3 deletions(-) diff --git a/ArduCopter/crash_check.cpp b/ArduCopter/crash_check.cpp index c6164d1ea1..81f80e0935 100644 --- a/ArduCopter/crash_check.cpp +++ b/ArduCopter/crash_check.cpp @@ -14,7 +14,6 @@ // Yaw imbalance check #define YAW_IMBALANCE_IMAX_THRESHOLD 0.75f -#define YAW_IMBALANCE_I_THERSHOLD 0.1f #define YAW_IMBALANCE_WARN_MS 10000 // crash_check - disarms motors if a crash has been detected @@ -209,8 +208,7 @@ void Copter::yaw_imbalance_check() } const float I_max = attitude_control->get_rate_yaw_pid().imax(); - if ((is_positive(I_max) && ((I > YAW_IMBALANCE_IMAX_THRESHOLD * I_max) || (is_equal(I_term,I_max)))) || - (I >YAW_IMBALANCE_I_THERSHOLD)) { + if ((is_positive(I_max) && ((I > YAW_IMBALANCE_IMAX_THRESHOLD * I_max) || (is_equal(I_term,I_max))))) { // filtered using over precentage of I max or unfiltered = I max // I makes up more than precentage of total available control power const uint32_t now = millis();