From d29ccba3f7318a0b20d6f698d6e4d5e3db0ecc49 Mon Sep 17 00:00:00 2001 From: Andy Piper Date: Sat, 24 Dec 2022 16:07:03 +0000 Subject: [PATCH] AP_Vehicle: constrain FFT notch updates to obey reference frequency --- libraries/AP_Vehicle/AP_Vehicle.cpp | 16 ++++++++++++++-- 1 file changed, 14 insertions(+), 2 deletions(-) diff --git a/libraries/AP_Vehicle/AP_Vehicle.cpp b/libraries/AP_Vehicle/AP_Vehicle.cpp index 4ad7a38908..ae69f8b76b 100644 --- a/libraries/AP_Vehicle/AP_Vehicle.cpp +++ b/libraries/AP_Vehicle/AP_Vehicle.cpp @@ -576,9 +576,21 @@ void AP_Vehicle::update_dynamic_notch(AP_InertialSensor::HarmonicNotch ¬ch) float notches[INS_MAX_NOTCHES]; const uint8_t peaks = gyro_fft.get_weighted_noise_center_frequencies_hz(notch.num_dynamic_notches, notches); - notch.update_frequencies_hz(peaks, notches); + if (peaks > 0) { + for (uint8_t i = 0; i < peaks; i++) { + notches[i] = MAX(ref_freq, notches[i]); + } + notch.update_frequencies_hz(peaks, notches); + } else { // since FFT can be used post-filter it is better to disable the notch when there is no data + notch.set_inactive(true); + } } else { - notch.update_freq_hz(gyro_fft.get_weighted_noise_center_freq_hz()); + float center_freq = gyro_fft.get_weighted_noise_center_freq_hz(); + if (!is_zero(center_freq)) { + notch.update_freq_hz(MAX(ref_freq, center_freq)); + } else { // since FFT can be used post-filter it is better to disable the notch when there is no data + notch.set_inactive(true); + } } break; #endif