Filter: add support for double harmonic notches to more effectively target wider noise peaks

libraries/Filter/HarmonicNotchFilter.cpp

@@ -16,7 +16,8 @@
 #include "HarmonicNotchFilter.h"
 #include <GCS_MAVLink/GCS.h>
 
-#define HNF_MAX_FILTERS 3
+#define HNF_MAX_FILTERS 6 // must be even for double-notch filters
+#define HNF_MAX_HARMONICS 8
 
 // table of user settable parameters
 const AP_Param::GroupInfo HarmonicNotchFilterParams::var_info[] = {
@@ -76,6 +77,14 @@ const AP_Param::GroupInfo HarmonicNotchFilterParams::var_info[] = {
     // @User: Advanced
     AP_GROUPINFO("MODE", 7, HarmonicNotchFilterParams, _tracking_mode, 1),
 
+    // @Param: OPTS
+    // @DisplayName: Harmonic Notch Filter options
+    // @Description: Harmonic Notch Filter options.
+    // @Bitmask: 0:Double notch
+    // @User: Advanced
+    // @RebootRequired: True
+    AP_GROUPINFO("OPTS", 8, HarmonicNotchFilterParams, _options, 0),
+
     AP_GROUPEND
 };
 
@@ -102,39 +111,41 @@ void HarmonicNotchFilter<T>::init(float sample_freq_hz, float center_freq_hz, fl
     }
 
     _sample_freq_hz = sample_freq_hz;
-    const float nyquist_limit = sample_freq_hz * 0.48f;
+    // Calculate spread required to achieve an equivalent single notch using two notches with Bandwidth/2
+    _notch_spread = bandwidth_hz / (32 * 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
-    center_freq_hz = constrain_float(center_freq_hz, bandwidth_hz * 0.52f, nyquist_limit);
-
-    // calculate attenuation and quality from the shaping constraints
-    NotchFilter<T>::calculate_A_and_Q(center_freq_hz, bandwidth_hz, attenuation_dB, _A, _Q);
-
-    _num_enabled_filters = 0;
-    // 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++) {
-        const float notch_center = center_freq_hz * (i+1);
-        if ((1U<<i) & _harmonics) {
-            // only enable the filter if its center frequency is below the nyquist frequency
-            if (notch_center < nyquist_limit) {
-                _filters[filt].init_with_A_and_Q(sample_freq_hz, notch_center, _A, _Q);
-                _num_enabled_filters++;
-            }
-            filt++;
-        }
+    center_freq_hz = constrain_float(center_freq_hz, bandwidth_limit, nyquist_limit);
+
+    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<T>::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<T>::calculate_A_and_Q(center_freq_hz, bandwidth_hz, attenuation_dB, _A, _Q);
     }
+
     _initialised = true;
+    update(center_freq_hz);
 }
 
 /*
   allocate a collection of, at most HNF_MAX_FILTERS, notch filters to be managed by this harmonic notch filter
  */
 template <class T>
-void HarmonicNotchFilter<T>::allocate_filters(uint8_t harmonics)
+void HarmonicNotchFilter<T>::allocate_filters(uint8_t harmonics, bool double_notch)
 {
+    _double_notch = double_notch;
+
     for (uint8_t i = 0; i < HNF_MAX_HARMONICS && _num_filters < HNF_MAX_FILTERS; i++) {
         if ((1U<<i) & harmonics) {
             _num_filters++;
+            if (_double_notch) {
+                _num_filters++;
+            }
         }
     }
     if (_num_filters > 0) {
@@ -166,12 +177,29 @@ void HarmonicNotchFilter<T>::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, filt = 0; i < HNF_MAX_HARMONICS && filt < _num_filters; i++) {
-        const float notch_center = center_freq_hz * (i+1);
         if ((1U<<i) & _harmonics) {
-            // only enable the filter if its center frequency is below the nyquist frequency
-            if (notch_center < nyquist_limit) {
-                _filters[filt].init_with_A_and_Q(_sample_freq_hz, notch_center, _A, _Q);
-                _num_enabled_filters++;
+            const float notch_center = center_freq_hz * (i+1);
+            if (!_double_notch) {
+                // only enable the filter if its center frequency is below the nyquist frequency
+                if (notch_center < nyquist_limit) {
+                    _filters[filt].init_with_A_and_Q(_sample_freq_hz, notch_center, _A, _Q);
+                    _num_enabled_filters++;
+                }
+            } else {
+                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[filt].init_with_A_and_Q(_sample_freq_hz, notch_center_double, _A, _Q);
+                    _num_enabled_filters++;
+                }
+                filt++;
+                // 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[filt].init_with_A_and_Q(_sample_freq_hz, notch_center_double, _A, _Q);
+                    _num_enabled_filters++;
+                }
             }
             filt++;
         }

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 harmonics);
+    void allocate_filters(uint8_t harmonics, bool double_notch);
     // 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
@@ -45,12 +45,16 @@ private:
     NotchFilter<T>*  _filters;
     // sample frequency for each filter
     float _sample_freq_hz;
+    // base double notch bandwidth for each filter
+    float _notch_spread;
     // attenuation for each filter
     float _A;
     // quality factor of each filter
     float _Q;
     // a bitmask of the harmonics to use
     uint8_t _harmonics;
+    // whether to use double-notches
+    bool _double_notch;
     // number of allocated filters
     uint8_t _num_filters;
     // number of enabled filters
@@ -72,6 +76,10 @@ enum class HarmonicNotchDynamicMode {
  */
 class HarmonicNotchFilterParams : public NotchFilterParams {
 public:
+    enum class Options {
+        DoubleNotch = 1<<0,
+    };
+
     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); }
@@ -79,6 +87,8 @@ public:
     uint8_t harmonics(void) const { return _harmonics; }
     // reference value of the harmonic notch
     float reference(void) const { return _reference; }
+    // notch options
+    bool hasOption(Options option) const { return _options & uint16_t(option); }
     // notch dynamic tracking mode
     HarmonicNotchDynamicMode tracking_mode(void) const { return HarmonicNotchDynamicMode(_tracking_mode.get()); }
     static const struct AP_Param::GroupInfo var_info[];
@@ -90,6 +100,8 @@ private:
     AP_Float _reference;
     // notch dynamic tracking mode
     AP_Int8 _tracking_mode;
+    // notch options
+    AP_Int16 _options;
 };
 
 typedef HarmonicNotchFilter<Vector3f> HarmonicNotchFilterVector3f;