Diff: PosVelFilter/PosVelFilter.cpp

Revision:

9:d5fcdcb3c89d

Child:

10:085ab7328054

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/PosVelFilter/PosVelFilter.cpp	Fri Oct 20 11:41:22 2017 +0000
@@ -0,0 +1,51 @@
+#include "PosVelFilter.hpp"
+#include "StaticDefs.hpp"
+#include "conversions.hpp"
+
+PosVelFilter::PosVelFilter()
+{
+    x1 = 0;
+    x2 = 0;
+    //w_n is the natural frequency of the filter bigger increases frequency response
+    w_n = 1.0;
+
+}
+
+void PosVelFilter::update(float deltaT, float counts)
+{
+    //run the pos-vel estimate filter
+    //this derives the timing from last run
+    //last_time = curr_time;
+    //curr_time = time;
+    dt = deltaT;
+
+    x1_dot = x2;
+    x2_dot = (-2.0*w_n*x2)-(w_n*w_n)*x1+(w_n*w_n)*counts;
+
+    position = x1;
+    velocity = x2;
+
+    x1 += x1_dot*dt;
+    x2 += x2_dot*dt;
+
+}
+
+float PosVelFilter::getPosition()
+{
+    return position;
+}
+
+float PosVelFilter::getVelocity()
+{
+    return velocity;
+}
+
+float PosVelFilter::getDt()
+{
+    return dt;
+}
+
+void PosVelFilter::writeWn(float wn)
+{
+    w_n = wn;
+}
\ No newline at end of file