From 492e203fd2d024fb9999a812c7abd7eb60e43656 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Fri, 15 Apr 2022 18:56:15 +1000 Subject: [PATCH] AP_GyroFFT: allow for 2 FFT based notches --- libraries/AP_GyroFFT/AP_GyroFFT.cpp | 5 ++--- 1 file changed, 2 insertions(+), 3 deletions(-) diff --git a/libraries/AP_GyroFFT/AP_GyroFFT.cpp b/libraries/AP_GyroFFT/AP_GyroFFT.cpp index 91c93bd5e4..24b37cb80c 100644 --- a/libraries/AP_GyroFFT/AP_GyroFFT.cpp +++ b/libraries/AP_GyroFFT/AP_GyroFFT.cpp @@ -254,9 +254,8 @@ void AP_GyroFFT::init(uint16_t loop_rate_hz) uint8_t num_notches = 0; for (auto ¬ch : _ins->harmonic_notches) { if (notch.params.tracking_mode() == HarmonicNotchDynamicMode::UpdateGyroFFT) { - harmonics = notch.params.harmonics(); - num_notches = notch.num_dynamic_notches; - break; + harmonics |= notch.params.harmonics(); + num_notches = MAX(num_notches, notch.num_dynamic_notches); } } // count the number of active harmonics or dynamic notchs