Code for autonomous ground vehicle, Data Bus, 3rd place winner in 2012 Sparkfun AVC.
Dependencies: Watchdog mbed Schedule SimpleFilter LSM303DLM PinDetect DebounceIn Servo
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