From cc7b3a97d5294fa6a4cfbbed22c77253d0d9ee82 Mon Sep 17 00:00:00 2001
From: philsson <philsson@gmail.com>
Date: Sun, 2 Sep 2018 19:01:31 +0200
Subject: [PATCH] pt1 filter

---
 src/control/lpf.cpp | 23 ++++++++++++++++++++---
 src/control/lpf.h   | 22 ++++++++++++++++++++--
 2 files changed, 40 insertions(+), 5 deletions(-)

diff --git a/src/control/lpf.cpp b/src/control/lpf.cpp
index 583e510..3cf836b 100644
--- a/src/control/lpf.cpp
+++ b/src/control/lpf.cpp
@@ -1,17 +1,34 @@
 #include "src/control/lpf.h"
+#include "src/math/Utilities.h"
+
+using namespace math;
 
 namespace control {
 
-
 incrementalLPF::incrementalLPF()
 : m_filtered(0)
 {
 }
 
-double incrementalLPF::filter(double latestValue)
+float incrementalLPF::filter(float latestValue)
 {
-    m_filtered = m_filtered*0.95 + latestValue*0.05;
+    m_filtered = m_filtered*0.95f + latestValue*0.05f;
     return m_filtered;
 }
 
+pt1FilterApply4::pt1FilterApply4(float freqCut)
+: m_freqCut(freqCut)
+, m_RC(1.0f / (2.0f * (float)PI * m_freqCut))
+, m_filtered(0.0f)
+{
+}
+
+float pt1FilterApply4::filter(float input, float dT)
+{
+	m_filtered = m_filtered + dT / (m_RC + dT) * (input - m_filtered);
+
+	return m_filtered;
+}
+
+
 } // namespace control
\ No newline at end of file
diff --git a/src/control/lpf.h b/src/control/lpf.h
index af15588..5007a56 100644
--- a/src/control/lpf.h
+++ b/src/control/lpf.h
@@ -9,11 +9,29 @@ public:
 
     incrementalLPF();
 
-    double filter(double latestValue);
+    float filter(float latestValue);
 
 private:
 
-    double m_filtered;
+    float m_filtered;
 };
 
+
+class pt1FilterApply4
+{
+
+public:
+
+    pt1FilterApply4(float freqCut);
+
+    float filter(float dT, float latestValue);
+
+private:
+
+    float m_freqCut;
+    float m_RC;
+    float m_filtered;
+};
+
+
 } // namespace control
\ No newline at end of file