From 71f99542a8a890d94123430f10eadfb28c2c6575 Mon Sep 17 00:00:00 2001
From: Andy Piper <github@andypiper.com>
Date: Fri, 29 May 2020 17:28:43 +0100
Subject: [PATCH] Filter: allow harmonic notch center frequencies to be updated
 individually

---
 libraries/Filter/HarmonicNotchFilter.cpp | 54 +++++++++++++++++++-----
 libraries/Filter/HarmonicNotchFilter.h   |  3 ++
 2 files changed, 47 insertions(+), 10 deletions(-)

diff --git a/libraries/Filter/HarmonicNotchFilter.cpp b/libraries/Filter/HarmonicNotchFilter.cpp
index e7cc80e5ed..520d753b37 100644
--- a/libraries/Filter/HarmonicNotchFilter.cpp
+++ b/libraries/Filter/HarmonicNotchFilter.cpp
@@ -80,7 +80,7 @@ const AP_Param::GroupInfo HarmonicNotchFilterParams::var_info[] = {
     // @Param: OPTS
     // @DisplayName: Harmonic Notch Filter options
     // @Description: Harmonic Notch Filter options.
-    // @Bitmask: 0:Double notch
+    // @Bitmask: 0:Double notch,1:Dynamic harmonic
     // @User: Advanced
     // @RebootRequired: True
     AP_GROUPINFO("OPTS", 8, HarmonicNotchFilterParams, _options, 0),
@@ -176,32 +176,66 @@ 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++) {
+    for (uint8_t i = 0; i < HNF_MAX_HARMONICS && _num_enabled_filters < _num_filters; i++) {
         if ((1U<<i) & _harmonics) {
             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++;
+                    _filters[_num_enabled_filters++].init_with_A_and_Q(_sample_freq_hz, notch_center, _A, _Q);
                 }
             } 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++;
+                    _filters[_num_enabled_filters++].init_with_A_and_Q(_sample_freq_hz, notch_center_double, _A, _Q);
                 }
-                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++;
+                    _filters[_num_enabled_filters++].init_with_A_and_Q(_sample_freq_hz, notch_center_double, _A, _Q);
                 }
             }
-            filt++;
+        }
+    }
+}
+
+/*
+  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 <class T>
+void HarmonicNotchFilter<T>::update(uint8_t num_centers, const float center_freq_hz[])
+{
+    if (!_initialised) {
+        return;
+    }
+
+    // adjust the frequencies to be in the allowable range
+    const float nyquist_limit = _sample_freq_hz * 0.48f;
+
+    _num_enabled_filters = 0;
+    // update all of the filters using the new center frequencies and existing A & Q
+    for (uint8_t i = 0; i < HNF_MAX_HARMONICS && i < num_centers && _num_enabled_filters < _num_filters; i++) {
+        const float notch_center = constrain_float(center_freq_hz[i], 1.0f, nyquist_limit);
+        if (!_double_notch) {
+            // 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 {
+            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[_num_enabled_filters++].init_with_A_and_Q(_sample_freq_hz, notch_center_double, _A, _Q);
+            }
+            // 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[_num_enabled_filters++].init_with_A_and_Q(_sample_freq_hz, notch_center_double, _A, _Q);
+            }
         }
     }
 }
diff --git a/libraries/Filter/HarmonicNotchFilter.h b/libraries/Filter/HarmonicNotchFilter.h
index e3365329a5..ad0833412c 100644
--- a/libraries/Filter/HarmonicNotchFilter.h
+++ b/libraries/Filter/HarmonicNotchFilter.h
@@ -35,6 +35,8 @@ public:
     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);
+    // update all o fthe underlying center frequencies individually
+    void update(uint8_t num_centers, const 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
@@ -78,6 +80,7 @@ class HarmonicNotchFilterParams : public NotchFilterParams {
 public:
     enum class Options {
         DoubleNotch = 1<<0,
+        DynamicHarmonic = 1<<1,
     };
 
     HarmonicNotchFilterParams(void);