Revision 1:cb84b477886c, committed 2013-05-28

Comitter:

shimniok

Date:

Tue May 28 13:58:35 2013 +0000

Parent:

0:a6a169de725f

Child:

2:fbc6e3cf3ed8

Commit message:

Changed initial next/prev waypoint to permit steering calc. Removed unnecessary code, added logging for steering angle to diagnose bug.

Changed in this revision

--- a/Actuators/Steering/Steering.cpp	Mon May 27 13:26:03 2013 +0000
+++ b/Actuators/Steering/Steering.cpp	Tue May 28 13:58:35 2013 +0000
@@ -1,6 +1,8 @@
 #include "Steering.h"
 #include "math.h"
 
+#include "mbed.h"
+
 /** create a new steering calculator for a particular vehicle
  *
  */
@@ -71,6 +73,7 @@
  */
 float Steering::crossTrack(float Bx, float By, float Ax, float Ay, float Cx, float Cy)
 {
+/*
     // Compute rise for prev wpt to bot; or compute vector offset by A(x,y)
     float Rx = (Bx - Ax);
     // compute run for prev wpt to bot; or compute vector offset by A(x,y)
@@ -93,8 +96,9 @@
     float NBx = Nx-Bx;
     float NBy = Ny-By;
     float cte = sqrtf(NBx*NBx + NBy*NBy);
-
     return cte;
+*/
+    return 0;
 }
 
 
@@ -106,7 +110,7 @@
     float Ly = Cy - Ay;
     // Robot vector
     float Rx = Bx - Ax;
-    float Ry = Cy - Ay;
+    float Ry = By - Ay;
 
     // Find the goal point, a projection of the bot vector onto the current leg, moved ahead
     // along the path by the lookahead distance
@@ -116,21 +120,19 @@
     float LAy = (proj+_intercept)*Ly/legLength + Ay;
     // Compute a circle that is tangential to bot heading and intercepts bot
     // and goal point (LAx,LAy), the intercept circle. Then compute the steering
-    // angle to trace that circle.
+    // angle to trace that circle. (x,y because 0 deg points up not right)
     float brg = 180*atan2(LAx-Rx,LAy-Ry)/PI;
     // would be nice to add in some noise to heading info
     float relBrg = brg-hdg;
-    if (relBrg < -180.0)
-        relBrg += 360.0;
-    if (relBrg >= 180.0)
-        relBrg -= 360.0;
+    if (relBrg < -180.0) relBrg += 360.0;
+    if (relBrg >= 180.0) relBrg -= 360.0;
     // I haven't had time to work out why the equation is asymmetrical, that is, 
     // negative angle produces slightly less steering angle. So instead, just use
     // absolute value and restore sign later.
     float sign = (relBrg < 0) ? -1 : 1;
     // The equation peaks out at 90* so clamp theta artifically to 90, so that
     // if theta is actually > 90, we select max steering
-    if (relBrg > 90.0) relBrg = 90.0;
+    if (fabs(relBrg) > 90.0) relBrg = 90.0;
     // Compute radius based on intercept distance and specified angle
     float radius = _intercept/(2*sin(fabs(relBrg)*PI/180));
     // optionally, limit radius min/max
@@ -138,6 +140,8 @@
     // Steering angle is based on wheelbase and track width
     SA = sign * angle_degrees(asin(_wheelbase / (radius - _track/2)));
 
+    //fprintf(stdout, "R(%.4f,%.4f) A(%.4f,%.4f) C(%.4f,%.4f) L(%.4f,%.4f) b=%.2f rb=%.2f sa=%.2f\n", Bx, By, Ax, Ay, Cx, Cy, Lx, Ly, brg, relBrg, SA);
+        
     return SA;
 }
 

--- a/Config/Config.cpp	Mon May 27 13:26:03 2013 +0000
+++ b/Config/Config.cpp	Tue May 28 13:58:35 2013 +0000
@@ -101,10 +101,6 @@
                     steerGain = cvstof(tmp);            // steering angle multiplier
                     p = split(tmp, p, MAXBUF, ',');
                     steerGainAngle = cvstof(tmp);       // angle below which steering gain takes effect
-                    p = split(tmp, p, MAXBUF, ',');
-                    cteKi = cvstof(tmp);                // integral term for cross track
-                    p = split(tmp, p, MAXBUF, ',');
-                    usePP = ( atoi(tmp) == 1 );         // use pure pursuit algorithm
                     break;
                 case 'S' :                              // Throttle configuration
                     p = split(tmp, p, MAXBUF, ',');

--- a/Config/Config.h	Mon May 27 13:26:03 2013 +0000
+++ b/Config/Config.h	Tue May 28 13:58:35 2013 +0000
@@ -34,8 +34,6 @@
         float steerZero;        // zero steering aka center point
         float steerGain;        // gain factor for steering algorithm
         float steerGainAngle;   // angle below which steering gain takes effect
-        float cteKi;            // cross track error integral gain
-        bool usePP;             // use pure pursuit algorithm? 
         float curbThreshold;    // distance at which curb avoid takes place
         float curbGain;         // gain of curb avoid steering
         // acceleration profile?

--- a/SystemState.h	Mon May 27 13:26:03 2013 +0000
+++ b/SystemState.h	Tue May 28 13:58:35 2013 +0000
@@ -1,7 +1,7 @@
 #ifndef _SYSTEMSTATE_H
 #define _SYSTEMSTATE_H
 
-#define SSBUF 0x2F
+#define SSBUF 0x3F   // 0b0000 0011 1111 1111
 
 /** System State is the main mechanism for communicating current realtime system state to
  * the rest of the system for logging, data display, etc.
@@ -36,7 +36,8 @@
  * rrEncSpeed           right rear encoder speed
  * encHeading           estimated heading based on encoder readings
  ****** Estimated Position and Heading
- * estHeading           estimated heading in degrees, this is the robot's reference for heading
+ * estLagHeading        estimated heading in degrees, lagged to sync with gps
+ * estHeading           estimated current heading
  * estLatitude          estimated latitude in fractional degrees (e.g., 39.123456)
  * estLongitude         estimated longitude in fractional degrees (e.g., -104.123456)
  * estNorthing          some algorithms use UTM.  Estimated UTM northing
@@ -66,18 +67,11 @@
     float gpsSpeed_mps;
     float gpsHDOP;
     int gpsSats;
-
-    double gpsLatitude2;
-    double gpsLongitude2;
-    float gpsCourse_deg2;
-    float gpsSpeed_mps2;
-    float gpsHDOP2;
-    int gpsSats2;
-
     float lrEncDistance, rrEncDistance;
     float lrEncSpeed, rrEncSpeed;
     float encHeading;
     float estHeading;
+    float estLagHeading;
     double estLatitude, estLongitude;
     //double estNorthing, estEasting;
     float estX, estY;
@@ -86,10 +80,7 @@
     float distance;
     float gbias;
     float errAngle;
-    float leftRanger;
-    float rightRanger;
-    float centerRanger;
-    float crossTrackErr;
+    float steerAngle;
 } SystemState;
 
 #endif
\ No newline at end of file

--- a/UI/Display/Display.cpp	Mon May 27 13:26:03 2013 +0000
+++ b/UI/Display/Display.cpp	Tue May 28 13:58:35 2013 +0000
@@ -71,14 +71,14 @@
     lcd.circle(90, 40, 22, true);
     lcd.circle(90, 40, 14, true);
     lcd.posXY(90-9,40-(8/2)); // 3 * 6 / 2, char width=6, 5 chars, half that size
-    lcd.printf("%03.0f", state.gHeading);
-    int nx = 90 - 18 * sin(3.141529 * state.gHeading / 180);
-    int ny = 40 - 18 * cos(-3.141529 * state.gHeading / 180);
+    lcd.printf("%03.0f", state.estHeading);
+    int nx = 90 - 18 * sin(3.141529 * state.estHeading / 180.0);
+    int ny = 40 - 18 * cos(-3.141529 * state.estHeading / 180.0);
     lcd.posXY(nx - 2, ny - 3);
     lcd.printf("N");
     //lcd.circle(nx, ny, 5, true);
-    int bx = 90 - 18 * sin(-3.141529 * (state.bearing-state.gHeading) / 180);
-    int by = 40 - 18 * cos(-3.141529 * (state.bearing-state.gHeading) / 180);
+    int bx = 90 - 18 * sin(-3.141529 * (state.bearing-state.estHeading) / 180.0);
+    int by = 40 - 18 * cos(-3.141529 * (state.bearing-state.estHeading) / 180.0);
     lcd.circle(bx, by, 2, true);
     
     /*

--- a/Watchdog/Watchdog.cpp	Mon May 27 13:26:03 2013 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,46 +0,0 @@
-/// @file Watchdog.cpp provides the interface to the Watchdog module
-///
-/// This provides basic Watchdog service for the mbed. You can configure
-/// various timeout intervals that meet your system needs. Additionally,
-/// it is possible to identify if the Watchdog was the cause of any 
-/// system restart.
-/// 
-/// Adapted from Simon's Watchdog code from http://mbed.org/forum/mbed/topic/508/
-///
-/// @note Copyright © 2011 by Smartware Computing, all rights reserved.
-///     This software may be used to derive new software, as long as
-///     this copyright statement remains in the source file.
-/// @author David Smart
-///
-#include "mbed.h"
-#include "Watchdog.h"
-
-
-
-/// Watchdog gets instantiated at the module level
-Watchdog::Watchdog() {
-    wdreset = (LPC_WDT->WDMOD >> 2) & 1;    // capture the cause of the previous reset
-}
-
-/// Load timeout value in watchdog timer and enable
-void Watchdog::Configure(float s) {
-    LPC_WDT->WDCLKSEL = 0x1;                // Set CLK src to PCLK
-    uint32_t clk = SystemCoreClock / 16;    // WD has a fixed /4 prescaler, PCLK default is /4
-    LPC_WDT->WDTC = (uint32_t)(s * (float)clk);
-    LPC_WDT->WDMOD = 0x3;                   // Enabled and Reset
-    Service();
-}
-
-/// "Service", "kick" or "feed" the dog - reset the watchdog timer
-/// by writing this required bit pattern
-void Watchdog::Service() {
-    LPC_WDT->WDFEED = 0xAA;
-    LPC_WDT->WDFEED = 0x55;
-}
-
-/// get the flag to indicate if the watchdog causes the reset
-bool Watchdog::WatchdogCausedReset() {
-    return wdreset;
-}
-
-

--- a/Watchdog/Watchdog.h	Mon May 27 13:26:03 2013 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,101 +0,0 @@
-/// @file Watchdog.h provides the interface to the Watchdog module
-///
-/// This provides basic Watchdog service for the mbed. You can configure
-/// various timeout intervals that meet your system needs. Additionally,
-/// it is possible to identify if the Watchdog was the cause of any 
-/// system restart, permitting the application code to take appropriate
-/// behavior.
-/// 
-/// Adapted from Simon's Watchdog code from http://mbed.org/forum/mbed/topic/508/
-///
-/// @note Copyright © 2011 by Smartware Computing, all rights reserved.
-///     This software may be used to derive new software, as long as
-///     this copyright statement remains in the source file.
-/// @author David Smart
-///
-/// History
-/// \li v1.00 - 20110616: initial release with some documentation improvements
-///
-#ifndef WATCHDOG_H
-#define WATCHDOG_H
-#include "mbed.h"
-
-/// The Watchdog class provides the interface to the Watchdog feature
-///
-/// Embedded programs, by their nature, are usually unattended. If things
-/// go wrong, it is usually important that the system attempts to recover.
-/// Aside from robust software, a hardware watchdog can monitor the
-/// system and initiate a system reset when appropriate.
-///
-/// This Watchdog is patterned after one found elsewhere on the mbed site,
-/// however this one also provides a method for the application software
-/// to determine the cause of the reset - watchdog or otherwise.
-///
-/// example:
-/// @code
-/// Watchdog wd;
-///
-/// ...
-/// main() {
-///    if (wd.WatchdogCausedReset())
-///        pc.printf("Watchdog caused reset.\r\n");
-///      
-///    wd.Configure(3.0);       // sets the timeout interval
-///    for (;;) {
-///         wd.Service();       // kick the dog before the timeout
-///         // do other work
-///    }
-/// }
-/// @endcode
-///
-class Watchdog {
-public:
-    /// Create a Watchdog object
-    ///
-    /// example:
-    /// @code
-    /// Watchdog wd;    // placed before main
-    /// @endcode
-    Watchdog();
-    
-    /// Configure the timeout for the Watchdog
-    ///
-    /// This configures the Watchdog service and starts it. It must
-    /// be serviced before the timeout, or the system will be restarted.
-    ///
-    /// example:
-    /// @code
-    ///     ...
-    ///     wd.Configure(1.4);  // configure for a 1.4 second timeout
-    ///     ...
-    /// @endcode
-    ///
-    /// @param timeout in seconds, as a floating point number
-    /// @returns none
-    ///
-    void Configure(float timeout);
-    
-    /// Service the Watchdog so it does not cause a system reset
-    ///
-    /// example:
-    /// @code
-    ///    wd.Service();
-    /// @endcode
-    /// @returns none
-    void Service();
-    
-    /// WatchdogCausedReset identifies if the cause of the system
-    /// reset was the Watchdog
-    ///
-    /// example:
-    /// @code
-    ///    if (wd.WatchdogCausedReset())) {
-    /// @endcode
-    ///
-    /// @returns true if the Watchdog was the cause of the reset
-    bool WatchdogCausedReset();
-private:
-    bool wdreset;
-};
-
-#endif // WATCHDOG_H
\ No newline at end of file

--- a/logging/logging.cpp	Mon May 27 13:26:03 2013 +0000
+++ b/logging/logging.cpp	Tue May 28 13:58:35 2013 +0000
@@ -20,7 +20,7 @@
     s->gHeading = s->cHeading = 0.0;
     //s->roll = s->pitch = s->yaw =0.0;
     s->gpsLatitude = s->gpsLongitude = s->gpsCourse_deg = s->gpsSpeed_mps = s->gpsHDOP = 0.0;
-    s->gpsLatitude2 = s->gpsLongitude2 = s->gpsCourse_deg2 = s->gpsSpeed_mps2 = s->gpsHDOP2 = 0.0;
+    //s->gpsLatitude2 = s->gpsLongitude2 = s->gpsCourse_deg2 = s->gpsSpeed_mps2 = s->gpsHDOP2 = 0.0;
     s->lrEncDistance = s->rrEncDistance = 0.0;
     s->lrEncSpeed = s->rrEncSpeed = s->encHeading = 0.0;
     s->estHeading = s->estLatitude = s->estLongitude = 0.0;
@@ -176,23 +176,7 @@
     fputc(',',logp);
     printInt(logp, s.gpsSats);
     fputc(',',logp);
-
-    // GPS 2
-    fprintf(logp, "%.7f,%.7f,", s.gpsLatitude2, s.gpsLongitude2);
-    //printFloat(logp, s.gpsLatitude2, 7);
-    //fputc(',',logp);
-    //printFloat(logp, s.gpsLongitude2, 7);
-    //fputc(',',logp);
-    printFloat(logp, s.gpsCourse_deg2, 2);
-    fputc(',',logp);
-    printFloat(logp, s.gpsSpeed_mps2, 2);
-    fputc(',',logp);
-    printFloat(logp, s.gpsHDOP2, 1);
-    fputc(',',logp);
-    printInt(logp, s.gpsSats2);
-    fputc(',',logp);
-
-
+    // Encoders
     printFloat(logp, s.lrEncDistance, 7);
     fputc(',',logp);
     printFloat(logp, s.rrEncDistance, 7);
@@ -203,8 +187,11 @@
     fputc(',',logp);
     printFloat(logp, s.encHeading, 2);
     fputc(',',logp);
+    // Estimates
     printFloat(logp, s.estHeading, 2);
     fputc(',',logp);
+    printFloat(logp, s.estLagHeading, 2);
+    fputc(',',logp);
     printFloat(logp, s.estLatitude,  7);
     fputc(',',logp);
     printFloat(logp, s.estLongitude, 7);
@@ -213,25 +200,15 @@
     fputc(',',logp);
     printFloat(logp, s.estY, 4);
     fputc(',',logp);
+    // Nav
     printInt(logp, s.nextWaypoint);
     fputc(',',logp);
     printFloat(logp, s.bearing, 2);
     fputc(',',logp);
     printFloat(logp, s.distance, 3);
     fputc(',',logp);
-/*
-    printFloat(logp, s.gbias, 3);
-    fputc(',',logp);
-    printFloat(logp, s.errAngle, 5);
-    fputc(',',logp);
-    printFloat(logp, s.leftRanger, 3);
+    printFloat(logp, s.steerAngle, 3);
     fputc(',',logp);
-    printFloat(logp, s.rightRanger, 3);
-    fputc(',',logp);
-    printFloat(logp, s.centerRanger, 3);
-    fputc(',',logp);
-    printFloat(logp, s.crossTrackErr, 3);
-*/
     fputc('\n',logp);
 
     t2 = logtimer.read_us();

--- a/main.cpp	Mon May 27 13:26:03 2013 +0000
+++ b/main.cpp	Tue May 28 13:58:35 2013 +0000
@@ -1,4 +1,4 @@
-/** Code for "Data Bus" UGV entry for Sparkfun AVC 2012
+/** Code for "Data Bus" UGV entry for Sparkfun AVC 2013
  *  http://www.bot-thoughts.com/
  */
 
@@ -188,12 +188,12 @@
 
     // Send data back to the PC
     pc.baud(115200);
-    fprintf(stdout, "Data Bus mAGV Control System\n");
+    fprintf(stdout, "Data Bus 2013\n");
 
     display.init();
-    display.status("Data Bus mAGV V3");
+    display.status("Data Bus 2013");
    
-    fprintf(stdout, "Initialization...\n");
+    fprintf(stdout, "Initializing...\n");
     display.status("Initializing");
     wait(0.2);
     
@@ -210,18 +210,21 @@
     wait(0.2);
     if (config.load())                          // Load various configurable parameters, e.g., waypoints, declination, etc.
         confStatus = 1;
-        
+
     // Something here is jacking up the I2C stuff
+    // Also when initializing with ESC powered, it causes motor to run which
+    // totally jacks up everything (noise?)
     initSteering();
     initThrottle();
     // initFlasher();                                   // Initialize autonomous mode flasher
         
     sensors.Compass_Calibrate(config.magOffset, config.magScale);
-    pc.printf("Declination: %.1f\n", config.declination);
+    //pc.printf("Declination: %.1f\n", config.declination);
     pc.printf("Speed: escZero=%d escMax=%d top=%.1f turn=%.1f Kp=%.4f Ki=%.4f Kd=%.4f\n", 
         config.escZero, config.escMax, config.topSpeed, config.turnSpeed, 
         config.speedKp, config.speedKi, config.speedKd);
-    pc.printf("Steering: steerZero=%0.2f steerGain=%.1f gainAngle=%.1f\n", config.steerZero, config.steerGain, config.steerGainAngle);
+    pc.printf("Steering: steerZero=%0.2f steerGain=%.1f gainAngle=%.1f\n",
+        config.steerZero, config.steerGain, config.steerGainAngle);
 
     // Convert lat/lon waypoints to cartesian
     mapper.init(config.wptCount, config.wpt);
@@ -231,9 +234,9 @@
                     w, config.cwpt[w]._x, config.cwpt[w]._y, config.wpt[w].latitude(), config.wpt[w].longitude());
     }
 
-    // TODO: 3 print mag and gyro calibrations
+    // TODO 3 print mag and gyro calibrations
 
-    // TODO 3: remove GPS configuration, all config will be in object itself I think
+    // TODO 3 remove GPS configuration, all config will be in object itself I think
 
     display.status("Nav configuration   ");
     steerCalc.setIntercept(config.interceptDist);               // Setup steering calculator based on intercept distance
@@ -245,8 +248,7 @@
     fprintf(stdout, "Calculating offsets...\n");
     display.status("Offset calculation  ");
     wait(0.2);
-    // TODO: 3 Really need to give the gyro more time to settle
-    // 5/20 - for some reason it is hanging here
+    // TODO 3 Really need to give the gyro more time to settle
     sensors.gps.disable();
     sensors.Calculate_Offsets();
 
@@ -303,20 +305,20 @@
         }*/
 
         if ((thisUpdate = timer.read_ms()) > nextUpdate) {
-            // TODO 1: wtf is this?!?!  why not pull out of state[outstate]
+            // Pulling out current state so we get the most current
             SystemState s = state[inState];
+            // Now populate in the current GPS data
             s.gpsHDOP = sensors.gps.hdop();
             s.gpsSats = sensors.gps.sat_count();
             display.update(s);
             nextUpdate = thisUpdate + 2000;
+            // TODO move this statistic into display class
             fprintf(stdout, "update time: %d\n", getUpdateTime());
         }
         
         if (keypad.pressed) {
             keypad.pressed = false;
             printLCDMenu = true;
-            //causes issue with servo library
-            //speaker.beep(3000.0, 0.1); // non-blocking
             switch (keypad.which) {
                 case NEXT_BUTTON:
                     menu.next();
@@ -338,7 +340,6 @@
 
             
         if (printLCDMenu) {
-        
             display.menu( menu.getItemName() );
             display.status("Ready.");
             display.redraw();
@@ -506,13 +507,9 @@
     timer.start();
     wait(0.1);
     
-    // Initialize logging buffer
-    // Needs to happen after we've reset the millisecond timer and after
-    // the schedHandler() fires off at least once more with the new time
-    inState = outState = 0;    
-    
     // Tell the navigation / position estimation stuff to reset to starting waypoint
-    restartNav();
+    // Disable 05/27/2013 to try and fix initial heading estimate
+    //restartNav();
                 
     // Main loop
     //
@@ -529,14 +526,6 @@
         //
         // set throttle only if goGoGo set
         if (goGoGo) {
-            /** acceleration curve */
-            /*
-            if (go.ticked(timer.read_ms())) {
-                throttle = go.get() / 1000.0;
-                //fprintf(stdout, "throttle: %0.3f\n", throttle.read());
-            }
-            */
-            
             // TODO: 1 Add additional condition of travel for N meters before
             // the HALT button is armed
             
@@ -554,23 +543,15 @@
                 display.status("GO GO GO!");
                 goGoGo = true;
                 keypad.pressed = false;
-                //restartNav();
                 beginRun();
-                // Doing this for collecting step response, hopefully an S curve... we'll see.
-                //throttle = config.escMax;
-                // TODO: 2 Improve encapsulation of the scheduler
-                // TODO: 2 can we do something clever with GPS position estimate since we know where we're starting?
-                // E.g. if dist to wpt0 < x then initialize to wpt0 else use gps
             }
         }        
 
         // Are we at the last waypoint?
         // 
         if (state[inState].nextWaypoint == config.wptCount) {
-            fprintf(stdout, "Arrived at final destination. Done.\n");
-            //causes issue with servo library
-            //speaker.beep(3000.0, 1.0); // non-blocking
-            display.status("Arrived. Done.");
+            fprintf(stdout, "Arrived at final destination.\n");
+            display.status("Arrived at end.");
             navDone = true;
             endRun();
         }
@@ -579,14 +560,15 @@
         // LOGGING
         //////////////////////////////////////////////////////////////////////////////
         // sensor reads are happening in the schedHandler();
-        // Are there more items to come out of the log buffer?
+        // Are there more items to come out of the log fifo?
         // Since this could take anywhere from a few hundred usec to
         // 150ms, we run it opportunistically and use a buffer. That way
         // the sensor updates, calculation, and control can continue to happen
         if (outState != inState) {
             logStatus = !logStatus;         // log indicator LED
             //fprintf(stdout, "FIFO: in=%d out=%d\n", inState, outState);
-            if (ssBufOverrun) {
+ 
+            if (ssBufOverrun) { // set in update()
                 fprintf(stdout, ">>> SystemState Buffer Overrun Condition\n");
                 ssBufOverrun = false;
             }
@@ -603,13 +585,11 @@
         }
 
     } // while
-    
     closeLogfile();
+    wait(2.0);
     logStatus = 0;
-    fprintf(stdout, "Completed, file saved.\n");
-    wait(2); // wait from last printout
-    display.status("Done. Saved.");
-    wait(2);        
+    display.status("Completed. Saved.");
+    wait(2.0);
 
     ahrsStatus = 0;
     gpsStatus = 0;
@@ -942,13 +922,13 @@
                 fprintf(stdout, "Rangers: L=%.2f R=%.2f C=%.2f", sensors.leftRanger, sensors.rightRanger, sensors.centerRanger);
                 fprintf(stdout, "\n");
                 //fprintf(stdout, "ahrs.MAG_Heading=%4.1f\n",  ahrs.MAG_Heading*180/PI);
-                fprintf(stdout, "raw m=(%d, %d, %d)\n", sensors.m[0], sensors.m[1], sensors.m[2]);
-                fprintf(stdout, "m=(%2.3f, %2.3f, %2.3f) %2.3f\n", sensors.mag[0], sensors.mag[1], sensors.mag[2],
-                        sqrt(sensors.mag[0]*sensors.mag[0] + sensors.mag[1]*sensors.mag[1] + sensors.mag[2]*sensors.mag[2] ));
+                //fprintf(stdout, "raw m=(%d, %d, %d)\n", sensors.m[0], sensors.m[1], sensors.m[2]);
+                //fprintf(stdout, "m=(%2.3f, %2.3f, %2.3f) %2.3f\n", sensors.mag[0], sensors.mag[1], sensors.mag[2],
+                //        sqrt(sensors.mag[0]*sensors.mag[0] + sensors.mag[1]*sensors.mag[1] + sensors.mag[2]*sensors.mag[2] ));
                 fprintf(stdout, "g=(%4d, %4d, %4d) %d\n", sensors.g[0], sensors.g[1], sensors.g[2], sensors.gTemp);
                 fprintf(stdout, "gc=(%.1f, %.1f, %.1f)\n", sensors.gyro[0], sensors.gyro[1], sensors.gyro[2]);
                 fprintf(stdout, "a=(%5d, %5d, %5d)\n", sensors.a[0], sensors.a[1], sensors.a[2]);
-                fprintf(stdout, "estHdg=%.2f\n", state[inState].estHeading);
+                fprintf(stdout, "estHdg=%.2f lagHdg=%.2f\n", state[inState].estHeading, state[inState].estLagHeading);
                 //fprintf(stdout, "roll=%.2f pitch=%.2f yaw=%.2f\n", ToDeg(ahrs.roll), ToDeg(ahrs.pitch), ToDeg(ahrs.yaw));
                 fprintf(stdout, "speed: left=%.3f  right=%.3f\n", sensors.lrEncSpeed, sensors.rrEncSpeed);
                 fprintf(stdout, "gps=(%.6f, %.6f, %.1f, %.1f, %.1f, %d) %02x\n", 

--- a/updater.cpp	Mon May 27 13:26:03 2013 +0000
+++ b/updater.cpp	Tue May 28 13:58:35 2013 +0000
@@ -5,7 +5,7 @@
 #include "Sensors.h"
 #include "SystemState.h"
 #include "Venus638flpx.h"
-#include "Ublox6.h"
+//#include "Ublox6.h"
 #include "Steering.h"
 #include "Servo.h"
 #include "Mapping.h"
@@ -34,7 +34,7 @@
 
 int control_count=CTRL_SKIP;
 int update_count=MAG_SKIP;              // call Update_mag() every update_count calls to schedHandler()
-int log_count=LOG_SKIP;                 // buffer a new status entry for logging every log_count calls to schedHandler
+int log_count=0;                        // buffer a new status entry for logging every log_count calls to schedHandler
 int tReal;                              // calculate real elapsed time
 int bufCount=0;
 
@@ -58,8 +58,6 @@
 double bearing;                         // bearing to next waypoint
 double distance;                        // distance to next waypoint
 float steerAngle;                       // steering angle
-float cte;                              // Cross Track error
-float cteI;                             // Integral of Cross Track Error
 
 // Throttle PID
 float speedDt=0;                        // dt for the speed PID
@@ -69,19 +67,17 @@
 float desiredSpeed;                     // speed set point
 float nowSpeed;
 
-
 // Pose Estimation
-bool initNav = true;
-float initHdg = true;                   // indiciates that heading needs to be initialized by gps
+bool initNav = true;                    // initialize navigation estimates
+bool doLog = false;                     // determines when to start and stop entering log data
 float initialHeading=-999;              // initial heading
-float myHeading=0;                      // heading sent to KF
 CartPosition cartHere;                  // position estimate, cartesian
 GeoPosition here;                       // position estimate, lat/lon
 int timeZero=0;
 int lastTime=-1;                        // used to calculate dt for KF
 int thisTime;                           // used to calculate dt for KF
 float dt;                               // dt for the KF
-float lagHeading = 0;                   // lagged heading estimate
+float estLagHeading = 0;                   // lagged heading estimate
 //GeoPosition lagHere;                  // lagged position estimate; use here as the current position estimate
 float errAngle;                         // error between gyro hdg estimate and gps hdg estimate
 float gyroBias=0;                       // exponentially weighted moving average of gyro error
@@ -92,47 +88,51 @@
 
 #define MAXHIST 128 // must be multiple of 0x08
 #define inc(x)  (x) = ((x)+1)&(MAXHIST-1)
-struct {
+struct history_rec {
     float x;        // x coordinate
     float y;        // y coordinate
     float hdg;      // heading
     float dist;     // distance
     float gyro;     // heading rate
-    float ghdg;     // uncorrected gyro heading
+    //float ghdg;   // uncorrected gyro heading
     float dt;       // delta time
 } history[MAXHIST]; // fifo for sensor data, position, heading, dt
 
-int hCount=0;       // history counter; one > 100, we can go back in time to reference gyro history
-int now = 0;        // fifo input index
-int prev = 0;      // previous fifo iput index
-int lag = 0;        // fifo output index
-int lagPrev = 0;    // previous fifo output index
+int hCount;         // history counter; one > 100, 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 lagPrev = 0;    // previous fifo output index, 101 entries prior
 
 /** attach update to Ticker */
 void startUpdater()
 {
+    // Initialize logging buffer
+    // Needs to happen after we've reset the millisecond timer and after
+    // the schedHandler() fires off at least once more with the new time
     sched.attach(&update, UPDATE_PERIOD);
 }
 
-/** set flag to initialize navigation at next schedHandler() call
- */
+/** set flag to initialize navigation at next schedHandler() call */
 void restartNav()
 {
+    go = false;
+    inState = outState = 0;    
     initNav = true;
-    initHdg = true;
     return;
 }
 
-/** instruct the controller to throttle up
- */
+/** instruct the controller to arm, begin estimation */
 void beginRun()
 {
     go = true;
+    inState = outState = 0;    
     timeZero = thisTime; // initialize 
     bufCount = 0;
     return;
 }
 
+/** instruct the controller that we're done with the run */
 void endRun()
 {
     go = false;
@@ -140,36 +140,37 @@
     return;
 }
 
-/** get elasped time in update loop
- */
+/** get elasped time in update loop */
 int getUpdateTime()
 {
     return tReal;
 }
 
+/** Set the desired speed for the controller to attain */
 void setSpeed(float speed) 
 {
     if (desiredSpeed != speed) {
         desiredSpeed = speed;
-        integral = 0;
+        integral = 0; // reset the error integral
     }
     return;
 }
 
-/** schedHandler fires off the various routines at appropriate schedules
- *
- */
+/** update() runs the data collection, estimation, steering control, and throttle control */
 void update()
 {
     tReal = timer.read_us();
     bool useGps=false;
 
+    // TODO 1 do we need to set up the dt stuff after initialization?
+
     ahrsStatus = 0;
     thisTime = timer.read_ms();
     dt = (lastTime < 0) ? 0 : ((float) thisTime - (float) lastTime) / 1000.0; // first pass let dt=0
     lastTime = thisTime;
 
     // Add up dt to speedDt
+    // We're adding up distance over several update() calls so have to keep track of total time
     speedDt += dt;
     
     // Log Data Timestamp    
@@ -186,28 +187,27 @@
         initNav = false;
         here.set(config.wpt[0]);
         nextWaypoint = 1; // Point to the next waypoint; 0th wpt is the starting point
-        lastWaypoint = 1; // Init to waypoint 1, we we don't start from wpt 0 at the turning speed
+        lastWaypoint = 0;
+        
+        // TODO 1 need to start off, briefly, at a slow speed
+        
         // Initialize lag estimates
         //lagHere.set( here );
-        // Initialize fifo
-        hCount = 0;
-        now = 0;
+        hCount = 2; // lag entry and first now entry are two entries
+        now = 0; 
         // initialize what will become lag data in 1 second from now
         history[now].dt = 0;
+        history[now].dist = 0;
         // initial position is waypoint 0
         history[now].x = config.cwpt[0]._x;
         history[now].y = config.cwpt[0]._y;
         cartHere.set(history[now].x, history[now].y);
         // initialize heading to bearing between waypoint 0 and 1
         //history[now].hdg = here.bearingTo(config.wpt[nextWaypoint]);
-        history[now].hdg = cartHere.bearingTo(config.cwpt[nextWaypoint]);
-        history[now].ghdg = history[now].hdg;
-        initialHeading = history[now].hdg;
+        initialHeading = history[now].hdg = cartHere.bearingTo(config.cwpt[nextWaypoint]);
+        //history[now].ghdg = history[now].hdg;
         // Initialize Kalman Filter
-        headingKalmanInit(history[now].hdg);
-        // initialize cross track error
-        cte = 0;
-        cteI = 0;
+        headingKalmanInit(initialHeading);
         // point next fifo input to slot 1, slot 0 occupied/initialized, now
         lag = 0;
         lagPrev = 0;
@@ -220,7 +220,7 @@
     // SENSOR UPDATES
     //////////////////////////////////////////////////////////////////////////////
 
-    // TODO: 3 This really should run infrequently
+    // TODO 3 This really should run infrequently
     sensors.Read_Power();
 
     sensors.Read_Encoders(); 
@@ -266,6 +266,8 @@
                   (thisTime-timeZero) > 3000); // gps hdg converges by 3-5 sec.                
     }
 
+    DigitalOut useGpsStat(LED1);
+    useGpsStat = useGps;
     
     //////////////////////////////////////////////////////////////////////////////
     // HEADING AND POSITION UPDATE
@@ -292,35 +294,27 @@
     // We'll find out how much error there is between gps heading and the integrated
     // gyro heading from a second ago.
 
-    // stick precalculated gyro data, with bias correction, into fifo
-    //history[now].gyro = sensors.gyro[_z_] - gyroBias;   
-    history[now].gyro = sensors.gyro[_z_];   
-
-    // Have to store dt history too (feh)
-    history[now].dt = dt;
-    
-    // Store distance travelled in a fifo for later use
-    history[now].dist = (sensors.lrEncDistance + sensors.rrEncDistance) / 2.0;
-
-
-    // Calc and store heading
-    history[now].ghdg = history[prev].ghdg + dt*history[now].gyro; // raw gyro calculated heading
-    //history[now].hdg = history[prev].ghdg - dt*gyroBias;           // bias-corrected gyro heading
-    history[now].hdg = history[prev].hdg + dt*history[now].gyro;
+    // Save current data into history fifo to use 1 second from now
+    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;
-
-    //fprintf(stdout, "%d %d %.4f %.4f\n", now, prev, history[now].hdg, history[prev].hdg);
+    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) {
+        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
     } else {
-        // Now that the buffer is full, we'll maintain a Kalman Filtered estimate that is
+        // Now that the buffer is full, we'll maintain a Kalman Filter estimate that is
         // time-shifted to the past because the GPS output represents the system state from
-        // the past. We'll also take our history of gyro readings from the most recent 
-        // filter update to the present time and update our current heading and position
+        // the past. We'll also correct our history of gyro readings from the past to the
+        // present
         
         ////////////////////////////////////////////////////////////////////////////////
         // UPDATE LAGGED ESTIMATE
@@ -337,22 +331,21 @@
         // Clamp heading to initial heading when we're not moving; hopefully this will
         // give the KF a head start figuring out how to deal with the gyro
         //
-        // TODO 1 we're getting gps course every 100ms, are we calling kalman every 10ms???
+        // TODO 1 maybe we should only call the gps version after moving
         if (go) {
-            lagHeading = headingKalman(history[lag].dt, state[inState].gpsCourse_deg, useGps, history[lag].gyro, true);
+            estLagHeading = headingKalman(history[lag].dt, state[inState].gpsCourse_deg, useGps, history[lag].gyro, true);
         } else {    
-            lagHeading = headingKalman(history[lag].dt, initialHeading, true, history[lag].gyro, true);
+            estLagHeading = headingKalman(history[lag].dt, initialHeading, true, history[lag].gyro, true);
         }
 
-        // TODO 1 need to figure out exactly how to update t-1 position correctly <-- wtf does this mean?!
+        // TODO 1 are we adding history lag to lagprev or should we add lag+1 to lag or what?
         // Update the lagged position estimate
-        //lagHere.move(lagHeading, history[lag].dist);
-        history[lag].x = history[lagPrev].x + history[lag].dist * sin(lagHeading);
-        history[lag].y = history[lagPrev].y + history[lag].dist * cos(lagHeading);
+        history[lag].x = history[lagPrev].x + history[lag].dist * sin(estLagHeading);
+        history[lag].y = history[lagPrev].y + history[lag].dist * cos(estLagHeading);
         
         ////////////////////////////////////////////////////////////////////////////////
         // UPDATE CURRENT ESTIMATE
-       
+
         // Now we need to re-calculate the current heading and position, starting with the most recent
         // heading estimate representing the heading 1 second ago.
         //        
@@ -367,16 +360,18 @@
         // matrix.
         //
         // initialize these once
-        errAngle = (lagHeading - history[lag].hdg);
+        errAngle = (estLagHeading - history[lag].hdg);
         if (errAngle <= -180.0) errAngle += 360.0;
         if (errAngle > 180) errAngle -= 360.0;
 
-        //fprintf(stdout, "%d %.2f, %.2f, %.4f %.4f\n", lag, lagHeading, history[lag].hdg, lagHeading - history[lag].hdg, errAngle);
-        float cosA = cos(errAngle * PI / 180);
-        float sinA = sin(errAngle * PI / 180);
-        // Start at the out side of the fifo which is from 1 second ago
+        //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++) {
+            //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;
             // Rotate x, y by errAngle around pivot point; no need to rotate pivot point (j=0)
             if (j > 0) {
@@ -388,81 +383,28 @@
             }
             inc(i);
         }
-        
-        // Gyro bias, correct only with shallow steering angles
-        // if we're not going, assume heading rate is 0 and correct accordingly
-        // If we are going, compare gyro-generated heading estimate with kalman
-        // heading estimate and correct bias accordingly using PI controller with
-        // fairly long time constant
-        // TODO: 3 Add something in here to stop updating if the gps is unreliable; need to find out what indicates poor gps heading
-        // Note, only time gps heading is paritcularly terrible is high bandwidth, at least for the Venus GPS, even with high dynamic
-        // firmware installed it still does fairly heavy handed low pass filtering
-        /*
-        if (history[lag].dt > 0 && fabs(steerAngle) < 5.0 && useGps) {
-            // I think we should normalize heading err here (4/7/2013) as this will be a mess if you 
-            // have, say, ghdg==359.0 and gpsCourse==1, herr should be 2 but would otherwise come out 358
-
-            // Calculate the error term between Gyro integrated heading (from 1 sec ago) and GPS heading (1 sec old)
-            float herr = history[lag].ghdg - state[inState].gpsCourse_deg;
-            if (herr <= -180.0) herr += 360.0;  // normalize to within -180 to 180 degrees
-            if (herr > 180.0) herr -= 360.0;
-
-            // Calculate a bias error angle, an exponential filtering of heading error over time.
-            biasErrAngle = Kbias*biasErrAngle + (1-Kbias)*herr; // can use this to compute gyro bias
-            if (biasErrAngle <= -180.0) biasErrAngle += 360.0; // normalize to within -180 to 180 degrees
-            if (biasErrAngle > 180) biasErrAngle -= 360.0;
-
-            // Calculate the error rate using the filtered bias error angle divided by delta time.
-            float errRate = biasErrAngle / history[lag].dt;
-
-            //if (!go) errRate = history[lag].gyro;
-
-            // Compute exponentially filtered gyro bias based on errRate which is based on filtered biasErrAngle
-            gyroBias = Kbias*gyroBias + (1-Kbias)*errRate;
-            //fprintf(stdout, "%d %.2f, %.2f, %.4f %.4f %.4f\n", lag, lagHeading, history[lag].hdg, errAngle, errRate, gyroBias);
-        }
-        */
-        
-        // make sure we update the lag heading with the new estimate
-        history[lag].hdg = lagHeading;
-        
         // increment lag pointer and wrap        
         lagPrev = lag;
         inc(lag);
-        
     }
-    state[inState].estHeading = history[lag].hdg;
-    // the variable "here" is the current position
-    //here.move(history[now].hdg, history[now].dist);
-    float r = PI/180 * history[now].hdg;
-    // update current position
-    history[now].x = history[prev].x + history[now].dist * sin(r);
-    history[now].y = history[prev].y + history[now].dist * cos(r);
+    // "here" is the current position
     cartHere.set(history[now].x, history[now].y);
-    mapper.cartToGeo(cartHere, &here);
-
-    // TODO: don't update gyro heading if speed ~0 -- or use this time to re-calc bias?
-    // (sensors.lrEncSpeed == 0 && sensors.rrEncSpeed == 0)
 
     //////////////////////////////////////////////////////////////////////////////
     // NAVIGATION UPDATE
     //////////////////////////////////////////////////////////////////////////////
     
-    //bearing = here.bearingTo(config.wpt[nextWaypoint]);
     bearing = cartHere.bearingTo(config.cwpt[nextWaypoint]);
-    //distance = here.distanceTo(config.wpt[nextWaypoint]);
     distance = cartHere.distanceTo(config.cwpt[nextWaypoint]);
-    //float prevDistance = here.distanceTo(config.wpt[lastWaypoint]);
     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
+    // TODO 3 turn this into a function
     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
+    // TODO 3 figure out how to output feedback on wpt arrival external to this function
     if (go) {
 
         // if we're within brakeDist of next or previous waypoint, run @ turn speed
@@ -481,7 +423,6 @@
             //speaker.beep(3000.0, 0.5); // non-blocking
             lastWaypoint = nextWaypoint;
             nextWaypoint++;
-            cteI = 0;
         }
         
     } else {
@@ -507,23 +448,14 @@
 
     if (--control_count == 0) {
   
-        // Compute cross track error
-        /*
-        cte = steerCalc.crossTrack(history[now].x, history[now].y,
-                                   config.cwpt[lastWaypoint]._x, config.cwpt[lastWaypoint]._y,
-                                   config.cwpt[nextWaypoint]._x, config.cwpt[nextWaypoint]._y);
-        cteI += cte;
-        */
-
-        steerAngle = steerCalc.pathPursuitSA(state[inState].estHeading, 
-                                             history[now].x, history[now].y,
+        steerAngle = steerCalc.pathPursuitSA(history[now].hdg, history[now].x, history[now].y,
                                              config.cwpt[lastWaypoint]._x, config.cwpt[lastWaypoint]._y,
                                              config.cwpt[nextWaypoint]._x, config.cwpt[nextWaypoint]._y);
         
         // TODO 3: eliminate pursuit config item
         /*
         if (config.usePP) {
-            steerAngle = steerCalc.purePursuitSA(state[inState].estHeading, 
+            steerAngle = steerCalc.purePursuitSA(history[now].hdg, 
                                                  history[now].x, history[now].y,
                                                  config.cwpt[lastWaypoint]._x, config.cwpt[lastWaypoint]._y,
                                                  config.cwpt[nextWaypoint]._x, config.cwpt[nextWaypoint]._y);
@@ -537,14 +469,14 @@
         if (fabs(steerAngle) < config.steerGainAngle) steerAngle *= config.steerGain;
 
         // Curb avoidance
+        /*
         if (sensors.rightRanger < config.curbThreshold) {
             steerAngle -= config.curbGain * (config.curbThreshold - sensors.rightRanger);
         }
+        */
                     
         setSteering( steerAngle );
 
-// void throttleUpdate( float speed, float dt )
-
         // PID control for throttle
         // TODO: 3 move all this PID crap into Actuators.cpp
         // TODO: 3 probably should do KF or something for speed/dist; need to address GPS lag, too
@@ -581,40 +513,37 @@
     // Periodically, we enter a new SystemState into the FIFO buffer
     // The main loop handles logging and will catch up to us provided 
     // we feed in new log entries slowly enough.
-    if (--log_count == 0) {
+    // log_count initialized to 0 to begin with forcing initialization below
+    // but no further updates until in go mode
+    if (go && (log_count > 0)) --log_count;
+    // Only enter new SystemState data when in "go" mode (armed to run, run,
+    // and end run)
+    // We should really only be adding to the state fifo if we're in 'go' mode
+    if (log_count <= 0) {
         // Copy data into system state for logging
         inState++;                      // Get next state struct in the buffer
         inState &= SSBUF;               // Wrap around
         ssBufOverrun = (inState == outState);
-        //
         // Need to clear out encoder distance counters; these are incremented
         // each time this routine is called.
+        // TODO 1 is there anything we can't clear out?
+        // TODO 1 use clearState()
         state[inState].lrEncDistance = 0;
         state[inState].rrEncDistance = 0;
-        //
-        // need to initialize gps data to be safe
-        //
+        // initialize gps data
         state[inState].gpsLatitude = 0;
         state[inState].gpsLongitude = 0;
         state[inState].gpsHDOP = 0;
         state[inState].gpsCourse_deg = 0;
         state[inState].gpsSpeed_mps = 0;
         state[inState].gpsSats = 0;
-
-        state[inState].gpsLatitude2 = 0;
-        state[inState].gpsLongitude2 = 0;
-        state[inState].gpsHDOP2 = 0;
-        state[inState].gpsCourse_deg2 = 0;
-        state[inState].gpsSpeed_mps2 = 0;
-        state[inState].gpsSats2 = 0;
-
         log_count = LOG_SKIP;       // reset counter
         bufCount++;
     }
 
     // Log Data Timestamp    
     state[inState].millis = timestamp;
-    
+
     // TODO: 3 recursive filtering on each of the state values
     state[inState].voltage = sensors.voltage;
     state[inState].current = sensors.current;
@@ -624,12 +553,14 @@
         state[inState].a[i] = sensors.a[i];
     }
     state[inState].gTemp = sensors.gTemp;
-    state[inState].gHeading = history[now].hdg;
     state[inState].lrEncSpeed = sensors.lrEncSpeed;
     state[inState].rrEncSpeed = sensors.rrEncSpeed;
     state[inState].lrEncDistance += sensors.lrEncDistance;
     state[inState].rrEncDistance += sensors.rrEncDistance;
     //state[inState].encHeading += (state[inState].lrEncDistance - state[inState].rrEncDistance) / TRACK;
+    state[inState].estHeading = history[now].hdg; // gyro heading estimate, current, corrected
+    state[inState].estLagHeading = history[lag].hdg; // gyro heading, estimate, lagged
+    mapper.cartToGeo(cartHere, &here);
     state[inState].estLatitude = here.latitude();
     state[inState].estLongitude = here.longitude();
     state[inState].estX = history[now].x;
@@ -637,12 +568,12 @@
     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;
-    state[inState].centerRanger = sensors.centerRanger;
-    state[inState].crossTrackErr = cte;
+    //state[inState].leftRanger = sensors.leftRanger;
+    //state[inState].rightRanger = sensors.rightRanger;
+    //state[inState].centerRanger = sensors.centerRanger;
+    state[inState].steerAngle = steerAngle;
     // Copy AHRS data into logging data
     //state[inState].roll = ToDeg(ahrs.roll);
     //state[inState].pitch = ToDeg(ahrs.pitch);