From 43e93ccf27496448553173d3711e19f5c059dde5 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Fri, 15 Apr 2022 17:38:56 +1000 Subject: [PATCH] AP_InertialSensor: switch to HarmonicNotch class this makes the logic much easier to follow, without indexes into arrays --- .../AP_InertialSensor/AP_InertialSensor.cpp | 99 ++++++++++++------- .../AP_InertialSensor/AP_InertialSensor.h | 87 +++++++--------- .../AP_InertialSensor_Backend.cpp | 42 ++------ .../AP_InertialSensor_Backend.h | 24 +---- .../AP_InertialSensor_Logging.cpp | 12 +-- 5 files changed, 117 insertions(+), 147 deletions(-) diff --git a/libraries/AP_InertialSensor/AP_InertialSensor.cpp b/libraries/AP_InertialSensor/AP_InertialSensor.cpp index 071e17120e..e68e1745d9 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor.cpp +++ b/libraries/AP_InertialSensor/AP_InertialSensor.cpp @@ -548,12 +548,12 @@ const AP_Param::GroupInfo AP_InertialSensor::var_info[] = { // @Group: HNTCH_ // @Path: ../Filter/HarmonicNotchFilter.cpp - AP_SUBGROUPINFO(_harmonic_notch_filter[0], "HNTCH_", 41, AP_InertialSensor, HarmonicNotchFilterParams), + AP_SUBGROUPINFO(harmonic_notches[0].params, "HNTCH_", 41, AP_InertialSensor, HarmonicNotchFilterParams), #if HAL_INS_NUM_HARMONIC_NOTCH_FILTERS > 1 // @Group: HNTC2_ // @Path: ../Filter/HarmonicNotchFilter.cpp - AP_SUBGROUPINFO(_harmonic_notch_filter[1], "HNTC2_", 53, AP_InertialSensor, HarmonicNotchFilterParams), + AP_SUBGROUPINFO(harmonic_notches[1].params, "HNTC2_", 53, AP_InertialSensor, HarmonicNotchFilterParams), #endif // @Param: GYRO_RATE @@ -876,28 +876,25 @@ 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 - uint8_t num_filters = 0; - bool double_notch[HAL_INS_NUM_HARMONIC_NOTCH_FILTERS] {}; - for (uint8_t i=0; iget_motor_mask()); + notch.num_dynamic_notches = __builtin_popcount(motors->get_motor_mask()); } } // avoid harmonics unless actually configured by the user - _harmonic_notch_filter[i].set_default_harmonics(1); + notch.params.set_default_harmonics(1); } #endif } @@ -907,11 +904,13 @@ AP_InertialSensor::init(uint16_t loop_rate) sensors_used += _use[i]; } - for (uint8_t i=0; i 1) { + filter[instance].update(num_calculated_notch_frequencies, calculated_notch_freq_hz); + } else { + filter[instance].update(center_freq); + } + last_center_freq_hz = center_freq; + } +} /* update gyro and accel values from backends @@ -1606,6 +1631,13 @@ void AP_InertialSensor::update(void) for (uint8_t i=0; i<_backend_count; i++) { _backends[i]->update(); } + for (uint8_t i=0; i<_gyro_count; i++) { + 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 (!_startup_error_counts_set) { for (uint8_t i=0; i= HAL_INS_NUM_HARMONIC_NOTCH_FILTERS) { - return; - } +void AP_InertialSensor::HarmonicNotch::update_freq_hz(float scaled_freq) +{ // protect against zero as the scaled frequency if (is_positive(scaled_freq)) { - _calculated_harmonic_notch_freq_hz[idx][0] = scaled_freq; + calculated_notch_freq_hz[0] = scaled_freq; } - _num_calculated_harmonic_notch_frequencies[idx] = 1; + num_calculated_notch_frequencies = 1; } // Update the harmonic notch frequency -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; - } +void AP_InertialSensor::HarmonicNotch::update_frequencies_hz(uint8_t num_freqs, const float scaled_freq[]) { // 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[idx][i] = scaled_freq[i]; + calculated_notch_freq_hz[i] = scaled_freq[i]; } } // any uncalculated frequencies will float at the previous value or the initialized freq if none - _num_calculated_harmonic_notch_frequencies[idx] = num_freqs; + num_calculated_notch_frequencies = num_freqs; } /* diff --git a/libraries/AP_InertialSensor/AP_InertialSensor.h b/libraries/AP_InertialSensor/AP_InertialSensor.h index 37e477df80..eddf939478 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor.h +++ b/libraries/AP_InertialSensor/AP_InertialSensor.h @@ -39,6 +39,11 @@ #define HAL_INS_NUM_HARMONIC_NOTCH_FILTERS 2 #endif +// time for the estimated gyro rates to converge +#ifndef HAL_INS_CONVERGANCE_MS +#define HAL_INS_CONVERGANCE_MS 30000 +#endif + #include #include @@ -243,46 +248,12 @@ public: uint8_t get_primary_accel(void) const { return _primary_accel; } uint8_t get_primary_gyro(void) const { return _primary_gyro; } - // Update the harmonic notch frequency - 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 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; } // get the accel filter rate in Hz 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(uint8_t idx) const { return idx 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); - } - } } /* diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_Backend.h b/libraries/AP_InertialSensor/AP_InertialSensor_Backend.h index 2e54ded740..2dcd56958b 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_Backend.h +++ b/libraries/AP_InertialSensor/AP_InertialSensor_Backend.h @@ -200,7 +200,7 @@ protected: void _update_sensor_rate(uint16_t &count, uint32_t &start_us, float &rate_hz) const __RAMFUNC__; // return true if the sensors are still converging and sampling rates could change significantly - bool sensors_converging() const { return AP_HAL::millis() < 30000; } + bool sensors_converging() const { return AP_HAL::millis() < HAL_INS_CONVERGANCE_MS; } // set accelerometer max absolute offset for calibration void _set_accel_max_abs_offset(uint8_t instance, float offset); @@ -257,23 +257,6 @@ protected: return (uint16_t)_imu._loop_rate; } - // return the harmonic notch filter center in Hz for the sample rate - float gyro_harmonic_notch_center_freq_hz(uint8_t idx) const { return idx