Diff: updater.cpp

Revision:

2:fbc6e3cf3ed8

Parent:

1:cb84b477886c

Child:

3:42f3821c4e54

--- a/updater.cpp	Tue May 28 13:58:35 2013 +0000
+++ b/updater.cpp	Thu Jun 06 13:40:23 2013 +0000
@@ -1,4 +1,6 @@
 #include "mbed.h"
+#include "util.h"
+#include "globals.h"
 #include "updater.h"
 #include "Config.h"
 #include "Actuators.h"
@@ -25,6 +27,7 @@
 //#define LOG_SKIP 1  // 50ms, 20hz, log entry entered into fifo
 
 // The following is for main loop at 10ms = 100hz
+#define HDG_LAG 100
 #define CTRL_SKIP 5 // 50ms (20hz), control update
 #define MAG_SKIP 2  // 20ms (50hz), magnetometer update
 #define LOG_SKIP 2  // 20ms (50hz), log entry entered into fifo
@@ -98,10 +101,10 @@
     float dt;       // delta time
 } history[MAXHIST]; // fifo for sensor data, position, heading, dt
 
-int hCount;         // history counter; one > 100, we can go back in time to reference gyro history
+int hCount;         // history counter; one > HDG_LAG, we can go back in time to reference gyro history
 int now = 0;        // fifo input index, latest entry
 int prev = 0;       // previous fifo iput index, next to latest entry
-int lag = 0;        // fifo output index, entry from 1 second ago (100 entries prior)
+int lag = 0;        // fifo output index, entry from 1 second ago (HDG_LAG entries prior)
 int lagPrev = 0;    // previous fifo output index, 101 entries prior
 
 /** attach update to Ticker */
@@ -122,7 +125,7 @@
     return;
 }
 
-/** instruct the controller to arm, begin estimation */
+/** instruct the controller to start running */
 void beginRun()
 {
     go = true;
@@ -298,15 +301,16 @@
     history[now].dist = (sensors.lrEncDistance + sensors.rrEncDistance) / 2.0; // current distance traveled
     history[now].gyro = sensors.gyro[_z_];  // current raw gyro data
     history[now].dt = dt; // current dt, to address jitter
-    history[now].hdg = history[prev].hdg + dt*history[now].gyro; // compute current heading from current gyro
-    if (history[now].hdg >= 360.0) history[now].hdg -= 360.0;
-    if (history[now].hdg < 0)      history[now].hdg += 360.0;
+    history[now].hdg = history[prev].hdg + dt*(history[now].gyro - gyroBias); // compute current heading from current gyro
+    clamp(history[now].hdg, 0, 360.0);
+    //if (history[now].hdg >= 360.0) history[now].hdg -= 360.0;
+    //if (history[now].hdg < 0)      history[now].hdg += 360.0;
     float r = PI/180 * history[now].hdg;
     history[now].x = history[prev].x + history[now].dist * sin(r);    // update current position
     history[now].y = history[prev].y + history[now].dist * cos(r);
 
     // We can't do anything until the history buffer is full
-    if (hCount < 100) {
+    if (hCount < HDG_LAG) {
         estLagHeading = history[now].hdg;
         // Until the fifo is full, only keep track of current gyro heading
         hCount++; // after n iterations the fifo will be full
@@ -354,25 +358,33 @@
         // Heading is easy. Original heading - estimated heading gives a tiny error. Add this to all the historical
         // heading data up to present.
         //
-        // For position re-calculation, we iterate 100 times up to present record. Haversine is way, way too slow,
+        // For position re-calculation, we iterate HDG_LAG times up to present record. Haversine is way, way too slow,
         // trig calcs is marginal. Update rate is 10ms and we can't hog more than maybe 2-3ms as the outer
         // loop has logging work to do. Rotating each point is faster; pre-calculate sin/cos, etc. for rotation
         // matrix.
-        //
-        // initialize these once
-        errAngle = (estLagHeading - history[lag].hdg);
-        if (errAngle <= -180.0) errAngle += 360.0;
-        if (errAngle > 180) errAngle -= 360.0;
+
+        // Low pass filter the error correction.  Multiplying by 0.01, it takes HDG_LAG updates to correct a 
+        // consistent error; that's 0.10s/0.01 = 1 sec.  0.005 is 2 sec, 0.0025 is 4 sec, etc.
+        errAngle = (estLagHeading - history[lag].hdg); 
+        // low pass filter error angle:
+        clamp(errAngle, -180.0, 180.0);
+        //if (errAngle > 180.0) errAngle -= 360.0;
+        //if (errAngle <= -180.0) errAngle += 360.0;
+        errAngle *= 0.01; // multiply only after clamping to -180:180 range.
 
         //fprintf(stdout, "%d %.2f, %.2f, %.4f %.4f\n", lag, estLagHeading, history[lag].hdg, estLagHeading - history[lag].hdg, errAngle);
         float cosA = cos(errAngle * PI / 180.0);
         float sinA = sin(errAngle * PI / 180.0);
         // Update position & heading from history[lag] through history[now]
         int i = lag;
-        for (int j=0; j < 100; j++) {
+        // TODO 2 parameterize heading lag -- for uBlox it seems to be ~ 600ms, for Venus, about 1000ms
+        for (int j=0; j < HDG_LAG; j++) {
             //fprintf(stdout, "i=%d n=%d l=%d\n", i, now, lag);
             // Correct gyro-calculated headings from past to present including history[lag].hdg
             history[i].hdg += errAngle;
+            clamp(history[i].hdg, 0, 360.0);
+            //if (history[i].hdg >= 360.0) history[i].hdg -= 360.0;
+            //if (history[i].hdg < 0) history[i].hdg += 360.0;
             // Rotate x, y by errAngle around pivot point; no need to rotate pivot point (j=0)
             if (j > 0) {
                 float dx = history[i].x-history[lag].x;
@@ -399,9 +411,9 @@
     float prevDistance = cartHere.distanceTo(config.cwpt[lastWaypoint]);
     double relativeBrg = bearing - history[now].hdg;
     // if correction angle is < -180, express as negative degree
-    // TODO 3 turn this into a function
-    if (relativeBrg < -180.0) relativeBrg += 360.0;
-    if (relativeBrg > 180.0)  relativeBrg -= 360.0;
+    clamp(relativeBrg, -180.0, 180.0);
+    //if (relativeBrg < -180.0) relativeBrg += 360.0;
+    //if (relativeBrg > 180.0)  relativeBrg -= 360.0;
 
     // if within config.waypointDist distance threshold move to next waypoint
     // TODO 3 figure out how to output feedback on wpt arrival external to this function
@@ -410,10 +422,10 @@
         // if we're within brakeDist of next or previous waypoint, run @ turn speed
         // This would normally mean we run at turn speed until we're brakeDist away
         // from waypoint 0, but we trick the algorithm by initializing prevWaypoint to waypoint 1
-        if (distance < config.brakeDist || prevDistance < config.brakeDist) {
+        if ( (thisTime - timeZero) < 3000 ) {
+            setSpeed( config.startSpeed );
+        } else if (distance < config.brakeDist || prevDistance < config.brakeDist) {
             setSpeed( config.turnSpeed );
-        } else if ( (thisTime - timeZero) < 1000 ) {
-            setSpeed( config.startSpeed );
         } else {
             setSpeed( config.topSpeed );
         }
@@ -568,7 +580,7 @@
     state[inState].bearing = bearing;
     state[inState].distance = distance;
     state[inState].nextWaypoint = nextWaypoint;
-    //state[inState].gbias = gyroBias;
+    state[inState].gbias = gyroBias;
     state[inState].errAngle = biasErrAngle;
     //state[inState].leftRanger = sensors.leftRanger;
     //state[inState].rightRanger = sensors.rightRanger;