From 80e591a54bd6769e1d2566f405aefcd59b881f9b Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Mon, 6 Nov 2023 18:24:43 +1100 Subject: [PATCH] Filter: adjust attenuation properly at low frequencies use sqrt() adjustment from Leonard --- libraries/Filter/HarmonicNotchFilter.cpp | 54 ++++++++++++------------ libraries/Filter/HarmonicNotchFilter.h | 8 ++-- 2 files changed, 30 insertions(+), 32 deletions(-) diff --git a/libraries/Filter/HarmonicNotchFilter.cpp b/libraries/Filter/HarmonicNotchFilter.cpp index b1279ff96966de..aa15e71dd6767f 100644 --- a/libraries/Filter/HarmonicNotchFilter.cpp +++ b/libraries/Filter/HarmonicNotchFilter.cpp @@ -129,8 +129,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; @@ -138,15 +141,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 @@ -161,8 +164,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; } @@ -240,45 +241,44 @@ 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 + // frequency scaled for the harmonic multiplier, the spread + // multiplier and the FM_RAT set by the user + const float harmonic_min_freq = _minimum_freq * harmonic_mul * spread_mul; - // 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. + const bool treat_low_freq_as_min = params->hasOption(HarmonicNotchFilterParams::Options::TreatLowAsMin); + + if (treat_low_freq_as_min) { + notch_center = MAX(notch_center, harmonic_min_freq); + } else { /* 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) { + if (notch_center < disable_threshold * harmonic_min_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) + See https://github.com/ArduPilot/ardupilot/issues/25461 */ - A = linear_interpolate(1.0, _A, - notch_center, - disable_threshold * _minimum_freq, - _minimum_freq); + const float bandwidth_ref_Hz = MIN(notch_center * 0.5, params->bandwidth_hz()); + const float ref = constrain_float((harmonic_min_freq - notch_center) / bandwidth_ref_Hz, 0.0, 1.0); + A = sqrtf(sq(A) * (1.0 - ref) + ref); } } - // 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); } diff --git a/libraries/Filter/HarmonicNotchFilter.h b/libraries/Filter/HarmonicNotchFilter.h index 1961387adeaf42..fa084c3b431bb4 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