Filter: align notch code with master

ArduPilot · Feb 2, 2024 · c7a6dc6 · c7a6dc6
1 parent 8d900ae
commit c7a6dc6
Show file tree

Hide file tree

Showing 2 changed files with 19 additions and 7 deletions.
diff --git 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
@@ -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;
 };
 
 /*