From ead011c61566dd8a45a2dca8d6ea9cc892d47e76 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Wed, 13 Apr 2022 13:36:18 +1000 Subject: [PATCH] AP_InertialSensor: support two full harmonic notch filters --- .../AP_InertialSensor/AP_InertialSensor.cpp | 89 ++++++++++--------- .../AP_InertialSensor/AP_InertialSensor.h | 43 +++++---- .../AP_InertialSensor_Backend.cpp | 75 +++++++--------- .../AP_InertialSensor_Backend.h | 32 ++----- .../AP_InertialSensor_Logging.cpp | 18 +++- 5 files changed, 124 insertions(+), 133 deletions(-) diff --git a/libraries/AP_InertialSensor/AP_InertialSensor.cpp b/libraries/AP_InertialSensor/AP_InertialSensor.cpp index 4f8379e4ed..071e17120e 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor.cpp +++ b/libraries/AP_InertialSensor/AP_InertialSensor.cpp @@ -548,11 +548,13 @@ const AP_Param::GroupInfo AP_InertialSensor::var_info[] = { // @Group: HNTCH_ // @Path: ../Filter/HarmonicNotchFilter.cpp - AP_SUBGROUPINFO(_harmonic_notch_filter, "HNTCH_", 41, AP_InertialSensor, HarmonicNotchFilterParams), + AP_SUBGROUPINFO(_harmonic_notch_filter[0], "HNTCH_", 41, AP_InertialSensor, HarmonicNotchFilterParams), +#if HAL_INS_NUM_HARMONIC_NOTCH_FILTERS > 1 // @Group: HNTC2_ // @Path: ../Filter/HarmonicNotchFilter.cpp - AP_SUBGROUPINFO(_notch_filter, "HNTC2_", 53, AP_InertialSensor, HarmonicNotchFilterParams), + AP_SUBGROUPINFO(_harmonic_notch_filter[1], "HNTC2_", 53, AP_InertialSensor, HarmonicNotchFilterParams), +#endif // @Param: GYRO_RATE // @DisplayName: Gyro rate for IMUs with Fast Sampling enabled @@ -874,45 +876,44 @@ AP_InertialSensor::init(uint16_t loop_rate) // the center frequency of the harmonic notch is always taken from the calculated value so that it can be updated // dynamically, the calculated value is always some multiple of the configured center frequency, so start with the // configured value - _calculated_harmonic_notch_freq_hz[0] = _harmonic_notch_filter.center_freq_hz(); - _num_calculated_harmonic_notch_frequencies = 1; - _num_dynamic_harmonic_notches = 1; - uint8_t num_filters = 0; - const bool double_notch = _harmonic_notch_filter.hasOption(HarmonicNotchFilterParams::Options::DoubleNotch); + bool double_notch[HAL_INS_NUM_HARMONIC_NOTCH_FILTERS] {}; + for (uint8_t i=0; iget_motor_mask()); + if (motors != nullptr) { + _num_dynamic_harmonic_notches[i] = __builtin_popcount(motors->get_motor_mask()); + } } // avoid harmonics unless actually configured by the user - _harmonic_notch_filter.set_default_harmonics(1); - } + _harmonic_notch_filter[i].set_default_harmonics(1); + } #endif + } // count number of used sensors uint8_t sensors_used = 0; for (uint8_t i = 0; i < INS_MAX_INSTANCES; i++) { sensors_used += _use[i]; } - // calculate number of notches we might want to use for harmonic notch - if (_harmonic_notch_filter.enabled()) { - num_filters = __builtin_popcount( _harmonic_notch_filter.harmonics()) - * _num_dynamic_harmonic_notches * (double_notch ? 2 : 1) - * sensors_used; - } - // add filters used by static notch - if (_notch_filter.enabled()) { - _notch_filter.set_default_harmonics(1); - num_filters += __builtin_popcount(_notch_filter.harmonics()) - * (_notch_filter.hasOption(HarmonicNotchFilterParams::Options::DoubleNotch) ? 2 : 1) - * sensors_used; + for (uint8_t i=0; i HAL_HNF_MAX_FILTERS) { @@ -923,18 +924,14 @@ AP_InertialSensor::init(uint16_t loop_rate) for (uint8_t i=0; i= HAL_INS_NUM_HARMONIC_NOTCH_FILTERS) { + return; + } // protect against zero as the scaled frequency if (is_positive(scaled_freq)) { - _calculated_harmonic_notch_freq_hz[0] = scaled_freq; + _calculated_harmonic_notch_freq_hz[idx][0] = scaled_freq; } - _num_calculated_harmonic_notch_frequencies = 1; + _num_calculated_harmonic_notch_frequencies[idx] = 1; } // Update the harmonic notch frequency -void AP_InertialSensor::update_harmonic_notch_frequencies_hz(uint8_t num_freqs, const float scaled_freq[]) { +void AP_InertialSensor::update_harmonic_notch_frequencies_hz(uint8_t idx, uint8_t num_freqs, const float scaled_freq[]) { + if (idx >= HAL_INS_NUM_HARMONIC_NOTCH_FILTERS) { + return; + } // protect against zero as the scaled frequency for (uint8_t i = 0; i < num_freqs; i++) { if (is_positive(scaled_freq[i])) { - _calculated_harmonic_notch_freq_hz[i] = scaled_freq[i]; + _calculated_harmonic_notch_freq_hz[idx][i] = scaled_freq[i]; } } // any uncalculated frequencies will float at the previous value or the initialized freq if none - _num_calculated_harmonic_notch_frequencies = num_freqs; + _num_calculated_harmonic_notch_frequencies[idx] = num_freqs; } /* diff --git a/libraries/AP_InertialSensor/AP_InertialSensor.h b/libraries/AP_InertialSensor/AP_InertialSensor.h index 81c7155931..37e477df80 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor.h +++ b/libraries/AP_InertialSensor/AP_InertialSensor.h @@ -35,6 +35,9 @@ #define HAL_INS_TEMPERATURE_CAL_ENABLE !HAL_MINIMIZE_FEATURES && BOARD_FLASH_SIZE > 1024 #endif +#ifndef HAL_INS_NUM_HARMONIC_NOTCH_FILTERS +#define HAL_INS_NUM_HARMONIC_NOTCH_FILTERS 2 +#endif #include @@ -241,9 +244,9 @@ public: uint8_t get_primary_gyro(void) const { return _primary_gyro; } // Update the harmonic notch frequency - void update_harmonic_notch_freq_hz(float scaled_freq); + void update_harmonic_notch_freq_hz(uint8_t idx, float scaled_freq); // Update the harmonic notch frequencies - void update_harmonic_notch_frequencies_hz(uint8_t num_freqs, const float scaled_freq[]); + void update_harmonic_notch_frequencies_hz(uint8_t idx, uint8_t num_freqs, const float scaled_freq[]); // get the gyro filter rate in Hz uint16_t get_gyro_filter_hz(void) const { return _gyro_filter_cutoff; } @@ -252,32 +255,32 @@ public: uint16_t get_accel_filter_hz(void) const { return _accel_filter_cutoff; } // harmonic notch current center frequency - float get_gyro_dynamic_notch_center_freq_hz(void) const { return _calculated_harmonic_notch_freq_hz[0]; } + float get_gyro_dynamic_notch_center_freq_hz(uint8_t idx) const { return idx 1) { - _imu._gyro_harmonic_notch_filter[instance].update(num_gyro_harmonic_notch_center_frequencies(), gyro_harmonic_notch_center_frequencies_hz()); - } else { - _imu._gyro_harmonic_notch_filter[instance].update(gyro_harmonic_notch_center_freq_hz()); + for (uint8_t i=0; i 1) { + _imu._gyro_harmonic_notch_filter[i][instance].update(num_gyro_harmonic_notch_center_frequencies(i), gyro_harmonic_notch_center_frequencies_hz(i)); + } else { + _imu._gyro_harmonic_notch_filter[i][instance].update(gyro_harmonic_notch_center_freq_hz(i)); + } + _last_harmonic_notch_center_freq_hz[i] = gyro_harmonic_notch_center_freq_hz(i); } - _last_harmonic_notch_center_freq_hz = gyro_harmonic_notch_center_freq_hz(); - } - // possily update the notch filter parameters - if (!is_equal(_last_notch_center_freq_hz, _gyro_notch_center_freq_hz()) || - !is_equal(_last_notch_bandwidth_hz, _gyro_notch_bandwidth_hz()) || - !is_equal(_last_notch_attenuation_dB, _gyro_notch_attenuation_dB()) || - sensors_converging()) { - _imu._gyro_notch_filter[instance].init(_gyro_raw_sample_rate(instance), _gyro_notch_center_freq_hz(), _gyro_notch_bandwidth_hz(), _gyro_notch_attenuation_dB()); - _last_notch_center_freq_hz = _gyro_notch_center_freq_hz(); - _last_notch_bandwidth_hz = _gyro_notch_bandwidth_hz(); - _last_notch_attenuation_dB = _gyro_notch_attenuation_dB(); } } diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_Backend.h b/libraries/AP_InertialSensor/AP_InertialSensor_Backend.h index db5d226269..2e54ded740 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_Backend.h +++ b/libraries/AP_InertialSensor/AP_InertialSensor_Backend.h @@ -257,33 +257,22 @@ protected: return (uint16_t)_imu._loop_rate; } - // return the notch filter center in Hz for the sample rate - float _gyro_notch_center_freq_hz(void) const { return _imu._notch_filter.center_freq_hz(); } - - // return the notch filter bandwidth in Hz for the sample rate - float _gyro_notch_bandwidth_hz(void) const { return _imu._notch_filter.bandwidth_hz(); } - - // return the notch filter attenuation in dB for the sample rate - float _gyro_notch_attenuation_dB(void) const { return _imu._notch_filter.attenuation_dB(); } - - bool _gyro_notch_enabled(void) const { return _imu._notch_filter.enabled(); } - // return the harmonic notch filter center in Hz for the sample rate - float gyro_harmonic_notch_center_freq_hz() const { return _imu.get_gyro_dynamic_notch_center_freq_hz(); } + float gyro_harmonic_notch_center_freq_hz(uint8_t idx) const { return idx