From 7fd478fc4ab431820dbac403dffca690ef517183 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 | 9 +++++++-- 1 file changed, 7 insertions(+), 2 deletions(-) diff --git a/libraries/AP_GyroFFT/AP_GyroFFT.cpp b/libraries/AP_GyroFFT/AP_GyroFFT.cpp index 7c9e483096..cc07c8859a 100644 --- a/libraries/AP_GyroFFT/AP_GyroFFT.cpp +++ b/libraries/AP_GyroFFT/AP_GyroFFT.cpp @@ -253,12 +253,17 @@ 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); @@ -384,7 +389,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);