From 3c6ddda5e8d564e06f7188c759212e62f5953aaa Mon Sep 17 00:00:00 2001
From: Iampete1 <iampete@hotmail.co.uk>
Date: Sat, 8 Jul 2023 02:28:32 +0100
Subject: [PATCH] Flter: notchfilter: remove unneeded value and pre-multiply
 for speed

---
 libraries/Filter/NotchFilter.cpp              | 20 ++++++++++++++-----
 libraries/Filter/NotchFilter.h                |  4 ++--
 .../TransferFunctionCheck.cpp                 |  2 +-
 3 files changed, 18 insertions(+), 8 deletions(-)

diff --git a/libraries/Filter/NotchFilter.cpp b/libraries/Filter/NotchFilter.cpp
index db9e2e234b..ea2a77d80f 100644
--- a/libraries/Filter/NotchFilter.cpp
+++ b/libraries/Filter/NotchFilter.cpp
@@ -77,9 +77,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;
         initialised = true;
@@ -100,16 +109,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 39ed64d4f2..dd6f319f78 100644
--- a/libraries/Filter/NotchFilter.h
+++ b/libraries/Filter/NotchFilter.h
@@ -41,9 +41,9 @@ public:
 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;
-    T ntchsig, ntchsig1, ntchsig2, signal2, signal1;
+    T ntchsig1, ntchsig2, signal2, signal1;
 };
 
 /*
diff --git a/libraries/Filter/examples/TransferFunctionCheck/TransferFunctionCheck.cpp b/libraries/Filter/examples/TransferFunctionCheck/TransferFunctionCheck.cpp
index d1518719f0..320f0b4456 100644
--- a/libraries/Filter/examples/TransferFunctionCheck/TransferFunctionCheck.cpp
+++ b/libraries/Filter/examples/TransferFunctionCheck/TransferFunctionCheck.cpp
@@ -61,7 +61,7 @@ public:
         hal.console->printf("NotchFilterFloat\n");
         hal.console->printf("Sample rate: %.9f Hz, Center: %.9f Hz\n", _sample_freq_hz, _center_freq_hz);
         hal.console->printf("Notch filter in the form: H(z) = (b0 + b1*z^-1 + b2*z^-2)/(a0 + a1*z^-1 + a2*z^-2)\n");
-        hal.console->printf("a0: %.9f, a1: %.9f, a2: %.9f, b0: %.9f, b1: %.9f, b2: %.9f\n", 1.0/a0_inv, a1, a2, b0, b1, b2);
+        hal.console->printf("a0: %.9f, a1: %.9f, a2: %.9f, b0: %.9f, b1: %.9f, b2: %.9f\n", 1.0, a1, a2, b0, b1, b2);
     }
 };