From 334ebadd5adb613335631618e1c442b12f9efb69 Mon Sep 17 00:00:00 2001 From: Andy Piper Date: Wed, 28 Aug 2019 21:18:00 +0100 Subject: [PATCH] Filter: add comments and address review comments for harmonic notch --- libraries/Filter/HarmonicNotchFilter.cpp | 64 +++++++++++++++--------- libraries/Filter/HarmonicNotchFilter.h | 34 ++++++++++--- 2 files changed, 65 insertions(+), 33 deletions(-) diff --git a/libraries/Filter/HarmonicNotchFilter.cpp b/libraries/Filter/HarmonicNotchFilter.cpp index 623061afbe..064bcfd2ac 100644 --- a/libraries/Filter/HarmonicNotchFilter.cpp +++ b/libraries/Filter/HarmonicNotchFilter.cpp @@ -22,47 +22,47 @@ const AP_Param::GroupInfo HarmonicNotchFilterParams::var_info[] = { // @Param: ENABLE - // @DisplayName: Enable - // @Description: Enable harmonic notch filter + // @DisplayName: Harmonic Notch Filter enable + // @Description: Harmonic Notch Filter enable // @Values: 0:Disabled,1:Enabled // @User: Advanced AP_GROUPINFO_FLAGS("ENABLE", 1, HarmonicNotchFilterParams, _enable, 0, AP_PARAM_FLAG_ENABLE), // @Param: FREQ - // @DisplayName: Base Frequency - // @Description: Notch base center frequency in Hz + // @DisplayName: Harmonic Notch Filter base frequency + // @Description: Harmonic Notch Filter base center frequency in Hz // @Range: 10 400 // @Units: Hz // @User: Advanced AP_GROUPINFO("FREQ", 2, HarmonicNotchFilterParams, _center_freq_hz, 80), // @Param: BW - // @DisplayName: Bandwidth - // @Description: Harmonic notch bandwidth in Hz + // @DisplayName: Harmonic Notch Filter bandwidth + // @Description: Harmonic Notch Filter bandwidth in Hz // @Range: 5 100 // @Units: Hz // @User: Advanced AP_GROUPINFO("BW", 3, HarmonicNotchFilterParams, _bandwidth_hz, 20), // @Param: ATT - // @DisplayName: Attenuation - // @Description: Harmonic notch attenuation in dB + // @DisplayName: Harmonic Notch Filter attenuation + // @Description: Harmonic Notch Filter attenuation in dB // @Range: 5 30 // @Units: dB // @User: Advanced AP_GROUPINFO("ATT", 4, HarmonicNotchFilterParams, _attenuation_dB, 15), // @Param: HMNCS - // @DisplayName: Harmonics - // @Description: Bitmask of harmonic frequencies to apply notches to. This option takes effect on the next reboot. A maximum of 3 harmonics can be used at any one time + // @DisplayName: Harmonic Notch Filter harmonics + // @Description: Bitmask of harmonic frequencies to apply Harmonic Notch Filter to. This option takes effect on the next reboot. A maximum of 3 harmonics can be used at any one time // @Bitmask: 0:1st harmonic,1:2nd harmonic,2:3rd harmonic,3:4th hamronic,4:5th harmonic,5:6th harmonic,6:7th harmonic,7:8th harmonic // @User: Advanced // @RebootRequired: True AP_GROUPINFO("HMNCS", 5, HarmonicNotchFilterParams, _harmonics, 3), // @Param: REF - // @DisplayName: Reference value - // @Description: Reference value associated with the specified frequency to facilitate frequency scaling + // @DisplayName: Harmonic Notch Filter reference value + // @Description: Reference value associated with the specified frequency to facilitate frequency scaling of the Harmonic Notch Filter // @User: Advanced // @Range: 0.1 0.9 // @RebootRequired: True @@ -71,9 +71,18 @@ const AP_Param::GroupInfo HarmonicNotchFilterParams::var_info[] = { AP_GROUPEND }; +/* + destroy all of the associated notch filters + */ +template +HarmonicNotchFilter::~HarmonicNotchFilter() { + delete[] _filters; + _num_filters = 0; +} /* - initialise filter + initialise the associated filters with the provided shaping constraints + 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) @@ -87,8 +96,10 @@ void HarmonicNotchFilter::init(float sample_freq_hz, float center_freq_hz, fl // adjust the center frequency to be in the allowable range center_freq_hz = constrain_float(center_freq_hz, bandwidth_hz * 0.52f, sample_freq_hz * 0.48f); + // calculate attenuation and quality from the shaping constraints NotchFilter::calculate_A_and_Q(center_freq_hz, bandwidth_hz, attenuation_dB, _A, _Q); + // initialize all the configured filters with the same A & Q and multiples of the center frequency for (uint8_t i = 0, filt = 0; i < HNF_MAX_HARMONICS && filt < _num_filters; i++) { if ((1U<::init(float sample_freq_hz, float center_freq_hz, fl } /* - initialise filter + allocate a collection of, at most HNF_MAX_FILTERS, notch filters to be managed by this harmonic notch filter */ template -void HarmonicNotchFilter::create(uint8_t harmonics) +void HarmonicNotchFilter::allocate_filters(uint8_t harmonics) { for (uint8_t i = 0; i < HNF_MAX_HARMONICS && _num_filters < HNF_MAX_FILTERS; i++) { if ((1U<::create(uint8_t harmonics) _harmonics = harmonics; } -template -HarmonicNotchFilter::~HarmonicNotchFilter() { - if (_num_filters > 0) { - delete[] _filters; - } -} - +/* + 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 + */ template void HarmonicNotchFilter::update(float center_freq_hz) { @@ -131,6 +139,7 @@ void HarmonicNotchFilter::update(float center_freq_hz) // adjust the center frequency to be in the allowable range center_freq_hz = constrain_float(center_freq_hz, 1.0f, _sample_freq_hz * 0.48f); + // update all of the filters using the new center frequency and existing A & Q for (uint8_t i = 0, filt = 0; i < HNF_MAX_HARMONICS && filt < _num_filters; i++) { if ((1U<::update(float center_freq_hz) } } +/* + apply a sample to each of the underlying filters in turn and return the output + */ template T HarmonicNotchFilter::apply(const T &sample) { @@ -152,6 +164,9 @@ T HarmonicNotchFilter::apply(const T &sample) return output; } +/* + reset all of the underlying filters + */ template void HarmonicNotchFilter::reset() { @@ -164,9 +179,8 @@ void HarmonicNotchFilter::reset() } } - /* - a notch filter with enable and filter parameters - constructor + create parameters for the harmonic notch filter and initialise defaults */ HarmonicNotchFilterParams::HarmonicNotchFilterParams(void) { @@ -174,6 +188,6 @@ HarmonicNotchFilterParams::HarmonicNotchFilterParams(void) } /* - instantiate template classes + instantiate template classes */ template class HarmonicNotchFilter; diff --git a/libraries/Filter/HarmonicNotchFilter.h b/libraries/Filter/HarmonicNotchFilter.h index e70003d4a0..6ad43ef669 100644 --- a/libraries/Filter/HarmonicNotchFilter.h +++ b/libraries/Filter/HarmonicNotchFilter.h @@ -16,44 +16,62 @@ #include #include -#include #include #include "NotchFilter.h" +/* + a filter that manages a set of notch filters targetted at a fundamental center frequency + and multiples of that fundamental frequency + */ template class HarmonicNotchFilter { public: - // set parameters - void create(uint8_t harmonics); - void init(float sample_freq_hz, float center_freq_hz, float bandwidth_hz, float attenuation_dB); - T apply(const T &sample); - void reset(); - void update(float center_freq_hz); ~HarmonicNotchFilter(); + // allocate a bank of notch filters for this harmonic notch filter + void allocate_filters(uint8_t harmonics); + // 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 + void update(float center_freq_hz); + // apply a sample to each of the underlying filters in turn + T apply(const T &sample); + // reset each of the underlying filters + void reset(); private: + // underlying bank of notch filters NotchFilter* _filters; + // sample frequency for each filter float _sample_freq_hz; + // attenuation for each filter float _A; + // quality factor of each filter float _Q; + // a bitmask of the harmonics to use uint8_t _harmonics; + // number of allocated filters uint8_t _num_filters; bool _initialised; }; /* - notch filter enable and filter parameters + harmonic notch filter configuration parameters */ class HarmonicNotchFilterParams : public NotchFilterParams { public: 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); } + // harmonics enabled on the harmonic notch uint8_t harmonics(void) const { return _harmonics; } + // reference value of the harmonic notch float reference(void) const { return _reference; } static const struct AP_Param::GroupInfo var_info[]; private: + // notch harmonics AP_Int8 _harmonics; + // notch reference value AP_Float _reference; };