Diff: PosVelFilter/PosVelFilter.cpp

Revision:

87:6d95f853dab3

Parent:

10:085ab7328054

--- a/PosVelFilter/PosVelFilter.cpp	Thu May 02 20:34:16 2019 +0000
+++ b/PosVelFilter/PosVelFilter.cpp	Wed May 08 13:24:04 2019 +0000
@@ -1,25 +1,61 @@
 #include "PosVelFilter.hpp"
 //#include "StaticDefs.hpp"
 
-PosVelFilter::PosVelFilter() {
+PosVelFilter::PosVelFilter() {  // if heading_flag is set, operate on sine and cosine, return atan2(sin, cos) as postion and velocity??
     x1 = 0; // pseudo position state
     x2 = 0; // pseudo velocity state
-    
-    w_n = 1.0; // natural frequency of the filter bigger increases frequency response
+    x1c = 0;
+    x2c = 0;
+    x1s = 0;
+    x2s = 0;
+    w_n = 1.0; // natural frequency of the filter:   bigger increases frequency response
+    _headingFlag = false;
 }
 
 //run the pos-vel estimate filter
 void PosVelFilter::update(float deltaT, float counts) {
     dt = deltaT;
+    
+    if(!_headingFlag) {
+      x1_dot = x2;
+      x2_dot = (-2.0*w_n*x2) - (w_n*w_n)*x1 + (w_n*w_n)*counts;
 
-    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;
+    }
+    if(_headingFlag) {
+        counts_c = cos(counts * _PI/180.0);  // math.h cos and sine operate on radians , counts is heading degrees
+        counts_s = sin(counts * _PI/180.0);
+        // the cosine part
+        x1c_dot = x2c;
+        x2c_dot = (-2.0*w_n*x2c) - (w_n*w_n)*x1c + (w_n*w_n)*counts_c;
+
+        position_c = x1c;
+        velocity_c = x2c;
 
-    position = x1;
-    velocity = x2;
+        x1c += x1c_dot*dt;
+        x2c += x2c_dot*dt;
+          //   now the sine portion
+        x1s_dot = x2s;
+        x2s_dot = (-2.0*w_n*x2s) - (w_n*w_n)*x1s + (w_n*w_n)*counts_s;
+
+        position_s = x1s;
+        velocity_s = x2s;
 
-    x1 += x1_dot*dt;
-    x2 += x2_dot*dt;
+        x1s += x1s_dot*dt;
+        x2s += x2s_dot*dt;
+      // now reconstitute the angle and the angular velocity
+        position = atan2(position_s, position_c)  * 180.0/_PI;
+        pnew = atan2(x1s,x1c) * 180/_PI;  // the next value, since the update has happened by now.
+        velocity = (pnew - position) ;  // don't multiply by dt yet, now just an angular difference
+        if(position < 0 ) { position = position +360; }
+        if(velocity  > 180 ) { velocity -= 360;  }
+        if(velocity <= -180)  { velocity += 360 ;}
+        velocity *= (1.0/dt);
+    }
 }
 
 float PosVelFilter::getPosition() {
@@ -29,6 +65,11 @@
 float PosVelFilter::getVelocity() {
     return velocity;
 }
+void PosVelFilter::setHeadingFlag(bool heading_flag) {
+    _headingFlag = heading_flag;
+}
+
+    
 
 float PosVelFilter::getDt() {
     return dt;
@@ -36,4 +77,4 @@
 
 void PosVelFilter::writeWn(float wn) {
     w_n = wn;
-}
\ No newline at end of file
+}