From 6cae97446af6334bef31c574aacb7a1d7a875dde Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <andrew@tridgell.net>
Date: Sun, 4 Apr 2021 09:14:57 +1000
Subject: [PATCH] Filter: added auto-init to LowPassFilter

make sure initial filter returns are not a long way off if filtered
input is far from zero
---
 libraries/Filter/LowPassFilter.cpp | 11 ++++++++++-
 libraries/Filter/LowPassFilter.h   |  8 ++++++--
 2 files changed, 16 insertions(+), 3 deletions(-)

diff --git a/libraries/Filter/LowPassFilter.cpp b/libraries/Filter/LowPassFilter.cpp
index 25c756cf8b..246dc4cc02 100644
--- a/libraries/Filter/LowPassFilter.cpp
+++ b/libraries/Filter/LowPassFilter.cpp
@@ -27,12 +27,20 @@ T DigitalLPF<T>::apply(const T &sample, float cutoff_freq, float dt) {
     float rc = 1.0f/(M_2PI*cutoff_freq);
     alpha = constrain_float(dt/(dt+rc), 0.0f, 1.0f);
     _output += (sample - _output) * alpha;
+    if (!initialised) {
+        initialised = true;
+        _output = sample;
+    }
     return _output;
 }
 
 template <class T>
 T DigitalLPF<T>::apply(const T &sample) {
     _output += (sample - _output) * alpha;
+    if (!initialised) {
+        initialised = true;
+        _output = sample;
+    }
     return _output;
 }
 
@@ -53,7 +61,8 @@ const T &DigitalLPF<T>::get() const {
 
 template <class T>
 void DigitalLPF<T>::reset(T value) { 
-    _output = value; 
+    _output = value;
+    initialised = true;
 }
     
 ////////////////////////////////////////////////////////////////////////////////////////////
diff --git a/libraries/Filter/LowPassFilter.h b/libraries/Filter/LowPassFilter.h
index 1ff19a3875..9e8b9c02a7 100644
--- a/libraries/Filter/LowPassFilter.h
+++ b/libraries/Filter/LowPassFilter.h
@@ -61,10 +61,14 @@ public:
     // get latest filtered value from filter (equal to the value returned by latest call to apply method)
     const T &get() const;
     void reset(T value);
+    void reset() {
+        initialised = false;
+    }
 
 private:
     T _output;
     float alpha = 1.0f;
+    bool initialised;
 };
 
 // LPF base class
@@ -85,8 +89,8 @@ public:
     T apply(T sample);
     const T &get() const;
     void reset(T value);
-    void reset(void) { reset(T()); }
-    
+    void reset(void) { _filter.reset(); }
+
 protected:
     float _cutoff_freq;