updater.c

00001 #include "Config.h"
00002 #include "Actuators.h"
00003 #include "Sensors.h"
00004 #include "SystemState.h"
00005 #include "GPS.h"
00006 #include "Steering.h"
00007 #include "Servo.h"
00008 #include "Mapping.h"
00009 #include "CartPosition.h"
00010 #include "GeoPosition.h"
00011 #include "kalman.h"
00012 
00013 #define _x_ 0
00014 #define _y_ 1
00015 #define _z_ 2
00016 
00017 // Every ...
00018 //  10ms (100Hz) -- update gyro, encoders; update AHRS; update navigation; set log data
00019 //  10ms (100Hz) -- camera (actually runs 30fps
00020 //  20ms  (50Hz) -- update compass sensor
00021 //  50ms  (20Hz) -- create new log entry in log FIFO
00022 // 100ms  (10Hz) -- GPS update
00023 // 100ms  (10Hz) -- control update
00024 
00025 #define CTRL_SKIP 5
00026 #define MAG_SKIP 2
00027 #define LOG_SKIP 5
00028 
00029 int control_count=CTRL_SKIP;
00030 int update_count=MAG_SKIP;              // call Update_mag() every update_count calls to schedHandler()
00031 int log_count=LOG_SKIP;                 // buffer a new status entry for logging every log_count calls to schedHandler
00032 int tReal;                              // calculate real elapsed time
00033 
00034 // TODO: 3 better encapsulation, please
00035 extern Sensors sensors;
00036 extern SystemState state[SSBUF];
00037 extern unsigned char inState;
00038 extern unsigned char outState;
00039 extern bool ssBufOverrun;
00040 extern GPS gps;
00041 extern Mapping mapper;
00042 extern Steering steerCalc;              // steering calculator
00043 extern Timer timer;
00044 extern DigitalOut ahrsStatus;           // AHRS status LED
00045 
00046 // Navigation
00047 extern Config config;
00048 int nextWaypoint = 0;                   // next waypoint destination
00049 int lastWaypoint = 1;
00050 double bearing;                         // bearing to next waypoint
00051 double distance;                        // distance to next waypoint
00052 float steerAngle;                       // steering angle
00053 float cte;                              // Cross Track error
00054 float cteI;                             // Integral of Cross Track Error
00055 
00056 // Throttle PID
00057 float speedDt=0;                        // dt for the speed PID
00058 float integral=0;                       // error integral for speed PID
00059 float lastError=0;                      // previous error, used for calculating derivative
00060 bool go=false;                          // initiate throttle (or not)
00061 float desiredSpeed;                     // speed set point
00062 float nowSpeed;
00063 
00064 
00065 // Pose Estimation
00066 bool initNav = true;
00067 float initHdg = true;                   // indiciates that heading needs to be initialized by gps
00068 float initialHeading=-999;              // initial heading
00069 float myHeading=0;                      // heading sent to KF
00070 CartPosition cartHere;                  // position estimate, cartesian
00071 GeoPosition here;                       // position estimate, lat/lon
00072 extern GeoPosition gps_here;
00073 int timeZero=0;
00074 int lastTime=-1;                        // used to calculate dt for KF
00075 int thisTime;                           // used to calculate dt for KF
00076 float dt;                               // dt for the KF
00077 float lagHeading = 0;                   // lagged heading estimate
00078 //GeoPosition lagHere;                  // lagged position estimate; use here as the current position estimate
00079 float errAngle;                         // error between gyro hdg estimate and gps hdg estimate
00080 float gyroBias=0;                       // exponentially weighted moving average of gyro error
00081 float Ag = (2.0/(1000.0+1.0));          // Gyro bias filter alpha, gyro; # of 10ms steps
00082 float Kbias = 0.995;            
00083 float filtErrRate = 0;
00084 float biasErrAngle = 0;
00085 
00086 #define MAXHIST 128 // must be multiple of 0x08
00087 #define inc(x)  (x) = ((x)+1)&(MAXHIST-1)
00088 struct {
00089     float x;        // x coordinate
00090     float y;        // y coordinate
00091     float hdg;      // heading
00092     float dist;     // distance
00093     float gyro;     // heading rate
00094     float ghdg;     // uncorrected gyro heading
00095     float dt;       // delta time
00096 } history[MAXHIST]; // fifo for sensor data, position, heading, dt
00097 
00098 int hCount=0;       // history counter; one > 100, we can go back in time to reference gyro history
00099 int now = 0;        // fifo input index
00100 int prev = 0;      // previous fifo iput index
00101 int lag = 0;        // fifo output index
00102 int lagPrev = 0;    // previous fifo output index
00103 
00104 
00105 /** set flag to initialize navigation at next schedHandler() call
00106  */
00107 void restartNav()
00108 {
00109     initNav = true;
00110     initHdg = true;
00111     return;
00112 }
00113 
00114 /** instruct the controller to throttle up
00115  */
00116 void beginRun()
00117 {
00118     go = true;
00119     timeZero = thisTime; // initialize 
00120     return;
00121 }
00122 
00123 void endRun()
00124 {
00125     go = false;
00126     initNav = true;
00127     return;
00128 }
00129 
00130 /** get elasped time in update loop
00131  */
00132 int getUpdateTime()
00133 {
00134     return tReal;
00135 }
00136 
00137 void setSpeed(float speed) 
00138 {
00139     if (desiredSpeed != speed) {
00140         desiredSpeed = speed;
00141         integral = 0;
00142     }
00143     return;
00144 }
00145 
00146 /** schedHandler fires off the various routines at appropriate schedules
00147  *
00148  */
00149 void update()
00150 {
00151     tReal = timer.read_us();
00152 
00153     ahrsStatus = 0;
00154     thisTime = timer.read_ms();
00155     dt = (lastTime < 0) ? 0 : ((float) thisTime - (float) lastTime) / 1000.0; // first pass let dt=0
00156     lastTime = thisTime;
00157 
00158     // Add up dt to speedDt
00159     speedDt += dt;
00160     
00161     // Log Data Timestamp    
00162     int timestamp = timer.read_ms();
00163 
00164     //////////////////////////////////////////////////////////////////////////////
00165     // NAVIGATION INIT
00166     //////////////////////////////////////////////////////////////////////////////
00167     // initNav is set with call to restartNav() when the "go" button is pressed.  Up
00168     // to 10ms later this function is called and the code below will initialize the
00169     // dead reckoning position and estimated position variables
00170     // 
00171     if (initNav == true) {
00172         initNav = false;
00173         here.set(config.wpt[0]);
00174         nextWaypoint = 1; // Point to the next waypoint; 0th wpt is the starting point
00175         lastWaypoint = 1; // Init to waypoint 1, we we don't start from wpt 0 at the turning speed
00176         // Initialize lag estimates
00177         //lagHere.set( here );
00178         // Initialize fifo
00179         hCount = 0;
00180         now = 0;
00181         // initialize what will become lag data in 1 second from now
00182         history[now].dt = 0;
00183         // initial position is waypoint 0
00184         history[now].x = config.cwpt[0]._x;
00185         history[now].y = config.cwpt[0]._y;
00186         cartHere.set(history[now].x, history[now].y);
00187         // initialize heading to bearing between waypoint 0 and 1
00188         //history[now].hdg = here.bearingTo(config.wpt[nextWaypoint]);
00189         history[now].hdg = cartHere.bearingTo(config.cwpt[nextWaypoint]);
00190         history[now].ghdg = history[now].hdg;
00191         initialHeading = history[now].hdg;
00192         // Initialize Kalman Filter
00193         headingKalmanInit(history[now].hdg);
00194         // initialize cross track error
00195         cte = 0;
00196         cteI = 0;
00197         // point next fifo input to slot 1, slot 0 occupied/initialized, now
00198         lag = 0;
00199         lagPrev = 0;
00200         prev = now; // point to the most recently entered data
00201         now = 1;    // new input slot
00202     }
00203 
00204 
00205     //////////////////////////////////////////////////////////////////////////////
00206     // SENSOR UPDATES
00207     //////////////////////////////////////////////////////////////////////////////
00208 
00209     // TODO: 3 This really should run infrequently
00210     sensors.Read_Power();
00211 
00212     sensors.Read_Encoders(); 
00213     nowSpeed = sensors.encSpeed;
00214 
00215     sensors.Read_Gyro(); 
00216 
00217     sensors.Read_Rangers();
00218 
00219     //sensors.Read_Camera();
00220 
00221     //////////////////////////////////////////////////////////////////////////////
00222     // Obtain GPS data                        
00223     //////////////////////////////////////////////////////////////////////////////
00224 
00225     // synchronize when RMC and GGA sentences received w/ AHRS
00226     // Do this in schedHandler??  GPS data is coming in not entirely in sync
00227     // with the logging info
00228     if (gps.nmea.rmc_ready() && gps.nmea.gga_ready()) {
00229         char gpsdate[32], gpstime[32];
00230 
00231         gps.process(gps_here, gpsdate, gpstime);
00232         //gpsStatus = (gps.hdop > 0.0 && gps.hdop < 3.0) ? 1 : 0;
00233 
00234         // update system status struct for logging
00235         state[inState].gpsLatitude = gps_here.latitude();
00236         state[inState].gpsLongitude = gps_here.longitude();
00237         state[inState].gpsHDOP = gps.hdop;
00238         state[inState].gpsCourse = gps.nmea.f_course();
00239         state[inState].gpsSpeed = gps.nmea.f_speed_mph(); // can we just do kph/1000 ? or mps?
00240         state[inState].gpsSats = gps.nmea.sat_count();
00241     }
00242     
00243     //////////////////////////////////////////////////////////////////////////////
00244     // HEADING AND POSITION UPDATE
00245     //////////////////////////////////////////////////////////////////////////////
00246 
00247     // TODO: 2 Position filtering
00248     //    position will be updated based on heading error from heading estimate
00249     // TODO: 2 Distance/speed filtering
00250     //    this might be useful, but not sure it's worth the effort
00251 
00252     // So the big pain in the ass is that the GPS data coming in represents the
00253     // state of the system ~1s ago. Yes, a full second of lag despite running
00254     // at 10hz (or whatever).  So if we try and fuse a lagged gps heading with a
00255     // relatively current gyro heading rate, the gyro is ignored and the heading
00256     // estimate lags reality
00257     //
00258     // In real life testing, the robot steering control was highly unstable with
00259     // too much gain (typical of any negative feedback system with a very high
00260     // phase shift and too much feedback). It'd drive around in circles trying to
00261     // hunt for the next waypoint.  Dropping the gain restored stability but the
00262     // steering overshot significantly, because of the lag.  It sort of worked but
00263     // it was ugly. So here's my lame attempt to fix all this. 
00264     // 
00265     // We'll find out how much error there is between gps heading and the integrated
00266     // gyro heading from a second ago.
00267 
00268     // stick precalculated gyro data, with bias correction, into fifo
00269     //history[now].gyro = sensors.gyro[_z_] - gyroBias;   
00270     history[now].gyro = sensors.gyro[_z_];   
00271 
00272     // Have to store dt history too (feh)
00273     history[now].dt = dt;
00274     
00275     // Store distance travelled in a fifo for later use
00276     history[now].dist = (sensors.lrEncDistance + sensors.rrEncDistance) / 2.0;
00277 
00278 
00279     // Calc and store heading
00280     history[now].ghdg = history[prev].ghdg + dt*history[now].gyro; // raw gyro calculated heading
00281     //history[now].hdg = history[prev].ghdg - dt*gyroBias;           // bias-corrected gyro heading
00282     history[now].hdg = history[prev].hdg + dt*history[now].gyro;
00283     if (history[now].hdg >= 360.0) history[now].hdg -= 360.0;
00284     if (history[now].hdg < 0)      history[now].hdg += 360.0;
00285 
00286     //fprintf(stdout, "%d %d %.4f %.4f\n", now, prev, history[now].hdg, history[prev].hdg);
00287 
00288     // We can't do anything until the history buffer is full
00289     if (hCount < 100) {
00290         // Until the fifo is full, only keep track of current gyro heading
00291         hCount++; // after n iterations the fifo will be full
00292     } else {
00293         // Now that the buffer is full, we'll maintain a Kalman Filtered estimate that is
00294         // time-shifted to the past because the GPS output represents the system state from
00295         // the past. We'll also take our history of gyro readings from the most recent 
00296         // filter update to the present time and update our current heading and position
00297         
00298         ////////////////////////////////////////////////////////////////////////////////
00299         // UPDATE LAGGED ESTIMATE
00300         
00301         // Recover data from 1 second ago which will be used to generate
00302         // updated lag estimates
00303 
00304         // GPS heading is unavailable from this particular GPS at speeds < 0.5 mph
00305         bool useGps = (state[inState].gpsLatitude != 0 &&
00306                        state[inState].lrEncSpeed > 1.0 &&
00307                        state[inState].rrEncSpeed > 1.0 &&
00308                        (thisTime-timeZero) > 3000); // gps hdg converges by 3-5 sec.
00309 
00310         // This represents the best estimate for heading... for one second ago
00311         // If we do nothing else, the robot will think it is located at a position 1 second behind
00312         // where it actually is. That means that any control feedback needs to have a longer time
00313         // constant than 1 sec or the robot will have unstable heading correctoin.
00314         // Use gps data when available, always use gyro data
00315 
00316         // Clamp heading to initial heading when we're not moving; hopefully this will
00317         // give the KF a head start figuring out how to deal with the gyro
00318         if (go) {
00319             lagHeading = headingKalman(history[lag].dt, state[inState].gpsCourse, useGps, history[lag].gyro, true);
00320         } else {    
00321             lagHeading = headingKalman(history[lag].dt, initialHeading, true, history[lag].gyro, true);
00322         }
00323 
00324         // TODO 1: need to figure out exactly how to update t-1 position correctly
00325         // Update the lagged position estimate
00326         //lagHere.move(lagHeading, history[lag].dist);
00327         history[lag].x = history[lagPrev].x + history[lag].dist * sin(lagHeading);
00328         history[lag].y = history[lagPrev].y + history[lag].dist * cos(lagHeading);
00329         
00330         ////////////////////////////////////////////////////////////////////////////////
00331         // UPDATE CURRENT ESTIMATE
00332        
00333         // Now we need to re-calculate the current heading and position, starting with the most recent
00334         // heading estimate representing the heading 1 second ago.
00335         //        
00336         // Nuance: lag and now have already been incremented so that works out beautifully for this loop
00337         //
00338         // Heading is easy. Original heading - estimated heading gives a tiny error. Add this to all the historical
00339         // heading data up to present.
00340         //
00341         // For position re-calculation, we iterate 100 times up to present record. Haversine is way, way too slow,
00342         // so is trig calcs is marginal. Update rate is 10ms and we can't hog more than maybe 2-3ms as the outer
00343         // loop has logging work to do. Rotating each point is faster; pre-calculate sin/cos, etc.
00344         //
00345         // initialize these once
00346         errAngle = (lagHeading - history[lag].hdg);
00347         if (errAngle <= -180.0) errAngle += 360.0;
00348         if (errAngle > 180) errAngle -= 360.0;
00349 
00350         //fprintf(stdout, "%d %.2f, %.2f, %.4f %.4f\n", lag, lagHeading, history[lag].hdg, lagHeading - history[lag].hdg, errAngle);
00351         float cosA = cos(errAngle * PI / 180);
00352         float sinA = sin(errAngle * PI / 180);
00353         // Start at the out side of the fifo which is from 1 second ago
00354         int i = lag;
00355         for (int j=0; j < 100; j++) {
00356             history[i].hdg += errAngle;
00357             // Rotate x, y by errAngle around pivot point; no need to rotate pivot point (j=0)
00358             if (j > 0) {
00359                 float dx = history[i].x-history[lag].x;
00360                 float dy = history[i].y-history[lag].y;
00361                 history[i].x = history[lag].x + dx*cosA - dy*sinA;
00362                 history[i].y = history[lag].y + dx*sinA + dy*cosA;
00363             }
00364             inc(i);
00365         }
00366         
00367         // gyro bias, correct only with shallow steering angles
00368         // if we're not going, assume heading rate is 0 and correct accordingly
00369         // If we are going, compare gyro-generated heading estimate with kalman
00370         // heading estimate and correct bias accordingly using PI controller with
00371         // fairly long time constant
00372         // TODO: 3 Add something in here to stop updating if the gps is unreliable; need to find out what indicates poor gps heading
00373         if (history[lag].dt > 0 && fabs(steerAngle) < 5.0 && useGps) {
00374             biasErrAngle = Kbias*biasErrAngle + (1-Kbias)*(history[lag].ghdg - state[inState].gpsCourse); // can use this to compute gyro bias
00375             if (biasErrAngle <= -180.0) biasErrAngle += 360.0;
00376             if (biasErrAngle > 180) biasErrAngle -= 360.0;
00377         
00378             float errRate = biasErrAngle / history[lag].dt;
00379 
00380             //if (!go) errRate = history[lag].gyro;
00381 
00382             // Filter bias based on errRate which is based on filtered biasErrAngle
00383             gyroBias = Kbias*gyroBias + (1-Kbias)*errRate;
00384             //fprintf(stdout, "%d %.2f, %.2f, %.4f %.4f %.4f\n", lag, lagHeading, history[lag].hdg, errAngle, errRate, gyroBias);
00385         }
00386         
00387         // make sure we update the lag heading with the new estimate
00388         history[lag].hdg = lagHeading;
00389         
00390         // increment lag pointer and wrap        
00391         lagPrev = lag;
00392         inc(lag);
00393         
00394     }
00395     state[inState].estHeading = history[lag].hdg;
00396     // the variable "here" is the current position
00397     //here.move(history[now].hdg, history[now].dist);
00398     float r = PI/180 * history[now].hdg;
00399     // update current position
00400     history[now].x = history[prev].x + history[now].dist * sin(r);
00401     history[now].y = history[prev].y + history[now].dist * cos(r);
00402     cartHere.set(history[now].x, history[now].y);
00403     mapper.cartToGeo(cartHere, &here);
00404 
00405     // TODO: don't update gyro heading if speed ~0 -- or use this time to re-calc bias?
00406     // (sensors.lrEncSpeed == 0 && sensors.rrEncSpeed == 0)
00407 
00408     //////////////////////////////////////////////////////////////////////////////
00409     // NAVIGATION UPDATE
00410     //////////////////////////////////////////////////////////////////////////////
00411     
00412     //bearing = here.bearingTo(config.wpt[nextWaypoint]);
00413     bearing = cartHere.bearingTo(config.cwpt[nextWaypoint]);
00414     //distance = here.distanceTo(config.wpt[nextWaypoint]);
00415     distance = cartHere.distanceTo(config.cwpt[nextWaypoint]);
00416     //float prevDistance = here.distanceTo(config.wpt[lastWaypoint]);
00417     float prevDistance = cartHere.distanceTo(config.cwpt[lastWaypoint]);
00418     double relativeBrg = bearing - history[now].hdg;
00419 
00420     // if correction angle is < -180, express as negative degree
00421     // TODO: 3 turn this into a function
00422     if (relativeBrg < -180.0) relativeBrg += 360.0;
00423     if (relativeBrg > 180.0)  relativeBrg -= 360.0;
00424 
00425     // if within config.waypointDist distance threshold move to next waypoint
00426     // TODO: 3 figure out how to output feedback on wpt arrival external to this function
00427     if (go) {
00428 
00429         // if we're within brakeDist of next or previous waypoint, run @ turn speed
00430         // This would normally mean we run at turn speed until we're brakeDist away
00431         // from waypoint 0, but we trick the algorithm by initializing prevWaypoint to waypoint 1
00432         if (distance < config.brakeDist || prevDistance < config.brakeDist) {
00433             setSpeed( config.turnSpeed );
00434         } else if ( (thisTime - timeZero) < 1000 ) {
00435             setSpeed( config.startSpeed );
00436         } else {
00437             setSpeed( config.topSpeed );
00438         }
00439 
00440         if (distance < config.waypointDist) {
00441             //fprintf(stdout, "Arrived at wpt %d\n", nextWaypoint);
00442             //speaker.beep(3000.0, 0.5); // non-blocking
00443             lastWaypoint = nextWaypoint;
00444             nextWaypoint++;
00445             cteI = 0;
00446         }
00447         
00448     } else {
00449         setSpeed( 0.0 );
00450     }
00451     // Are we at the last waypoint?
00452     // currently handled external to this routine
00453         
00454     //////////////////////////////////////////////////////////////////////////////
00455     // OBSTACLE DETECTION & AVOIDANCE
00456     //////////////////////////////////////////////////////////////////////////////
00457     // TODO: 1 limit steering angle based on object detection ?
00458     // or limit relative brg perhaps?
00459     // TODO: 1 add vision obstacle detection and filtering
00460     // TODO: 1 add ranger obstacle detection and filtering/fusion with vision
00461 
00462 
00463     //////////////////////////////////////////////////////////////////////////////
00464     // CONTROL UPDATE
00465     //////////////////////////////////////////////////////////////////////////////
00466 
00467     if (--control_count == 0) {
00468   
00469         // Compute cross track error
00470         cte = steerCalc.crossTrack(history[now].x, history[now].y,
00471                                    config.cwpt[lastWaypoint]._x, config.cwpt[lastWaypoint]._y,
00472                                    config.cwpt[nextWaypoint]._x, config.cwpt[nextWaypoint]._y);
00473         cteI += cte;
00474 
00475         // Use either pure pursuit algorithm or original algorithm  
00476         if (config.usePP) {
00477             steerAngle = steerCalc.purePursuitSA(state[inState].estHeading, 
00478                                                  history[now].x, history[now].y,
00479                                                  config.cwpt[lastWaypoint]._x, config.cwpt[lastWaypoint]._y,
00480                                                  config.cwpt[nextWaypoint]._x, config.cwpt[nextWaypoint]._y);
00481         } else {
00482             steerAngle = steerCalc.calcSA(relativeBrg, config.minRadius); // use the configured minimum turn radius
00483         }
00484         
00485         // Apply gain factor for near straight line
00486         if (fabs(steerAngle) < config.steerGainAngle) steerAngle *= config.steerGain;
00487 
00488         // Curb avoidance
00489         if (sensors.rightRanger < config.curbThreshold) {
00490             steerAngle -= config.curbGain * (config.curbThreshold - sensors.rightRanger);
00491         }
00492                     
00493         setSteering( steerAngle );
00494 
00495 // void throttleUpdate( float speed, float dt )
00496 
00497         // PID control for throttle
00498         // TODO: 3 move all this PID crap into Actuators.cpp
00499         // TODO: 3 probably should do KF or something for speed/dist; need to address GPS lag, too
00500         // if nothing else, at least average the encoder speed over mult. samples
00501         if (desiredSpeed <= 0.1) {
00502             setThrottle( config.escZero );
00503         } else {
00504             // PID loop for throttle control
00505             // http://www.codeproject.com/Articles/36459/PID-process-control-a-Cruise-Control-example
00506             float error = desiredSpeed - nowSpeed; 
00507             // track error over time, scaled to the timer interval
00508             integral += (error * speedDt);
00509             // determine the amount of change from the last time checked
00510             float derivative = (error - lastError) / speedDt; 
00511             // calculate how much to drive the output in order to get to the 
00512             // desired setpoint. 
00513             int output = config.escZero + (config.speedKp * error) + (config.speedKi * integral) + (config.speedKd * derivative);
00514             if (output > config.escMax) output = config.escMax;
00515             if (output < config.escMin) output = config.escMin;
00516             //fprintf(stdout, "s=%.1f d=%.1f o=%d\n", nowSpeed, desiredSpeed, output);
00517             setThrottle( output );
00518             // remember the error for the next time around.
00519             lastError = error; 
00520         }
00521 
00522         speedDt = 0; // reset dt to begin counting for next time
00523         control_count = CTRL_SKIP;
00524     }      
00525 
00526     //////////////////////////////////////////////////////////////////////////////
00527     // DATA FOR LOGGING
00528     //////////////////////////////////////////////////////////////////////////////
00529 
00530     // Periodically, we enter a new SystemState into the FIFO buffer
00531     // The main loop handles logging and will catch up to us provided 
00532     // we feed in new log entries slowly enough.
00533     if (--log_count == 0) {
00534         // Copy data into system state for logging
00535         inState++;                      // Get next state struct in the buffer
00536         inState &= SSBUF;               // Wrap around
00537         ssBufOverrun = (inState == outState);
00538         //
00539         // Need to clear out encoder distance counters; these are incremented
00540         // each time this routine is called.
00541         state[inState].lrEncDistance = 0;
00542         state[inState].rrEncDistance = 0;
00543         //
00544         // need to initialize gps data to be safe
00545         //
00546         state[inState].gpsLatitude = 0;
00547         state[inState].gpsLongitude = 0;
00548         state[inState].gpsHDOP = 0;
00549         state[inState].gpsCourse = 0;
00550         state[inState].gpsSpeed = 0;
00551         state[inState].gpsSats = 0;
00552         log_count = LOG_SKIP;       // reset counter
00553     }
00554 
00555     // Log Data Timestamp    
00556     state[inState].millis = timestamp;
00557     
00558     // TODO: 3 recursive filtering on each of the state values
00559     state[inState].voltage = sensors.voltage;
00560     state[inState].current = sensors.current;
00561     for (int i=0; i < 3; i++) {
00562         state[inState].m[i] = sensors.m[i];
00563         state[inState].g[i] = sensors.g[i];
00564         state[inState].a[i] = sensors.a[i];
00565     }
00566     state[inState].gTemp = sensors.gTemp;
00567     state[inState].gHeading = history[now].hdg;
00568     state[inState].lrEncSpeed = sensors.lrEncSpeed;
00569     state[inState].rrEncSpeed = sensors.rrEncSpeed;
00570     state[inState].lrEncDistance += sensors.lrEncDistance;
00571     state[inState].rrEncDistance += sensors.rrEncDistance;
00572     //state[inState].encHeading += (state[inState].lrEncDistance - state[inState].rrEncDistance) / TRACK;
00573     state[inState].estLatitude = here.latitude();
00574     state[inState].estLongitude = here.longitude();
00575     state[inState].estX = history[now].x;
00576     state[inState].estY = history[now].y;
00577     state[inState].bearing = bearing;
00578     state[inState].distance = distance;
00579     state[inState].nextWaypoint = nextWaypoint;
00580     state[inState].gbias = gyroBias;
00581     state[inState].errAngle = biasErrAngle;
00582     state[inState].leftRanger = sensors.leftRanger;
00583     state[inState].rightRanger = sensors.rightRanger;
00584     state[inState].centerRanger = sensors.centerRanger;
00585     state[inState].crossTrackErr = cte;
00586     // Copy AHRS data into logging data
00587     //state[inState].roll = ToDeg(ahrs.roll);
00588     //state[inState].pitch = ToDeg(ahrs.pitch);
00589     //state[inState].yaw = ToDeg(ahrs.yaw);
00590 
00591     // increment fifo pointers with wrap-around
00592     prev = now;
00593     inc(now);
00594 
00595     // timing
00596     tReal = timer.read_us() - tReal;
00597 
00598     ahrsStatus = 1;
00599 }