From 97d42308a72a7edd4d9e44b3ab838db4be847f0f Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sat, 16 Apr 2022 12:08:22 +1000 Subject: [PATCH] AP_GyroFFT: skip disabled notches --- libraries/AP_GyroFFT/AP_GyroFFT.cpp | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/libraries/AP_GyroFFT/AP_GyroFFT.cpp b/libraries/AP_GyroFFT/AP_GyroFFT.cpp index 9f11994930..7c9e483096 100644 --- a/libraries/AP_GyroFFT/AP_GyroFFT.cpp +++ b/libraries/AP_GyroFFT/AP_GyroFFT.cpp @@ -253,7 +253,7 @@ void AP_GyroFFT::init(uint16_t loop_rate_hz) uint8_t harmonics = 0; uint8_t num_notches = 0; for (auto ¬ch : _ins->harmonic_notches) { - if (notch.params.tracking_mode() == HarmonicNotchDynamicMode::UpdateGyroFFT) { + if (notch.params.enabled() && notch.params.tracking_mode() == HarmonicNotchDynamicMode::UpdateGyroFFT) { harmonics |= notch.params.harmonics(); num_notches = MAX(num_notches, notch.num_dynamic_notches); } @@ -386,7 +386,9 @@ void AP_GyroFFT::update() } else { uint8_t num_notches = 0; for (auto ¬ch : _ins->harmonic_notches) { - num_notches = MAX(num_notches, notch.num_dynamic_notches); + if (notch.params.enabled()) { + num_notches = MAX(num_notches, notch.num_dynamic_notches); + } } _health = MIN(_global_state._health, num_notches); }