From cb111504e25b65022e0afce39fb731f3a04123c2 Mon Sep 17 00:00:00 2001 From: Andy Piper Date: Fri, 4 Oct 2024 20:38:16 +0100 Subject: [PATCH] AP_InertialSensor: cope with negative ESC frequencies in notch updates --- libraries/AP_InertialSensor/AP_InertialSensor.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/libraries/AP_InertialSensor/AP_InertialSensor.cpp b/libraries/AP_InertialSensor/AP_InertialSensor.cpp index 6d254dccec..f717d23b81 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor.cpp +++ b/libraries/AP_InertialSensor/AP_InertialSensor.cpp @@ -2327,7 +2327,7 @@ void AP_InertialSensor::acal_update() */ void AP_InertialSensor::HarmonicNotch::update_freq_hz(float scaled_freq) { - calculated_notch_freq_hz[0] = scaled_freq; + calculated_notch_freq_hz[0] = fabsf(scaled_freq); num_calculated_notch_frequencies = 1; } @@ -2335,7 +2335,7 @@ void AP_InertialSensor::HarmonicNotch::update_freq_hz(float scaled_freq) void AP_InertialSensor::HarmonicNotch::update_frequencies_hz(uint8_t num_freqs, const float scaled_freq[]) { // note that we allow zero through, which will disable the notch for (uint8_t i = 0; i < num_freqs; i++) { - calculated_notch_freq_hz[i] = scaled_freq[i]; + calculated_notch_freq_hz[i] = fabsf(scaled_freq[i]); } // any uncalculated frequencies will float at the previous value or the initialized freq if none num_calculated_notch_frequencies = num_freqs;