diff --git a/libraries/AP_InertialSensor/AP_InertialSensor.cpp b/libraries/AP_InertialSensor/AP_InertialSensor.cpp index ccc9ac2425..e2855b73d6 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor.cpp +++ b/libraries/AP_InertialSensor/AP_InertialSensor.cpp @@ -1615,6 +1615,7 @@ 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)) || converging) { filter[instance].init(gyro_rate, center_freq, @@ -1623,7 +1624,7 @@ void AP_InertialSensor::HarmonicNotch::update_params(uint8_t instance, bool conv last_center_freq_hz[instance] = center_freq; last_bandwidth_hz[instance] = params.bandwidth_hz(); last_attenuation_dB[instance] = params.attenuation_dB(); - } else if (!is_equal(last_center_freq_hz[instance], center_freq)) { + } else if (params.tracking_mode() != HarmonicNotchDynamicMode::Fixed) { if (num_calculated_notch_frequencies > 1) { filter[instance].update(num_calculated_notch_frequencies, calculated_notch_freq_hz); } else {