Diff: PosVelFilter/PosVelFilter.cpp

Revision:

10:085ab7328054

Parent:

9:d5fcdcb3c89d

Child:

87:6d95f853dab3

--- a/PosVelFilter/PosVelFilter.cpp	Fri Oct 20 11:41:22 2017 +0000
+++ b/PosVelFilter/PosVelFilter.cpp	Mon Oct 23 12:50:53 2017 +0000
@@ -1,51 +1,39 @@
 #include "PosVelFilter.hpp"
-#include "StaticDefs.hpp"
-#include "conversions.hpp"
+//#include "StaticDefs.hpp"
 
-PosVelFilter::PosVelFilter()
-{
-    x1 = 0;
-    x2 = 0;
-    //w_n is the natural frequency of the filter bigger increases frequency response
-    w_n = 1.0;
-
+PosVelFilter::PosVelFilter() {
+    x1 = 0; // pseudo position state
+    x2 = 0; // pseudo velocity state
+    
+    w_n = 1.0; // natural frequency of the filter bigger increases frequency response
 }
 
-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;
+//run the pos-vel estimate filter
+void PosVelFilter::update(float deltaT, float counts) {
     dt = deltaT;
 
     x1_dot = x2;
-    x2_dot = (-2.0*w_n*x2)-(w_n*w_n)*x1+(w_n*w_n)*counts;
+    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()
-{
+float PosVelFilter::getPosition() {
     return position;
 }
 
-float PosVelFilter::getVelocity()
-{
+float PosVelFilter::getVelocity() {
     return velocity;
 }
 
-float PosVelFilter::getDt()
-{
+float PosVelFilter::getDt() {
     return dt;
 }
 
-void PosVelFilter::writeWn(float wn)
-{
+void PosVelFilter::writeWn(float wn) {
     w_n = wn;
 }
\ No newline at end of file