AP_InertialSensor: make sure notch is still attenuating noise during …

…warmup
tridge · Nov 28, 2024 · 0984dce · 0984dce
1 parent fa828a7
commit 0984dce
Showing 1 changed file with 3 additions and 2 deletions.
diff --git a/libraries/AP_InertialSensor/AP_InertialSensor.cpp b/libraries/AP_InertialSensor/AP_InertialSensor.cpp
@@ -1788,13 +1788,14 @@ void AP_InertialSensor::HarmonicNotch::update_params(uint8_t instance, bool conv
     const float center_freq = calculated_notch_freq_hz[0];
     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], center_freq)) ||
+        (params.tracking_mode() == HarmonicNotchDynamicMode::Fixed && !is_equal(last_center_freq_hz[instance], params.center_freq_hz())) ||
         converging) {
         filter[instance].init(gyro_rate,
                               params.center_freq_hz(),
                               params.bandwidth_hz(),
                               params.attenuation_dB());
-        last_center_freq_hz[instance] = center_freq;
+        filter[instance].update(center_freq);
+        last_center_freq_hz[instance] = params.center_freq_hz();
         last_bandwidth_hz[instance] = params.bandwidth_hz();
         last_attenuation_dB[instance] = params.attenuation_dB();
     }