From 13e62ef265bb1912891f7f98ebfec028c6001ffe Mon Sep 17 00:00:00 2001 From: Andy Piper Date: Fri, 3 Nov 2023 17:05:23 +0000 Subject: [PATCH 01/15] Filter: allow zero centre frequency to passthrough and disable notch --- libraries/Filter/HarmonicNotchFilter.cpp | 4 ++-- libraries/Filter/NotchFilter.cpp | 2 +- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/libraries/Filter/HarmonicNotchFilter.cpp b/libraries/Filter/HarmonicNotchFilter.cpp index f5e58f492596a7..1700f3b1bf789f 100644 --- a/libraries/Filter/HarmonicNotchFilter.cpp +++ b/libraries/Filter/HarmonicNotchFilter.cpp @@ -203,7 +203,7 @@ void HarmonicNotchFilter::update(float center_freq_hz) // adjust the fundamental center frequency to be in the allowable range const float nyquist_limit = _sample_freq_hz * 0.48f; - center_freq_hz = constrain_float(center_freq_hz, 1.0f, nyquist_limit); + center_freq_hz = constrain_float(center_freq_hz, 0.0f, nyquist_limit); _num_enabled_filters = 0; // update all of the filters using the new center frequency and existing A & Q @@ -264,7 +264,7 @@ void HarmonicNotchFilter::update(uint8_t num_centers, const float center_freq continue; } - const float notch_center = constrain_float(center_freq_hz[center_n] * (harmonic_n+1), 1.0f, nyquist_limit); + const float notch_center = constrain_float(center_freq_hz[center_n] * (harmonic_n+1), 0.0f, nyquist_limit); if (_composite_notches != 2) { // only enable the filter if its center frequency is below the nyquist frequency if (notch_center < nyquist_limit) { diff --git a/libraries/Filter/NotchFilter.cpp b/libraries/Filter/NotchFilter.cpp index 538eb02deef752..37097dffafb1c0 100644 --- a/libraries/Filter/NotchFilter.cpp +++ b/libraries/Filter/NotchFilter.cpp @@ -71,7 +71,7 @@ void NotchFilter::init_with_A_and_Q(float sample_freq_hz, float center_freq_h _center_freq_hz * NOTCH_MAX_SLEW_UPPER); } - if ((new_center_freq > 2.0) && (new_center_freq < 0.5 * sample_freq_hz) && (Q > 0.0)) { + if (is_positive(new_center_freq) && (new_center_freq < 0.5 * sample_freq_hz) && (Q > 0.0)) { float omega = 2.0 * M_PI * new_center_freq / sample_freq_hz; float alpha = sinf(omega) / (2 * Q); b0 = 1.0 + alpha*sq(A); From b95b98315893bfe416a866de2d019bc92833117f Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Tue, 7 Nov 2023 17:19:58 +1100 Subject: [PATCH 02/15] Filter: added optional notch debug logging this has helped find multiple bugs --- libraries/Filter/HarmonicNotchFilter.cpp | 31 ++++++++++++++++++++++++ libraries/Filter/NotchFilter.h | 4 +++ 2 files changed, 35 insertions(+) diff --git a/libraries/Filter/HarmonicNotchFilter.cpp b/libraries/Filter/HarmonicNotchFilter.cpp index 1700f3b1bf789f..b2ecc098214a9f 100644 --- a/libraries/Filter/HarmonicNotchFilter.cpp +++ b/libraries/Filter/HarmonicNotchFilter.cpp @@ -20,10 +20,22 @@ #include "HarmonicNotchFilter.h" #include +#include +#include +#include +#include #define HNF_MAX_FILTERS HAL_HNF_MAX_FILTERS // must be even for double-notch filters #define HNF_MAX_HARMONICS 8 +/* + optional logging for SITL only of all notch frequencies + */ +#ifndef NOTCH_DEBUG_LOGGING +#define NOTCH_DEBUG_LOGGING 0 +#endif + + // table of user settable parameters const AP_Param::GroupInfo HarmonicNotchFilterParams::var_info[] = { @@ -297,10 +309,29 @@ T HarmonicNotchFilter::apply(const T &sample) return sample; } +#if NOTCH_DEBUG_LOGGING + static int dfd = -1; + if (dfd == -1) { + dfd = ::open("notch.txt", O_WRONLY|O_CREAT|O_TRUNC, 0644); + } +#endif + T output = sample; for (uint8_t i = 0; i < _num_enabled_filters; i++) { +#if NOTCH_DEBUG_LOGGING + if (!_filters[i].initialised) { + ::dprintf(dfd, "------- "); + } else { + ::dprintf(dfd, "%.4f ", _filters[i]._center_freq_hz); + } +#endif output = _filters[i].apply(output); } +#if NOTCH_DEBUG_LOGGING + if (_num_enabled_filters > 0) { + ::dprintf(dfd, "\n"); + } +#endif return output; } diff --git a/libraries/Filter/NotchFilter.h b/libraries/Filter/NotchFilter.h index 53bacc8e7419f3..db8c07fe52b16a 100644 --- a/libraries/Filter/NotchFilter.h +++ b/libraries/Filter/NotchFilter.h @@ -26,9 +26,13 @@ #include +template +class HarmonicNotchFilter; + template class NotchFilter { public: + friend class HarmonicNotchFilter; // set parameters void init(float sample_freq_hz, float center_freq_hz, float bandwidth_hz, float attenuation_dB); void init_with_A_and_Q(float sample_freq_hz, float center_freq_hz, float A, float Q); From c8976b0b23096eb6c2486fcab61988b329a27d1e Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Tue, 7 Nov 2023 17:29:55 +1100 Subject: [PATCH 03/15] Filter: fixed uint8_t bug in total filters and expand_filter_count bug this gets the right number of notches on quadplanes, but is still very bad in fwd flight --- libraries/Filter/HarmonicNotchFilter.cpp | 22 +++++++++------------- libraries/Filter/HarmonicNotchFilter.h | 6 +++--- 2 files changed, 12 insertions(+), 16 deletions(-) diff --git a/libraries/Filter/HarmonicNotchFilter.cpp b/libraries/Filter/HarmonicNotchFilter.cpp index b2ecc098214a9f..348e65c84613c8 100644 --- a/libraries/Filter/HarmonicNotchFilter.cpp +++ b/libraries/Filter/HarmonicNotchFilter.cpp @@ -175,13 +175,8 @@ void HarmonicNotchFilter::allocate_filters(uint8_t num_notches, uint8_t harmo expand the number of filters at runtime, allowing for RPM sources such as lua scripts */ template -void HarmonicNotchFilter::expand_filter_count(uint8_t num_notches) +void HarmonicNotchFilter::expand_filter_count(uint16_t total_notches) { - uint8_t num_filters = _num_harmonics * num_notches * _composite_notches; - if (num_filters <= _num_filters) { - // enough already - return; - } if (_alloc_has_failed) { // we've failed to allocate before, don't try again return; @@ -190,7 +185,7 @@ void HarmonicNotchFilter::expand_filter_count(uint8_t num_notches) note that we rely on the semaphore in AP_InertialSensor_Backend.cpp to make this thread safe */ - auto filters = new NotchFilter[num_filters]; + auto filters = new NotchFilter[total_notches]; if (filters == nullptr) { _alloc_has_failed = true; return; @@ -198,7 +193,7 @@ void HarmonicNotchFilter::expand_filter_count(uint8_t num_notches) memcpy(filters, _filters, sizeof(filters[0])*_num_filters); auto _old_filters = _filters; _filters = filters; - _num_filters = num_filters; + _num_filters = total_notches; delete[] _old_filters; } @@ -259,15 +254,16 @@ void HarmonicNotchFilter::update(uint8_t num_centers, const float center_freq // adjust the frequencies to be in the allowable range const float nyquist_limit = _sample_freq_hz * 0.48f; - if (num_centers > _num_filters) { + uint16_t total_notches = num_centers * _num_harmonics * _composite_notches; + if (total_notches > _num_filters) { // alloc realloc of filters - expand_filter_count(num_centers); + expand_filter_count(total_notches); } _num_enabled_filters = 0; // update all of the filters using the new center frequencies and existing A & Q - for (uint8_t i = 0; i < num_centers * HNF_MAX_HARMONICS && _num_enabled_filters < _num_filters; i++) { + for (uint16_t i = 0; i < num_centers * HNF_MAX_HARMONICS && _num_enabled_filters < _num_filters; i++) { const uint8_t harmonic_n = i / num_centers; const uint8_t center_n = i % num_centers; // the filters are ordered by center and then harmonic so @@ -317,7 +313,7 @@ T HarmonicNotchFilter::apply(const T &sample) #endif T output = sample; - for (uint8_t i = 0; i < _num_enabled_filters; i++) { + for (uint16_t i = 0; i < _num_enabled_filters; i++) { #if NOTCH_DEBUG_LOGGING if (!_filters[i].initialised) { ::dprintf(dfd, "------- "); @@ -345,7 +341,7 @@ void HarmonicNotchFilter::reset() return; } - for (uint8_t i = 0; i < _num_filters; i++) { + for (uint16_t i = 0; i < _num_filters; i++) { _filters[i].reset(); } } diff --git a/libraries/Filter/HarmonicNotchFilter.h b/libraries/Filter/HarmonicNotchFilter.h index 064365f0b27713..500d82809f57f1 100644 --- a/libraries/Filter/HarmonicNotchFilter.h +++ b/libraries/Filter/HarmonicNotchFilter.h @@ -32,7 +32,7 @@ class HarmonicNotchFilter { // allocate a bank of notch filters for this harmonic notch filter void allocate_filters(uint8_t num_notches, uint8_t harmonics, uint8_t composite_notches); // expand filter bank with new filters - void expand_filter_count(uint8_t num_notches); + void expand_filter_count(uint16_t total_notches); // initialize the underlying filters using the provided filter parameters void init(float sample_freq_hz, float center_freq_hz, float bandwidth_hz, float attenuation_dB); // update the underlying filters' center frequencies using center_freq_hz as the fundamental @@ -60,11 +60,11 @@ class HarmonicNotchFilter { // number of notches that make up a composite notch uint8_t _composite_notches; // number of allocated filters - uint8_t _num_filters; + uint16_t _num_filters; // pre-calculated number of harmonics uint8_t _num_harmonics; // number of enabled filters - uint8_t _num_enabled_filters; + uint16_t _num_enabled_filters; bool _initialised; // have we failed to expand filters? From 313093b4f2ace74168da468c3b5c439b7fa62893 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Wed, 31 Jan 2024 09:00:02 +1100 Subject: [PATCH 04/15] Filter: rework harmonic notch do all frequenct clamping in one place in set_center_frequency(). Allow for zero frequency to disable the notch. Add an option to treat inactive RPM source as min frequency # Conflicts: # libraries/Filter/HarmonicNotchFilter.cpp # libraries/Filter/NotchFilter.h --- libraries/Filter/HarmonicNotchFilter.cpp | 129 ++++++++++++++++------- libraries/Filter/HarmonicNotchFilter.h | 23 +++- libraries/Filter/NotchFilter.h | 6 +- 3 files changed, 119 insertions(+), 39 deletions(-) diff --git a/libraries/Filter/HarmonicNotchFilter.cpp b/libraries/Filter/HarmonicNotchFilter.cpp index 348e65c84613c8..853e7c3a9a9275 100644 --- a/libraries/Filter/HarmonicNotchFilter.cpp +++ b/libraries/Filter/HarmonicNotchFilter.cpp @@ -96,8 +96,8 @@ const AP_Param::GroupInfo HarmonicNotchFilterParams::var_info[] = { // @Param: OPTS // @DisplayName: Harmonic Notch Filter options - // @Description: Harmonic Notch Filter options. Triple and double-notches can provide deeper attenuation across a wider bandwidth with reduced latency than single notches and are suitable for larger aircraft. Dynamic harmonics attaches a harmonic notch to each detected noise frequency instead of simply being multiples of the base frequency, in the case of FFT it will attach notches to each of three detected noise peaks, in the case of ESC it will attach notches to each of four motor RPM values. Loop rate update changes the notch center frequency at the scheduler loop rate rather than at the default of 200Hz. If both double and triple notches are specified only double notches will take effect. - // @Bitmask: 0:Double notch,1:Dynamic harmonic,2:Update at loop rate,3:EnableOnAllIMUs,4:Triple notch + // @Description: Harmonic Notch Filter options. Triple and double-notches can provide deeper attenuation across a wider bandwidth with reduced latency than single notches and are suitable for larger aircraft. Multi-Source attaches a harmonic notch to each detected noise frequency instead of simply being multiples of the base frequency, in the case of FFT it will attach notches to each of three detected noise peaks, in the case of ESC it will attach notches to each of four motor RPM values. Loop rate update changes the notch center frequency at the scheduler loop rate rather than at the default of 200Hz. If both double and triple notches are specified only double notches will take effect. + // @Bitmask: 0:Double notch,1:Multi-Source,2:Update at loop rate,3:EnableOnAllIMUs,4:Triple notch, 5:Use min freq on RPM failure // @User: Advanced // @RebootRequired: True AP_GROUPINFO("OPTS", 8, HarmonicNotchFilterParams, _options, 0), @@ -127,7 +127,7 @@ HarmonicNotchFilter::~HarmonicNotchFilter() { the constraints are used to determine attenuation (A) and quality (Q) factors for the filter */ template -void HarmonicNotchFilter::init(float sample_freq_hz, float center_freq_hz, float bandwidth_hz, float attenuation_dB) +void HarmonicNotchFilter::init(float sample_freq_hz, const HarmonicNotchFilterParams ¶ms) { // sanity check the input if (_filters == nullptr || is_zero(sample_freq_hz) || isnan(sample_freq_hz)) { @@ -136,10 +136,22 @@ void HarmonicNotchFilter::init(float sample_freq_hz, float center_freq_hz, fl _sample_freq_hz = sample_freq_hz; + const float bandwidth_hz = params.bandwidth_hz(); + const float attenuation_dB = params.attenuation_dB(); + float center_freq_hz = params.center_freq_hz(); + const float nyquist_limit = sample_freq_hz * 0.48f; const float bandwidth_limit = bandwidth_hz * 0.52f; - // adjust the fundamental center frequency to be in the allowable range + + // remember the lowest frequency we will have a notch enabled at + _minimum_freq = center_freq_hz * params.freq_min_ratio(); + + /* + adjust the fundamental center frequency we use for the initial + calculation of A and Q to be in the allowable range + */ center_freq_hz = constrain_float(center_freq_hz, bandwidth_limit, nyquist_limit); + // Calculate spread required to achieve an equivalent single notch using two notches with Bandwidth/2 _notch_spread = bandwidth_hz / (32 * center_freq_hz); @@ -147,8 +159,9 @@ void HarmonicNotchFilter::init(float sample_freq_hz, float center_freq_hz, fl // calculate attenuation and quality from the shaping constraints NotchFilter::calculate_A_and_Q(center_freq_hz, bandwidth_hz / _composite_notches, attenuation_dB, _A, _Q); + _treat_low_freq_as_min = params.hasOption(HarmonicNotchFilterParams::Options::TreatLowAsMin); + _initialised = true; - update(center_freq_hz); } /* @@ -197,6 +210,72 @@ void HarmonicNotchFilter::expand_filter_count(uint16_t total_notches) delete[] _old_filters; } +/* + set the center frequency of a single notch + */ +template +void HarmonicNotchFilter::set_center_frequency(uint8_t idx, float notch_center, float spread_mul, uint8_t harmonic_mul) +{ + const float nyquist_limit = _sample_freq_hz * 0.48f; + auto ¬ch = _filters[idx]; + + // scale the notch with the harmonic multiplier + notch_center *= harmonic_mul; + + /* disable the filter if its center frequency is above the nyquist + frequency + + NOTE: should this be notch_center*spread_mul ? As it is here we + do sometimes run the upper notch in a double or triple notch at + higher than the nyquist. + */ + if (notch_center >= nyquist_limit) { + notch.disable(); + return; + } + + // the minimum frequency for a harmonic is the base minimum frequency + // scaled for the harmonic multiplier + const float harmonic_min_freq = _minimum_freq * harmonic_mul; + + // on some vehicles we can adjust the attenuation at low frequencies + float A = _A; + + // on some vehicles we want to treat zero or very low frequency as + // the minimum frequency, on others we want to disable the + // notch. That is controlled by the _treat_low_freq_as_min flag + if (!_treat_low_freq_as_min) { + /* + disable if the notch is less than half of the min frequency + */ + const float disable_threshold = 0.5; + if (notch_center < disable_threshold * _minimum_freq) { + notch.disable(); + return; + } + + if (notch_center < _minimum_freq) { + /* + scale the attenuation so that we fade out the notch as + we get further below the min frequency + + TODO: determine if this linear scaling is the right + function. Very likely we will need something more complex + */ + A = linear_interpolate(1.0, _A, + notch_center, + disable_threshold * _minimum_freq, + _minimum_freq); + } + } + + // never run the center of a notch at a lower frequency than the + // minimum specified in the parameters. + notch_center = MAX(notch_center, harmonic_min_freq) * spread_mul; + + notch.init_with_A_and_Q(_sample_freq_hz, notch_center, A, _Q); +} + /* update the underlying filters' center frequency using the current attenuation and quality this function is cheaper than init() because A & Q do not need to be recalculated @@ -216,25 +295,14 @@ void HarmonicNotchFilter::update(float center_freq_hz) // update all of the filters using the new center frequency and existing A & Q for (uint8_t i = 0; i < HNF_MAX_HARMONICS && _num_enabled_filters < _num_filters; i++) { if ((1U< 1) { - float notch_center_double; - // only enable the filter if its center frequency is below the nyquist frequency - notch_center_double = notch_center * (1.0 - _notch_spread); - if (notch_center_double < nyquist_limit) { - _filters[_num_enabled_filters++].init_with_A_and_Q(_sample_freq_hz, notch_center_double, _A, _Q); - } - // only enable the filter if its center frequency is below the nyquist frequency - notch_center_double = notch_center * (1.0 + _notch_spread); - if (notch_center_double < nyquist_limit) { - _filters[_num_enabled_filters++].init_with_A_and_Q(_sample_freq_hz, notch_center_double, _A, _Q); - } + set_center_frequency(_num_enabled_filters++, notch_center, 1.0 - _notch_spread, harmonic_mul); + set_center_frequency(_num_enabled_filters++, notch_center, 1.0 + _notch_spread, harmonic_mul); } } } @@ -273,24 +341,13 @@ void HarmonicNotchFilter::update(uint8_t num_centers, const float center_freq } const float notch_center = constrain_float(center_freq_hz[center_n] * (harmonic_n+1), 0.0f, nyquist_limit); + const uint8_t harmonic_mul = (harmonic_n+1); if (_composite_notches != 2) { - // only enable the filter if its center frequency is below the nyquist frequency - if (notch_center < nyquist_limit) { - _filters[_num_enabled_filters++].init_with_A_and_Q(_sample_freq_hz, notch_center, _A, _Q); - } + set_center_frequency(_num_enabled_filters++, notch_center, 1.0, harmonic_mul); } if (_composite_notches > 1) { - float notch_center_double; - // only enable the filter if its center frequency is below the nyquist frequency - notch_center_double = notch_center * (1.0 - _notch_spread); - if (notch_center_double < nyquist_limit) { - _filters[_num_enabled_filters++].init_with_A_and_Q(_sample_freq_hz, notch_center_double, _A, _Q); - } - // only enable the filter if its center frequency is below the nyquist frequency - notch_center_double = notch_center * (1.0 + _notch_spread); - if (notch_center_double < nyquist_limit) { - _filters[_num_enabled_filters++].init_with_A_and_Q(_sample_freq_hz, notch_center_double, _A, _Q); - } + set_center_frequency(_num_enabled_filters++, notch_center, 1.0 - _notch_spread, harmonic_mul); + set_center_frequency(_num_enabled_filters++, notch_center, 1.0 + _notch_spread, harmonic_mul); } } } diff --git a/libraries/Filter/HarmonicNotchFilter.h b/libraries/Filter/HarmonicNotchFilter.h index 500d82809f57f1..e6a1219718f7de 100644 --- a/libraries/Filter/HarmonicNotchFilter.h +++ b/libraries/Filter/HarmonicNotchFilter.h @@ -21,6 +21,8 @@ #define HNF_MAX_HARMONICS 8 +class HarmonicNotchFilterParams; + /* a filter that manages a set of notch filters targetted at a fundamental center frequency and multiples of that fundamental frequency @@ -34,11 +36,19 @@ class HarmonicNotchFilter { // expand filter bank with new filters void expand_filter_count(uint16_t total_notches); // initialize the underlying filters using the provided filter parameters - void init(float sample_freq_hz, float center_freq_hz, float bandwidth_hz, float attenuation_dB); + void init(float sample_freq_hz, const HarmonicNotchFilterParams ¶ms); // update the underlying filters' center frequencies using center_freq_hz as the fundamental void update(float center_freq_hz); - // update all o fthe underlying center frequencies individually + // update all of the underlying center frequencies individually void update(uint8_t num_centers, const float center_freq_hz[]); + + /* + set center frequency of one notch. + spread_mul is a scale factor for spreading of double or triple notch + harmonic_mul is the multiplier for harmonics, 1 is for the fundamental + */ + void set_center_frequency(uint8_t idx, float center_freq_hz, float spread_mul, uint8_t harmonic_mul); + // apply a sample to each of the underlying filters in turn T apply(const T &sample); // reset each of the underlying filters @@ -69,6 +79,14 @@ class HarmonicNotchFilter { // have we failed to expand filters? bool _alloc_has_failed; + + // minimum frequency (from INS_HNTCH_FREQ * INS_HNTCH_FM_RAT) + float _minimum_freq; + + /* + flag for treating a very low frequency as the min frequency + */ + bool _treat_low_freq_as_min; }; // Harmonic notch update mode @@ -92,6 +110,7 @@ class HarmonicNotchFilterParams : public NotchFilterParams { LoopRateUpdate = 1<<2, EnableOnAllIMUs = 1<<3, TripleNotch = 1<<4, + TreatLowAsMin = 1<<5, }; HarmonicNotchFilterParams(void); diff --git a/libraries/Filter/NotchFilter.h b/libraries/Filter/NotchFilter.h index db8c07fe52b16a..48480238ffc50e 100644 --- a/libraries/Filter/NotchFilter.h +++ b/libraries/Filter/NotchFilter.h @@ -42,7 +42,11 @@ class NotchFilter { // calculate attenuation and quality from provided center frequency and bandwidth static void calculate_A_and_Q(float center_freq_hz, float bandwidth_hz, float attenuation_dB, float& A, float& Q); -private: + void disable(void) { + initialised = false; + } + +protected: bool initialised, need_reset; float b0, b1, b2, a1, a2, a0_inv; From a4099f6050116e98e6114bd5244309e2a5b92221 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Wed, 31 Jan 2024 09:00:28 +1100 Subject: [PATCH 05/15] AP_Vehicle: removed the clamping of notch filters at vehicle level moved the claiming down into HarmonicNotchFilter # Conflicts: # libraries/AP_Vehicle/AP_Vehicle.cpp --- libraries/AP_Vehicle/AP_Vehicle.cpp | 35 +++++------------------------ 1 file changed, 6 insertions(+), 29 deletions(-) diff --git a/libraries/AP_Vehicle/AP_Vehicle.cpp b/libraries/AP_Vehicle/AP_Vehicle.cpp index 309d76d42b01c1..35c9ae040f503b 100644 --- a/libraries/AP_Vehicle/AP_Vehicle.cpp +++ b/libraries/AP_Vehicle/AP_Vehicle.cpp @@ -512,7 +512,6 @@ void AP_Vehicle::update_throttle_notch(AP_InertialSensor::HarmonicNotch ¬ch) #if APM_BUILD_TYPE(APM_BUILD_ArduPlane)||APM_BUILD_COPTER_OR_HELI||APM_BUILD_TYPE(APM_BUILD_Rover) const float ref_freq = notch.params.center_freq_hz(); const float ref = notch.params.reference(); - const float min_ratio = notch.params.freq_min_ratio(); #if APM_BUILD_TYPE(APM_BUILD_ArduPlane)||APM_BUILD_COPTER_OR_HELI const AP_Motors* motors = AP::motors(); @@ -522,7 +521,7 @@ void AP_Vehicle::update_throttle_notch(AP_InertialSensor::HarmonicNotch ¬ch) const float motors_throttle = motors != nullptr ? abs(motors->get_throttle() / 100.0f) : 0; #endif - float throttle_freq = ref_freq * MAX(min_ratio, sqrtf(motors_throttle / ref)); + float throttle_freq = ref_freq * sqrtf(MAX(0,motors_throttle) / ref); notch.update_freq_hz(throttle_freq); #endif @@ -542,17 +541,6 @@ void AP_Vehicle::update_dynamic_notch(AP_InertialSensor::HarmonicNotch ¬ch) return; } -#if APM_BUILD_TYPE(APM_BUILD_ArduPlane)||APM_BUILD_COPTER_OR_HELI - const AP_Motors* motors = AP::motors(); - if (motors != nullptr && motors->get_spool_state() == AP_Motors::SpoolState::SHUT_DOWN) { - notch.set_inactive(true); - } else { - notch.set_inactive(false); - } -#else // APM_BUILD_Rover: keep notch active - notch.set_inactive(false); -#endif - switch (notch.params.tracking_mode()) { case HarmonicNotchDynamicMode::UpdateThrottle: // throttle based tracking // set the harmonic notch filter frequency approximately scaled on motor rpm implied by throttle @@ -567,9 +555,9 @@ void AP_Vehicle::update_dynamic_notch(AP_InertialSensor::HarmonicNotch ¬ch) float rpm; if (rpm_sensor != nullptr && rpm_sensor->get_rpm(sensor, rpm)) { // set the harmonic notch filter frequency from the main rotor rpm - notch.update_freq_hz(MAX(ref_freq, rpm * ref * (1.0/60))); + notch.update_freq_hz(rpm * ref * (1.0/60)); } else { - notch.update_freq_hz(ref_freq); + notch.update_freq_hz(0); } break; } @@ -581,18 +569,13 @@ void AP_Vehicle::update_dynamic_notch(AP_InertialSensor::HarmonicNotch ¬ch) float notches[INS_MAX_NOTCHES]; // ESC telemetry will return 0 for missing data, but only after 1s const uint8_t num_notches = AP::esc_telem().get_motor_frequencies_hz(INS_MAX_NOTCHES, notches); - for (uint8_t i = 0; i < num_notches; i++) { - if (!is_zero(notches[i])) { - notches[i] = MAX(ref_freq, notches[i]); - } - } if (num_notches > 0) { notch.update_frequencies_hz(num_notches, notches); } else { // throttle fallback update_throttle_notch(notch); } } else { - notch.update_freq_hz(MAX(ref_freq, AP::esc_telem().get_average_motor_frequency_hz() * ref)); + notch.update_freq_hz(AP::esc_telem().get_average_motor_frequency_hz() * ref); } break; #endif @@ -604,20 +587,14 @@ void AP_Vehicle::update_dynamic_notch(AP_InertialSensor::HarmonicNotch ¬ch) const uint8_t peaks = gyro_fft.get_weighted_noise_center_frequencies_hz(notch.num_dynamic_notches, notches); if (peaks > 0) { - for (uint8_t i = 0; i < peaks; i++) { - notches[i] = MAX(ref_freq, notches[i]); - } + notch.set_inactive(false); notch.update_frequencies_hz(peaks, notches); } else { // since FFT can be used post-filter it is better to disable the notch when there is no data notch.set_inactive(true); } } else { float center_freq = gyro_fft.get_weighted_noise_center_freq_hz(); - if (!is_zero(center_freq)) { - notch.update_freq_hz(MAX(ref_freq, center_freq)); - } else { // since FFT can be used post-filter it is better to disable the notch when there is no data - notch.set_inactive(true); - } + notch.update_freq_hz(center_freq); } break; #endif From 3507e32aa892b97649e00a46da524784fe6e90ab Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Wed, 31 Jan 2024 09:00:58 +1100 Subject: [PATCH 06/15] AP_InertialSensor: removed zero checks and clamping on notch filters and pass params object down into HarmonicNotchFilter # Conflicts: # libraries/AP_InertialSensor/AP_InertialSensor.cpp --- .../AP_InertialSensor/AP_InertialSensor.cpp | 27 ++++++------------- 1 file changed, 8 insertions(+), 19 deletions(-) diff --git a/libraries/AP_InertialSensor/AP_InertialSensor.cpp b/libraries/AP_InertialSensor/AP_InertialSensor.cpp index ed98f5fab2fe56..6c16cdabd0ac40 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor.cpp +++ b/libraries/AP_InertialSensor/AP_InertialSensor.cpp @@ -950,7 +950,6 @@ AP_InertialSensor::init(uint16_t loop_rate) if (!notch.params.enabled() && !fft_enabled) { continue; } - notch.calculated_notch_freq_hz[0] = notch.params.center_freq_hz(); notch.num_calculated_notch_frequencies = 1; notch.num_dynamic_notches = 1; #if APM_BUILD_COPTER_OR_HELI || APM_BUILD_TYPE(APM_BUILD_ArduPlane) @@ -1005,8 +1004,7 @@ AP_InertialSensor::init(uint16_t loop_rate) notch.filter[i].allocate_filters(notch.num_dynamic_notches, notch.params.harmonics(), double_notch ? 2 : triple_notch ? 3 : 1); // initialise default settings, these will be subsequently changed in AP_InertialSensor_Backend::update_gyro() - notch.filter[i].init(_gyro_raw_sample_rates[i], notch.calculated_notch_freq_hz[0], - notch.params.bandwidth_hz(), notch.params.attenuation_dB()); + notch.filter[i].init(_gyro_raw_sample_rates[i], notch.params); } } } @@ -1702,25 +1700,21 @@ void AP_InertialSensor::_save_gyro_calibration() */ void AP_InertialSensor::HarmonicNotch::update_params(uint8_t instance, bool converging, float gyro_rate) { - const float center_freq = calculated_notch_freq_hz[0]; if (!is_equal(last_bandwidth_hz[instance], params.bandwidth_hz()) || !is_equal(last_attenuation_dB[instance], params.attenuation_dB()) || - (params.tracking_mode() == HarmonicNotchDynamicMode::Fixed && !is_equal(last_center_freq_hz[instance], center_freq)) || + (params.tracking_mode() == HarmonicNotchDynamicMode::Fixed && + !is_equal(last_center_freq_hz[instance], params.center_freq_hz())) || converging) { - filter[instance].init(gyro_rate, - center_freq, - params.bandwidth_hz(), - params.attenuation_dB()); - last_center_freq_hz[instance] = center_freq; + filter[instance].init(gyro_rate, params); + 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 (num_calculated_notch_frequencies > 1) { filter[instance].update(num_calculated_notch_frequencies, calculated_notch_freq_hz); } else { - filter[instance].update(center_freq); + filter[instance].update(calculated_notch_freq_hz[0]); } - last_center_freq_hz[instance] = center_freq; } } @@ -2155,10 +2149,7 @@ void AP_InertialSensor::acal_update() // Update the harmonic notch frequency void AP_InertialSensor::HarmonicNotch::update_freq_hz(float scaled_freq) { - // protect against zero as the scaled frequency - if (is_positive(scaled_freq)) { - calculated_notch_freq_hz[0] = scaled_freq; - } + calculated_notch_freq_hz[0] = scaled_freq; num_calculated_notch_frequencies = 1; } @@ -2166,9 +2157,7 @@ void AP_InertialSensor::HarmonicNotch::update_freq_hz(float scaled_freq) 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_notch_freq_hz[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_notch_frequencies = num_freqs; From 869a7019a68189f00f171f9cb9362ea38eb9f086 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Wed, 31 Jan 2024 09:01:21 +1100 Subject: [PATCH 07/15] Filter: fixed test suite fill in a parameters structure # Conflicts: # libraries/Filter/HarmonicNotchFilter.h --- libraries/Filter/HarmonicNotchFilter.h | 18 +++++++++++++++--- libraries/Filter/tests/test_notchfilter.cpp | 9 ++++++++- 2 files changed, 23 insertions(+), 4 deletions(-) diff --git a/libraries/Filter/HarmonicNotchFilter.h b/libraries/Filter/HarmonicNotchFilter.h index e6a1219718f7de..aea18e610d4d1e 100644 --- a/libraries/Filter/HarmonicNotchFilter.h +++ b/libraries/Filter/HarmonicNotchFilter.h @@ -116,17 +116,26 @@ class HarmonicNotchFilterParams : public NotchFilterParams { HarmonicNotchFilterParams(void); // set the fundamental center frequency of the harmonic notch void set_center_freq_hz(float center_freq) { _center_freq_hz.set(center_freq); } + // set the bandwidth of the harmonic notch void set_bandwidth_hz(float bandwidth_hz) { _bandwidth_hz.set(bandwidth_hz); } + + // set the attenuation of the harmonic notch + void set_attenuation(float attenuation_dB) { _attenuation_dB.set(attenuation_dB); } + // harmonics enabled on the harmonic notch - uint8_t harmonics(void) const { return _harmonics; } + uint32_t harmonics(void) const { return _harmonics; } + // set the harmonics value - void set_harmonics(uint8_t hmncs) { _harmonics.set(hmncs); } + void set_harmonics(uint32_t hmncs) { _harmonics.set(hmncs); } + // has the user set the harmonics value - void set_default_harmonics(uint8_t hmncs) { _harmonics.set_default(hmncs); } + void set_default_harmonics(uint32_t hmncs) { _harmonics.set_default(hmncs); } + // reference value of the harmonic notch float reference(void) const { return _reference; } void set_reference(float ref) { _reference.set(ref); } + // notch options bool hasOption(Options option) const { return _options & uint16_t(option); } // notch dynamic tracking mode @@ -139,6 +148,9 @@ class HarmonicNotchFilterParams : public NotchFilterParams { } void set_freq_min_ratio(float ratio) { _freq_min_ratio.set(ratio); } + // set options flags + void set_options(uint16_t options) { _options.set(options); } + // save parameters void save_params(); diff --git a/libraries/Filter/tests/test_notchfilter.cpp b/libraries/Filter/tests/test_notchfilter.cpp index 4cc0c0dbf6c87a..2f266fa88105d9 100644 --- a/libraries/Filter/tests/test_notchfilter.cpp +++ b/libraries/Filter/tests/test_notchfilter.cpp @@ -106,7 +106,14 @@ TEST(NotchFilterTest, HarmonicNotchTest) for (uint8_t c=0; c Date: Mon, 6 Nov 2023 16:51:02 +1100 Subject: [PATCH 08/15] Filter: fixed filter expansion for other than motors based RPM sources this also fixes the uint8_t storage of a number than can be greater than 256. Max total notches in a single HarmonicNotchFilter is currently 12*16*3 for 12 ESCs, with INS_HNTCH_HMNCS=0xFFFF and triple notch --- libraries/Filter/HarmonicNotchFilter.cpp | 10 +++++++--- libraries/Filter/HarmonicNotchFilter.h | 2 +- 2 files changed, 8 insertions(+), 4 deletions(-) diff --git a/libraries/Filter/HarmonicNotchFilter.cpp b/libraries/Filter/HarmonicNotchFilter.cpp index 853e7c3a9a9275..bbba850eeb58f0 100644 --- a/libraries/Filter/HarmonicNotchFilter.cpp +++ b/libraries/Filter/HarmonicNotchFilter.cpp @@ -190,6 +190,10 @@ void HarmonicNotchFilter::allocate_filters(uint8_t num_notches, uint8_t harmo template void HarmonicNotchFilter::expand_filter_count(uint16_t total_notches) { + if (total_notches <= _num_filters) { + // enough already + return; + } if (_alloc_has_failed) { // we've failed to allocate before, don't try again return; @@ -214,7 +218,7 @@ void HarmonicNotchFilter::expand_filter_count(uint16_t total_notches) set the center frequency of a single notch */ template -void HarmonicNotchFilter::set_center_frequency(uint8_t idx, float notch_center, float spread_mul, uint8_t harmonic_mul) +void HarmonicNotchFilter::set_center_frequency(uint16_t idx, float notch_center, float spread_mul, uint8_t harmonic_mul) { const float nyquist_limit = _sample_freq_hz * 0.48f; auto ¬ch = _filters[idx]; @@ -293,7 +297,7 @@ void HarmonicNotchFilter::update(float center_freq_hz) _num_enabled_filters = 0; // update all of the filters using the new center frequency and existing A & Q - for (uint8_t i = 0; i < HNF_MAX_HARMONICS && _num_enabled_filters < _num_filters; i++) { + for (uint16_t i = 0; i < HNF_MAX_HARMONICS && _num_enabled_filters < _num_filters; i++) { if ((1U<::update(uint8_t num_centers, const float center_freq // adjust the frequencies to be in the allowable range const float nyquist_limit = _sample_freq_hz * 0.48f; - uint16_t total_notches = num_centers * _num_harmonics * _composite_notches; + const uint16_t total_notches = num_centers * _num_harmonics * _composite_notches; if (total_notches > _num_filters) { // alloc realloc of filters expand_filter_count(total_notches); diff --git a/libraries/Filter/HarmonicNotchFilter.h b/libraries/Filter/HarmonicNotchFilter.h index aea18e610d4d1e..ba2bd2df6d4da7 100644 --- a/libraries/Filter/HarmonicNotchFilter.h +++ b/libraries/Filter/HarmonicNotchFilter.h @@ -47,7 +47,7 @@ class HarmonicNotchFilter { spread_mul is a scale factor for spreading of double or triple notch harmonic_mul is the multiplier for harmonics, 1 is for the fundamental */ - void set_center_frequency(uint8_t idx, float center_freq_hz, float spread_mul, uint8_t harmonic_mul); + void set_center_frequency(uint16_t idx, float center_freq_hz, float spread_mul, uint8_t harmonic_mul); // apply a sample to each of the underlying filters in turn T apply(const T &sample); From d88e25e9ca66e6dca56a40b2147145e305267490 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Mon, 6 Nov 2023 18:23:56 +1100 Subject: [PATCH 09/15] AP_InertialSensor: fixed num_filters to be uint16_t max is more than 256 --- libraries/AP_InertialSensor/AP_InertialSensor.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/libraries/AP_InertialSensor/AP_InertialSensor.cpp b/libraries/AP_InertialSensor/AP_InertialSensor.cpp index 6c16cdabd0ac40..b1e025f6f350f3 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor.cpp +++ b/libraries/AP_InertialSensor/AP_InertialSensor.cpp @@ -976,7 +976,7 @@ AP_InertialSensor::init(uint16_t loop_rate) sensors_used += _use(i); } - uint8_t num_filters = 0; + uint16_t num_filters = 0; for (auto ¬ch : harmonic_notches) { // calculate number of notches we might want to use for harmonic notch if (notch.params.enabled() || fft_enabled) { From 5d294eed601a39ac7b8517bd8748303a51d431a1 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Mon, 22 Jan 2024 07:55:03 +1100 Subject: [PATCH 10/15] AP_InertialSensor: use num_composite_notches() this prevents duplication of the logic for the priority of the double notch vs the triple notch option --- libraries/AP_InertialSensor/AP_InertialSensor.cpp | 8 ++------ 1 file changed, 2 insertions(+), 6 deletions(-) diff --git a/libraries/AP_InertialSensor/AP_InertialSensor.cpp b/libraries/AP_InertialSensor/AP_InertialSensor.cpp index b1e025f6f350f3..9c6bb0dd64eb5d 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor.cpp +++ b/libraries/AP_InertialSensor/AP_InertialSensor.cpp @@ -980,11 +980,9 @@ AP_InertialSensor::init(uint16_t loop_rate) for (auto ¬ch : harmonic_notches) { // calculate number of notches we might want to use for harmonic notch if (notch.params.enabled() || fft_enabled) { - const bool double_notch = notch.params.hasOption(HarmonicNotchFilterParams::Options::DoubleNotch); - const bool triple_notch = notch.params.hasOption(HarmonicNotchFilterParams::Options::TripleNotch); const bool all_sensors = notch.params.hasOption(HarmonicNotchFilterParams::Options::EnableOnAllIMUs); num_filters += __builtin_popcount(notch.params.harmonics()) - * notch.num_dynamic_notches * (double_notch ? 2 : triple_notch ? 3 : 1) + * notch.num_dynamic_notches * notch.params.num_composite_notches() * (all_sensors?sensors_used:1); } } @@ -999,10 +997,8 @@ AP_InertialSensor::init(uint16_t loop_rate) if (_use(i)) { for (auto ¬ch : harmonic_notches) { if (notch.params.enabled() || fft_enabled) { - const bool double_notch = notch.params.hasOption(HarmonicNotchFilterParams::Options::DoubleNotch); - const bool triple_notch = notch.params.hasOption(HarmonicNotchFilterParams::Options::TripleNotch); notch.filter[i].allocate_filters(notch.num_dynamic_notches, - notch.params.harmonics(), double_notch ? 2 : triple_notch ? 3 : 1); + notch.params.harmonics(), notch.params.num_composite_notches()); // initialise default settings, these will be subsequently changed in AP_InertialSensor_Backend::update_gyro() notch.filter[i].init(_gyro_raw_sample_rates[i], notch.params); } From 6eebcfb238c41e68216775028e1ff4a2503a814e Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Wed, 31 Jan 2024 09:02:27 +1100 Subject: [PATCH 11/15] AP_Logger: added VER.FV for filter version # Conflicts: # libraries/AP_Logger/AP_Logger_Backend.cpp --- libraries/AP_Logger/AP_Logger_Backend.cpp | 7 +++++++ libraries/AP_Logger/LogStructure.h | 3 ++- 2 files changed, 9 insertions(+), 1 deletion(-) diff --git a/libraries/AP_Logger/AP_Logger_Backend.cpp b/libraries/AP_Logger/AP_Logger_Backend.cpp index c054f44955000b..b5d5e4ca531e43 100644 --- a/libraries/AP_Logger/AP_Logger_Backend.cpp +++ b/libraries/AP_Logger/AP_Logger_Backend.cpp @@ -7,6 +7,12 @@ #include #include #include +#include +#include "AP_Logger.h" + +#if HAL_LOGGER_FENCE_ENABLED + #include +#endif extern const AP_HAL::HAL& hal; @@ -562,6 +568,7 @@ bool AP_Logger_Backend::Write_VER() fw_type: fwver.fw_type, git_hash: fwver.fw_hash, build_type: fwver.vehicle_type, + filter_version: AP_FILTER_VERSION, }; strncpy(pkt.fw_string, fwver.fw_string, ARRAY_SIZE(pkt.fw_string)-1); diff --git a/libraries/AP_Logger/LogStructure.h b/libraries/AP_Logger/LogStructure.h index d44aa7156bbc4e..c893283ced68d8 100644 --- a/libraries/AP_Logger/LogStructure.h +++ b/libraries/AP_Logger/LogStructure.h @@ -685,6 +685,7 @@ struct PACKED log_VER { char fw_string[64]; uint16_t _APJ_BOARD_ID; uint8_t build_type; + uint8_t filter_version; }; @@ -1345,7 +1346,7 @@ LOG_STRUCTURE_FROM_AIS \ { LOG_SCRIPTING_MSG, sizeof(log_Scripting), \ "SCR", "QNIii", "TimeUS,Name,Runtime,Total_mem,Run_mem", "s-sbb", "F-F--", true }, \ { LOG_VER_MSG, sizeof(log_VER), \ - "VER", "QBHBBBBIZHB", "TimeUS,BT,BST,Maj,Min,Pat,FWT,GH,FWS,APJ,BU", "s----------", "F----------", false }, \ + "VER", "QBHBBBBIZHBB", "TimeUS,BT,BST,Maj,Min,Pat,FWT,GH,FWS,APJ,BU,FV", "s-----------", "F-----------", false }, \ { LOG_MOTBATT_MSG, sizeof(log_MotBatt), \ "MOTB", "QfffffB", "TimeUS,LiftMax,BatVolt,ThLimit,ThrAvMx,ThrOut,FailFlags", "s------", "F------" , true } From 7931c1e54276d1b13fd6a44939431ba4f132fcf1 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Wed, 31 Jan 2024 07:34:25 +1100 Subject: [PATCH 12/15] AP_InertialSensor: added comments --- libraries/AP_InertialSensor/AP_InertialSensor.cpp | 9 +++++++-- 1 file changed, 7 insertions(+), 2 deletions(-) diff --git a/libraries/AP_InertialSensor/AP_InertialSensor.cpp b/libraries/AP_InertialSensor/AP_InertialSensor.cpp index 9c6bb0dd64eb5d..53c3dff3135cfb 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor.cpp +++ b/libraries/AP_InertialSensor/AP_InertialSensor.cpp @@ -2142,7 +2142,12 @@ void AP_InertialSensor::acal_update() } #endif -// Update the harmonic notch frequency +/* + Update the harmonic notch frequency + + Note that zero is a valid value and will disable the notch unless + the TreatLowAsMin option is set +*/ void AP_InertialSensor::HarmonicNotch::update_freq_hz(float scaled_freq) { calculated_notch_freq_hz[0] = scaled_freq; @@ -2151,7 +2156,7 @@ void AP_InertialSensor::HarmonicNotch::update_freq_hz(float scaled_freq) // Update the harmonic notch frequency void AP_InertialSensor::HarmonicNotch::update_frequencies_hz(uint8_t num_freqs, const float scaled_freq[]) { - // protect against zero as the scaled frequency + // note that we allow zero through, which will disable the notch for (uint8_t i = 0; i < num_freqs; i++) { calculated_notch_freq_hz[i] = scaled_freq[i]; } From 6fd91ade5447cb9626fece8d5ae657ffe3b39134 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sat, 3 Feb 2024 08:35:24 +1100 Subject: [PATCH 13/15] Filter: adjust attenuation properly at low frequencies use sqrt() adjustment from Leonard --- libraries/Filter/Filter.h | 9 ++ libraries/Filter/HarmonicNotchFilter.cpp | 103 +++++++++++------------ libraries/Filter/HarmonicNotchFilter.h | 13 +-- libraries/Filter/NotchFilter.cpp | 6 +- libraries/Filter/NotchFilter.h | 2 +- 5 files changed, 73 insertions(+), 60 deletions(-) diff --git a/libraries/Filter/Filter.h b/libraries/Filter/Filter.h index 7f11ed4852b859..ed1f00b4489a38 100644 --- a/libraries/Filter/Filter.h +++ b/libraries/Filter/Filter.h @@ -9,3 +9,12 @@ #include "LowPassFilter.h" #include "ModeFilter.h" #include "Butter.h" + +/* + the filter version is logged in the VER message to assist the online + analysis tools, so they can display the right filter formulas for + this version of the code + This should be incremented on significant filtering changes + */ +#define AP_FILTER_VERSION 2 + diff --git a/libraries/Filter/HarmonicNotchFilter.cpp b/libraries/Filter/HarmonicNotchFilter.cpp index bbba850eeb58f0..873379958b46b6 100644 --- a/libraries/Filter/HarmonicNotchFilter.cpp +++ b/libraries/Filter/HarmonicNotchFilter.cpp @@ -35,6 +35,11 @@ #define NOTCH_DEBUG_LOGGING 0 #endif +/* + point at which the harmonic notch goes to zero attenuation + */ +#define NOTCHFILTER_ATTENUATION_CUTOFF 0.25 + // table of user settable parameters const AP_Param::GroupInfo HarmonicNotchFilterParams::var_info[] = { @@ -127,8 +132,11 @@ HarmonicNotchFilter::~HarmonicNotchFilter() { the constraints are used to determine attenuation (A) and quality (Q) factors for the filter */ template -void HarmonicNotchFilter::init(float sample_freq_hz, const HarmonicNotchFilterParams ¶ms) +void HarmonicNotchFilter::init(float sample_freq_hz, HarmonicNotchFilterParams &_params) { + // keep a copy of the params object + params = &_params; + // sanity check the input if (_filters == nullptr || is_zero(sample_freq_hz) || isnan(sample_freq_hz)) { return; @@ -136,15 +144,15 @@ void HarmonicNotchFilter::init(float sample_freq_hz, const HarmonicNotchFilte _sample_freq_hz = sample_freq_hz; - const float bandwidth_hz = params.bandwidth_hz(); - const float attenuation_dB = params.attenuation_dB(); - float center_freq_hz = params.center_freq_hz(); + const float bandwidth_hz = params->bandwidth_hz(); + const float attenuation_dB = params->attenuation_dB(); + float center_freq_hz = params->center_freq_hz(); const float nyquist_limit = sample_freq_hz * 0.48f; const float bandwidth_limit = bandwidth_hz * 0.52f; // remember the lowest frequency we will have a notch enabled at - _minimum_freq = center_freq_hz * params.freq_min_ratio(); + _minimum_freq = center_freq_hz * params->freq_min_ratio(); /* adjust the fundamental center frequency we use for the initial @@ -159,8 +167,6 @@ void HarmonicNotchFilter::init(float sample_freq_hz, const HarmonicNotchFilte // calculate attenuation and quality from the shaping constraints NotchFilter::calculate_A_and_Q(center_freq_hz, bandwidth_hz / _composite_notches, attenuation_dB, _A, _Q); - _treat_low_freq_as_min = params.hasOption(HarmonicNotchFilterParams::Options::TreatLowAsMin); - _initialised = true; } @@ -215,7 +221,11 @@ void HarmonicNotchFilter::expand_filter_count(uint16_t total_notches) } /* - set the center frequency of a single notch + set the center frequency of a single notch harmonic + + The spread_mul is the frequency multiplier from the spread of the + double or triple notch. The harmonic_mul is the multiplier for the + frequency for this harmonic */ template void HarmonicNotchFilter::set_center_frequency(uint16_t idx, float notch_center, float spread_mul, uint8_t harmonic_mul) @@ -238,44 +248,53 @@ void HarmonicNotchFilter::set_center_frequency(uint16_t idx, float notch_cent return; } - // the minimum frequency for a harmonic is the base minimum frequency - // scaled for the harmonic multiplier - const float harmonic_min_freq = _minimum_freq * harmonic_mul; + // the minimum frequency for a harmonic is the base minimum + float harmonic_min_freq = _minimum_freq; - // on some vehicles we can adjust the attenuation at low frequencies + // we can adjust the attenuation at low frequencies float A = _A; // on some vehicles we want to treat zero or very low frequency as // the minimum frequency, on others we want to disable the - // notch. That is controlled by the _treat_low_freq_as_min flag - if (!_treat_low_freq_as_min) { + // notch for low frequencies + const bool treat_low_freq_as_min = params->hasOption(HarmonicNotchFilterParams::Options::TreatLowAsMin); + + if (treat_low_freq_as_min) { + /* + when we are treating low as min we don't want to collapse + the harmonics down to low frequencies + */ + harmonic_min_freq *= harmonic_mul; + } else { /* - disable if the notch is less than half of the min frequency + disable if the notch is less than 25% of the min frequency */ - const float disable_threshold = 0.5; - if (notch_center < disable_threshold * _minimum_freq) { + const float disable_freq = harmonic_min_freq * NOTCHFILTER_ATTENUATION_CUTOFF; + if (notch_center < disable_freq) { notch.disable(); return; } - if (notch_center < _minimum_freq) { + if (notch_center < harmonic_min_freq) { /* scale the attenuation so that we fade out the notch as - we get further below the min frequency - - TODO: determine if this linear scaling is the right - function. Very likely we will need something more complex + we get further below the min frequency. The attenuation + factor A goes to 1.0 (which means no attenuation) + Scaling the attenuation in this way means we don't get a + glitch at the disable point */ - A = linear_interpolate(1.0, _A, - notch_center, - disable_threshold * _minimum_freq, - _minimum_freq); + A = linear_interpolate(A, 1.0, notch_center, harmonic_min_freq, disable_freq); } } - // never run the center of a notch at a lower frequency than the - // minimum specified in the parameters. - notch_center = MAX(notch_center, harmonic_min_freq) * spread_mul; + // don't let the notch go below the min frequency + notch_center = MAX(notch_center, harmonic_min_freq); + + /* adjust notch center for spread for double and triple notch. + This adjustment is applied last to maintain the symmetry of the + double and triple notches + */ + notch_center *= spread_mul; notch.init_with_A_and_Q(_sample_freq_hz, notch_center, A, _Q); } @@ -287,29 +306,7 @@ void HarmonicNotchFilter::set_center_frequency(uint16_t idx, float notch_cent template void HarmonicNotchFilter::update(float center_freq_hz) { - if (!_initialised) { - return; - } - - // adjust the fundamental center frequency to be in the allowable range - const float nyquist_limit = _sample_freq_hz * 0.48f; - center_freq_hz = constrain_float(center_freq_hz, 0.0f, nyquist_limit); - - _num_enabled_filters = 0; - // update all of the filters using the new center frequency and existing A & Q - for (uint16_t i = 0; i < HNF_MAX_HARMONICS && _num_enabled_filters < _num_filters; i++) { - if ((1U< 1) { - set_center_frequency(_num_enabled_filters++, notch_center, 1.0 - _notch_spread, harmonic_mul); - set_center_frequency(_num_enabled_filters++, notch_center, 1.0 + _notch_spread, harmonic_mul); - } - } - } + update(1, ¢er_freq_hz); } /* @@ -344,7 +341,7 @@ void HarmonicNotchFilter::update(uint8_t num_centers, const float center_freq continue; } - const float notch_center = constrain_float(center_freq_hz[center_n] * (harmonic_n+1), 0.0f, nyquist_limit); + const float notch_center = constrain_float(center_freq_hz[center_n], 0.0f, nyquist_limit); const uint8_t harmonic_mul = (harmonic_n+1); if (_composite_notches != 2) { set_center_frequency(_num_enabled_filters++, notch_center, 1.0, harmonic_mul); diff --git a/libraries/Filter/HarmonicNotchFilter.h b/libraries/Filter/HarmonicNotchFilter.h index ba2bd2df6d4da7..e2fcc8148c61fb 100644 --- a/libraries/Filter/HarmonicNotchFilter.h +++ b/libraries/Filter/HarmonicNotchFilter.h @@ -36,7 +36,7 @@ class HarmonicNotchFilter { // expand filter bank with new filters void expand_filter_count(uint16_t total_notches); // initialize the underlying filters using the provided filter parameters - void init(float sample_freq_hz, const HarmonicNotchFilterParams ¶ms); + void init(float sample_freq_hz, HarmonicNotchFilterParams ¶ms); // update the underlying filters' center frequencies using center_freq_hz as the fundamental void update(float center_freq_hz); // update all of the underlying center frequencies individually @@ -83,10 +83,8 @@ class HarmonicNotchFilter { // minimum frequency (from INS_HNTCH_FREQ * INS_HNTCH_FM_RAT) float _minimum_freq; - /* - flag for treating a very low frequency as the min frequency - */ - bool _treat_low_freq_as_min; + // pointer to params object for this filter + HarmonicNotchFilterParams *params; }; // Harmonic notch update mode @@ -154,6 +152,11 @@ class HarmonicNotchFilterParams : public NotchFilterParams { // save parameters void save_params(); + // return the number of composite notches given the options + uint8_t num_composite_notches(void) const { + return hasOption(Options::DoubleNotch) ? 2 : hasOption(Options::TripleNotch) ? 3: 1; + } + private: // configured notch harmonics AP_Int8 _harmonics; diff --git a/libraries/Filter/NotchFilter.cpp b/libraries/Filter/NotchFilter.cpp index 37097dffafb1c0..64aac746ce5348 100644 --- a/libraries/Filter/NotchFilter.cpp +++ b/libraries/Filter/NotchFilter.cpp @@ -59,7 +59,10 @@ template void NotchFilter::init_with_A_and_Q(float sample_freq_hz, float center_freq_hz, float A, float Q) { // don't update if no updates required - if (initialised && is_equal(center_freq_hz, _center_freq_hz) && is_equal(sample_freq_hz, _sample_freq_hz)) { + if (initialised && + is_equal(center_freq_hz, _center_freq_hz) && + is_equal(sample_freq_hz, _sample_freq_hz) && + is_equal(A, _A)) { return; } @@ -82,6 +85,7 @@ void NotchFilter::init_with_A_and_Q(float sample_freq_hz, float center_freq_h a2 = 1.0 - alpha; _center_freq_hz = new_center_freq; _sample_freq_hz = sample_freq_hz; + _A = A; initialised = true; } else { // leave center_freq_hz at last value diff --git a/libraries/Filter/NotchFilter.h b/libraries/Filter/NotchFilter.h index 48480238ffc50e..4911c4a9734253 100644 --- a/libraries/Filter/NotchFilter.h +++ b/libraries/Filter/NotchFilter.h @@ -50,7 +50,7 @@ class NotchFilter { bool initialised, need_reset; float b0, b1, b2, a1, a2, a0_inv; - float _center_freq_hz, _sample_freq_hz; + float _center_freq_hz, _sample_freq_hz, _A; T ntchsig, ntchsig1, ntchsig2, signal2, signal1; }; From 8d900ae253b9653778c86bf9625e4d32c73eb5e8 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Mon, 29 Jan 2024 17:12:42 +1100 Subject: [PATCH 14/15] Filter: added test for attenuation adjustment --- libraries/Filter/tests/plot_harmonictest2.gnu | 11 ++ libraries/Filter/tests/plot_harmonictest3.gnu | 12 ++ libraries/Filter/tests/plot_harmonictest4.gnu | 12 ++ libraries/Filter/tests/test_notchfilter.cpp | 172 +++++++++++++++++- 4 files changed, 202 insertions(+), 5 deletions(-) create mode 100644 libraries/Filter/tests/plot_harmonictest2.gnu create mode 100644 libraries/Filter/tests/plot_harmonictest3.gnu create mode 100644 libraries/Filter/tests/plot_harmonictest4.gnu diff --git a/libraries/Filter/tests/plot_harmonictest2.gnu b/libraries/Filter/tests/plot_harmonictest2.gnu new file mode 100644 index 00000000000000..2df39eb16c5b1a --- /dev/null +++ b/libraries/Filter/tests/plot_harmonictest2.gnu @@ -0,0 +1,11 @@ +#!/usr/bin/gnuplot -persist +set y2tics 0,10 +set ytics nomirror +set style data linespoints +set key autotitle +set datafile separator "," +set key autotitle columnhead +set xlabel "Freq(Hz)" +set ylabel "Attenuation(dB)" +#set ylabel2 "PhaseLag(deg)" +plot "harmonicnotch_test2.csv" using 1:2 axis x1y1, "harmonicnotch_test2.csv" using 1:3 axis x1y2 diff --git a/libraries/Filter/tests/plot_harmonictest3.gnu b/libraries/Filter/tests/plot_harmonictest3.gnu new file mode 100644 index 00000000000000..d2f7cc337a8652 --- /dev/null +++ b/libraries/Filter/tests/plot_harmonictest3.gnu @@ -0,0 +1,12 @@ +#!/usr/bin/gnuplot -persist +set y2tics 0,10 +set ytics nomirror +set style data linespoints +set key autotitle +set datafile separator "," +set key autotitle columnhead +set xlabel "Freq(Hz)" +set ylabel "Attenuation(dB)" +#set ylabel2 "PhaseLag(deg)" +plot "harmonicnotch_test3.csv" using 1:2 axis x1y1, "harmonicnotch_test3.csv" using 1:3 axis x1y2, "harmonicnotch_test3.csv" using 1:4 axis x1y1, "harmonicnotch_test3.csv" using 1:5 axis x1y2 + diff --git a/libraries/Filter/tests/plot_harmonictest4.gnu b/libraries/Filter/tests/plot_harmonictest4.gnu new file mode 100644 index 00000000000000..cfc8b5117e8e30 --- /dev/null +++ b/libraries/Filter/tests/plot_harmonictest4.gnu @@ -0,0 +1,12 @@ +#!/usr/bin/gnuplot -persist +set y2tics 0,10 +set ytics nomirror +set style data linespoints +set key autotitle +set datafile separator "," +set key autotitle columnhead +set xlabel "Freq(Hz)" +set ylabel "Attenuation(dB)" +set key left bottom +plot "harmonicnotch_test4.csv" using 1:2, "harmonicnotch_test4.csv" using 1:3, "harmonicnotch_test4.csv" using 1:4, "harmonicnotch_test4.csv" using 1:5, "harmonicnotch_test4.csv" using 1:6, "harmonicnotch_test4.csv" using 1:7, "harmonicnotch_test4.csv" using 1:8 + diff --git a/libraries/Filter/tests/test_notchfilter.cpp b/libraries/Filter/tests/test_notchfilter.cpp index 2f266fa88105d9..79d2325fa54600 100644 --- a/libraries/Filter/tests/test_notchfilter.cpp +++ b/libraries/Filter/tests/test_notchfilter.cpp @@ -6,6 +6,11 @@ const AP_HAL::HAL& hal = AP_HAL::get_HAL(); +static double ratio_to_dB(double ratio) +{ + return 10*log(ratio); +} + /* test if a reset of a notch filter results in no glitch in the following samples with constant input @@ -74,7 +79,6 @@ TEST(NotchFilterTest, HarmonicNotchTest) { const uint8_t num_test_freq = 150; const uint8_t harmonics = 15; - const uint8_t num_harmonics = __builtin_popcount(harmonics); const float base_freq = 46; const float bandwidth = base_freq/2; const float attenuation_dB = 60; @@ -86,7 +90,6 @@ TEST(NotchFilterTest, HarmonicNotchTest) const float test_amplitude = 0.7; const double dt = 1.0 / rate_hz; - bool double_notch = true; HarmonicNotchFilter filters[num_test_freq][chained_filters] {}; struct { double in; @@ -105,13 +108,14 @@ TEST(NotchFilterTest, HarmonicNotchTest) for (uint8_t i=0; i filter {}; + struct { + double last_in; + double last_out; + double v_max; + uint32_t last_crossing; + uint32_t total_lag_samples; + uint32_t lag_count; + float get_lag_degrees(const float freq) const { + const float lag_avg = total_lag_samples/float(lag_count); + return (360.0 * lag_avg * freq) / rate_hz; + } + } integral {}; + + auto &f = filter; + + + HarmonicNotchFilterParams notch_params {}; + notch_params.set_options(options); + notch_params.set_attenuation(attenuation_dB); + notch_params.set_bandwidth_hz(bandwidth); + notch_params.set_center_freq_hz(base_freq); + notch_params.set_freq_min_ratio(1.0); + f.allocate_filters(1, harmonics, notch_params.num_composite_notches()); + f.init(rate_hz, notch_params); + f.update(source_freq); + + for (uint32_t s=0; s= samples/10) { + integral.v_max = MAX(integral.v_max, v); + } + if (sample >= 0 && integral.last_in < 0) { + integral.last_crossing = s; + } + if (v >= 0 && integral.last_out < 0 && integral.last_crossing != 0) { + integral.total_lag_samples += s - integral.last_crossing; + integral.lag_count++; + } + integral.last_in = sample; + integral.last_out = v; + f.update(source_freq); + } + phase_lag = integral.get_lag_degrees(test_freq); + out_attenuation_dB = ratio_to_dB(integral.v_max/test_amplitude); +} + +/* + test the test_one_filter function + */ +TEST(NotchFilterTest, HarmonicNotchTest2) +{ + const float min_freq = 1.0; + const float max_freq = 200; + const uint16_t steps = 2000; + + const char *csv_file = "harmonicnotch_test2.csv"; + FILE *f = fopen(csv_file, "w"); + fprintf(f, "Freq(Hz),Attenuation(dB),Lag(deg)\n"); + + for (uint16_t i=0; i Date: Sat, 3 Feb 2024 08:41:23 +1100 Subject: [PATCH 15/15] Filter: align notch code with master --- libraries/Filter/NotchFilter.cpp | 20 +++++++++++++++----- libraries/Filter/NotchFilter.h | 6 ++++-- 2 files changed, 19 insertions(+), 7 deletions(-) diff --git a/libraries/Filter/NotchFilter.cpp b/libraries/Filter/NotchFilter.cpp index 64aac746ce5348..dbbec6f80a802b 100644 --- a/libraries/Filter/NotchFilter.cpp +++ b/libraries/Filter/NotchFilter.cpp @@ -80,9 +80,18 @@ void NotchFilter::init_with_A_and_Q(float sample_freq_hz, float center_freq_h b0 = 1.0 + alpha*sq(A); b1 = -2.0 * cosf(omega); b2 = 1.0 - alpha*sq(A); - a0_inv = 1.0/(1.0 + alpha); a1 = b1; a2 = 1.0 - alpha; + + const float a0_inv = 1.0/(1.0 + alpha); + + // Pre-multiply to save runtime calc + b0 *= a0_inv; + b1 *= a0_inv; + b2 *= a0_inv; + a1 *= a0_inv; + a2 *= a0_inv; + _center_freq_hz = new_center_freq; _sample_freq_hz = sample_freq_hz; _A = A; @@ -104,16 +113,17 @@ T NotchFilter::apply(const T &sample) // sample as output and update delayed samples signal1 = sample; signal2 = sample; - ntchsig = sample; ntchsig1 = sample; ntchsig2 = sample; need_reset = false; return sample; } + + T output = sample*b0 + ntchsig1*b1 + ntchsig2*b2 - signal1*a1 - signal2*a2; + ntchsig2 = ntchsig1; - ntchsig1 = ntchsig; - ntchsig = sample; - T output = (ntchsig*b0 + ntchsig1*b1 + ntchsig2*b2 - signal1*a1 - signal2*a2) * a0_inv; + ntchsig1 = sample; + signal2 = signal1; signal1 = output; return output; diff --git a/libraries/Filter/NotchFilter.h b/libraries/Filter/NotchFilter.h index 4911c4a9734253..c397f6b40ccf8a 100644 --- a/libraries/Filter/NotchFilter.h +++ b/libraries/Filter/NotchFilter.h @@ -38,6 +38,8 @@ class NotchFilter { void init_with_A_and_Q(float sample_freq_hz, float center_freq_hz, float A, float Q); T apply(const T &sample); void reset(); + float center_freq_hz() const { return _center_freq_hz; } + float sample_freq_hz() const { return _sample_freq_hz; } // calculate attenuation and quality from provided center frequency and bandwidth static void calculate_A_and_Q(float center_freq_hz, float bandwidth_hz, float attenuation_dB, float& A, float& Q); @@ -49,9 +51,9 @@ class NotchFilter { protected: bool initialised, need_reset; - float b0, b1, b2, a1, a2, a0_inv; + float b0, b1, b2, a1, a2; float _center_freq_hz, _sample_freq_hz, _A; - T ntchsig, ntchsig1, ntchsig2, signal2, signal1; + T ntchsig1, ntchsig2, signal2, signal1; }; /*