From c7a6dc6dbd05543f4371fa6785055d4ff3a2ae93 Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <andrew@tridgell.net>
Date: Sat, 3 Feb 2024 08:41:23 +1100
Subject: [PATCH] Filter: align notch code with master

---
 libraries/Filter/NotchFilter.cpp | 20 +++++++++++++++-----
 libraries/Filter/NotchFilter.h   |  6 ++++--
 2 files changed, 19 insertions(+), 7 deletions(-)

diff --git a/libraries/Filter/NotchFilter.cpp b/libraries/Filter/NotchFilter.cpp
index 64aac746ce534..dbbec6f80a802 100644
--- a/libraries/Filter/NotchFilter.cpp
+++ b/libraries/Filter/NotchFilter.cpp
@@ -80,9 +80,18 @@ void NotchFilter<T>::init_with_A_and_Q(float sample_freq_hz, float center_freq_h
         b0 =  1.0 + alpha*sq(A);
         b1 = -2.0 * cosf(omega);
         b2 =  1.0 - alpha*sq(A);
-        a0_inv =  1.0/(1.0 + alpha);
         a1 = b1;
         a2 =  1.0 - alpha;
+
+        const float a0_inv =  1.0/(1.0 + alpha);
+
+        // Pre-multiply to save runtime calc
+        b0 *= a0_inv;
+        b1 *= a0_inv;
+        b2 *= a0_inv;
+        a1 *= a0_inv;
+        a2 *= a0_inv;
+
         _center_freq_hz = new_center_freq;
         _sample_freq_hz = sample_freq_hz;
         _A = A;
@@ -104,16 +113,17 @@ T NotchFilter<T>::apply(const T &sample)
         // sample as output and update delayed samples
         signal1 = sample;
         signal2 = sample;
-        ntchsig = sample;
         ntchsig1 = sample;
         ntchsig2 = sample;
         need_reset = false;
         return sample;
     }
+
+    T output = sample*b0 + ntchsig1*b1 + ntchsig2*b2 - signal1*a1 - signal2*a2;
+
     ntchsig2 = ntchsig1;
-    ntchsig1 = ntchsig;
-    ntchsig = sample;
-    T output = (ntchsig*b0 + ntchsig1*b1 + ntchsig2*b2 - signal1*a1 - signal2*a2) * a0_inv;
+    ntchsig1 = sample;
+
     signal2 = signal1;
     signal1 = output;
     return output;
diff --git a/libraries/Filter/NotchFilter.h b/libraries/Filter/NotchFilter.h
index 4911c4a973425..c397f6b40ccf8 100644
--- a/libraries/Filter/NotchFilter.h
+++ b/libraries/Filter/NotchFilter.h
@@ -38,6 +38,8 @@ class NotchFilter {
     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();
+    float center_freq_hz() const { return _center_freq_hz; }
+    float sample_freq_hz() const { return _sample_freq_hz; }
 
     // 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); 
@@ -49,9 +51,9 @@ class NotchFilter {
 protected:
 
     bool initialised, need_reset;
-    float b0, b1, b2, a1, a2, a0_inv;
+    float b0, b1, b2, a1, a2;
     float _center_freq_hz, _sample_freq_hz, _A;
-    T ntchsig, ntchsig1, ntchsig2, signal2, signal1;
+    T ntchsig1, ntchsig2, signal2, signal1;
 };
 
 /*