Diff: updater.c

Revision:

0:826c6171fc1b

diff -r 000000000000 -r 826c6171fc1b updater.c
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/updater.c	Wed Jun 20 14:57:48 2012 +0000
@@ -0,0 +1,599 @@
+#include "Config.h"
+#include "Actuators.h"
+#include "Sensors.h"
+#include "SystemState.h"
+#include "GPS.h"
+#include "Steering.h"
+#include "Servo.h"
+#include "Mapping.h"
+#include "CartPosition.h"
+#include "GeoPosition.h"
+#include "kalman.h"
+
+#define _x_ 0
+#define _y_ 1
+#define _z_ 2
+
+// Every ...
+//  10ms (100Hz) -- update gyro, encoders; update AHRS; update navigation; set log data
+//  10ms (100Hz) -- camera (actually runs 30fps
+//  20ms  (50Hz) -- update compass sensor
+//  50ms  (20Hz) -- create new log entry in log FIFO
+// 100ms  (10Hz) -- GPS update
+// 100ms  (10Hz) -- control update
+
+#define CTRL_SKIP 5
+#define MAG_SKIP 2
+#define LOG_SKIP 5
+
+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 tReal;                              // calculate real elapsed time
+
+// TODO: 3 better encapsulation, please
+extern Sensors sensors;
+extern SystemState state[SSBUF];
+extern unsigned char inState;
+extern unsigned char outState;
+extern bool ssBufOverrun;
+extern GPS gps;
+extern Mapping mapper;
+extern Steering steerCalc;              // steering calculator
+extern Timer timer;
+extern DigitalOut ahrsStatus;           // AHRS status LED
+
+// Navigation
+extern Config config;
+int nextWaypoint = 0;                   // next waypoint destination
+int lastWaypoint = 1;
+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
+float integral=0;                       // error integral for speed PID
+float lastError=0;                      // previous error, used for calculating derivative
+bool go=false;                          // initiate throttle (or not)
+float desiredSpeed;                     // speed set point
+float nowSpeed;
+
+
+// Pose Estimation
+bool initNav = true;
+float initHdg = true;                   // indiciates that heading needs to be initialized by gps
+float initialHeading=-999;              // initial heading
+float myHeading=0;                      // heading sent to KF
+CartPosition cartHere;                  // position estimate, cartesian
+GeoPosition here;                       // position estimate, lat/lon
+extern GeoPosition gps_here;
+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
+//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
+float Ag = (2.0/(1000.0+1.0));          // Gyro bias filter alpha, gyro; # of 10ms steps
+float Kbias = 0.995;            
+float filtErrRate = 0;
+float biasErrAngle = 0;
+
+#define MAXHIST 128 // must be multiple of 0x08
+#define inc(x)  (x) = ((x)+1)&(MAXHIST-1)
+struct {
+    float x;        // x coordinate
+    float y;        // y coordinate
+    float hdg;      // heading
+    float dist;     // distance
+    float gyro;     // heading rate
+    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
+
+
+/** set flag to initialize navigation at next schedHandler() call
+ */
+void restartNav()
+{
+    initNav = true;
+    initHdg = true;
+    return;
+}
+
+/** instruct the controller to throttle up
+ */
+void beginRun()
+{
+    go = true;
+    timeZero = thisTime; // initialize 
+    return;
+}
+
+void endRun()
+{
+    go = false;
+    initNav = true;
+    return;
+}
+
+/** get elasped time in update loop
+ */
+int getUpdateTime()
+{
+    return tReal;
+}
+
+void setSpeed(float speed) 
+{
+    if (desiredSpeed != speed) {
+        desiredSpeed = speed;
+        integral = 0;
+    }
+    return;
+}
+
+/** schedHandler fires off the various routines at appropriate schedules
+ *
+ */
+void update()
+{
+    tReal = timer.read_us();
+
+    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
+    speedDt += dt;
+    
+    // Log Data Timestamp    
+    int timestamp = timer.read_ms();
+
+    //////////////////////////////////////////////////////////////////////////////
+    // NAVIGATION INIT
+    //////////////////////////////////////////////////////////////////////////////
+    // initNav is set with call to restartNav() when the "go" button is pressed.  Up
+    // to 10ms later this function is called and the code below will initialize the
+    // dead reckoning position and estimated position variables
+    // 
+    if (initNav == true) {
+        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
+        // Initialize lag estimates
+        //lagHere.set( here );
+        // Initialize fifo
+        hCount = 0;
+        now = 0;
+        // initialize what will become lag data in 1 second from now
+        history[now].dt = 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;
+        // Initialize Kalman Filter
+        headingKalmanInit(history[now].hdg);
+        // initialize cross track error
+        cte = 0;
+        cteI = 0;
+        // point next fifo input to slot 1, slot 0 occupied/initialized, now
+        lag = 0;
+        lagPrev = 0;
+        prev = now; // point to the most recently entered data
+        now = 1;    // new input slot
+    }
+
+
+    //////////////////////////////////////////////////////////////////////////////
+    // SENSOR UPDATES
+    //////////////////////////////////////////////////////////////////////////////
+
+    // TODO: 3 This really should run infrequently
+    sensors.Read_Power();
+
+    sensors.Read_Encoders(); 
+    nowSpeed = sensors.encSpeed;
+
+    sensors.Read_Gyro(); 
+
+    sensors.Read_Rangers();
+
+    //sensors.Read_Camera();
+
+    //////////////////////////////////////////////////////////////////////////////
+    // Obtain GPS data                        
+    //////////////////////////////////////////////////////////////////////////////
+
+    // synchronize when RMC and GGA sentences received w/ AHRS
+    // Do this in schedHandler??  GPS data is coming in not entirely in sync
+    // with the logging info
+    if (gps.nmea.rmc_ready() && gps.nmea.gga_ready()) {
+        char gpsdate[32], gpstime[32];
+
+        gps.process(gps_here, gpsdate, gpstime);
+        //gpsStatus = (gps.hdop > 0.0 && gps.hdop < 3.0) ? 1 : 0;
+
+        // update system status struct for logging
+        state[inState].gpsLatitude = gps_here.latitude();
+        state[inState].gpsLongitude = gps_here.longitude();
+        state[inState].gpsHDOP = gps.hdop;
+        state[inState].gpsCourse = gps.nmea.f_course();
+        state[inState].gpsSpeed = gps.nmea.f_speed_mph(); // can we just do kph/1000 ? or mps?
+        state[inState].gpsSats = gps.nmea.sat_count();
+    }
+    
+    //////////////////////////////////////////////////////////////////////////////
+    // HEADING AND POSITION UPDATE
+    //////////////////////////////////////////////////////////////////////////////
+
+    // TODO: 2 Position filtering
+    //    position will be updated based on heading error from heading estimate
+    // TODO: 2 Distance/speed filtering
+    //    this might be useful, but not sure it's worth the effort
+
+    // So the big pain in the ass is that the GPS data coming in represents the
+    // state of the system ~1s ago. Yes, a full second of lag despite running
+    // at 10hz (or whatever).  So if we try and fuse a lagged gps heading with a
+    // relatively current gyro heading rate, the gyro is ignored and the heading
+    // estimate lags reality
+    //
+    // In real life testing, the robot steering control was highly unstable with
+    // too much gain (typical of any negative feedback system with a very high
+    // phase shift and too much feedback). It'd drive around in circles trying to
+    // hunt for the next waypoint.  Dropping the gain restored stability but the
+    // steering overshot significantly, because of the lag.  It sort of worked but
+    // it was ugly. So here's my lame attempt to fix all this. 
+    // 
+    // 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;
+    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);
+
+    // We can't do anything until the history buffer is full
+    if (hCount < 100) {
+        // 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
+        // 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
+        
+        ////////////////////////////////////////////////////////////////////////////////
+        // UPDATE LAGGED ESTIMATE
+        
+        // Recover data from 1 second ago which will be used to generate
+        // updated lag estimates
+
+        // GPS heading is unavailable from this particular GPS at speeds < 0.5 mph
+        bool useGps = (state[inState].gpsLatitude != 0 &&
+                       state[inState].lrEncSpeed > 1.0 &&
+                       state[inState].rrEncSpeed > 1.0 &&
+                       (thisTime-timeZero) > 3000); // gps hdg converges by 3-5 sec.
+
+        // This represents the best estimate for heading... for one second ago
+        // If we do nothing else, the robot will think it is located at a position 1 second behind
+        // where it actually is. That means that any control feedback needs to have a longer time
+        // constant than 1 sec or the robot will have unstable heading correctoin.
+        // Use gps data when available, always use gyro data
+
+        // 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
+        if (go) {
+            lagHeading = headingKalman(history[lag].dt, state[inState].gpsCourse, useGps, history[lag].gyro, true);
+        } else {    
+            lagHeading = headingKalman(history[lag].dt, initialHeading, true, history[lag].gyro, true);
+        }
+
+        // TODO 1: need to figure out exactly how to update t-1 position correctly
+        // 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);
+        
+        ////////////////////////////////////////////////////////////////////////////////
+        // 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.
+        //        
+        // Nuance: lag and now have already been incremented so that works out beautifully for this loop
+        //
+        // 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,
+        // so is 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.
+        //
+        // initialize these once
+        errAngle = (lagHeading - 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
+        int i = lag;
+        for (int j=0; j < 100; j++) {
+            history[i].hdg += errAngle;
+            // 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;
+                float dy = history[i].y-history[lag].y;
+                history[i].x = history[lag].x + dx*cosA - dy*sinA;
+                history[i].y = history[lag].y + dx*sinA + dy*cosA;
+            }
+            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
+        if (history[lag].dt > 0 && fabs(steerAngle) < 5.0 && useGps) {
+            biasErrAngle = Kbias*biasErrAngle + (1-Kbias)*(history[lag].ghdg - state[inState].gpsCourse); // can use this to compute gyro bias
+            if (biasErrAngle <= -180.0) biasErrAngle += 360.0;
+            if (biasErrAngle > 180) biasErrAngle -= 360.0;
+        
+            float errRate = biasErrAngle / history[lag].dt;
+
+            //if (!go) errRate = history[lag].gyro;
+
+            // Filter 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);
+    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
+    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
+    if (go) {
+
+        // 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) {
+            setSpeed( config.turnSpeed );
+        } else if ( (thisTime - timeZero) < 1000 ) {
+            setSpeed( config.startSpeed );
+        } else {
+            setSpeed( config.topSpeed );
+        }
+
+        if (distance < config.waypointDist) {
+            //fprintf(stdout, "Arrived at wpt %d\n", nextWaypoint);
+            //speaker.beep(3000.0, 0.5); // non-blocking
+            lastWaypoint = nextWaypoint;
+            nextWaypoint++;
+            cteI = 0;
+        }
+        
+    } else {
+        setSpeed( 0.0 );
+    }
+    // Are we at the last waypoint?
+    // currently handled external to this routine
+        
+    //////////////////////////////////////////////////////////////////////////////
+    // OBSTACLE DETECTION & AVOIDANCE
+    //////////////////////////////////////////////////////////////////////////////
+    // TODO: 1 limit steering angle based on object detection ?
+    // or limit relative brg perhaps?
+    // TODO: 1 add vision obstacle detection and filtering
+    // TODO: 1 add ranger obstacle detection and filtering/fusion with vision
+
+
+    //////////////////////////////////////////////////////////////////////////////
+    // CONTROL UPDATE
+    //////////////////////////////////////////////////////////////////////////////
+
+    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;
+
+        // Use either pure pursuit algorithm or original algorithm  
+        if (config.usePP) {
+            steerAngle = steerCalc.purePursuitSA(state[inState].estHeading, 
+                                                 history[now].x, history[now].y,
+                                                 config.cwpt[lastWaypoint]._x, config.cwpt[lastWaypoint]._y,
+                                                 config.cwpt[nextWaypoint]._x, config.cwpt[nextWaypoint]._y);
+        } else {
+            steerAngle = steerCalc.calcSA(relativeBrg, config.minRadius); // use the configured minimum turn radius
+        }
+        
+        // Apply gain factor for near straight line
+        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
+        // if nothing else, at least average the encoder speed over mult. samples
+        if (desiredSpeed <= 0.1) {
+            setThrottle( config.escZero );
+        } else {
+            // PID loop for throttle control
+            // http://www.codeproject.com/Articles/36459/PID-process-control-a-Cruise-Control-example
+            float error = desiredSpeed - nowSpeed; 
+            // track error over time, scaled to the timer interval
+            integral += (error * speedDt);
+            // determine the amount of change from the last time checked
+            float derivative = (error - lastError) / speedDt; 
+            // calculate how much to drive the output in order to get to the 
+            // desired setpoint. 
+            int output = config.escZero + (config.speedKp * error) + (config.speedKi * integral) + (config.speedKd * derivative);
+            if (output > config.escMax) output = config.escMax;
+            if (output < config.escMin) output = config.escMin;
+            //fprintf(stdout, "s=%.1f d=%.1f o=%d\n", nowSpeed, desiredSpeed, output);
+            setThrottle( output );
+            // remember the error for the next time around.
+            lastError = error; 
+        }
+
+        speedDt = 0; // reset dt to begin counting for next time
+        control_count = CTRL_SKIP;
+    }      
+
+    //////////////////////////////////////////////////////////////////////////////
+    // DATA FOR LOGGING
+    //////////////////////////////////////////////////////////////////////////////
+
+    // 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) {
+        // 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.
+        state[inState].lrEncDistance = 0;
+        state[inState].rrEncDistance = 0;
+        //
+        // need to initialize gps data to be safe
+        //
+        state[inState].gpsLatitude = 0;
+        state[inState].gpsLongitude = 0;
+        state[inState].gpsHDOP = 0;
+        state[inState].gpsCourse = 0;
+        state[inState].gpsSpeed = 0;
+        state[inState].gpsSats = 0;
+        log_count = LOG_SKIP;       // reset counter
+    }
+
+    // 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;
+    for (int i=0; i < 3; i++) {
+        state[inState].m[i] = sensors.m[i];
+        state[inState].g[i] = sensors.g[i];
+        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].estLatitude = here.latitude();
+    state[inState].estLongitude = here.longitude();
+    state[inState].estX = history[now].x;
+    state[inState].estY = history[now].y;
+    state[inState].bearing = bearing;
+    state[inState].distance = distance;
+    state[inState].nextWaypoint = nextWaypoint;
+    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;
+    // Copy AHRS data into logging data
+    //state[inState].roll = ToDeg(ahrs.roll);
+    //state[inState].pitch = ToDeg(ahrs.pitch);
+    //state[inState].yaw = ToDeg(ahrs.yaw);
+
+    // increment fifo pointers with wrap-around
+    prev = now;
+    inc(now);
+
+    // timing
+    tReal = timer.read_us() - tReal;
+
+    ahrsStatus = 1;
+}
\ No newline at end of file