Michael Shimniok / Mbed 2 deprecated AVC_2012

Code for autonomous ground vehicle, Data Bus, 3rd place winner in 2012 Sparkfun AVC.

Dependencies:   Watchdog mbed Schedule SimpleFilter LSM303DLM PinDetect DebounceIn Servo

updater.c@0:826c6171fc1b, 2012-06-20 (annotated)

Committer:
shimniok
Date:
Wed Jun 20 14:57:48 2012 +0000
Revision:
0:826c6171fc1b
Updated documentation

Who changed what in which revision?

UserRevisionLine numberNew contents of line
shimniok 0:826c6171fc1b 1 #include "Config.h"
shimniok 0:826c6171fc1b 2 #include "Actuators.h"
shimniok 0:826c6171fc1b 3 #include "Sensors.h"
shimniok 0:826c6171fc1b 4 #include "SystemState.h"
shimniok 0:826c6171fc1b 5 #include "GPS.h"
shimniok 0:826c6171fc1b 6 #include "Steering.h"
shimniok 0:826c6171fc1b 7 #include "Servo.h"
shimniok 0:826c6171fc1b 8 #include "Mapping.h"
shimniok 0:826c6171fc1b 9 #include "CartPosition.h"
shimniok 0:826c6171fc1b 10 #include "GeoPosition.h"
shimniok 0:826c6171fc1b 11 #include "kalman.h"
shimniok 0:826c6171fc1b 12
shimniok 0:826c6171fc1b 13 #define _x_ 0
shimniok 0:826c6171fc1b 14 #define _y_ 1
shimniok 0:826c6171fc1b 15 #define _z_ 2
shimniok 0:826c6171fc1b 16
shimniok 0:826c6171fc1b 17 // Every ...
shimniok 0:826c6171fc1b 18 // 10ms (100Hz) -- update gyro, encoders; update AHRS; update navigation; set log data
shimniok 0:826c6171fc1b 19 // 10ms (100Hz) -- camera (actually runs 30fps
shimniok 0:826c6171fc1b 20 // 20ms (50Hz) -- update compass sensor
shimniok 0:826c6171fc1b 21 // 50ms (20Hz) -- create new log entry in log FIFO
shimniok 0:826c6171fc1b 22 // 100ms (10Hz) -- GPS update
shimniok 0:826c6171fc1b 23 // 100ms (10Hz) -- control update
shimniok 0:826c6171fc1b 24
shimniok 0:826c6171fc1b 25 #define CTRL_SKIP 5
shimniok 0:826c6171fc1b 26 #define MAG_SKIP 2
shimniok 0:826c6171fc1b 27 #define LOG_SKIP 5
shimniok 0:826c6171fc1b 28
shimniok 0:826c6171fc1b 29 int control_count=CTRL_SKIP;
shimniok 0:826c6171fc1b 30 int update_count=MAG_SKIP; // call Update_mag() every update_count calls to schedHandler()
shimniok 0:826c6171fc1b 31 int log_count=LOG_SKIP; // buffer a new status entry for logging every log_count calls to schedHandler
shimniok 0:826c6171fc1b 32 int tReal; // calculate real elapsed time
shimniok 0:826c6171fc1b 33
shimniok 0:826c6171fc1b 34 // TODO: 3 better encapsulation, please
shimniok 0:826c6171fc1b 35 extern Sensors sensors;
shimniok 0:826c6171fc1b 36 extern SystemState state[SSBUF];
shimniok 0:826c6171fc1b 37 extern unsigned char inState;
shimniok 0:826c6171fc1b 38 extern unsigned char outState;
shimniok 0:826c6171fc1b 39 extern bool ssBufOverrun;
shimniok 0:826c6171fc1b 40 extern GPS gps;
shimniok 0:826c6171fc1b 41 extern Mapping mapper;
shimniok 0:826c6171fc1b 42 extern Steering steerCalc; // steering calculator
shimniok 0:826c6171fc1b 43 extern Timer timer;
shimniok 0:826c6171fc1b 44 extern DigitalOut ahrsStatus; // AHRS status LED
shimniok 0:826c6171fc1b 45
shimniok 0:826c6171fc1b 46 // Navigation
shimniok 0:826c6171fc1b 47 extern Config config;
shimniok 0:826c6171fc1b 48 int nextWaypoint = 0; // next waypoint destination
shimniok 0:826c6171fc1b 49 int lastWaypoint = 1;
shimniok 0:826c6171fc1b 50 double bearing; // bearing to next waypoint
shimniok 0:826c6171fc1b 51 double distance; // distance to next waypoint
shimniok 0:826c6171fc1b 52 float steerAngle; // steering angle
shimniok 0:826c6171fc1b 53 float cte; // Cross Track error
shimniok 0:826c6171fc1b 54 float cteI; // Integral of Cross Track Error
shimniok 0:826c6171fc1b 55
shimniok 0:826c6171fc1b 56 // Throttle PID
shimniok 0:826c6171fc1b 57 float speedDt=0; // dt for the speed PID
shimniok 0:826c6171fc1b 58 float integral=0; // error integral for speed PID
shimniok 0:826c6171fc1b 59 float lastError=0; // previous error, used for calculating derivative
shimniok 0:826c6171fc1b 60 bool go=false; // initiate throttle (or not)
shimniok 0:826c6171fc1b 61 float desiredSpeed; // speed set point
shimniok 0:826c6171fc1b 62 float nowSpeed;
shimniok 0:826c6171fc1b 63
shimniok 0:826c6171fc1b 64
shimniok 0:826c6171fc1b 65 // Pose Estimation
shimniok 0:826c6171fc1b 66 bool initNav = true;
shimniok 0:826c6171fc1b 67 float initHdg = true; // indiciates that heading needs to be initialized by gps
shimniok 0:826c6171fc1b 68 float initialHeading=-999; // initial heading
shimniok 0:826c6171fc1b 69 float myHeading=0; // heading sent to KF
shimniok 0:826c6171fc1b 70 CartPosition cartHere; // position estimate, cartesian
shimniok 0:826c6171fc1b 71 GeoPosition here; // position estimate, lat/lon
shimniok 0:826c6171fc1b 72 extern GeoPosition gps_here;
shimniok 0:826c6171fc1b 73 int timeZero=0;
shimniok 0:826c6171fc1b 74 int lastTime=-1; // used to calculate dt for KF
shimniok 0:826c6171fc1b 75 int thisTime; // used to calculate dt for KF
shimniok 0:826c6171fc1b 76 float dt; // dt for the KF
shimniok 0:826c6171fc1b 77 float lagHeading = 0; // lagged heading estimate
shimniok 0:826c6171fc1b 78 //GeoPosition lagHere; // lagged position estimate; use here as the current position estimate
shimniok 0:826c6171fc1b 79 float errAngle; // error between gyro hdg estimate and gps hdg estimate
shimniok 0:826c6171fc1b 80 float gyroBias=0; // exponentially weighted moving average of gyro error
shimniok 0:826c6171fc1b 81 float Ag = (2.0/(1000.0+1.0)); // Gyro bias filter alpha, gyro; # of 10ms steps
shimniok 0:826c6171fc1b 82 float Kbias = 0.995;
shimniok 0:826c6171fc1b 83 float filtErrRate = 0;
shimniok 0:826c6171fc1b 84 float biasErrAngle = 0;
shimniok 0:826c6171fc1b 85
shimniok 0:826c6171fc1b 86 #define MAXHIST 128 // must be multiple of 0x08
shimniok 0:826c6171fc1b 87 #define inc(x) (x) = ((x)+1)&(MAXHIST-1)
shimniok 0:826c6171fc1b 88 struct {
shimniok 0:826c6171fc1b 89 float x; // x coordinate
shimniok 0:826c6171fc1b 90 float y; // y coordinate
shimniok 0:826c6171fc1b 91 float hdg; // heading
shimniok 0:826c6171fc1b 92 float dist; // distance
shimniok 0:826c6171fc1b 93 float gyro; // heading rate
shimniok 0:826c6171fc1b 94 float ghdg; // uncorrected gyro heading
shimniok 0:826c6171fc1b 95 float dt; // delta time
shimniok 0:826c6171fc1b 96 } history[MAXHIST]; // fifo for sensor data, position, heading, dt
shimniok 0:826c6171fc1b 97
shimniok 0:826c6171fc1b 98 int hCount=0; // history counter; one > 100, we can go back in time to reference gyro history
shimniok 0:826c6171fc1b 99 int now = 0; // fifo input index
shimniok 0:826c6171fc1b 100 int prev = 0; // previous fifo iput index
shimniok 0:826c6171fc1b 101 int lag = 0; // fifo output index
shimniok 0:826c6171fc1b 102 int lagPrev = 0; // previous fifo output index
shimniok 0:826c6171fc1b 103
shimniok 0:826c6171fc1b 104
shimniok 0:826c6171fc1b 105 /** set flag to initialize navigation at next schedHandler() call
shimniok 0:826c6171fc1b 106 */
shimniok 0:826c6171fc1b 107 void restartNav()
shimniok 0:826c6171fc1b 108 {
shimniok 0:826c6171fc1b 109 initNav = true;
shimniok 0:826c6171fc1b 110 initHdg = true;
shimniok 0:826c6171fc1b 111 return;
shimniok 0:826c6171fc1b 112 }
shimniok 0:826c6171fc1b 113
shimniok 0:826c6171fc1b 114 /** instruct the controller to throttle up
shimniok 0:826c6171fc1b 115 */
shimniok 0:826c6171fc1b 116 void beginRun()
shimniok 0:826c6171fc1b 117 {
shimniok 0:826c6171fc1b 118 go = true;
shimniok 0:826c6171fc1b 119 timeZero = thisTime; // initialize
shimniok 0:826c6171fc1b 120 return;
shimniok 0:826c6171fc1b 121 }
shimniok 0:826c6171fc1b 122
shimniok 0:826c6171fc1b 123 void endRun()
shimniok 0:826c6171fc1b 124 {
shimniok 0:826c6171fc1b 125 go = false;
shimniok 0:826c6171fc1b 126 initNav = true;
shimniok 0:826c6171fc1b 127 return;
shimniok 0:826c6171fc1b 128 }
shimniok 0:826c6171fc1b 129
shimniok 0:826c6171fc1b 130 /** get elasped time in update loop
shimniok 0:826c6171fc1b 131 */
shimniok 0:826c6171fc1b 132 int getUpdateTime()
shimniok 0:826c6171fc1b 133 {
shimniok 0:826c6171fc1b 134 return tReal;
shimniok 0:826c6171fc1b 135 }
shimniok 0:826c6171fc1b 136
shimniok 0:826c6171fc1b 137 void setSpeed(float speed)
shimniok 0:826c6171fc1b 138 {
shimniok 0:826c6171fc1b 139 if (desiredSpeed != speed) {
shimniok 0:826c6171fc1b 140 desiredSpeed = speed;
shimniok 0:826c6171fc1b 141 integral = 0;
shimniok 0:826c6171fc1b 142 }
shimniok 0:826c6171fc1b 143 return;
shimniok 0:826c6171fc1b 144 }
shimniok 0:826c6171fc1b 145
shimniok 0:826c6171fc1b 146 /** schedHandler fires off the various routines at appropriate schedules
shimniok 0:826c6171fc1b 147 *
shimniok 0:826c6171fc1b 148 */
shimniok 0:826c6171fc1b 149 void update()
shimniok 0:826c6171fc1b 150 {
shimniok 0:826c6171fc1b 151 tReal = timer.read_us();
shimniok 0:826c6171fc1b 152
shimniok 0:826c6171fc1b 153 ahrsStatus = 0;
shimniok 0:826c6171fc1b 154 thisTime = timer.read_ms();
shimniok 0:826c6171fc1b 155 dt = (lastTime < 0) ? 0 : ((float) thisTime - (float) lastTime) / 1000.0; // first pass let dt=0
shimniok 0:826c6171fc1b 156 lastTime = thisTime;
shimniok 0:826c6171fc1b 157
shimniok 0:826c6171fc1b 158 // Add up dt to speedDt
shimniok 0:826c6171fc1b 159 speedDt += dt;
shimniok 0:826c6171fc1b 160
shimniok 0:826c6171fc1b 161 // Log Data Timestamp
shimniok 0:826c6171fc1b 162 int timestamp = timer.read_ms();
shimniok 0:826c6171fc1b 163
shimniok 0:826c6171fc1b 164 //////////////////////////////////////////////////////////////////////////////
shimniok 0:826c6171fc1b 165 // NAVIGATION INIT
shimniok 0:826c6171fc1b 166 //////////////////////////////////////////////////////////////////////////////
shimniok 0:826c6171fc1b 167 // initNav is set with call to restartNav() when the "go" button is pressed. Up
shimniok 0:826c6171fc1b 168 // to 10ms later this function is called and the code below will initialize the
shimniok 0:826c6171fc1b 169 // dead reckoning position and estimated position variables
shimniok 0:826c6171fc1b 170 //
shimniok 0:826c6171fc1b 171 if (initNav == true) {
shimniok 0:826c6171fc1b 172 initNav = false;
shimniok 0:826c6171fc1b 173 here.set(config.wpt[0]);
shimniok 0:826c6171fc1b 174 nextWaypoint = 1; // Point to the next waypoint; 0th wpt is the starting point
shimniok 0:826c6171fc1b 175 lastWaypoint = 1; // Init to waypoint 1, we we don't start from wpt 0 at the turning speed
shimniok 0:826c6171fc1b 176 // Initialize lag estimates
shimniok 0:826c6171fc1b 177 //lagHere.set( here );
shimniok 0:826c6171fc1b 178 // Initialize fifo
shimniok 0:826c6171fc1b 179 hCount = 0;
shimniok 0:826c6171fc1b 180 now = 0;
shimniok 0:826c6171fc1b 181 // initialize what will become lag data in 1 second from now
shimniok 0:826c6171fc1b 182 history[now].dt = 0;
shimniok 0:826c6171fc1b 183 // initial position is waypoint 0
shimniok 0:826c6171fc1b 184 history[now].x = config.cwpt[0]._x;
shimniok 0:826c6171fc1b 185 history[now].y = config.cwpt[0]._y;
shimniok 0:826c6171fc1b 186 cartHere.set(history[now].x, history[now].y);
shimniok 0:826c6171fc1b 187 // initialize heading to bearing between waypoint 0 and 1
shimniok 0:826c6171fc1b 188 //history[now].hdg = here.bearingTo(config.wpt[nextWaypoint]);
shimniok 0:826c6171fc1b 189 history[now].hdg = cartHere.bearingTo(config.cwpt[nextWaypoint]);
shimniok 0:826c6171fc1b 190 history[now].ghdg = history[now].hdg;
shimniok 0:826c6171fc1b 191 initialHeading = history[now].hdg;
shimniok 0:826c6171fc1b 192 // Initialize Kalman Filter
shimniok 0:826c6171fc1b 193 headingKalmanInit(history[now].hdg);
shimniok 0:826c6171fc1b 194 // initialize cross track error
shimniok 0:826c6171fc1b 195 cte = 0;
shimniok 0:826c6171fc1b 196 cteI = 0;
shimniok 0:826c6171fc1b 197 // point next fifo input to slot 1, slot 0 occupied/initialized, now
shimniok 0:826c6171fc1b 198 lag = 0;
shimniok 0:826c6171fc1b 199 lagPrev = 0;
shimniok 0:826c6171fc1b 200 prev = now; // point to the most recently entered data
shimniok 0:826c6171fc1b 201 now = 1; // new input slot
shimniok 0:826c6171fc1b 202 }
shimniok 0:826c6171fc1b 203
shimniok 0:826c6171fc1b 204
shimniok 0:826c6171fc1b 205 //////////////////////////////////////////////////////////////////////////////
shimniok 0:826c6171fc1b 206 // SENSOR UPDATES
shimniok 0:826c6171fc1b 207 //////////////////////////////////////////////////////////////////////////////
shimniok 0:826c6171fc1b 208
shimniok 0:826c6171fc1b 209 // TODO: 3 This really should run infrequently
shimniok 0:826c6171fc1b 210 sensors.Read_Power();
shimniok 0:826c6171fc1b 211
shimniok 0:826c6171fc1b 212 sensors.Read_Encoders();
shimniok 0:826c6171fc1b 213 nowSpeed = sensors.encSpeed;
shimniok 0:826c6171fc1b 214
shimniok 0:826c6171fc1b 215 sensors.Read_Gyro();
shimniok 0:826c6171fc1b 216
shimniok 0:826c6171fc1b 217 sensors.Read_Rangers();
shimniok 0:826c6171fc1b 218
shimniok 0:826c6171fc1b 219 //sensors.Read_Camera();
shimniok 0:826c6171fc1b 220
shimniok 0:826c6171fc1b 221 //////////////////////////////////////////////////////////////////////////////
shimniok 0:826c6171fc1b 222 // Obtain GPS data
shimniok 0:826c6171fc1b 223 //////////////////////////////////////////////////////////////////////////////
shimniok 0:826c6171fc1b 224
shimniok 0:826c6171fc1b 225 // synchronize when RMC and GGA sentences received w/ AHRS
shimniok 0:826c6171fc1b 226 // Do this in schedHandler?? GPS data is coming in not entirely in sync
shimniok 0:826c6171fc1b 227 // with the logging info
shimniok 0:826c6171fc1b 228 if (gps.nmea.rmc_ready() && gps.nmea.gga_ready()) {
shimniok 0:826c6171fc1b 229 char gpsdate[32], gpstime[32];
shimniok 0:826c6171fc1b 230
shimniok 0:826c6171fc1b 231 gps.process(gps_here, gpsdate, gpstime);
shimniok 0:826c6171fc1b 232 //gpsStatus = (gps.hdop > 0.0 && gps.hdop < 3.0) ? 1 : 0;
shimniok 0:826c6171fc1b 233
shimniok 0:826c6171fc1b 234 // update system status struct for logging
shimniok 0:826c6171fc1b 235 state[inState].gpsLatitude = gps_here.latitude();
shimniok 0:826c6171fc1b 236 state[inState].gpsLongitude = gps_here.longitude();
shimniok 0:826c6171fc1b 237 state[inState].gpsHDOP = gps.hdop;
shimniok 0:826c6171fc1b 238 state[inState].gpsCourse = gps.nmea.f_course();
shimniok 0:826c6171fc1b 239 state[inState].gpsSpeed = gps.nmea.f_speed_mph(); // can we just do kph/1000 ? or mps?
shimniok 0:826c6171fc1b 240 state[inState].gpsSats = gps.nmea.sat_count();
shimniok 0:826c6171fc1b 241 }
shimniok 0:826c6171fc1b 242
shimniok 0:826c6171fc1b 243 //////////////////////////////////////////////////////////////////////////////
shimniok 0:826c6171fc1b 244 // HEADING AND POSITION UPDATE
shimniok 0:826c6171fc1b 245 //////////////////////////////////////////////////////////////////////////////
shimniok 0:826c6171fc1b 246
shimniok 0:826c6171fc1b 247 // TODO: 2 Position filtering
shimniok 0:826c6171fc1b 248 // position will be updated based on heading error from heading estimate
shimniok 0:826c6171fc1b 249 // TODO: 2 Distance/speed filtering
shimniok 0:826c6171fc1b 250 // this might be useful, but not sure it's worth the effort
shimniok 0:826c6171fc1b 251
shimniok 0:826c6171fc1b 252 // So the big pain in the ass is that the GPS data coming in represents the
shimniok 0:826c6171fc1b 253 // state of the system ~1s ago. Yes, a full second of lag despite running
shimniok 0:826c6171fc1b 254 // at 10hz (or whatever). So if we try and fuse a lagged gps heading with a
shimniok 0:826c6171fc1b 255 // relatively current gyro heading rate, the gyro is ignored and the heading
shimniok 0:826c6171fc1b 256 // estimate lags reality
shimniok 0:826c6171fc1b 257 //
shimniok 0:826c6171fc1b 258 // In real life testing, the robot steering control was highly unstable with
shimniok 0:826c6171fc1b 259 // too much gain (typical of any negative feedback system with a very high
shimniok 0:826c6171fc1b 260 // phase shift and too much feedback). It'd drive around in circles trying to
shimniok 0:826c6171fc1b 261 // hunt for the next waypoint. Dropping the gain restored stability but the
shimniok 0:826c6171fc1b 262 // steering overshot significantly, because of the lag. It sort of worked but
shimniok 0:826c6171fc1b 263 // it was ugly. So here's my lame attempt to fix all this.
shimniok 0:826c6171fc1b 264 //
shimniok 0:826c6171fc1b 265 // We'll find out how much error there is between gps heading and the integrated
shimniok 0:826c6171fc1b 266 // gyro heading from a second ago.
shimniok 0:826c6171fc1b 267
shimniok 0:826c6171fc1b 268 // stick precalculated gyro data, with bias correction, into fifo
shimniok 0:826c6171fc1b 269 //history[now].gyro = sensors.gyro[_z_] - gyroBias;
shimniok 0:826c6171fc1b 270 history[now].gyro = sensors.gyro[_z_];
shimniok 0:826c6171fc1b 271
shimniok 0:826c6171fc1b 272 // Have to store dt history too (feh)
shimniok 0:826c6171fc1b 273 history[now].dt = dt;
shimniok 0:826c6171fc1b 274
shimniok 0:826c6171fc1b 275 // Store distance travelled in a fifo for later use
shimniok 0:826c6171fc1b 276 history[now].dist = (sensors.lrEncDistance + sensors.rrEncDistance) / 2.0;
shimniok 0:826c6171fc1b 277
shimniok 0:826c6171fc1b 278
shimniok 0:826c6171fc1b 279 // Calc and store heading
shimniok 0:826c6171fc1b 280 history[now].ghdg = history[prev].ghdg + dt*history[now].gyro; // raw gyro calculated heading
shimniok 0:826c6171fc1b 281 //history[now].hdg = history[prev].ghdg - dt*gyroBias; // bias-corrected gyro heading
shimniok 0:826c6171fc1b 282 history[now].hdg = history[prev].hdg + dt*history[now].gyro;
shimniok 0:826c6171fc1b 283 if (history[now].hdg >= 360.0) history[now].hdg -= 360.0;
shimniok 0:826c6171fc1b 284 if (history[now].hdg < 0) history[now].hdg += 360.0;
shimniok 0:826c6171fc1b 285
shimniok 0:826c6171fc1b 286 //fprintf(stdout, "%d %d %.4f %.4f\n", now, prev, history[now].hdg, history[prev].hdg);
shimniok 0:826c6171fc1b 287
shimniok 0:826c6171fc1b 288 // We can't do anything until the history buffer is full
shimniok 0:826c6171fc1b 289 if (hCount < 100) {
shimniok 0:826c6171fc1b 290 // Until the fifo is full, only keep track of current gyro heading
shimniok 0:826c6171fc1b 291 hCount++; // after n iterations the fifo will be full
shimniok 0:826c6171fc1b 292 } else {
shimniok 0:826c6171fc1b 293 // Now that the buffer is full, we'll maintain a Kalman Filtered estimate that is
shimniok 0:826c6171fc1b 294 // time-shifted to the past because the GPS output represents the system state from
shimniok 0:826c6171fc1b 295 // the past. We'll also take our history of gyro readings from the most recent
shimniok 0:826c6171fc1b 296 // filter update to the present time and update our current heading and position
shimniok 0:826c6171fc1b 297
shimniok 0:826c6171fc1b 298 ////////////////////////////////////////////////////////////////////////////////
shimniok 0:826c6171fc1b 299 // UPDATE LAGGED ESTIMATE
shimniok 0:826c6171fc1b 300
shimniok 0:826c6171fc1b 301 // Recover data from 1 second ago which will be used to generate
shimniok 0:826c6171fc1b 302 // updated lag estimates
shimniok 0:826c6171fc1b 303
shimniok 0:826c6171fc1b 304 // GPS heading is unavailable from this particular GPS at speeds < 0.5 mph
shimniok 0:826c6171fc1b 305 bool useGps = (state[inState].gpsLatitude != 0 &&
shimniok 0:826c6171fc1b 306 state[inState].lrEncSpeed > 1.0 &&
shimniok 0:826c6171fc1b 307 state[inState].rrEncSpeed > 1.0 &&
shimniok 0:826c6171fc1b 308 (thisTime-timeZero) > 3000); // gps hdg converges by 3-5 sec.
shimniok 0:826c6171fc1b 309
shimniok 0:826c6171fc1b 310 // This represents the best estimate for heading... for one second ago
shimniok 0:826c6171fc1b 311 // If we do nothing else, the robot will think it is located at a position 1 second behind
shimniok 0:826c6171fc1b 312 // where it actually is. That means that any control feedback needs to have a longer time
shimniok 0:826c6171fc1b 313 // constant than 1 sec or the robot will have unstable heading correctoin.
shimniok 0:826c6171fc1b 314 // Use gps data when available, always use gyro data
shimniok 0:826c6171fc1b 315
shimniok 0:826c6171fc1b 316 // Clamp heading to initial heading when we're not moving; hopefully this will
shimniok 0:826c6171fc1b 317 // give the KF a head start figuring out how to deal with the gyro
shimniok 0:826c6171fc1b 318 if (go) {
shimniok 0:826c6171fc1b 319 lagHeading = headingKalman(history[lag].dt, state[inState].gpsCourse, useGps, history[lag].gyro, true);
shimniok 0:826c6171fc1b 320 } else {
shimniok 0:826c6171fc1b 321 lagHeading = headingKalman(history[lag].dt, initialHeading, true, history[lag].gyro, true);
shimniok 0:826c6171fc1b 322 }
shimniok 0:826c6171fc1b 323
shimniok 0:826c6171fc1b 324 // TODO 1: need to figure out exactly how to update t-1 position correctly
shimniok 0:826c6171fc1b 325 // Update the lagged position estimate
shimniok 0:826c6171fc1b 326 //lagHere.move(lagHeading, history[lag].dist);
shimniok 0:826c6171fc1b 327 history[lag].x = history[lagPrev].x + history[lag].dist * sin(lagHeading);
shimniok 0:826c6171fc1b 328 history[lag].y = history[lagPrev].y + history[lag].dist * cos(lagHeading);
shimniok 0:826c6171fc1b 329
shimniok 0:826c6171fc1b 330 ////////////////////////////////////////////////////////////////////////////////
shimniok 0:826c6171fc1b 331 // UPDATE CURRENT ESTIMATE
shimniok 0:826c6171fc1b 332
shimniok 0:826c6171fc1b 333 // Now we need to re-calculate the current heading and position, starting with the most recent
shimniok 0:826c6171fc1b 334 // heading estimate representing the heading 1 second ago.
shimniok 0:826c6171fc1b 335 //
shimniok 0:826c6171fc1b 336 // Nuance: lag and now have already been incremented so that works out beautifully for this loop
shimniok 0:826c6171fc1b 337 //
shimniok 0:826c6171fc1b 338 // Heading is easy. Original heading - estimated heading gives a tiny error. Add this to all the historical
shimniok 0:826c6171fc1b 339 // heading data up to present.
shimniok 0:826c6171fc1b 340 //
shimniok 0:826c6171fc1b 341 // For position re-calculation, we iterate 100 times up to present record. Haversine is way, way too slow,
shimniok 0:826c6171fc1b 342 // so is trig calcs is marginal. Update rate is 10ms and we can't hog more than maybe 2-3ms as the outer
shimniok 0:826c6171fc1b 343 // loop has logging work to do. Rotating each point is faster; pre-calculate sin/cos, etc.
shimniok 0:826c6171fc1b 344 //
shimniok 0:826c6171fc1b 345 // initialize these once
shimniok 0:826c6171fc1b 346 errAngle = (lagHeading - history[lag].hdg);
shimniok 0:826c6171fc1b 347 if (errAngle <= -180.0) errAngle += 360.0;
shimniok 0:826c6171fc1b 348 if (errAngle > 180) errAngle -= 360.0;
shimniok 0:826c6171fc1b 349
shimniok 0:826c6171fc1b 350 //fprintf(stdout, "%d %.2f, %.2f, %.4f %.4f\n", lag, lagHeading, history[lag].hdg, lagHeading - history[lag].hdg, errAngle);
shimniok 0:826c6171fc1b 351 float cosA = cos(errAngle * PI / 180);
shimniok 0:826c6171fc1b 352 float sinA = sin(errAngle * PI / 180);
shimniok 0:826c6171fc1b 353 // Start at the out side of the fifo which is from 1 second ago
shimniok 0:826c6171fc1b 354 int i = lag;
shimniok 0:826c6171fc1b 355 for (int j=0; j < 100; j++) {
shimniok 0:826c6171fc1b 356 history[i].hdg += errAngle;
shimniok 0:826c6171fc1b 357 // Rotate x, y by errAngle around pivot point; no need to rotate pivot point (j=0)
shimniok 0:826c6171fc1b 358 if (j > 0) {
shimniok 0:826c6171fc1b 359 float dx = history[i].x-history[lag].x;
shimniok 0:826c6171fc1b 360 float dy = history[i].y-history[lag].y;
shimniok 0:826c6171fc1b 361 history[i].x = history[lag].x + dx*cosA - dy*sinA;
shimniok 0:826c6171fc1b 362 history[i].y = history[lag].y + dx*sinA + dy*cosA;
shimniok 0:826c6171fc1b 363 }
shimniok 0:826c6171fc1b 364 inc(i);
shimniok 0:826c6171fc1b 365 }
shimniok 0:826c6171fc1b 366
shimniok 0:826c6171fc1b 367 // gyro bias, correct only with shallow steering angles
shimniok 0:826c6171fc1b 368 // if we're not going, assume heading rate is 0 and correct accordingly
shimniok 0:826c6171fc1b 369 // If we are going, compare gyro-generated heading estimate with kalman
shimniok 0:826c6171fc1b 370 // heading estimate and correct bias accordingly using PI controller with
shimniok 0:826c6171fc1b 371 // fairly long time constant
shimniok 0:826c6171fc1b 372 // TODO: 3 Add something in here to stop updating if the gps is unreliable; need to find out what indicates poor gps heading
shimniok 0:826c6171fc1b 373 if (history[lag].dt > 0 && fabs(steerAngle) < 5.0 && useGps) {
shimniok 0:826c6171fc1b 374 biasErrAngle = Kbias*biasErrAngle + (1-Kbias)*(history[lag].ghdg - state[inState].gpsCourse); // can use this to compute gyro bias
shimniok 0:826c6171fc1b 375 if (biasErrAngle <= -180.0) biasErrAngle += 360.0;
shimniok 0:826c6171fc1b 376 if (biasErrAngle > 180) biasErrAngle -= 360.0;
shimniok 0:826c6171fc1b 377
shimniok 0:826c6171fc1b 378 float errRate = biasErrAngle / history[lag].dt;
shimniok 0:826c6171fc1b 379
shimniok 0:826c6171fc1b 380 //if (!go) errRate = history[lag].gyro;
shimniok 0:826c6171fc1b 381
shimniok 0:826c6171fc1b 382 // Filter bias based on errRate which is based on filtered biasErrAngle
shimniok 0:826c6171fc1b 383 gyroBias = Kbias*gyroBias + (1-Kbias)*errRate;
shimniok 0:826c6171fc1b 384 //fprintf(stdout, "%d %.2f, %.2f, %.4f %.4f %.4f\n", lag, lagHeading, history[lag].hdg, errAngle, errRate, gyroBias);
shimniok 0:826c6171fc1b 385 }
shimniok 0:826c6171fc1b 386
shimniok 0:826c6171fc1b 387 // make sure we update the lag heading with the new estimate
shimniok 0:826c6171fc1b 388 history[lag].hdg = lagHeading;
shimniok 0:826c6171fc1b 389
shimniok 0:826c6171fc1b 390 // increment lag pointer and wrap
shimniok 0:826c6171fc1b 391 lagPrev = lag;
shimniok 0:826c6171fc1b 392 inc(lag);
shimniok 0:826c6171fc1b 393
shimniok 0:826c6171fc1b 394 }
shimniok 0:826c6171fc1b 395 state[inState].estHeading = history[lag].hdg;
shimniok 0:826c6171fc1b 396 // the variable "here" is the current position
shimniok 0:826c6171fc1b 397 //here.move(history[now].hdg, history[now].dist);
shimniok 0:826c6171fc1b 398 float r = PI/180 * history[now].hdg;
shimniok 0:826c6171fc1b 399 // update current position
shimniok 0:826c6171fc1b 400 history[now].x = history[prev].x + history[now].dist * sin(r);
shimniok 0:826c6171fc1b 401 history[now].y = history[prev].y + history[now].dist * cos(r);
shimniok 0:826c6171fc1b 402 cartHere.set(history[now].x, history[now].y);
shimniok 0:826c6171fc1b 403 mapper.cartToGeo(cartHere, &here);
shimniok 0:826c6171fc1b 404
shimniok 0:826c6171fc1b 405 // TODO: don't update gyro heading if speed ~0 -- or use this time to re-calc bias?
shimniok 0:826c6171fc1b 406 // (sensors.lrEncSpeed == 0 && sensors.rrEncSpeed == 0)
shimniok 0:826c6171fc1b 407
shimniok 0:826c6171fc1b 408 //////////////////////////////////////////////////////////////////////////////
shimniok 0:826c6171fc1b 409 // NAVIGATION UPDATE
shimniok 0:826c6171fc1b 410 //////////////////////////////////////////////////////////////////////////////
shimniok 0:826c6171fc1b 411
shimniok 0:826c6171fc1b 412 //bearing = here.bearingTo(config.wpt[nextWaypoint]);
shimniok 0:826c6171fc1b 413 bearing = cartHere.bearingTo(config.cwpt[nextWaypoint]);
shimniok 0:826c6171fc1b 414 //distance = here.distanceTo(config.wpt[nextWaypoint]);
shimniok 0:826c6171fc1b 415 distance = cartHere.distanceTo(config.cwpt[nextWaypoint]);
shimniok 0:826c6171fc1b 416 //float prevDistance = here.distanceTo(config.wpt[lastWaypoint]);
shimniok 0:826c6171fc1b 417 float prevDistance = cartHere.distanceTo(config.cwpt[lastWaypoint]);
shimniok 0:826c6171fc1b 418 double relativeBrg = bearing - history[now].hdg;
shimniok 0:826c6171fc1b 419
shimniok 0:826c6171fc1b 420 // if correction angle is < -180, express as negative degree
shimniok 0:826c6171fc1b 421 // TODO: 3 turn this into a function
shimniok 0:826c6171fc1b 422 if (relativeBrg < -180.0) relativeBrg += 360.0;
shimniok 0:826c6171fc1b 423 if (relativeBrg > 180.0) relativeBrg -= 360.0;
shimniok 0:826c6171fc1b 424
shimniok 0:826c6171fc1b 425 // if within config.waypointDist distance threshold move to next waypoint
shimniok 0:826c6171fc1b 426 // TODO: 3 figure out how to output feedback on wpt arrival external to this function
shimniok 0:826c6171fc1b 427 if (go) {
shimniok 0:826c6171fc1b 428
shimniok 0:826c6171fc1b 429 // if we're within brakeDist of next or previous waypoint, run @ turn speed
shimniok 0:826c6171fc1b 430 // This would normally mean we run at turn speed until we're brakeDist away
shimniok 0:826c6171fc1b 431 // from waypoint 0, but we trick the algorithm by initializing prevWaypoint to waypoint 1
shimniok 0:826c6171fc1b 432 if (distance < config.brakeDist || prevDistance < config.brakeDist) {
shimniok 0:826c6171fc1b 433 setSpeed( config.turnSpeed );
shimniok 0:826c6171fc1b 434 } else if ( (thisTime - timeZero) < 1000 ) {
shimniok 0:826c6171fc1b 435 setSpeed( config.startSpeed );
shimniok 0:826c6171fc1b 436 } else {
shimniok 0:826c6171fc1b 437 setSpeed( config.topSpeed );
shimniok 0:826c6171fc1b 438 }
shimniok 0:826c6171fc1b 439
shimniok 0:826c6171fc1b 440 if (distance < config.waypointDist) {
shimniok 0:826c6171fc1b 441 //fprintf(stdout, "Arrived at wpt %d\n", nextWaypoint);
shimniok 0:826c6171fc1b 442 //speaker.beep(3000.0, 0.5); // non-blocking
shimniok 0:826c6171fc1b 443 lastWaypoint = nextWaypoint;
shimniok 0:826c6171fc1b 444 nextWaypoint++;
shimniok 0:826c6171fc1b 445 cteI = 0;
shimniok 0:826c6171fc1b 446 }
shimniok 0:826c6171fc1b 447
shimniok 0:826c6171fc1b 448 } else {
shimniok 0:826c6171fc1b 449 setSpeed( 0.0 );
shimniok 0:826c6171fc1b 450 }
shimniok 0:826c6171fc1b 451 // Are we at the last waypoint?
shimniok 0:826c6171fc1b 452 // currently handled external to this routine
shimniok 0:826c6171fc1b 453
shimniok 0:826c6171fc1b 454 //////////////////////////////////////////////////////////////////////////////
shimniok 0:826c6171fc1b 455 // OBSTACLE DETECTION & AVOIDANCE
shimniok 0:826c6171fc1b 456 //////////////////////////////////////////////////////////////////////////////
shimniok 0:826c6171fc1b 457 // TODO: 1 limit steering angle based on object detection ?
shimniok 0:826c6171fc1b 458 // or limit relative brg perhaps?
shimniok 0:826c6171fc1b 459 // TODO: 1 add vision obstacle detection and filtering
shimniok 0:826c6171fc1b 460 // TODO: 1 add ranger obstacle detection and filtering/fusion with vision
shimniok 0:826c6171fc1b 461
shimniok 0:826c6171fc1b 462
shimniok 0:826c6171fc1b 463 //////////////////////////////////////////////////////////////////////////////
shimniok 0:826c6171fc1b 464 // CONTROL UPDATE
shimniok 0:826c6171fc1b 465 //////////////////////////////////////////////////////////////////////////////
shimniok 0:826c6171fc1b 466
shimniok 0:826c6171fc1b 467 if (--control_count == 0) {
shimniok 0:826c6171fc1b 468
shimniok 0:826c6171fc1b 469 // Compute cross track error
shimniok 0:826c6171fc1b 470 cte = steerCalc.crossTrack(history[now].x, history[now].y,
shimniok 0:826c6171fc1b 471 config.cwpt[lastWaypoint]._x, config.cwpt[lastWaypoint]._y,
shimniok 0:826c6171fc1b 472 config.cwpt[nextWaypoint]._x, config.cwpt[nextWaypoint]._y);
shimniok 0:826c6171fc1b 473 cteI += cte;
shimniok 0:826c6171fc1b 474
shimniok 0:826c6171fc1b 475 // Use either pure pursuit algorithm or original algorithm
shimniok 0:826c6171fc1b 476 if (config.usePP) {
shimniok 0:826c6171fc1b 477 steerAngle = steerCalc.purePursuitSA(state[inState].estHeading,
shimniok 0:826c6171fc1b 478 history[now].x, history[now].y,
shimniok 0:826c6171fc1b 479 config.cwpt[lastWaypoint]._x, config.cwpt[lastWaypoint]._y,
shimniok 0:826c6171fc1b 480 config.cwpt[nextWaypoint]._x, config.cwpt[nextWaypoint]._y);
shimniok 0:826c6171fc1b 481 } else {
shimniok 0:826c6171fc1b 482 steerAngle = steerCalc.calcSA(relativeBrg, config.minRadius); // use the configured minimum turn radius
shimniok 0:826c6171fc1b 483 }
shimniok 0:826c6171fc1b 484
shimniok 0:826c6171fc1b 485 // Apply gain factor for near straight line
shimniok 0:826c6171fc1b 486 if (fabs(steerAngle) < config.steerGainAngle) steerAngle *= config.steerGain;
shimniok 0:826c6171fc1b 487
shimniok 0:826c6171fc1b 488 // Curb avoidance
shimniok 0:826c6171fc1b 489 if (sensors.rightRanger < config.curbThreshold) {
shimniok 0:826c6171fc1b 490 steerAngle -= config.curbGain * (config.curbThreshold - sensors.rightRanger);
shimniok 0:826c6171fc1b 491 }
shimniok 0:826c6171fc1b 492
shimniok 0:826c6171fc1b 493 setSteering( steerAngle );
shimniok 0:826c6171fc1b 494
shimniok 0:826c6171fc1b 495 // void throttleUpdate( float speed, float dt )
shimniok 0:826c6171fc1b 496
shimniok 0:826c6171fc1b 497 // PID control for throttle
shimniok 0:826c6171fc1b 498 // TODO: 3 move all this PID crap into Actuators.cpp
shimniok 0:826c6171fc1b 499 // TODO: 3 probably should do KF or something for speed/dist; need to address GPS lag, too
shimniok 0:826c6171fc1b 500 // if nothing else, at least average the encoder speed over mult. samples
shimniok 0:826c6171fc1b 501 if (desiredSpeed <= 0.1) {
shimniok 0:826c6171fc1b 502 setThrottle( config.escZero );
shimniok 0:826c6171fc1b 503 } else {
shimniok 0:826c6171fc1b 504 // PID loop for throttle control
shimniok 0:826c6171fc1b 505 // http://www.codeproject.com/Articles/36459/PID-process-control-a-Cruise-Control-example
shimniok 0:826c6171fc1b 506 float error = desiredSpeed - nowSpeed;
shimniok 0:826c6171fc1b 507 // track error over time, scaled to the timer interval
shimniok 0:826c6171fc1b 508 integral += (error * speedDt);
shimniok 0:826c6171fc1b 509 // determine the amount of change from the last time checked
shimniok 0:826c6171fc1b 510 float derivative = (error - lastError) / speedDt;
shimniok 0:826c6171fc1b 511 // calculate how much to drive the output in order to get to the
shimniok 0:826c6171fc1b 512 // desired setpoint.
shimniok 0:826c6171fc1b 513 int output = config.escZero + (config.speedKp * error) + (config.speedKi * integral) + (config.speedKd * derivative);
shimniok 0:826c6171fc1b 514 if (output > config.escMax) output = config.escMax;
shimniok 0:826c6171fc1b 515 if (output < config.escMin) output = config.escMin;
shimniok 0:826c6171fc1b 516 //fprintf(stdout, "s=%.1f d=%.1f o=%d\n", nowSpeed, desiredSpeed, output);
shimniok 0:826c6171fc1b 517 setThrottle( output );
shimniok 0:826c6171fc1b 518 // remember the error for the next time around.
shimniok 0:826c6171fc1b 519 lastError = error;
shimniok 0:826c6171fc1b 520 }
shimniok 0:826c6171fc1b 521
shimniok 0:826c6171fc1b 522 speedDt = 0; // reset dt to begin counting for next time
shimniok 0:826c6171fc1b 523 control_count = CTRL_SKIP;
shimniok 0:826c6171fc1b 524 }
shimniok 0:826c6171fc1b 525
shimniok 0:826c6171fc1b 526 //////////////////////////////////////////////////////////////////////////////
shimniok 0:826c6171fc1b 527 // DATA FOR LOGGING
shimniok 0:826c6171fc1b 528 //////////////////////////////////////////////////////////////////////////////
shimniok 0:826c6171fc1b 529
shimniok 0:826c6171fc1b 530 // Periodically, we enter a new SystemState into the FIFO buffer
shimniok 0:826c6171fc1b 531 // The main loop handles logging and will catch up to us provided
shimniok 0:826c6171fc1b 532 // we feed in new log entries slowly enough.
shimniok 0:826c6171fc1b 533 if (--log_count == 0) {
shimniok 0:826c6171fc1b 534 // Copy data into system state for logging
shimniok 0:826c6171fc1b 535 inState++; // Get next state struct in the buffer
shimniok 0:826c6171fc1b 536 inState &= SSBUF; // Wrap around
shimniok 0:826c6171fc1b 537 ssBufOverrun = (inState == outState);
shimniok 0:826c6171fc1b 538 //
shimniok 0:826c6171fc1b 539 // Need to clear out encoder distance counters; these are incremented
shimniok 0:826c6171fc1b 540 // each time this routine is called.
shimniok 0:826c6171fc1b 541 state[inState].lrEncDistance = 0;
shimniok 0:826c6171fc1b 542 state[inState].rrEncDistance = 0;
shimniok 0:826c6171fc1b 543 //
shimniok 0:826c6171fc1b 544 // need to initialize gps data to be safe
shimniok 0:826c6171fc1b 545 //
shimniok 0:826c6171fc1b 546 state[inState].gpsLatitude = 0;
shimniok 0:826c6171fc1b 547 state[inState].gpsLongitude = 0;
shimniok 0:826c6171fc1b 548 state[inState].gpsHDOP = 0;
shimniok 0:826c6171fc1b 549 state[inState].gpsCourse = 0;
shimniok 0:826c6171fc1b 550 state[inState].gpsSpeed = 0;
shimniok 0:826c6171fc1b 551 state[inState].gpsSats = 0;
shimniok 0:826c6171fc1b 552 log_count = LOG_SKIP; // reset counter
shimniok 0:826c6171fc1b 553 }
shimniok 0:826c6171fc1b 554
shimniok 0:826c6171fc1b 555 // Log Data Timestamp
shimniok 0:826c6171fc1b 556 state[inState].millis = timestamp;
shimniok 0:826c6171fc1b 557
shimniok 0:826c6171fc1b 558 // TODO: 3 recursive filtering on each of the state values
shimniok 0:826c6171fc1b 559 state[inState].voltage = sensors.voltage;
shimniok 0:826c6171fc1b 560 state[inState].current = sensors.current;
shimniok 0:826c6171fc1b 561 for (int i=0; i < 3; i++) {
shimniok 0:826c6171fc1b 562 state[inState].m[i] = sensors.m[i];
shimniok 0:826c6171fc1b 563 state[inState].g[i] = sensors.g[i];
shimniok 0:826c6171fc1b 564 state[inState].a[i] = sensors.a[i];
shimniok 0:826c6171fc1b 565 }
shimniok 0:826c6171fc1b 566 state[inState].gTemp = sensors.gTemp;
shimniok 0:826c6171fc1b 567 state[inState].gHeading = history[now].hdg;
shimniok 0:826c6171fc1b 568 state[inState].lrEncSpeed = sensors.lrEncSpeed;
shimniok 0:826c6171fc1b 569 state[inState].rrEncSpeed = sensors.rrEncSpeed;
shimniok 0:826c6171fc1b 570 state[inState].lrEncDistance += sensors.lrEncDistance;
shimniok 0:826c6171fc1b 571 state[inState].rrEncDistance += sensors.rrEncDistance;
shimniok 0:826c6171fc1b 572 //state[inState].encHeading += (state[inState].lrEncDistance - state[inState].rrEncDistance) / TRACK;
shimniok 0:826c6171fc1b 573 state[inState].estLatitude = here.latitude();
shimniok 0:826c6171fc1b 574 state[inState].estLongitude = here.longitude();
shimniok 0:826c6171fc1b 575 state[inState].estX = history[now].x;
shimniok 0:826c6171fc1b 576 state[inState].estY = history[now].y;
shimniok 0:826c6171fc1b 577 state[inState].bearing = bearing;
shimniok 0:826c6171fc1b 578 state[inState].distance = distance;
shimniok 0:826c6171fc1b 579 state[inState].nextWaypoint = nextWaypoint;
shimniok 0:826c6171fc1b 580 state[inState].gbias = gyroBias;
shimniok 0:826c6171fc1b 581 state[inState].errAngle = biasErrAngle;
shimniok 0:826c6171fc1b 582 state[inState].leftRanger = sensors.leftRanger;
shimniok 0:826c6171fc1b 583 state[inState].rightRanger = sensors.rightRanger;
shimniok 0:826c6171fc1b 584 state[inState].centerRanger = sensors.centerRanger;
shimniok 0:826c6171fc1b 585 state[inState].crossTrackErr = cte;
shimniok 0:826c6171fc1b 586 // Copy AHRS data into logging data
shimniok 0:826c6171fc1b 587 //state[inState].roll = ToDeg(ahrs.roll);
shimniok 0:826c6171fc1b 588 //state[inState].pitch = ToDeg(ahrs.pitch);
shimniok 0:826c6171fc1b 589 //state[inState].yaw = ToDeg(ahrs.yaw);
shimniok 0:826c6171fc1b 590
shimniok 0:826c6171fc1b 591 // increment fifo pointers with wrap-around
shimniok 0:826c6171fc1b 592 prev = now;
shimniok 0:826c6171fc1b 593 inc(now);
shimniok 0:826c6171fc1b 594
shimniok 0:826c6171fc1b 595 // timing
shimniok 0:826c6171fc1b 596 tReal = timer.read_us() - tReal;
shimniok 0:826c6171fc1b 597
shimniok 0:826c6171fc1b 598 ahrsStatus = 1;
shimniok 0:826c6171fc1b 599 }