From dfba938e63a24a5bdfffc30a73c42535b9510ca7 Mon Sep 17 00:00:00 2001 From: Andy Piper Date: Thu, 23 Jun 2022 17:52:40 +0100 Subject: [PATCH] Filter: add triple harmonic notches --- libraries/Filter/HarmonicNotchFilter.cpp | 31 +++++++++++------------- libraries/Filter/HarmonicNotchFilter.h | 7 +++--- 2 files changed, 18 insertions(+), 20 deletions(-) diff --git a/libraries/Filter/HarmonicNotchFilter.cpp b/libraries/Filter/HarmonicNotchFilter.cpp index 9617b3af49..9a51f0c4c0 100644 --- a/libraries/Filter/HarmonicNotchFilter.cpp +++ b/libraries/Filter/HarmonicNotchFilter.cpp @@ -84,8 +84,8 @@ const AP_Param::GroupInfo HarmonicNotchFilterParams::var_info[] = { // @Param: OPTS // @DisplayName: Harmonic Notch Filter options - // @Description: Harmonic Notch Filter options. Double-notches can provide deeper attenuation across a wider bandwidth 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. - // @Bitmask: 0:Double notch,1:Dynamic harmonic,2:Update at loop rate,3:EnableOnAllIMUs + // @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 // @User: Advanced // @RebootRequired: True AP_GROUPINFO("OPTS", 8, HarmonicNotchFilterParams, _options, 0), @@ -131,14 +131,9 @@ void HarmonicNotchFilter::init(float sample_freq_hz, float center_freq_hz, fl // Calculate spread required to achieve an equivalent single notch using two notches with Bandwidth/2 _notch_spread = bandwidth_hz / (32 * center_freq_hz); - if (_double_notch) { - // position the individual notches so that the attenuation is no worse than a single notch - // calculate attenuation and quality from the shaping constraints - NotchFilter::calculate_A_and_Q(center_freq_hz, bandwidth_hz * 0.5, attenuation_dB, _A, _Q); - } else { - // calculate attenuation and quality from the shaping constraints - NotchFilter::calculate_A_and_Q(center_freq_hz, bandwidth_hz, attenuation_dB, _A, _Q); - } + // position the individual notches so that the attenuation is no worse than a single notch + // calculate attenuation and quality from the shaping constraints + NotchFilter::calculate_A_and_Q(center_freq_hz, bandwidth_hz / _composite_notches, attenuation_dB, _A, _Q); _initialised = true; update(center_freq_hz); @@ -148,11 +143,11 @@ void HarmonicNotchFilter::init(float sample_freq_hz, float center_freq_hz, fl allocate a collection of, at most HNF_MAX_FILTERS, notch filters to be managed by this harmonic notch filter */ template -void HarmonicNotchFilter::allocate_filters(uint8_t num_notches, uint8_t harmonics, bool double_notch) +void HarmonicNotchFilter::allocate_filters(uint8_t num_notches, uint8_t harmonics, uint8_t composite_notches) { - _double_notch = double_notch; + _composite_notches = MIN(composite_notches, 3); _num_harmonics = __builtin_popcount(harmonics); - _num_filters = _num_harmonics * num_notches * (double_notch ? 2 : 1); + _num_filters = _num_harmonics * num_notches * _composite_notches; _harmonics = harmonics; if (_num_filters > 0) { @@ -184,12 +179,13 @@ void HarmonicNotchFilter::update(float center_freq_hz) 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); @@ -233,12 +229,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), 1.0f, nyquist_limit); - if (!_double_notch) { + 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); } - } else { + } + 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); diff --git a/libraries/Filter/HarmonicNotchFilter.h b/libraries/Filter/HarmonicNotchFilter.h index 75818b2339..f211078f61 100644 --- a/libraries/Filter/HarmonicNotchFilter.h +++ b/libraries/Filter/HarmonicNotchFilter.h @@ -30,7 +30,7 @@ class HarmonicNotchFilter { public: ~HarmonicNotchFilter(); // allocate a bank of notch filters for this harmonic notch filter - void allocate_filters(uint8_t num_notches, uint8_t harmonics, bool double_notch); + void allocate_filters(uint8_t num_notches, uint8_t harmonics, uint8_t composite_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 @@ -55,8 +55,8 @@ private: float _Q; // a bitmask of the harmonics to use uint8_t _harmonics; - // whether to use double-notches - bool _double_notch; + // number of notches that make up a composite notch + uint8_t _composite_notches; // number of allocated filters uint8_t _num_filters; // pre-calculated number of harmonics @@ -86,6 +86,7 @@ public: DynamicHarmonic = 1<<1, LoopRateUpdate = 1<<2, EnableOnAllIMUs = 1<<3, + TripleNotch = 1<<4, }; HarmonicNotchFilterParams(void);