Filter: New HarmonicNotchFilter

This delegates updates to a bank of NotchFilters located at an rpm frequency and harmonics. Center frequency can be updated dynamically. Notch parameters are configurable, including the number of harmonics to filter on. Updates to the filter parameters are optimized across the notch bank. Convert notch bandwidth and frequency to floats.

allow all filter harmonics to be controlled.

add destructor to harmonic notch.

don't allocate sub-filters for harmonic notch if no harmonics set.

libraries/Filter/HarmonicNotchFilter.cpp

@@ -0,0 +1,179 @@
+/*
+   This program is free software: you can redistribute it and/or modify
+   it under the terms of the GNU General Public License as published by
+   the Free Software Foundation, either version 3 of the License, or
+   (at your option) any later version.
+
+   This program is distributed in the hope that it will be useful,
+   but WITHOUT ANY WARRANTY; without even the implied warranty of
+   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+   GNU General Public License for more details.
+
+   You should have received a copy of the GNU General Public License
+   along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#include "HarmonicNotchFilter.h"
+
+#define HNF_MAX_FILTERS 3
+#define HNF_MAX_HARMONICS 8
+
+// table of user settable parameters
+const AP_Param::GroupInfo HarmonicNotchFilterParams::var_info[] = {
+
+    // @Param: ENABLE
+    // @DisplayName: Enable
+    // @Description: Enable harmonic notch filter
+    // @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
+    // @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
+    // @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
+    // @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
+    // @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
+    // @User: Advanced
+    // @Range: 0.1 0.9
+    // @RebootRequired: True
+    AP_GROUPINFO("REF", 6, HarmonicNotchFilterParams, _reference, 0.1f),
+
+    AP_GROUPEND
+};
+
+
+/*
+  initialise filter
+ */
+template <class T>
+void HarmonicNotchFilter<T>::init(float sample_freq_hz, float center_freq_hz, float bandwidth_hz, float attenuation_dB)
+{
+    if (_filters == nullptr) {
+        return;
+    }
+
+    _sample_freq_hz = sample_freq_hz;
+
+    // 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);
+
+    NotchFilter<T>::calculate_A_and_Q(center_freq_hz, bandwidth_hz, attenuation_dB, _A, _Q);
+
+    for (uint8_t i = 0, filt = 0; i < HNF_MAX_HARMONICS && filt < _num_filters; i++) {
+        if ((1U<<i) & _harmonics) {
+            _filters[filt++].init_with_A_and_Q(sample_freq_hz, center_freq_hz * (i+1), _A, _Q);
+        }
+    }
+    _initialised = true;
+}
+
+/*
+  initialise filter
+ */
+template <class T>
+void HarmonicNotchFilter<T>::create(uint8_t harmonics)
+{
+    for (uint8_t i = 0; i < HNF_MAX_HARMONICS && _num_filters < HNF_MAX_FILTERS; i++) {
+        if ((1U<<i) & harmonics) {
+            _num_filters++;
+        }
+    }
+    if (_num_filters > 0) {
+        _filters = new NotchFilter<T>[_num_filters];
+    }
+    _harmonics = harmonics;
+}
+
+template <class T>
+HarmonicNotchFilter<T>::~HarmonicNotchFilter() {
+    if (_num_filters > 0) {
+        delete[] _filters;
+    }
+}
+
+template <class T>
+void HarmonicNotchFilter<T>::update(float center_freq_hz)
+{
+    if (!_initialised) {
+        return;
+    }
+
+    // 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);
+
+    for (uint8_t i = 0, filt = 0; i < HNF_MAX_HARMONICS && filt < _num_filters; i++) {
+        if ((1U<<i) & _harmonics) {
+            _filters[filt++].init_with_A_and_Q(_sample_freq_hz, center_freq_hz * (i+1), _A, _Q);
+        }
+    }
+}
+
+template <class T>
+T HarmonicNotchFilter<T>::apply(const T &sample)
+{
+    if (!_initialised) {
+        return sample;
+    }
+
+    T output = sample;
+    for (uint8_t i = 0; i < _num_filters; i++) {
+        output = _filters[i].apply(output);
+    }
+    return output;
+}
+
+template <class T>
+void HarmonicNotchFilter<T>::reset()
+{
+    if (!_initialised) {
+        return;
+    }
+
+    for (uint8_t i = 0; i < _num_filters; i++) {
+        _filters[i].reset();
+    }
+}
+
+
+/*
+  a notch filter with enable and filter parameters - constructor
+ */
+HarmonicNotchFilterParams::HarmonicNotchFilterParams(void)
+{
+    AP_Param::setup_object_defaults(this, var_info);
+}
+
+/* 
+   instantiate template classes
+ */
+template class HarmonicNotchFilter<Vector3f>;

libraries/Filter/HarmonicNotchFilter.h

@@ -0,0 +1,61 @@
+/*
+   This program is free software: you can redistribute it and/or modify
+   it under the terms of the GNU General Public License as published by
+   the Free Software Foundation, either version 3 of the License, or
+   (at your option) any later version.
+
+   This program is distributed in the hope that it will be useful,
+   but WITHOUT ANY WARRANTY; without even the implied warranty of
+   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+   GNU General Public License for more details.
+
+   You should have received a copy of the GNU General Public License
+   along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+#pragma once
+
+#include <AP_Math/AP_Math.h>
+#include <cmath>
+#include <inttypes.h>
+#include <AP_Param/AP_Param.h>
+#include "NotchFilter.h"
+
+template <class T>
+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();
+
+private:
+    NotchFilter<T>*  _filters;
+    float _sample_freq_hz;
+    float _A;
+    float _Q;
+    uint8_t _harmonics;
+    uint8_t _num_filters;
+    bool _initialised;
+};
+
+/*
+  notch filter enable and filter parameters
+ */
+class HarmonicNotchFilterParams : public NotchFilterParams {
+public:
+    HarmonicNotchFilterParams(void);
+    void set_center_freq_hz(float center_freq) { _center_freq_hz.set(center_freq); }
+    uint8_t harmonics(void) const { return _harmonics; }
+    float reference(void) const { return _reference; }
+    static const struct AP_Param::GroupInfo var_info[];
+
+private:
+    AP_Int8 _harmonics;
+    AP_Float _reference;
+};
+
+typedef HarmonicNotchFilter<Vector3f> HarmonicNotchFilterVector3f;
+

libraries/Filter/NotchFilter.cpp

@@ -15,16 +15,34 @@
 
 #include "NotchFilter.h"
 
+/*
+   calculate the attenuation and quality factors of the filter
+ */
+template <class T>
+void NotchFilter<T>::calculate_A_and_Q(float center_freq_hz, float bandwidth_hz, float attenuation_dB, float& A, float& Q) {
+    const float octaves = log2f(center_freq_hz / (center_freq_hz - bandwidth_hz / 2.0f)) * 2.0f;
+    A = powf(10, -attenuation_dB / 40.0f);
+    Q = sqrtf(powf(2, octaves)) / (powf(2, octaves) - 1.0f);
+}
+
 /*
   initialise filter
  */
 template <class T>
 void NotchFilter<T>::init(float sample_freq_hz, float center_freq_hz, float bandwidth_hz, float attenuation_dB)
+{
+    // 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);
+
+    float A, Q;
+    calculate_A_and_Q(center_freq_hz, bandwidth_hz, attenuation_dB, A, Q);
+    init_with_A_and_Q(sample_freq_hz, center_freq_hz, A, Q);
+}
+
+template <class T>
+void NotchFilter<T>::init_with_A_and_Q(float sample_freq_hz, float center_freq_hz, float A, float Q)
 {
     float omega = 2.0 * M_PI * center_freq_hz / sample_freq_hz;
-    float octaves = log2f(center_freq_hz  / (center_freq_hz - bandwidth_hz/2)) * 2;
-    float A = powf(10, -attenuation_dB/40);
-    float Q = sqrtf(powf(2, octaves)) / (powf(2,octaves) - 1);
     float alpha = sinf(omega) / (2 * Q/A);
     b0 =  1.0 + alpha*A;
     b1 = -2.0 * cosf(omega);
@@ -72,22 +90,6 @@ const AP_Param::GroupInfo NotchFilterParams::var_info[] = {
     // @User: Advanced
     AP_GROUPINFO_FLAGS("ENABLE", 1, NotchFilterParams, _enable, 0, AP_PARAM_FLAG_ENABLE),
 
-    // @Param: FREQ
-    // @DisplayName: Frequency
-    // @Description: Notch center frequency in Hz
-    // @Range: 10 400
-    // @Units: Hz
-    // @User: Advanced
-    AP_GROUPINFO("FREQ", 2, NotchFilterParams, _center_freq_hz, 80),
-
-    // @Param: BW
-    // @DisplayName: Bandwidth
-    // @Description: Notch bandwidth in Hz
-    // @Range: 5 100
-    // @Units: Hz
-    // @User: Advanced
-    AP_GROUPINFO("BW", 3, NotchFilterParams, _bandwidth_hz, 20),
-
     // @Param: ATT
     // @DisplayName: Attenuation
     // @Description: Notch attenuation in dB
@@ -95,7 +97,23 @@ const AP_Param::GroupInfo NotchFilterParams::var_info[] = {
     // @Units: dB
     // @User: Advanced
     AP_GROUPINFO("ATT", 4, NotchFilterParams, _attenuation_dB, 15),
-    
+
+    // @Param: FREQ
+    // @DisplayName: Frequency
+    // @Description: Notch center frequency in Hz
+    // @Range: 10 400
+    // @Units: Hz
+    // @User: Advanced
+    AP_GROUPINFO("FREQ", 5, NotchFilterParams, _center_freq_hz, 80),
+
+    // @Param: BW
+    // @DisplayName: Bandwidth
+    // @Description: Notch bandwidth in Hz
+    // @Range: 5 100
+    // @Units: Hz
+    // @User: Advanced
+    AP_GROUPINFO("BW", 6, NotchFilterParams, _bandwidth_hz, 20),
+
     AP_GROUPEND
 };
 

libraries/Filter/NotchFilter.h

@@ -31,10 +31,15 @@ class NotchFilter {
 public:
     // 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);
     T apply(const T &sample);
     void reset();
 
+    // 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:
+
     bool initialised;
     float b0, b1, b2, a1, a2, a0_inv;
     T ntchsig, ntchsig1, ntchsig2, signal2, signal1;
@@ -48,15 +53,15 @@ public:
     NotchFilterParams(void);
     static const struct AP_Param::GroupInfo var_info[];
 
-    uint16_t center_freq_hz(void) const { return _center_freq_hz; }
+    float center_freq_hz(void) const { return _center_freq_hz; }
-    uint16_t bandwidth_hz(void) const { return _bandwidth_hz; }
+    float bandwidth_hz(void) const { return _bandwidth_hz; }
     float attenuation_dB(void) const { return _attenuation_dB; }
     uint8_t enabled(void) const { return _enable; }
     
-private:
+protected:
     AP_Int8 _enable;
-    AP_Int16 _center_freq_hz;
+    AP_Float _center_freq_hz;
-    AP_Int16 _bandwidth_hz;
+    AP_Float _bandwidth_hz;
     AP_Float _attenuation_dB;
 };