pt1 filter

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

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