From cf47c19c0d90e742397c7b4c47efff86799971df Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <andrew@tridgell.net>
Date: Mon, 11 Jan 2021 08:09:28 +1100
Subject: [PATCH] Filter: fixed low pass 2p filter to auto-init

this prevents the init to zero for temperature filters
---
 libraries/Filter/LowPassFilter2p.cpp | 7 +++++++
 libraries/Filter/LowPassFilter2p.h   | 1 +
 2 files changed, 8 insertions(+)

diff --git a/libraries/Filter/LowPassFilter2p.cpp b/libraries/Filter/LowPassFilter2p.cpp
index 484f64e8b9..d427d346ed 100644
--- a/libraries/Filter/LowPassFilter2p.cpp
+++ b/libraries/Filter/LowPassFilter2p.cpp
@@ -17,6 +17,11 @@ T DigitalBiquadFilter<T>::apply(const T &sample, const struct biquad_params &par
         return sample;
     }
 
+    if (!initialised) {
+        reset(sample);
+        initialised = true;
+    }
+
     T delay_element_0 = sample - _delay_element_1 * params.a1 - _delay_element_2 * params.a2;
     T output = delay_element_0 * params.b0 + _delay_element_1 * params.b1 + _delay_element_2 * params.b2;
 
@@ -29,11 +34,13 @@ T DigitalBiquadFilter<T>::apply(const T &sample, const struct biquad_params &par
 template <class T>
 void DigitalBiquadFilter<T>::reset() { 
     _delay_element_1 = _delay_element_2 = T();
+    initialised = false;
 }
 
 template <class T>
 void DigitalBiquadFilter<T>::reset(const T &value) {
     _delay_element_1 = _delay_element_2 = value;
+    initialised = true;
 }
 
 template <class T>
diff --git a/libraries/Filter/LowPassFilter2p.h b/libraries/Filter/LowPassFilter2p.h
index fcc8d644da..1024c634b8 100644
--- a/libraries/Filter/LowPassFilter2p.h
+++ b/libraries/Filter/LowPassFilter2p.h
@@ -45,6 +45,7 @@ public:
 private:
     T _delay_element_1;
     T _delay_element_2;
+    bool initialised;
 };
 
 template <class T>