From f78eb58fb41473db1a20a3002b99d52942b4afad Mon Sep 17 00:00:00 2001 From: Andy Piper Date: Thu, 2 May 2024 15:06:27 +0100 Subject: [PATCH] AP_InertialSensor: ensure that notches get updated while converging --- libraries/AP_InertialSensor/AP_InertialSensor.cpp | 4 +++- libraries/AP_InertialSensor/AP_InertialSensor.h | 6 ------ 2 files changed, 3 insertions(+), 7 deletions(-) diff --git a/libraries/AP_InertialSensor/AP_InertialSensor.cpp b/libraries/AP_InertialSensor/AP_InertialSensor.cpp index b2a76a1f26..bf62d4cdfc 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor.cpp +++ b/libraries/AP_InertialSensor/AP_InertialSensor.cpp @@ -1850,7 +1850,9 @@ void AP_InertialSensor::HarmonicNotch::update_params(uint8_t instance, bool conv last_center_freq_hz[instance] = params.center_freq_hz(); last_bandwidth_hz[instance] = params.bandwidth_hz(); last_attenuation_dB[instance] = params.attenuation_dB(); - } else if (params.tracking_mode() != HarmonicNotchDynamicMode::Fixed) { + } + + if (params.tracking_mode() != HarmonicNotchDynamicMode::Fixed) { if (num_calculated_notch_frequencies > 1) { filter[instance].update(num_calculated_notch_frequencies, calculated_notch_freq_hz); } else { diff --git a/libraries/AP_InertialSensor/AP_InertialSensor.h b/libraries/AP_InertialSensor/AP_InertialSensor.h index 9bce50d4ac..aa103bb608 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor.h +++ b/libraries/AP_InertialSensor/AP_InertialSensor.h @@ -449,12 +449,6 @@ public: float calculated_notch_freq_hz[INS_MAX_NOTCHES]; uint8_t num_calculated_notch_frequencies; - // Update the harmonic notch frequency - void update_notch_freq_hz(float scaled_freq); - - // Update the harmonic notch frequencies - void update_notch_frequencies_hz(uint8_t num_freqs, const float scaled_freq[]); - // runtime update of notch parameters void update_params(uint8_t instance, bool converging, float gyro_rate);