From d07761cfd97235a47fc518fbdb4186cbe39a9f86 Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <andrew@tridgell.net>
Date: Mon, 6 Jun 2022 09:40:34 +1000
Subject: [PATCH] Filter: fixed reset of notch filters

when we reset a notch we need to init the stored values to the current
value, rather than assuming that zero is the right value

this matters when switching IMUs in flight when we are only running
notch filters on the active gyro
---
 libraries/Filter/NotchFilter.cpp | 6 +++---
 libraries/Filter/NotchFilter.h   | 2 +-
 2 files changed, 4 insertions(+), 4 deletions(-)

diff --git a/libraries/Filter/NotchFilter.cpp b/libraries/Filter/NotchFilter.cpp
index ea818ec68b..510f085094 100644
--- a/libraries/Filter/NotchFilter.cpp
+++ b/libraries/Filter/NotchFilter.cpp
@@ -74,7 +74,7 @@ void NotchFilter<T>::init_with_A_and_Q(float sample_freq_hz, float center_freq_h
 template <class T>
 T NotchFilter<T>::apply(const T &sample)
 {
-    if (!initialised) {
+    if (!initialised || need_reset) {
         // if we have not been initialised when return the input
         // sample as output and update delayed samples
         ntchsig2 = ntchsig1;
@@ -82,6 +82,7 @@ T NotchFilter<T>::apply(const T &sample)
         ntchsig = sample;
         signal2 = signal1;
         signal1 = sample;
+        need_reset = false;
         return sample;
     }
     ntchsig2 = ntchsig1;
@@ -96,8 +97,7 @@ T NotchFilter<T>::apply(const T &sample)
 template <class T>
 void NotchFilter<T>::reset()
 {
-    ntchsig2 = ntchsig1 = T();
-    signal2 = signal1 = T();
+    need_reset = true;
 }
 
 /*
diff --git a/libraries/Filter/NotchFilter.h b/libraries/Filter/NotchFilter.h
index 62352b60ee..4c74fe7f5d 100644
--- a/libraries/Filter/NotchFilter.h
+++ b/libraries/Filter/NotchFilter.h
@@ -40,7 +40,7 @@ public:
 
 private:
 
-    bool initialised;
+    bool initialised, need_reset;
     float b0, b1, b2, a1, a2, a0_inv;
     T ntchsig, ntchsig1, ntchsig2, signal2, signal1;
 };