Diff: PosVelFilter.cpp

Revision:

3:8107bb13278b

Parent:

2:992e774dc62a

--- a/PosVelFilter.cpp	Mon Jul 17 14:24:27 2017 +0000
+++ b/PosVelFilter.cpp	Fri Jul 28 18:49:50 2017 +0000
@@ -43,6 +43,34 @@
 
 }
 
+void PosVelFilter::update_la()
+{
+    //run the pos/vel estimate filter
+    //this derives the timing from last run
+    last_time = curr_time;
+    curr_time = systemTime().read();
+    dt = curr_time-last_time;
+
+    // fetch the current distance reading from adc and convert to mm
+    conv_distance = counts_to_dist(adc().ch1_filt);
+    //conv_distance = counts_to_dist(300);  //testing
+
+    x1_dot = x2*1.0;
+    x2_dot = (-2.0*w_n*x2)-(w_n*w_n)*x1+(w_n*w_n)*conv_distance;
+
+    position = x1;
+    velocity = x2;
+    
+    //JUST A TEST (TROY)
+    //position = dt;
+    //velocity = 0;   //velocity needs to be zero, stabilize
+    //conv_distance = 1234;
+    
+
+    x1 += x1_dot*dt;
+    x2 += x2_dot*dt;
+
+}
 void PosVelFilter::init()
 {
     // run filter for 2 seconds on startup
@@ -71,6 +99,33 @@
 
 }
 
+void PosVelFilter::init_la()
+{
+    // run filter for 2 seconds on startup
+    float start = systemTime().read();
+    float time = start;
+    pc().printf("\n\rWarming Up \n\r");
+ 
+    while ((time - start) < 1.0) {
+        update_la();
+
+        pc().printf("%5.3f \r", velocity);
+        time = systemTime().read();
+    }
+    pc().printf("\n\r");
+    while (1) {
+        update();
+
+        if (abs(velocity) > 0.005) {
+            pc().printf("Waiting to stablize. velocity: %5.3f \r", velocity);
+
+        } else {
+            pc().printf("\n\rreading stabilized\n\r");
+            break;
+        }
+    }
+
+}
 
 float PosVelFilter::getPosition()
 {