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

@@ -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;
 }
 
 /*

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;
 };