From 6144136b6173bc561e6a43e5efb950f0405922f2 Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <andrew@tridgell.net>
Date: Fri, 5 Feb 2021 13:57:02 +1100
Subject: [PATCH] Filter: fixed reset of filters to first value

the delay elements were set incorrectly
---
 libraries/Filter/LowPassFilter2p.cpp                   |  8 ++++----
 libraries/Filter/LowPassFilter2p.h                     |  2 +-
 .../examples/LowPassFilter2p/LowPassFilter2p.cpp       | 10 +++-------
 3 files changed, 8 insertions(+), 12 deletions(-)

diff --git a/libraries/Filter/LowPassFilter2p.cpp b/libraries/Filter/LowPassFilter2p.cpp
index d427d346ed..93f9bb86ef 100644
--- a/libraries/Filter/LowPassFilter2p.cpp
+++ b/libraries/Filter/LowPassFilter2p.cpp
@@ -18,7 +18,7 @@ T DigitalBiquadFilter<T>::apply(const T &sample, const struct biquad_params &par
     }
 
     if (!initialised) {
-        reset(sample);
+        reset(sample, params);
         initialised = true;
     }
 
@@ -38,8 +38,8 @@ void DigitalBiquadFilter<T>::reset() {
 }
 
 template <class T>
-void DigitalBiquadFilter<T>::reset(const T &value) {
-    _delay_element_1 = _delay_element_2 = value;
+void DigitalBiquadFilter<T>::reset(const T &value, const struct biquad_params &params) {
+    _delay_element_1 = _delay_element_2 = value * (1.0 / (1 + params.a1 + params.a2));
     initialised = true;
 }
 
@@ -113,7 +113,7 @@ void LowPassFilter2p<T>::reset(void) {
 
 template <class T>
 void LowPassFilter2p<T>::reset(const T &value) {
-    return _filter.reset(value);
+    return _filter.reset(value, _params);
 }
 
 /* 
diff --git a/libraries/Filter/LowPassFilter2p.h b/libraries/Filter/LowPassFilter2p.h
index 1024c634b8..5197f97579 100644
--- a/libraries/Filter/LowPassFilter2p.h
+++ b/libraries/Filter/LowPassFilter2p.h
@@ -39,7 +39,7 @@ public:
 
     T apply(const T &sample, const struct biquad_params &params);
     void reset();
-    void reset(const T &value);
+    void reset(const T &value, const struct biquad_params &params);
     static void compute_params(float sample_freq, float cutoff_freq, biquad_params &ret);
     
 private:
diff --git a/libraries/Filter/examples/LowPassFilter2p/LowPassFilter2p.cpp b/libraries/Filter/examples/LowPassFilter2p/LowPassFilter2p.cpp
index 4347c703e3..bdbe5a7c26 100644
--- a/libraries/Filter/examples/LowPassFilter2p/LowPassFilter2p.cpp
+++ b/libraries/Filter/examples/LowPassFilter2p/LowPassFilter2p.cpp
@@ -26,16 +26,12 @@ void loop()
     for(int16_t i = 0; i < 300; i++ ) {
 
         // new data value
-        const float new_value = sinf((float)i * 2 * M_PI * 5 / 50.0f);  // 5hz
-
-        // output to user
-        hal.console->printf("applying: %6.4f", (double)new_value);
-
+        const float new_value = 17 + sinf((float)i * 2 * M_PI * 5 / 50.0f);  // 5hz
         // apply new value and retrieved filtered result
         const float filtered_value = low_pass_filter.apply(new_value);
 
-        // display results
-        hal.console->printf("\toutput: %6.4f\n", (double)filtered_value);
+        // output to user
+        hal.console->printf("applying: %6.4f -> %6.4f\n", (double)new_value, filtered_value);
 
         hal.scheduler->delay(10);
     }