From 9ac3472b473cd57ad96a49011100ee933188d9a6 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sun, 12 Jun 2022 12:32:46 +1000 Subject: [PATCH] AP_InertialSensor: add set_inactive() on notch filters --- libraries/AP_InertialSensor/AP_InertialSensor.h | 12 +++++++++++- .../AP_InertialSensor/AP_InertialSensor_Backend.cpp | 12 +++++++++--- 2 files changed, 20 insertions(+), 4 deletions(-) diff --git a/libraries/AP_InertialSensor/AP_InertialSensor.h b/libraries/AP_InertialSensor/AP_InertialSensor.h index a398d707db..cad3d951ef 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor.h +++ b/libraries/AP_InertialSensor/AP_InertialSensor.h @@ -446,12 +446,22 @@ public: // Update the harmonic notch frequencies void update_freq_hz(float scaled_freq); void update_frequencies_hz(uint8_t num_freqs, const float scaled_freq[]); - + + // enable/disable the notch + void set_inactive(bool _inactive) { + inactive = _inactive; + } + + bool is_inactive(void) const { + return inactive; + } + private: // support for updating harmonic filter at runtime float last_center_freq_hz[INS_MAX_INSTANCES]; float last_bandwidth_hz[INS_MAX_INSTANCES]; float last_attenuation_dB[INS_MAX_INSTANCES]; + bool inactive; } harmonic_notches[HAL_INS_NUM_HARMONIC_NOTCH_FILTERS]; private: diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_Backend.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_Backend.cpp index 923a32814c..be915129a8 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_Backend.cpp +++ b/libraries/AP_InertialSensor/AP_InertialSensor_Backend.cpp @@ -186,17 +186,23 @@ void AP_InertialSensor_Backend::apply_gyro_filters(const uint8_t instance, const if (!notch.params.enabled()) { continue; } + bool inactive = notch.is_inactive(); #ifndef HAL_BUILD_AP_PERIPH // by default we only run the expensive notch filters on the // currently active IMU we reset the inactive notch filters so // that if we switch IMUs we're not left with old data if (!notch.params.hasOption(HarmonicNotchFilterParams::Options::EnableOnAllIMUs) && instance != AP::ahrs().get_primary_gyro_index()) { - notch.filter[instance].reset(); - continue; + inactive = true; } #endif - gyro_filtered = notch.filter[instance].apply(gyro_filtered); + if (inactive) { + // while inactive we reset the filter so when it activates the first output + // will be the first input sample + notch.filter[instance].reset(); + } else { + gyro_filtered = notch.filter[instance].apply(gyro_filtered); + } } // apply the low pass filter last to attentuate any notch induced noise