From afe0c02835b74b2f7b5b5c04a58a2a1d787e5736 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sat, 16 Apr 2022 12:08:09 +1000 Subject: [PATCH] AP_InertialSensor: don't update disabled notches --- libraries/AP_InertialSensor/AP_InertialSensor.cpp | 7 ++++++- 1 file changed, 6 insertions(+), 1 deletion(-) diff --git a/libraries/AP_InertialSensor/AP_InertialSensor.cpp b/libraries/AP_InertialSensor/AP_InertialSensor.cpp index e68e1745d9..034e1cb1ab 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor.cpp +++ b/libraries/AP_InertialSensor/AP_InertialSensor.cpp @@ -877,6 +877,9 @@ AP_InertialSensor::init(uint16_t loop_rate) // dynamically, the calculated value is always some multiple of the configured center frequency, so start with the // configured value for (auto ¬ch : harmonic_notches) { + if (!notch.params.enabled()) { + continue; + } notch.calculated_notch_freq_hz[0] = notch.params.center_freq_hz(); notch.num_calculated_notch_frequencies = 1; notch.num_dynamic_notches = 1; @@ -1635,7 +1638,9 @@ void AP_InertialSensor::update(void) const bool converging = AP_HAL::millis() < HAL_INS_CONVERGANCE_MS; const float gyro_rate = _gyro_raw_sample_rates[i]; for (auto ¬ch : harmonic_notches) { - notch.update_params(i, converging, gyro_rate); + if (notch.params.enabled()) { + notch.update_params(i, converging, gyro_rate); + } } }