From 53ee7d6e75a09714594a8f9d6665bba5987b5163 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sat, 30 Nov 2024 07:28:06 +1100 Subject: [PATCH] AP_InertialSensor: fixed check for changes to notch filters if the configured freq changes on any type of notch then A and Q change, so init must be called. This does not affect only Fixed notches --- libraries/AP_InertialSensor/AP_InertialSensor.cpp | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/libraries/AP_InertialSensor/AP_InertialSensor.cpp b/libraries/AP_InertialSensor/AP_InertialSensor.cpp index 8c6f05fd1e..0d8e6029e8 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor.cpp +++ b/libraries/AP_InertialSensor/AP_InertialSensor.cpp @@ -1863,8 +1863,7 @@ void AP_InertialSensor::HarmonicNotch::update_params(uint8_t instance, bool conv { if (!is_equal(last_bandwidth_hz[instance], params.bandwidth_hz()) || !is_equal(last_attenuation_dB[instance], params.attenuation_dB()) || - (params.tracking_mode() == HarmonicNotchDynamicMode::Fixed && - !is_equal(last_center_freq_hz[instance], params.center_freq_hz())) || + !is_equal(last_center_freq_hz[instance], params.center_freq_hz()) || converging) { filter[instance].init(gyro_rate, params); last_center_freq_hz[instance] = params.center_freq_hz();