From 447af29ef1b63c029b184d4fa9f5afe04a51c8c4 Mon Sep 17 00:00:00 2001 From: Iampete1 Date: Sat, 7 Nov 2020 18:33:10 +0000 Subject: [PATCH] Copter: add yaw imbalance check --- ArduCopter/Copter.h | 3 +++ ArduCopter/crash_check.cpp | 52 ++++++++++++++++++++++++++++++++++++ ArduCopter/land_detector.cpp | 1 + 3 files changed, 56 insertions(+) diff --git a/ArduCopter/Copter.h b/ArduCopter/Copter.h index 79656bfc91..8c9a543466 100644 --- a/ArduCopter/Copter.h +++ b/ArduCopter/Copter.h @@ -690,6 +690,9 @@ private: // crash_check.cpp void crash_check(); void thrust_loss_check(); + void yaw_imbalance_check(); + LowPassFilterFloat yaw_I_filt{0.05f}; + uint32_t last_yaw_warn_ms; void parachute_check(); void parachute_release(); void parachute_manual_release(); diff --git a/ArduCopter/crash_check.cpp b/ArduCopter/crash_check.cpp index dbef41a7eb..217a62b0c8 100644 --- a/ArduCopter/crash_check.cpp +++ b/ArduCopter/crash_check.cpp @@ -12,6 +12,11 @@ #define THRUST_LOSS_CHECK_ANGLE_DEVIATION_CD 1500 // we can't expect to maintain altitude beyond 15 degrees on all aircraft #define THRUST_LOSS_CHECK_MINIMUM_THROTTLE 0.9f // we can expect to maintain altitude above 90 % throttle +// 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 // crashes are detected by the vehicle being more than 20 degrees beyond it's angle limits continuously for more than 1 second // called at MAIN_LOOP_RATE @@ -159,6 +164,53 @@ void Copter::thrust_loss_check() } } +// check for a large yaw imbalance, could be due to badly calibrated ESC or misaligned motors +void Copter::yaw_imbalance_check() +{ + // If I is disabled it is unlikely that the issue is not obvious + if (!is_positive(attitude_control->get_rate_yaw_pid().kI())) { + return; + } + + // thrust loss is trigerred, yaw issues are expected + if (motors->get_thrust_boost()) { + yaw_I_filt.reset(0.0f); + return; + } + + // return immediately if disarmed + if (!motors->armed() || ap.land_complete) { + yaw_I_filt.reset(0.0f); + return; + } + + // exit immediately if in standby + if (standby_active) { + yaw_I_filt.reset(0.0f); + return; + } + + // magnitude of low pass filtered I term + const float I_term = attitude_control->get_rate_yaw_pid().get_pid_info().I; + const float I = fabsf(yaw_I_filt.apply(attitude_control->get_rate_yaw_pid().get_pid_info().I,G_Dt)); + if (I > fabsf(I_term)) { + // never allow to be larger than I + yaw_I_filt.reset(I_term); + } + + 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)) { + // 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(); + if (now - last_yaw_warn_ms > YAW_IMBALANCE_WARN_MS) { + last_yaw_warn_ms = now; + gcs().send_text(MAV_SEVERITY_EMERGENCY, "Yaw Imbalance %0.0f%%", I *100); + } + } +} + #if PARACHUTE == ENABLED // Code to detect a crash main ArduCopter code diff --git a/ArduCopter/land_detector.cpp b/ArduCopter/land_detector.cpp index f807cdd31f..875b03b9af 100644 --- a/ArduCopter/land_detector.cpp +++ b/ArduCopter/land_detector.cpp @@ -27,6 +27,7 @@ void Copter::update_land_and_crash_detectors() crash_check(); thrust_loss_check(); + yaw_imbalance_check(); } // update_land_detector - checks if we have landed and updates the ap.land_complete flag