From bccca9ed2e3444d40e7ee9d6ee2354ba99056b62 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sun, 17 Apr 2022 17:24:45 +1000 Subject: [PATCH] AP_GyroFFT: added defaults for FFT with no notch allow for testing with FFT enabled, and defaulting number of frequencies to look for --- libraries/AP_GyroFFT/AP_GyroFFT.cpp | 8 ++++++-- 1 file changed, 6 insertions(+), 2 deletions(-) diff --git a/libraries/AP_GyroFFT/AP_GyroFFT.cpp b/libraries/AP_GyroFFT/AP_GyroFFT.cpp index a14f00c5d3..71c2512d6b 100644 --- a/libraries/AP_GyroFFT/AP_GyroFFT.cpp +++ b/libraries/AP_GyroFFT/AP_GyroFFT.cpp @@ -253,11 +253,15 @@ 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.enabled() && notch.params.tracking_mode() == HarmonicNotchDynamicMode::UpdateGyroFFT) { + if (notch.params.enabled()) { harmonics |= notch.params.harmonics(); num_notches = MAX(num_notches, notch.num_dynamic_notches); } } + if (harmonics == 0) { + // this allows use of FFT to find peaks with all notch filters disabled + harmonics = 3; + } // count the number of active harmonics or dynamic notchs _tracked_peaks = constrain_int16(MAX(__builtin_popcount(harmonics), num_notches), 1, FrequencyPeak::MAX_TRACKED_PEAKS); @@ -383,7 +387,7 @@ void AP_GyroFFT::update() if (!_rpy_health.x && !_rpy_health.y) { _health = 0; } else { - uint8_t num_notches = 0; + uint8_t num_notches = 1; for (auto ¬ch : _ins->harmonic_notches) { if (notch.params.enabled()) { num_notches = MAX(num_notches, notch.num_dynamic_notches);