Michael Shimniok / Mbed 2 deprecated AVC_20110423

The software I used to run my Sparkfun AVC 2011 entry, a 1:10 scale RC truck called \"Data Bus\". This is the final revision of the code as-run on April 23, 2011, the date of the contest, on the 3rd heat.

Dependencies:   TinyCHR6dm PinDetect mbed IncrementalEncoder Servo Schedule DebounceIn SimpleFilter LSM303DLH Steering

main.cpp@0:9b27ebe1ce17, 2011-04-27 (annotated)

Committer:
shimniok
Date:
Wed Apr 27 19:23:43 2011 +0000
Revision:
0:9b27ebe1ce17
Initial release

Who changed what in which revision?

UserRevisionLine numberNew contents of line
shimniok 0:9b27ebe1ce17 1 /** Code for "Data Bus" UGV entry for Sparkfun AVC 2011
shimniok 0:9b27ebe1ce17 2 * http://bot-thoughts.com/
shimniok 0:9b27ebe1ce17 3 */
shimniok 0:9b27ebe1ce17 4
shimniok 0:9b27ebe1ce17 5 ///////////////////////////////////////////////////////////////////////////////////////////////////////
shimniok 0:9b27ebe1ce17 6 // INCLUDES
shimniok 0:9b27ebe1ce17 7 ///////////////////////////////////////////////////////////////////////////////////////////////////////
shimniok 0:9b27ebe1ce17 8
shimniok 0:9b27ebe1ce17 9 #include "mbed.h"
shimniok 0:9b27ebe1ce17 10 #include "TinyGPS.h"
shimniok 0:9b27ebe1ce17 11 #include "SDFileSystem.h"
shimniok 0:9b27ebe1ce17 12 #include "ADC128S.h"
shimniok 0:9b27ebe1ce17 13 #include "PinDetect.h"
shimniok 0:9b27ebe1ce17 14 #include "LSM303DLH.h"
shimniok 0:9b27ebe1ce17 15 #include "Servo.h"
shimniok 0:9b27ebe1ce17 16 #include "IncrementalEncoder.h"
shimniok 0:9b27ebe1ce17 17 #include "Steering.h"
shimniok 0:9b27ebe1ce17 18 #include "Schedule.h"
shimniok 0:9b27ebe1ce17 19 #include "GeoPosition.h"
shimniok 0:9b27ebe1ce17 20 #include "TinyCHR6dm.h"
shimniok 0:9b27ebe1ce17 21 #include "SimpleFilter.h"
shimniok 0:9b27ebe1ce17 22
shimniok 0:9b27ebe1ce17 23 ///////////////////////////////////////////////////////////////////////////////////////////////////////
shimniok 0:9b27ebe1ce17 24 // DEFINES
shimniok 0:9b27ebe1ce17 25 ///////////////////////////////////////////////////////////////////////////////////////////////////////
shimniok 0:9b27ebe1ce17 26
shimniok 0:9b27ebe1ce17 27 #define WHERE(x) debug->printf("%d\n", __LINE__);
shimniok 0:9b27ebe1ce17 28
shimniok 0:9b27ebe1ce17 29 #define clamp360(x) \
shimniok 0:9b27ebe1ce17 30 while ((x) >= 360.0) (x) -= 360.0; \
shimniok 0:9b27ebe1ce17 31 while ((x) < 0) (x) += 360.0;
shimniok 0:9b27ebe1ce17 32 #define clamp180(x) ((x) - floor((x)/360.0) * 360.0 - 180.0);
shimniok 0:9b27ebe1ce17 33
shimniok 0:9b27ebe1ce17 34 #define absf(x) (x *= (x < 0.0) ? -1 : 1)
shimniok 0:9b27ebe1ce17 35
shimniok 0:9b27ebe1ce17 36 #define UPDATE_PERIOD 50 // update period in ms
shimniok 0:9b27ebe1ce17 37 #define GYRO_UPDATE UPDATE_PERIOD/5 // gyro update period in ms
shimniok 0:9b27ebe1ce17 38
shimniok 0:9b27ebe1ce17 39 #define GPS_MIN_SPEED 2.0 // speed below which we won't trust GPS course
shimniok 0:9b27ebe1ce17 40 #define GPS_MAX_HDOP 2.0 // HDOP above which we won't trust GPS course/position
shimniok 0:9b27ebe1ce17 41
shimniok 0:9b27ebe1ce17 42 // Error correction gains
shimniok 0:9b27ebe1ce17 43 #define COMPASS_GAIN 0.25
shimniok 0:9b27ebe1ce17 44 #define YAW_GAIN 0.25
shimniok 0:9b27ebe1ce17 45
shimniok 0:9b27ebe1ce17 46 // Driver configuration parameters
shimniok 0:9b27ebe1ce17 47 #define SONARLEFT_CHAN 0
shimniok 0:9b27ebe1ce17 48 #define SONARRIGHT_CHAN 1
shimniok 0:9b27ebe1ce17 49 #define IRLEFT_CHAN 2
shimniok 0:9b27ebe1ce17 50 #define IRRIGHT_CHAN 3
shimniok 0:9b27ebe1ce17 51 #define TEMP_CHAN 4
shimniok 0:9b27ebe1ce17 52 #define GYRO_CHAN 5
shimniok 0:9b27ebe1ce17 53
shimniok 0:9b27ebe1ce17 54 // Waypoint queue parameters
shimniok 0:9b27ebe1ce17 55 #define MAXWPT 10
shimniok 0:9b27ebe1ce17 56 #define ENDWPT 9999.0
shimniok 0:9b27ebe1ce17 57
shimniok 0:9b27ebe1ce17 58 // Chassis specific parameters
shimniok 0:9b27ebe1ce17 59 #define WHEEL_CIRC 0.321537 // m; calibrated with 4 12.236m runs. Measured 13.125" or 0.333375 wheel dia
shimniok 0:9b27ebe1ce17 60 #define WHEELBASE 0.290
shimniok 0:9b27ebe1ce17 61 #define TRACK 0.280
shimniok 0:9b27ebe1ce17 62
shimniok 0:9b27ebe1ce17 63 ///////////////////////////////////////////////////////////////////////////////////////////////////////
shimniok 0:9b27ebe1ce17 64 // GLOBAL OBJECTS
shimniok 0:9b27ebe1ce17 65 ///////////////////////////////////////////////////////////////////////////////////////////////////////
shimniok 0:9b27ebe1ce17 66
shimniok 0:9b27ebe1ce17 67 // OUTPUT
shimniok 0:9b27ebe1ce17 68 DigitalOut confStatus(LED1); // Config file status LED
shimniok 0:9b27ebe1ce17 69 DigitalOut logStatus(LED2); // Log file status LED
shimniok 0:9b27ebe1ce17 70 DigitalOut gps2Status(LED3); // GPS fix status LED
shimniok 0:9b27ebe1ce17 71 DigitalOut ahrsStatus(LED4); // GPS fix status LED
shimniok 0:9b27ebe1ce17 72 DigitalOut sonarStart(p18); // Sends signal to start sonar array pings
shimniok 0:9b27ebe1ce17 73 DigitalOut flasher(p10); // Autonomous mode warning flasher
shimniok 0:9b27ebe1ce17 74 DigitalOut steerServo(p21); // After shutdown, pass pulses through to servo
shimniok 0:9b27ebe1ce17 75 //Serial lcd(p9, NC); // LCD module, TX only
shimniok 0:9b27ebe1ce17 76
shimniok 0:9b27ebe1ce17 77 // INPUT
shimniok 0:9b27ebe1ce17 78 InterruptIn receiver(p23); // RC Receiver steering channel
shimniok 0:9b27ebe1ce17 79 DigitalOut buttonPower(p19); // Power the button(s)
shimniok 0:9b27ebe1ce17 80 //PinDetect upButton(p17);
shimniok 0:9b27ebe1ce17 81 PinDetect selectButton(p20); // Input selectButton
shimniok 0:9b27ebe1ce17 82 //PinDetect downButton(p18);
shimniok 0:9b27ebe1ce17 83
shimniok 0:9b27ebe1ce17 84 // VEHICLE
shimniok 0:9b27ebe1ce17 85 Servo steering(p21); // Steering Servo
shimniok 0:9b27ebe1ce17 86 Servo throttle(p22); // ESC
shimniok 0:9b27ebe1ce17 87 Schedule go; // Throttle profile, dead stop to full speed
shimniok 0:9b27ebe1ce17 88 Schedule stop; // Throttle profile, full speed to dead stop
shimniok 0:9b27ebe1ce17 89 Steering steerCalc(TRACK, WHEELBASE); // steering calculator
shimniok 0:9b27ebe1ce17 90 int maxSpeed=520; // Servo setting for max speed
shimniok 0:9b27ebe1ce17 91
shimniok 0:9b27ebe1ce17 92 // SENSORS
shimniok 0:9b27ebe1ce17 93 //HMC6352 compass(p28, p27); // Driver for compass
shimniok 0:9b27ebe1ce17 94 LSM303DLH compass3d(p28, p27); // Driver for compass
shimniok 0:9b27ebe1ce17 95 //I2C cam(p28, p27); // CMUcam I2C bridge
shimniok 0:9b27ebe1ce17 96 //ADC128S adc(p5, p6, p7, p15); // ADC128S102 8ch ADC driver; mosi, miso, sclk, cs
shimniok 0:9b27ebe1ce17 97 ADC128S adc(p11, p12, p13, p14); // ADC128S102 8ch ADC driver; mosi, miso, sclk, cs
shimniok 0:9b27ebe1ce17 98 IncrementalEncoder left(p30); // Left wheel encoder
shimniok 0:9b27ebe1ce17 99 IncrementalEncoder right(p29); // Right wheel encoder
shimniok 0:9b27ebe1ce17 100 //Serial gps1(p26, p25); // GPS1, Locosys LS20031
shimniok 0:9b27ebe1ce17 101 Serial ahrs(p26, p25); // CHR-6dm AHRS
shimniok 0:9b27ebe1ce17 102 Serial gps2(p9, p10); // GPS2, iGPS-500
shimniok 0:9b27ebe1ce17 103 Serial *dev; // For use with bridge
shimniok 0:9b27ebe1ce17 104 //TinyGPS gps1Parse; // GPS NMEA parser
shimniok 0:9b27ebe1ce17 105 TinyCHR6dm ahrsParser; // CHR-6dm AHRS parser
shimniok 0:9b27ebe1ce17 106 TinyGPS gps2Parse; // GPS NMEA parser
shimniok 0:9b27ebe1ce17 107
shimniok 0:9b27ebe1ce17 108 // COMM
shimniok 0:9b27ebe1ce17 109 Serial pc(USBTX, USBRX); // PC usb communications
shimniok 0:9b27ebe1ce17 110 Serial *debug = &pc;
shimniok 0:9b27ebe1ce17 111
shimniok 0:9b27ebe1ce17 112 // MISC
shimniok 0:9b27ebe1ce17 113 Timer timer; // For main loop scheduling
shimniok 0:9b27ebe1ce17 114 SDFileSystem sd(p5, p6, p7, p8, "log"); // mosi, miso, sclk, cs
shimniok 0:9b27ebe1ce17 115 LocalFileSystem local("etc"); // Create the local filesystem under the name "local"
shimniok 0:9b27ebe1ce17 116
shimniok 0:9b27ebe1ce17 117 ///////////////////////////////////////////////////////////////////////////////////////////////////////
shimniok 0:9b27ebe1ce17 118 // GLOBAL VARIABLES
shimniok 0:9b27ebe1ce17 119 ///////////////////////////////////////////////////////////////////////////////////////////////////////
shimniok 0:9b27ebe1ce17 120
shimniok 0:9b27ebe1ce17 121 // GPS Variables
shimniok 0:9b27ebe1ce17 122 double gps1_lat; // latitude
shimniok 0:9b27ebe1ce17 123 double gps1_lon; // longitude
shimniok 0:9b27ebe1ce17 124 double gps2_lat; // latitude
shimniok 0:9b27ebe1ce17 125 double gps2_lon; // longitude
shimniok 0:9b27ebe1ce17 126 unsigned long age = 0; // gps fix age
shimniok 0:9b27ebe1ce17 127 float gps1_hdop = 0.0; // gps horizontal dilution of precision
shimniok 0:9b27ebe1ce17 128 float gps2_hdop = 0.0; // gps horizontal dilution of precision
shimniok 0:9b27ebe1ce17 129 int year; // gps date variables
shimniok 0:9b27ebe1ce17 130 byte month;
shimniok 0:9b27ebe1ce17 131 byte day;
shimniok 0:9b27ebe1ce17 132 byte hour;
shimniok 0:9b27ebe1ce17 133 byte minute;
shimniok 0:9b27ebe1ce17 134 byte second;
shimniok 0:9b27ebe1ce17 135 byte hundredths;
shimniok 0:9b27ebe1ce17 136
shimniok 0:9b27ebe1ce17 137 // schedule for LED warning flasher
shimniok 0:9b27ebe1ce17 138 Schedule blink;
shimniok 0:9b27ebe1ce17 139
shimniok 0:9b27ebe1ce17 140 // Useful globals
shimniok 0:9b27ebe1ce17 141 float declination; // compass declination for local correction
shimniok 0:9b27ebe1ce17 142 bool goGoGo = false; // signal to start moving
shimniok 0:9b27ebe1ce17 143 bool done = false; // signal that we're done navigating
shimniok 0:9b27ebe1ce17 144
shimniok 0:9b27ebe1ce17 145 // Gyro Variables
shimniok 0:9b27ebe1ce17 146 float gyro = 0; // gyro reading
shimniok 0:9b27ebe1ce17 147 float gyroBias = 2027.0; // gyro bias in raw 12-bit ADC value
shimniok 0:9b27ebe1ce17 148 unsigned int temp = 0; // gyro temp reading
shimniok 0:9b27ebe1ce17 149 float gyroSens = 4.89; // in LSB/deg/sec since we're using ratiometric ADC and gyro
shimniok 0:9b27ebe1ce17 150 float gyroSum[UPDATE_PERIOD/GYRO_UPDATE]; // for summijng/averaging gyro between DR / compass updates
shimniok 0:9b27ebe1ce17 151 int gyroCount = 0; // counter for gyroSum array
shimniok 0:9b27ebe1ce17 152
shimniok 0:9b27ebe1ce17 153 // Navigation Variables
shimniok 0:9b27ebe1ce17 154 GeoPosition wpt[MAXWPT]; // course waypoints
shimniok 0:9b27ebe1ce17 155 unsigned int wptCount = 0; // number of waypoints configured
shimniok 0:9b27ebe1ce17 156 int wptCurrent = 0; // current waypoint
shimniok 0:9b27ebe1ce17 157 GeoPosition here1; // current gps position
shimniok 0:9b27ebe1ce17 158 GeoPosition here2; // current gps position
shimniok 0:9b27ebe1ce17 159 GeoPosition dr_here; // current dead reckoning position
shimniok 0:9b27ebe1ce17 160 GeoPosition dr_here_last; // DR position at last GPS packet
shimniok 0:9b27ebe1ce17 161
shimniok 0:9b27ebe1ce17 162 // Misc
shimniok 0:9b27ebe1ce17 163 FILE *fp = 0;
shimniok 0:9b27ebe1ce17 164 bool buttonPressed = false;
shimniok 0:9b27ebe1ce17 165 bool buttonReleased = false;
shimniok 0:9b27ebe1ce17 166
shimniok 0:9b27ebe1ce17 167 ///////////////////////////////////////////////////////////////////////////////////////////////////////
shimniok 0:9b27ebe1ce17 168 // FUNCTION DEFINITIONS
shimniok 0:9b27ebe1ce17 169 ///////////////////////////////////////////////////////////////////////////////////////////////////////
shimniok 0:9b27ebe1ce17 170
shimniok 0:9b27ebe1ce17 171 FILE *initLogfile(void);
shimniok 0:9b27ebe1ce17 172 void initButtons(void);
shimniok 0:9b27ebe1ce17 173 void initCompass(void);
shimniok 0:9b27ebe1ce17 174 void initGPS(void);
shimniok 0:9b27ebe1ce17 175 void initAHRS(void);
shimniok 0:9b27ebe1ce17 176 void initFlasher(void);
shimniok 0:9b27ebe1ce17 177 void initSteering(void);
shimniok 0:9b27ebe1ce17 178 void initThrottle(void);
shimniok 0:9b27ebe1ce17 179 void initDR(void);
shimniok 0:9b27ebe1ce17 180 void loadConfig(float &interceptDist, float &declination, float &compassGain, float &yawGain);
shimniok 0:9b27ebe1ce17 181 void doGPS(TinyGPS &parse, GeoPosition &here, DigitalOut status, char *date, char *time);
shimniok 0:9b27ebe1ce17 182 void autonomousMode(void);
shimniok 0:9b27ebe1ce17 183 void findGPSBias(double *brg, double *dist, int n, GeoPosition place);
shimniok 0:9b27ebe1ce17 184 void idleMode(void);
shimniok 0:9b27ebe1ce17 185 void compassCalibrate(void);
shimniok 0:9b27ebe1ce17 186 void servoCalibrate(void);
shimniok 0:9b27ebe1ce17 187 void serialBridge(Serial &gps);
shimniok 0:9b27ebe1ce17 188 void instrumentCheck(void);
shimniok 0:9b27ebe1ce17 189 float compassHeading(void);
shimniok 0:9b27ebe1ce17 190 float gyroRate(unsigned int adc);
shimniok 0:9b27ebe1ce17 191 float sonarDistance(unsigned int adc);
shimniok 0:9b27ebe1ce17 192 float irDistance(unsigned int adc);
shimniok 0:9b27ebe1ce17 193
shimniok 0:9b27ebe1ce17 194 // If we don't close the log file, when we restart, all the written data
shimniok 0:9b27ebe1ce17 195 // will be lost. So we have to use a button to force mbed to close the
shimniok 0:9b27ebe1ce17 196 // file and preserve the data.
shimniok 0:9b27ebe1ce17 197 //
shimniok 0:9b27ebe1ce17 198 void assertButton() {
shimniok 0:9b27ebe1ce17 199 buttonPressed = true;
shimniok 0:9b27ebe1ce17 200 buttonReleased = false;
shimniok 0:9b27ebe1ce17 201 }
shimniok 0:9b27ebe1ce17 202
shimniok 0:9b27ebe1ce17 203 void deassertButton() {
shimniok 0:9b27ebe1ce17 204 buttonPressed = false;
shimniok 0:9b27ebe1ce17 205 buttonReleased = true;
shimniok 0:9b27ebe1ce17 206 }
shimniok 0:9b27ebe1ce17 207
shimniok 0:9b27ebe1ce17 208 // Use interrupt handler to receive AHRS serial comm and parse
shimniok 0:9b27ebe1ce17 209 // it using TinyCHR6dm library
shimniok 0:9b27ebe1ce17 210 //
shimniok 0:9b27ebe1ce17 211 void recv1() {
shimniok 0:9b27ebe1ce17 212 while (ahrs.readable())
shimniok 0:9b27ebe1ce17 213 ahrsParser.parse(ahrs.getc());
shimniok 0:9b27ebe1ce17 214 }
shimniok 0:9b27ebe1ce17 215
shimniok 0:9b27ebe1ce17 216 void recv2() {
shimniok 0:9b27ebe1ce17 217 while (gps2.readable())
shimniok 0:9b27ebe1ce17 218 gps2Parse.encode(gps2.getc());
shimniok 0:9b27ebe1ce17 219 }
shimniok 0:9b27ebe1ce17 220
shimniok 0:9b27ebe1ce17 221 char getInput(const char *prompt)
shimniok 0:9b27ebe1ce17 222 {
shimniok 0:9b27ebe1ce17 223 char c;
shimniok 0:9b27ebe1ce17 224
shimniok 0:9b27ebe1ce17 225 if (prompt) pc.printf("%s ", prompt);
shimniok 0:9b27ebe1ce17 226 while (!pc.readable())
shimniok 0:9b27ebe1ce17 227 wait(0.001);
shimniok 0:9b27ebe1ce17 228 c = pc.getc();
shimniok 0:9b27ebe1ce17 229 pc.printf("%c\n", c);
shimniok 0:9b27ebe1ce17 230
shimniok 0:9b27ebe1ce17 231 return c;
shimniok 0:9b27ebe1ce17 232 }
shimniok 0:9b27ebe1ce17 233
shimniok 0:9b27ebe1ce17 234 int main()
shimniok 0:9b27ebe1ce17 235 {
shimniok 0:9b27ebe1ce17 236 // Send data back to the PC
shimniok 0:9b27ebe1ce17 237 pc.baud(115200);
shimniok 0:9b27ebe1ce17 238
shimniok 0:9b27ebe1ce17 239 //receiver.rise(&killSwitch); // Detects remote kill switch / rc takeover
shimniok 0:9b27ebe1ce17 240 initButtons(); // Initialize input buttons
shimniok 0:9b27ebe1ce17 241 initCompass();
shimniok 0:9b27ebe1ce17 242 initSteering();
shimniok 0:9b27ebe1ce17 243 initThrottle();
shimniok 0:9b27ebe1ce17 244 // initFlasher(); // Initialize autonomous mode flasher
shimniok 0:9b27ebe1ce17 245 initGPS();
shimniok 0:9b27ebe1ce17 246
shimniok 0:9b27ebe1ce17 247 // Insert menu system here w/ timeout
shimniok 0:9b27ebe1ce17 248 bool autoBoot=true;
shimniok 0:9b27ebe1ce17 249 /*
shimniok 0:9b27ebe1ce17 250 pc.printf("Booting to autonomous mode in 10 seconds, any key to abort.\n ");
shimniok 0:9b27ebe1ce17 251 for (int i=1; i > 0; i--) {
shimniok 0:9b27ebe1ce17 252 pc.printf("%d...", i);
shimniok 0:9b27ebe1ce17 253 if (pc.readable()) {
shimniok 0:9b27ebe1ce17 254 autoBoot = false;
shimniok 0:9b27ebe1ce17 255 while (pc.readable()) pc.getc(); // clear buffer
shimniok 0:9b27ebe1ce17 256 break;
shimniok 0:9b27ebe1ce17 257 }
shimniok 0:9b27ebe1ce17 258 wait(1.0);
shimniok 0:9b27ebe1ce17 259 }
shimniok 0:9b27ebe1ce17 260 pc.printf("\n");
shimniok 0:9b27ebe1ce17 261 */
shimniok 0:9b27ebe1ce17 262
shimniok 0:9b27ebe1ce17 263 char cmd;
shimniok 0:9b27ebe1ce17 264
shimniok 0:9b27ebe1ce17 265 while (1) {
shimniok 0:9b27ebe1ce17 266
shimniok 0:9b27ebe1ce17 267 if (autoBoot) {
shimniok 0:9b27ebe1ce17 268 autoBoot = false;
shimniok 0:9b27ebe1ce17 269 cmd = 'a';
shimniok 0:9b27ebe1ce17 270 } else {
shimniok 0:9b27ebe1ce17 271 pc.printf("==============\nData Bus Menu\n==============\n");
shimniok 0:9b27ebe1ce17 272 pc.printf("(a)utonomous mode\n");
shimniok 0:9b27ebe1ce17 273 pc.printf("(s)erial bridge\n (1) Port 1 (2) Port 2 (GPS)\n");
shimniok 0:9b27ebe1ce17 274 pc.printf("(c)ompass calibration\n");
shimniok 0:9b27ebe1ce17 275 pc.printf("(i)nstrument check\n");
shimniok 0:9b27ebe1ce17 276 //pc.printf("(s)ervo calibration\n");
shimniok 0:9b27ebe1ce17 277 cmd = getInput("\nSelect from the above:");
shimniok 0:9b27ebe1ce17 278 }
shimniok 0:9b27ebe1ce17 279
shimniok 0:9b27ebe1ce17 280 switch (cmd) {
shimniok 0:9b27ebe1ce17 281 case 'a' :
shimniok 0:9b27ebe1ce17 282 autonomousMode();
shimniok 0:9b27ebe1ce17 283 break;
shimniok 0:9b27ebe1ce17 284 case 's' :
shimniok 0:9b27ebe1ce17 285 // which GPS
shimniok 0:9b27ebe1ce17 286 cmd = getInput("GPS2 (2) or AHRS(1)?");
shimniok 0:9b27ebe1ce17 287 switch (cmd) {
shimniok 0:9b27ebe1ce17 288 case '1' :
shimniok 0:9b27ebe1ce17 289 serialBridge(ahrs);
shimniok 0:9b27ebe1ce17 290 break;
shimniok 0:9b27ebe1ce17 291 case '2' :
shimniok 0:9b27ebe1ce17 292 serialBridge(gps2);
shimniok 0:9b27ebe1ce17 293 break;
shimniok 0:9b27ebe1ce17 294 default :
shimniok 0:9b27ebe1ce17 295 break;
shimniok 0:9b27ebe1ce17 296 }
shimniok 0:9b27ebe1ce17 297 break;
shimniok 0:9b27ebe1ce17 298 case 'i' :
shimniok 0:9b27ebe1ce17 299 instrumentCheck();
shimniok 0:9b27ebe1ce17 300 break;
shimniok 0:9b27ebe1ce17 301 case 'c' :
shimniok 0:9b27ebe1ce17 302 compassCalibrate();
shimniok 0:9b27ebe1ce17 303 break;
shimniok 0:9b27ebe1ce17 304 default :
shimniok 0:9b27ebe1ce17 305 break;
shimniok 0:9b27ebe1ce17 306 }
shimniok 0:9b27ebe1ce17 307 }
shimniok 0:9b27ebe1ce17 308
shimniok 0:9b27ebe1ce17 309 }
shimniok 0:9b27ebe1ce17 310
shimniok 0:9b27ebe1ce17 311 // Handle data from a GPS (there may be two GPS's so needed to put the code in one routine
shimniok 0:9b27ebe1ce17 312 //
shimniok 0:9b27ebe1ce17 313 void doGPS(TinyGPS &parse, GeoPosition &here, DigitalOut status, char *date, char *time)
shimniok 0:9b27ebe1ce17 314 {
shimniok 0:9b27ebe1ce17 315 double lat, lon;
shimniok 0:9b27ebe1ce17 316 unsigned long age;
shimniok 0:9b27ebe1ce17 317
shimniok 0:9b27ebe1ce17 318 parse.reset_ready(); // reset the flags
shimniok 0:9b27ebe1ce17 319 //pc.printf("%d GPS RMC are ready\n", millis);
shimniok 0:9b27ebe1ce17 320 parse.f_get_position(&lat, &lon, &age);
shimniok 0:9b27ebe1ce17 321 parse.crack_datetime(&year, &month, &day, &hour, &minute, &second, &hundredths, &age);
shimniok 0:9b27ebe1ce17 322
shimniok 0:9b27ebe1ce17 323 sprintf(date, "%02d/%02d/%4d", month, day, year);
shimniok 0:9b27ebe1ce17 324 sprintf(time, "%02d:%02d:%02d.%03d", hour, minute, second, hundredths);
shimniok 0:9b27ebe1ce17 325
shimniok 0:9b27ebe1ce17 326 float hdop = parse.f_hdop();
shimniok 0:9b27ebe1ce17 327
shimniok 0:9b27ebe1ce17 328 // Want to blink the LED when GPS update arrives
shimniok 0:9b27ebe1ce17 329 // must toggle opposite
shimniok 0:9b27ebe1ce17 330 //status = (hdop > 0.0 && hdop < 10.0) ? 0 : 1;
shimniok 0:9b27ebe1ce17 331
shimniok 0:9b27ebe1ce17 332 // Bearing and distance to waypoint
shimniok 0:9b27ebe1ce17 333 here.set(lat, lon);
shimniok 0:9b27ebe1ce17 334
shimniok 0:9b27ebe1ce17 335 //pc.printf("HDOP: %.1f gyro: %d\n", gps1_hdop, gyro);
shimniok 0:9b27ebe1ce17 336
shimniok 0:9b27ebe1ce17 337 return;
shimniok 0:9b27ebe1ce17 338 }
shimniok 0:9b27ebe1ce17 339
shimniok 0:9b27ebe1ce17 340
shimniok 0:9b27ebe1ce17 341
shimniok 0:9b27ebe1ce17 342 // convert character to an int
shimniok 0:9b27ebe1ce17 343 //
shimniok 0:9b27ebe1ce17 344 int ctoi(char c)
shimniok 0:9b27ebe1ce17 345 {
shimniok 0:9b27ebe1ce17 346 int i=-1;
shimniok 0:9b27ebe1ce17 347
shimniok 0:9b27ebe1ce17 348 if (c >= '0' && c <= '9') {
shimniok 0:9b27ebe1ce17 349 i = c - '0';
shimniok 0:9b27ebe1ce17 350 }
shimniok 0:9b27ebe1ce17 351
shimniok 0:9b27ebe1ce17 352 //printf("char: %c int %d\n", c, i);
shimniok 0:9b27ebe1ce17 353
shimniok 0:9b27ebe1ce17 354 return i;
shimniok 0:9b27ebe1ce17 355 }
shimniok 0:9b27ebe1ce17 356
shimniok 0:9b27ebe1ce17 357
shimniok 0:9b27ebe1ce17 358 // convert string to floating point
shimniok 0:9b27ebe1ce17 359 //
shimniok 0:9b27ebe1ce17 360 double cvstof(char *s)
shimniok 0:9b27ebe1ce17 361 {
shimniok 0:9b27ebe1ce17 362 double f=0.0;
shimniok 0:9b27ebe1ce17 363 double mult = 0.1;
shimniok 0:9b27ebe1ce17 364 bool neg = false;
shimniok 0:9b27ebe1ce17 365 //char dec = 1;
shimniok 0:9b27ebe1ce17 366
shimniok 0:9b27ebe1ce17 367 // leading spaces
shimniok 0:9b27ebe1ce17 368 while (*s == ' ' || *s == '\t') {
shimniok 0:9b27ebe1ce17 369 s++;
shimniok 0:9b27ebe1ce17 370 if (*s == 0) break;
shimniok 0:9b27ebe1ce17 371 }
shimniok 0:9b27ebe1ce17 372
shimniok 0:9b27ebe1ce17 373 // What about negative numbers?
shimniok 0:9b27ebe1ce17 374 if (*s == '-') {
shimniok 0:9b27ebe1ce17 375 neg = true;
shimniok 0:9b27ebe1ce17 376 s++;
shimniok 0:9b27ebe1ce17 377 }
shimniok 0:9b27ebe1ce17 378
shimniok 0:9b27ebe1ce17 379 // before the decimal
shimniok 0:9b27ebe1ce17 380 //
shimniok 0:9b27ebe1ce17 381 while (*s != 0) {
shimniok 0:9b27ebe1ce17 382 if (*s == '.') {
shimniok 0:9b27ebe1ce17 383 s++;
shimniok 0:9b27ebe1ce17 384 break;
shimniok 0:9b27ebe1ce17 385 }
shimniok 0:9b27ebe1ce17 386 f = (f * 10.0) + (double) ctoi(*s);
shimniok 0:9b27ebe1ce17 387 s++;
shimniok 0:9b27ebe1ce17 388 }
shimniok 0:9b27ebe1ce17 389 // after the decimal
shimniok 0:9b27ebe1ce17 390 while (*s != 0 && *s >= '0' && *s <= '9') {
shimniok 0:9b27ebe1ce17 391 f += (double) ctoi(*s) * mult;
shimniok 0:9b27ebe1ce17 392 mult /= 10;
shimniok 0:9b27ebe1ce17 393 s++;
shimniok 0:9b27ebe1ce17 394 }
shimniok 0:9b27ebe1ce17 395
shimniok 0:9b27ebe1ce17 396 // if we were negative...
shimniok 0:9b27ebe1ce17 397 if (neg) f = -f;
shimniok 0:9b27ebe1ce17 398
shimniok 0:9b27ebe1ce17 399 return f;
shimniok 0:9b27ebe1ce17 400 }
shimniok 0:9b27ebe1ce17 401
shimniok 0:9b27ebe1ce17 402 // copy t to s until delimiter is reached
shimniok 0:9b27ebe1ce17 403 // return location of delimiter+1 in t
shimniok 0:9b27ebe1ce17 404 char *split(char *s, char *t, int max, char delim)
shimniok 0:9b27ebe1ce17 405 {
shimniok 0:9b27ebe1ce17 406 int i = 0;
shimniok 0:9b27ebe1ce17 407
shimniok 0:9b27ebe1ce17 408 if (s == 0 || t == 0)
shimniok 0:9b27ebe1ce17 409 return 0;
shimniok 0:9b27ebe1ce17 410
shimniok 0:9b27ebe1ce17 411 while (*t != 0 && *t != delim && i < max) {
shimniok 0:9b27ebe1ce17 412 *s++ = *t++;
shimniok 0:9b27ebe1ce17 413 i++;
shimniok 0:9b27ebe1ce17 414 }
shimniok 0:9b27ebe1ce17 415 *s = 0;
shimniok 0:9b27ebe1ce17 416
shimniok 0:9b27ebe1ce17 417 return t+1;
shimniok 0:9b27ebe1ce17 418 }
shimniok 0:9b27ebe1ce17 419
shimniok 0:9b27ebe1ce17 420 #define MAXBUF 64
shimniok 0:9b27ebe1ce17 421 // load configuration from filesystem
shimniok 0:9b27ebe1ce17 422 void loadConfig(float &interceptDist, float &declination, float &compassGain, float &yawGain)
shimniok 0:9b27ebe1ce17 423 {
shimniok 0:9b27ebe1ce17 424 // FILE *fp;
shimniok 0:9b27ebe1ce17 425 char buf[MAXBUF]; // buffer to read in data
shimniok 0:9b27ebe1ce17 426 char tmp[MAXBUF]; // temp buffer
shimniok 0:9b27ebe1ce17 427 char *p;
shimniok 0:9b27ebe1ce17 428 double lat, lon;
shimniok 0:9b27ebe1ce17 429 bool declFound = false;
shimniok 0:9b27ebe1ce17 430
shimniok 0:9b27ebe1ce17 431 // Just to be safe let's wait
shimniok 0:9b27ebe1ce17 432 //wait(2.0);
shimniok 0:9b27ebe1ce17 433
shimniok 0:9b27ebe1ce17 434 pc.printf("opening config file...\n");
shimniok 0:9b27ebe1ce17 435
shimniok 0:9b27ebe1ce17 436 fp = fopen("/etc/config.txt", "r");
shimniok 0:9b27ebe1ce17 437 if (fp == 0) {
shimniok 0:9b27ebe1ce17 438 pc.printf("Could not open config.txt\n");
shimniok 0:9b27ebe1ce17 439 } else {
shimniok 0:9b27ebe1ce17 440 wptCount = 0;
shimniok 0:9b27ebe1ce17 441 declination = 0.0;
shimniok 0:9b27ebe1ce17 442 while (!feof(fp)) {
shimniok 0:9b27ebe1ce17 443 fgets(buf, MAXBUF-1, fp);
shimniok 0:9b27ebe1ce17 444 p = split(tmp, buf, MAXBUF, ','); // split off the first field
shimniok 0:9b27ebe1ce17 445 switch (tmp[0]) {
shimniok 0:9b27ebe1ce17 446 case 'W' : // Waypoint
shimniok 0:9b27ebe1ce17 447 p = split(tmp, p, MAXBUF, ','); // split off the latitude to tmp
shimniok 0:9b27ebe1ce17 448 lat = cvstof(tmp);
shimniok 0:9b27ebe1ce17 449 p = split(tmp, p, MAXBUF, ','); // split off the longitude to tmp
shimniok 0:9b27ebe1ce17 450 lon = cvstof(tmp);
shimniok 0:9b27ebe1ce17 451 if (wptCount < MAXWPT) {
shimniok 0:9b27ebe1ce17 452 wpt[wptCount].set(lat, lon);
shimniok 0:9b27ebe1ce17 453 wptCount++;
shimniok 0:9b27ebe1ce17 454 }
shimniok 0:9b27ebe1ce17 455 break;
shimniok 0:9b27ebe1ce17 456 case 'G' : // Gyro Bias
shimniok 0:9b27ebe1ce17 457 p = split(tmp, p, MAXBUF, ','); // split off the declination to tmp
shimniok 0:9b27ebe1ce17 458 gyroBias = (float) cvstof(tmp);
shimniok 0:9b27ebe1ce17 459 break;
shimniok 0:9b27ebe1ce17 460 case 'D' : // Compass Declination
shimniok 0:9b27ebe1ce17 461 p = split(tmp, p, MAXBUF, ','); // split off the declination to tmp
shimniok 0:9b27ebe1ce17 462 declination = (float) cvstof(tmp);
shimniok 0:9b27ebe1ce17 463 declFound = true;
shimniok 0:9b27ebe1ce17 464 break;
shimniok 0:9b27ebe1ce17 465 case 'I' : // Intercept distance
shimniok 0:9b27ebe1ce17 466 p = split(tmp, p, MAXBUF, ','); // split off the number to tmp
shimniok 0:9b27ebe1ce17 467 interceptDist = (float) cvstof(tmp);
shimniok 0:9b27ebe1ce17 468 break;
shimniok 0:9b27ebe1ce17 469 case 'S' : // Speed maximum
shimniok 0:9b27ebe1ce17 470 p = split(tmp, p, MAXBUF, ','); // split off the number to tmp
shimniok 0:9b27ebe1ce17 471 maxSpeed = atoi(tmp);
shimniok 0:9b27ebe1ce17 472 //pc.printf("tmp:%s maxSpeed:%d\n", tmp, maxSpeed);
shimniok 0:9b27ebe1ce17 473 break;
shimniok 0:9b27ebe1ce17 474 case 'E' :
shimniok 0:9b27ebe1ce17 475 p = split(tmp, p, MAXBUF, ','); // split off the number to tmp
shimniok 0:9b27ebe1ce17 476 compassGain = (float) cvstof(tmp);
shimniok 0:9b27ebe1ce17 477 p = split(tmp, p, MAXBUF, ','); // split off the number to tmp
shimniok 0:9b27ebe1ce17 478 yawGain = (float) cvstof(tmp);
shimniok 0:9b27ebe1ce17 479 default :
shimniok 0:9b27ebe1ce17 480 break;
shimniok 0:9b27ebe1ce17 481 } // switch
shimniok 0:9b27ebe1ce17 482 } // while
shimniok 0:9b27ebe1ce17 483
shimniok 0:9b27ebe1ce17 484 // Did we get the values we were looking for?
shimniok 0:9b27ebe1ce17 485 if (wptCount > 0 && declFound) {
shimniok 0:9b27ebe1ce17 486 confStatus = 1;
shimniok 0:9b27ebe1ce17 487 }
shimniok 0:9b27ebe1ce17 488
shimniok 0:9b27ebe1ce17 489 } // if fp
shimniok 0:9b27ebe1ce17 490
shimniok 0:9b27ebe1ce17 491 if (fp != 0)
shimniok 0:9b27ebe1ce17 492 fclose(fp);
shimniok 0:9b27ebe1ce17 493
shimniok 0:9b27ebe1ce17 494 pc.printf("Intercept Dist: %.1f\n", interceptDist);
shimniok 0:9b27ebe1ce17 495 pc.printf("Declination: %.1f\n", declination);
shimniok 0:9b27ebe1ce17 496 pc.printf("MaxSpeed: %d\n", maxSpeed);
shimniok 0:9b27ebe1ce17 497 pc.printf("CompassGain; %.3f YawGain: %.3f\n", compassGain, yawGain);
shimniok 0:9b27ebe1ce17 498 for (int w = 0; w < MAXWPT && w < wptCount; w++) {
shimniok 0:9b27ebe1ce17 499 pc.printf("Waypoint #%d lat: %.6f lon: %.6f\n", w, wpt[w].latitude(), wpt[w].longitude());
shimniok 0:9b27ebe1ce17 500 }
shimniok 0:9b27ebe1ce17 501
shimniok 0:9b27ebe1ce17 502 }
shimniok 0:9b27ebe1ce17 503
shimniok 0:9b27ebe1ce17 504
shimniok 0:9b27ebe1ce17 505 ///////////////////////////////////////////////////////////////////////////////////////////////////////
shimniok 0:9b27ebe1ce17 506 // INITIALIZATION ROUTINES
shimniok 0:9b27ebe1ce17 507 ///////////////////////////////////////////////////////////////////////////////////////////////////////
shimniok 0:9b27ebe1ce17 508
shimniok 0:9b27ebe1ce17 509 // Find the next unused filename of the form logger##.csv where # is 0-9
shimniok 0:9b27ebe1ce17 510 //
shimniok 0:9b27ebe1ce17 511 FILE *initLogfile()
shimniok 0:9b27ebe1ce17 512 {
shimniok 0:9b27ebe1ce17 513 FILE *fp = 0;
shimniok 0:9b27ebe1ce17 514 char myname[64];
shimniok 0:9b27ebe1ce17 515
shimniok 0:9b27ebe1ce17 516 pc.printf("Opening log file...\n");
shimniok 0:9b27ebe1ce17 517
shimniok 0:9b27ebe1ce17 518 while (fp == 0) {
shimniok 0:9b27ebe1ce17 519 if ((fp = fopen("/log/test.txt", "r")) == 0) {
shimniok 0:9b27ebe1ce17 520 pc.printf("Waiting for filesystem to come online...");
shimniok 0:9b27ebe1ce17 521 wait(0.200);
shimniok 0:9b27ebe1ce17 522 }
shimniok 0:9b27ebe1ce17 523 }
shimniok 0:9b27ebe1ce17 524 fclose(fp);
shimniok 0:9b27ebe1ce17 525
shimniok 0:9b27ebe1ce17 526 for (int i = 0; i < 1000; i++) {
shimniok 0:9b27ebe1ce17 527 sprintf(myname, "/log/log%04d.csv", i);
shimniok 0:9b27ebe1ce17 528 //pc.printf("Try file: <%s>\n", myname);
shimniok 0:9b27ebe1ce17 529 if ((fp = fopen(myname, "r")) == 0) {
shimniok 0:9b27ebe1ce17 530 //pc.printf("File not found: <%s>\n", myname);
shimniok 0:9b27ebe1ce17 531 break;
shimniok 0:9b27ebe1ce17 532 } else {
shimniok 0:9b27ebe1ce17 533 //pc.printf("File exists: <%s>\n", myname);
shimniok 0:9b27ebe1ce17 534 fclose(fp);
shimniok 0:9b27ebe1ce17 535 }
shimniok 0:9b27ebe1ce17 536 }
shimniok 0:9b27ebe1ce17 537
shimniok 0:9b27ebe1ce17 538 fp = fopen(myname, "w");
shimniok 0:9b27ebe1ce17 539 if (fp == 0) {
shimniok 0:9b27ebe1ce17 540 pc.printf("file write failed: %s\n", myname);
shimniok 0:9b27ebe1ce17 541 } else {
shimniok 0:9b27ebe1ce17 542 //status = true;
shimniok 0:9b27ebe1ce17 543 pc.printf("opened %s for writing\n", myname);
shimniok 0:9b27ebe1ce17 544 fprintf(fp, "Millis, Gyro, GyroHdg, CompassHeading, Latitude, Longitude, Age, Date, Time, Altitude, Course, Speed, HDOP, Bearing, Distance, Left Enc, Right Enc, Gyro Temp, OdoHdg\n");
shimniok 0:9b27ebe1ce17 545 //fclose(fp);
shimniok 0:9b27ebe1ce17 546 }
shimniok 0:9b27ebe1ce17 547
shimniok 0:9b27ebe1ce17 548 return fp;
shimniok 0:9b27ebe1ce17 549 }
shimniok 0:9b27ebe1ce17 550
shimniok 0:9b27ebe1ce17 551
shimniok 0:9b27ebe1ce17 552 void initButtons()
shimniok 0:9b27ebe1ce17 553 {
shimniok 0:9b27ebe1ce17 554 // Set up button (plugs into two GPIOs, active low
shimniok 0:9b27ebe1ce17 555 buttonPower = 0;
shimniok 0:9b27ebe1ce17 556 selectButton.mode(PullUp);
shimniok 0:9b27ebe1ce17 557 selectButton.setSamplesTillAssert(20);
shimniok 0:9b27ebe1ce17 558 selectButton.setAssertValue(0); // active low logic
shimniok 0:9b27ebe1ce17 559 selectButton.setSampleFrequency(20); // 20us
shimniok 0:9b27ebe1ce17 560 selectButton.attach_asserted( &assertButton );
shimniok 0:9b27ebe1ce17 561 selectButton.attach_deasserted( &deassertButton );
shimniok 0:9b27ebe1ce17 562 }
shimniok 0:9b27ebe1ce17 563
shimniok 0:9b27ebe1ce17 564 void initCompass()
shimniok 0:9b27ebe1ce17 565 {
shimniok 0:9b27ebe1ce17 566 // Initialize compass; continuous mode, periodic set/reset, 20Hz measurement rate.
shimniok 0:9b27ebe1ce17 567 //compass.setOpMode(HMC6352_CONTINUOUS, 1, 20);
shimniok 0:9b27ebe1ce17 568
shimniok 0:9b27ebe1ce17 569 // Set calibration parameters for this particular LSM303DLH
shimniok 0:9b27ebe1ce17 570 //compass3d.setOffset(29.50, -0.50, 4.00);
shimniok 0:9b27ebe1ce17 571 //compass3d.setScale(1.00, 1.03, 1.21);
shimniok 0:9b27ebe1ce17 572 compass3d.setOffset(44.50, 5.00, -0.50); // Apr 11 testing
shimniok 0:9b27ebe1ce17 573 compass3d.setScale(1.00, 1.04, 1.29); // Apr 11 testing
shimniok 0:9b27ebe1ce17 574
shimniok 0:9b27ebe1ce17 575 //compass3d._debug = &pc;
shimniok 0:9b27ebe1ce17 576 }
shimniok 0:9b27ebe1ce17 577
shimniok 0:9b27ebe1ce17 578
shimniok 0:9b27ebe1ce17 579 void initGPS()
shimniok 0:9b27ebe1ce17 580 {
shimniok 0:9b27ebe1ce17 581 // Initialize the GPS comm and handler
shimniok 0:9b27ebe1ce17 582 //gps1.baud(57600); // LOCOSYS LS20031
shimniok 0:9b27ebe1ce17 583
shimniok 0:9b27ebe1ce17 584 // Set LCD baud rate ; has to be 4800
shimniok 0:9b27ebe1ce17 585 // because we share with 4800 bps GPS\
shimniok 0:9b27ebe1ce17 586 // send chr(124) and CTRL-L
shimniok 0:9b27ebe1ce17 587 //gps2.baud(9600);
shimniok 0:9b27ebe1ce17 588 //gps2.printf("Data Bus");
shimniok 0:9b27ebe1ce17 589 //gps2.printf("%c%c", 124, 12);
shimniok 0:9b27ebe1ce17 590
shimniok 0:9b27ebe1ce17 591 gps2.baud(4800); // Pharos iGPS-500
shimniok 0:9b27ebe1ce17 592
shimniok 0:9b27ebe1ce17 593 // Synchronize with GPS
shimniok 0:9b27ebe1ce17 594 //gps1Parse.reset_ready();
shimniok 0:9b27ebe1ce17 595 gps2Parse.reset_ready();
shimniok 0:9b27ebe1ce17 596 }
shimniok 0:9b27ebe1ce17 597
shimniok 0:9b27ebe1ce17 598 void initAHRS()
shimniok 0:9b27ebe1ce17 599 {
shimniok 0:9b27ebe1ce17 600 char data[MAX_BYTES];
shimniok 0:9b27ebe1ce17 601 int status;
shimniok 0:9b27ebe1ce17 602 int ok = 0; // counts number of command_complete messages
shimniok 0:9b27ebe1ce17 603 int c = 10; // timeout for status
shimniok 0:9b27ebe1ce17 604
shimniok 0:9b27ebe1ce17 605 ahrs.baud(115200);
shimniok 0:9b27ebe1ce17 606 ahrs.attach(recv1, Serial::RxIrq);
shimniok 0:9b27ebe1ce17 607 wait(0.5);
shimniok 0:9b27ebe1ce17 608
shimniok 0:9b27ebe1ce17 609 // Configure AHRS to use only acceleromters and gyro, no magnetometer
shimniok 0:9b27ebe1ce17 610 data[0] = Accel_EN;
shimniok 0:9b27ebe1ce17 611 ahrsParser.send_packet(&ahrs, SET_EKF_CONFIG, 1, data);
shimniok 0:9b27ebe1ce17 612 c = 10;
shimniok 0:9b27ebe1ce17 613 while (!ahrsParser.statusReady() && c-- > 0)
shimniok 0:9b27ebe1ce17 614 wait(0.1);
shimniok 0:9b27ebe1ce17 615 status = ahrsParser.status();
shimniok 0:9b27ebe1ce17 616 if (status == PT_COMMAND_COMPLETE) ok++;
shimniok 0:9b27ebe1ce17 617 pc.printf("SET_EKF_CONFIG: %02x %s\n", status, ahrsParser.statusString(status));
shimniok 0:9b27ebe1ce17 618
shimniok 0:9b27ebe1ce17 619 ahrsParser.send_packet(&ahrs, ZERO_RATE_GYROS, 0, 0);
shimniok 0:9b27ebe1ce17 620 c = 10;
shimniok 0:9b27ebe1ce17 621 while (!ahrsParser.statusReady() && c-- > 0)
shimniok 0:9b27ebe1ce17 622 wait(0.1);
shimniok 0:9b27ebe1ce17 623 status = ahrsParser.status();
shimniok 0:9b27ebe1ce17 624 if (status == PT_COMMAND_COMPLETE) ok++;
shimniok 0:9b27ebe1ce17 625 pc.printf("ZERO_RATE_GYROS: %02x %s\n", status, ahrsParser.statusString(status));
shimniok 0:9b27ebe1ce17 626
shimniok 0:9b27ebe1ce17 627 data[0] = 0; // 20Hz
shimniok 0:9b27ebe1ce17 628 ahrsParser.send_packet(&ahrs, SET_BROADCAST_MODE, 1, data);
shimniok 0:9b27ebe1ce17 629 c = 10;
shimniok 0:9b27ebe1ce17 630 while (!ahrsParser.statusReady() && c-- > 0)
shimniok 0:9b27ebe1ce17 631 wait(0.1);
shimniok 0:9b27ebe1ce17 632 status = ahrsParser.status();
shimniok 0:9b27ebe1ce17 633 if (status == PT_COMMAND_COMPLETE) ok++;
shimniok 0:9b27ebe1ce17 634 pc.printf("SET_BROADCAST_MODE: %02x %s\n", status, ahrsParser.statusString(status));
shimniok 0:9b27ebe1ce17 635
shimniok 0:9b27ebe1ce17 636 ahrsParser.send_packet(&ahrs, EKF_RESET, 0, 0);
shimniok 0:9b27ebe1ce17 637 c = 10;
shimniok 0:9b27ebe1ce17 638 while (!ahrsParser.statusReady() && c-- > 0)
shimniok 0:9b27ebe1ce17 639 wait(0.1);
shimniok 0:9b27ebe1ce17 640 status = ahrsParser.status();
shimniok 0:9b27ebe1ce17 641 if (status == PT_COMMAND_COMPLETE) ok++;
shimniok 0:9b27ebe1ce17 642 pc.printf("EKF_RESET: %02x %s\n", status, ahrsParser.statusString(status));
shimniok 0:9b27ebe1ce17 643 //ahrs.printf("%s", ahrsParser.send_packet()) // <--mag calibration here//
shimniok 0:9b27ebe1ce17 644
shimniok 0:9b27ebe1ce17 645 if (ok == 4) ahrsStatus = 1; // turn on status LED
shimniok 0:9b27ebe1ce17 646
shimniok 0:9b27ebe1ce17 647 ahrsParser.resetReady();
shimniok 0:9b27ebe1ce17 648 }
shimniok 0:9b27ebe1ce17 649
shimniok 0:9b27ebe1ce17 650
shimniok 0:9b27ebe1ce17 651 void initDR()
shimniok 0:9b27ebe1ce17 652 {
shimniok 0:9b27ebe1ce17 653 dr_here.set(wpt[0]); // Initialize Dead Reckoning to starting waypoint
shimniok 0:9b27ebe1ce17 654 dr_here_last.set(wpt[0]);
shimniok 0:9b27ebe1ce17 655 }
shimniok 0:9b27ebe1ce17 656
shimniok 0:9b27ebe1ce17 657
shimniok 0:9b27ebe1ce17 658 void initFlasher()
shimniok 0:9b27ebe1ce17 659 {
shimniok 0:9b27ebe1ce17 660 // Set up flasher schedule; 3 flashes every 80ms
shimniok 0:9b27ebe1ce17 661 // for 80ms total, with a 9x80ms period
shimniok 0:9b27ebe1ce17 662 blink.max(9);
shimniok 0:9b27ebe1ce17 663 blink.scale(80);
shimniok 0:9b27ebe1ce17 664 blink.mode(Schedule::repeat);
shimniok 0:9b27ebe1ce17 665 blink.set(0, 1); blink.set(2, 1); blink.set(4, 1);
shimniok 0:9b27ebe1ce17 666 }
shimniok 0:9b27ebe1ce17 667
shimniok 0:9b27ebe1ce17 668
shimniok 0:9b27ebe1ce17 669 void initSteering()
shimniok 0:9b27ebe1ce17 670 {
shimniok 0:9b27ebe1ce17 671 // Setup steering servo
shimniok 0:9b27ebe1ce17 672 steering = 0.5;
shimniok 0:9b27ebe1ce17 673 steering.calibrate(0.005, 45.0);
shimniok 0:9b27ebe1ce17 674 }
shimniok 0:9b27ebe1ce17 675
shimniok 0:9b27ebe1ce17 676
shimniok 0:9b27ebe1ce17 677 void initThrottle()
shimniok 0:9b27ebe1ce17 678 {
shimniok 0:9b27ebe1ce17 679 throttle = 0.5;
shimniok 0:9b27ebe1ce17 680 throttle.calibrate(0.0005, 45.0);
shimniok 0:9b27ebe1ce17 681 }
shimniok 0:9b27ebe1ce17 682
shimniok 0:9b27ebe1ce17 683
shimniok 0:9b27ebe1ce17 684 ///////////////////////////////////////////////////////////////////////////////////////////////////////
shimniok 0:9b27ebe1ce17 685 // OPERATIONAL MODE FUNCTIONS
shimniok 0:9b27ebe1ce17 686 ///////////////////////////////////////////////////////////////////////////////////////////////////////
shimniok 0:9b27ebe1ce17 687
shimniok 0:9b27ebe1ce17 688
shimniok 0:9b27ebe1ce17 689 void autonomousMode()
shimniok 0:9b27ebe1ce17 690 {
shimniok 0:9b27ebe1ce17 691 // Navigation
shimniok 0:9b27ebe1ce17 692 float interceptDist = 1.5; // Course correction intercept distance ; GPS_MAX_HDOP calculates to 20* SA
shimniok 0:9b27ebe1ce17 693
shimniok 0:9b27ebe1ce17 694 // Compass Variables
shimniok 0:9b27ebe1ce17 695 float compassHdg = 0.0; // compass heading
shimniok 0:9b27ebe1ce17 696 float compassHdg_last = -999; // heading at last GPS packet
shimniok 0:9b27ebe1ce17 697 float compassGain = COMPASS_GAIN;
shimniok 0:9b27ebe1ce17 698
shimniok 0:9b27ebe1ce17 699 // AHRS Variables
shimniok 0:9b27ebe1ce17 700 float yaw = 0.0; // course
shimniok 0:9b27ebe1ce17 701 float yawHdg = -1.0; // heading calculated from AHRS
shimniok 0:9b27ebe1ce17 702 float yawRate = 0.0; // calculated rate of change in yaw
shimniok 0:9b27ebe1ce17 703 float initialHdg = -999; // initial heading (course)
shimniok 0:9b27ebe1ce17 704 float yawGain = YAW_GAIN;
shimniok 0:9b27ebe1ce17 705
shimniok 0:9b27ebe1ce17 706 // Variables we calculate
shimniok 0:9b27ebe1ce17 707 double bearing = 0.0; // bearing to next waypoint
shimniok 0:9b27ebe1ce17 708 double distance = 0.0; // distance to next waypoint
shimniok 0:9b27ebe1ce17 709 float heading = 0.0; // estimated heading
shimniok 0:9b27ebe1ce17 710 float relativeBrg = 0.0; // relative bearing to next waypoint
shimniok 0:9b27ebe1ce17 711 double gps2_bearing = 0.0; // bearing, gps2
shimniok 0:9b27ebe1ce17 712 double gps2_distance = 0.0; // distance, gps2
shimniok 0:9b27ebe1ce17 713 float gps2_heading = 0.0; // current heading, gps2
shimniok 0:9b27ebe1ce17 714 double dr_bearing = 0.0; // bearing, dead reckoning
shimniok 0:9b27ebe1ce17 715 double dr_distance = 0.0; // distance, dead reckoning
shimniok 0:9b27ebe1ce17 716 float dr_heading = 0.0; // dead reckoning heading
shimniok 0:9b27ebe1ce17 717 float odo_heading = 0.0; // heading calc from odometry
shimniok 0:9b27ebe1ce17 718 double err_bearing = 0.0; // bearing between dr and gps
shimniok 0:9b27ebe1ce17 719 double err_distance = 0.0; // distance between dr and gps
shimniok 0:9b27ebe1ce17 720 float err_yaw = 0.0; // error in ahrs yaw versus gps heading.
shimniok 0:9b27ebe1ce17 721 float err_compass = 0.0; // error in compass versus gps heading
shimniok 0:9b27ebe1ce17 722 double gps2_off_dist = 0.0; // distance from gps fix to starting point
shimniok 0:9b27ebe1ce17 723 double gps2_off_brg = 0.0; // bearing from gps fix to starting point
shimniok 0:9b27ebe1ce17 724 double encDistance = 0.0; // encoder average distance recorded
shimniok 0:9b27ebe1ce17 725 double encSpeed = 0.0; // encoder calculated speed
shimniok 0:9b27ebe1ce17 726
shimniok 0:9b27ebe1ce17 727 goGoGo = false;
shimniok 0:9b27ebe1ce17 728
shimniok 0:9b27ebe1ce17 729 loadConfig(interceptDist, declination, compassGain, yawGain); // Load the waypoints, declination, and other config stuff
shimniok 0:9b27ebe1ce17 730 steerCalc.setIntercept(interceptDist); // Setup steering calculator based on intercept distance
shimniok 0:9b27ebe1ce17 731
shimniok 0:9b27ebe1ce17 732 go.set(UPDATE_PERIOD, 20, 650, maxSpeed, Schedule::hold); // Set throttle profile
shimniok 0:9b27ebe1ce17 733 //stop.set(UPDATE_PERIOD, 10, 500, 400, Schedule::hold); // Set brake profile
shimniok 0:9b27ebe1ce17 734 // TODO: Parameterize max brake
shimniok 0:9b27ebe1ce17 735 // obstacle: brake, speed up
shimniok 0:9b27ebe1ce17 736 // turn: brake or slowdown, speed up again
shimniok 0:9b27ebe1ce17 737 // brake speed up
shimniok 0:9b27ebe1ce17 738
shimniok 0:9b27ebe1ce17 739 initAHRS();
shimniok 0:9b27ebe1ce17 740
shimniok 0:9b27ebe1ce17 741 wptCurrent = 0;
shimniok 0:9b27ebe1ce17 742 initDR(); // initalize dead reckoning
shimniok 0:9b27ebe1ce17 743 wptCurrent++; // Point to the next waypoint; first wpt is the starting point
shimniok 0:9b27ebe1ce17 744 done = false;
shimniok 0:9b27ebe1ce17 745 buttonReleased = buttonPressed = false;
shimniok 0:9b27ebe1ce17 746 bool started = false; // flag to indicate robot has exceeded min speed.
shimniok 0:9b27ebe1ce17 747
shimniok 0:9b27ebe1ce17 748 // TODO--make sure all variables are initialized here
shimniok 0:9b27ebe1ce17 749 yawHdg = -1.0;
shimniok 0:9b27ebe1ce17 750 gyroCount = 0;
shimniok 0:9b27ebe1ce17 751 err_bearing = 0.0;
shimniok 0:9b27ebe1ce17 752 err_distance = 0.0;
shimniok 0:9b27ebe1ce17 753 err_compass = 0.0;
shimniok 0:9b27ebe1ce17 754 err_yaw = 0.0;
shimniok 0:9b27ebe1ce17 755
shimniok 0:9b27ebe1ce17 756
shimniok 0:9b27ebe1ce17 757 fp = initLogfile(); // Open the log file in sprintf format string style; numbers go in %d
shimniok 0:9b27ebe1ce17 758 wait(0.2);
shimniok 0:9b27ebe1ce17 759
shimniok 0:9b27ebe1ce17 760 //gps1.attach(recv1, Serial::RxIrq);
shimniok 0:9b27ebe1ce17 761 ahrs.attach(recv1, Serial::RxIrq);
shimniok 0:9b27ebe1ce17 762 gps2.attach(recv2, Serial::RxIrq);
shimniok 0:9b27ebe1ce17 763
shimniok 0:9b27ebe1ce17 764 // Figure out the "offset" or "bias" of the GPS reading
shimniok 0:9b27ebe1ce17 765 // since we know the robot's starting point
shimniok 0:9b27ebe1ce17 766
shimniok 0:9b27ebe1ce17 767 // 20110419 1052
shimniok 0:9b27ebe1ce17 768 findGPSBias(&gps2_off_brg, &gps2_off_dist, 2, wpt[0]);
shimniok 0:9b27ebe1ce17 769
shimniok 0:9b27ebe1ce17 770 // Find average compass heading and use for
shimniok 0:9b27ebe1ce17 771 // initializing base heading used by AHRS gyro+accel
shimniok 0:9b27ebe1ce17 772 // Compass LSM303DLH runs at 15Hz = .06666 sec
shimniok 0:9b27ebe1ce17 773 pc.printf("Computing initial yaw heading...\n");
shimniok 0:9b27ebe1ce17 774 initialHdg = 0.0;
shimniok 0:9b27ebe1ce17 775 for (int i = 0; i < 10; i++) {
shimniok 0:9b27ebe1ce17 776 float hdg = compassHeading();
shimniok 0:9b27ebe1ce17 777 initialHdg += hdg / 10.0;
shimniok 0:9b27ebe1ce17 778 pc.printf("Compass: %.2f Initial Heading: %.2f\n", hdg, initialHdg);
shimniok 0:9b27ebe1ce17 779 wait(0.05);
shimniok 0:9b27ebe1ce17 780 }
shimniok 0:9b27ebe1ce17 781 initialHdg = 91.0; // hard code due to issues
shimniok 0:9b27ebe1ce17 782 pc.printf("Initial Heading: %.2f\n", initialHdg);
shimniok 0:9b27ebe1ce17 783 odo_heading = initialHdg;
shimniok 0:9b27ebe1ce17 784
shimniok 0:9b27ebe1ce17 785 //while (!ahrsParser.dataReady()); // sync to ahrs
shimniok 0:9b27ebe1ce17 786 //ahrsParser.resetReady();
shimniok 0:9b27ebe1ce17 787 timer.reset(); // Keep track of milliseconds, elapsed
shimniok 0:9b27ebe1ce17 788 timer.start();
shimniok 0:9b27ebe1ce17 789
shimniok 0:9b27ebe1ce17 790 float lastYaw = ahrsParser.readYaw(); // try to capture rate of change
shimniok 0:9b27ebe1ce17 791 float initialHdgFilt = initialHdg; // initialize the initial heading filter
shimniok 0:9b27ebe1ce17 792 bool gpsSync = false; // Allows us to sync closer to actual fix time
shimniok 0:9b27ebe1ce17 793 int syncTime = 0; // stores next millisecond count at which to save DR data
shimniok 0:9b27ebe1ce17 794 bool firstGPS = 0; // is this the first GPS packet? If so, don't calc errors
shimniok 0:9b27ebe1ce17 795
shimniok 0:9b27ebe1ce17 796 // Main loop
shimniok 0:9b27ebe1ce17 797 //
shimniok 0:9b27ebe1ce17 798 while(done == false) {
shimniok 0:9b27ebe1ce17 799
shimniok 0:9b27ebe1ce17 800 int millis = timer.read_ms();
shimniok 0:9b27ebe1ce17 801
shimniok 0:9b27ebe1ce17 802 // reset led status; allows blinking, 10ms long
shimniok 0:9b27ebe1ce17 803 if ((millis % UPDATE_PERIOD) == 10) {
shimniok 0:9b27ebe1ce17 804 logStatus = (fp != 0) ? 1 : 0;
shimniok 0:9b27ebe1ce17 805 //ahrsStatus = (gps1Parse.f_hdop() > 0.0 && gps1Parse.f_hdop() < 10.0) ? 1 : 0;
shimniok 0:9b27ebe1ce17 806 gps2Status = (gps2Parse.f_hdop() > 0.0 && gps2Parse.f_hdop() < 10.0) ? 1 : 0;
shimniok 0:9b27ebe1ce17 807 }
shimniok 0:9b27ebe1ce17 808
shimniok 0:9b27ebe1ce17 809 // Warning flasher
shimniok 0:9b27ebe1ce17 810 //if (blink.ticked(millis)) {
shimniok 0:9b27ebe1ce17 811 // flasher = blink.get();
shimniok 0:9b27ebe1ce17 812 //}
shimniok 0:9b27ebe1ce17 813
shimniok 0:9b27ebe1ce17 814 // Button state machine
shimniok 0:9b27ebe1ce17 815 // if we've not started going, button starts us
shimniok 0:9b27ebe1ce17 816 // if we have started going, button stops us
shimniok 0:9b27ebe1ce17 817 // but only if we've released it first
shimniok 0:9b27ebe1ce17 818 //
shimniok 0:9b27ebe1ce17 819 // set throttle only if goGoGo set
shimniok 0:9b27ebe1ce17 820 if (goGoGo) {
shimniok 0:9b27ebe1ce17 821 if (go.ticked(millis)) {
shimniok 0:9b27ebe1ce17 822 throttle = go.get() / 1000.0;
shimniok 0:9b27ebe1ce17 823 //pc.printf("throttle: %0.3f\n", throttle.read());
shimniok 0:9b27ebe1ce17 824 }
shimniok 0:9b27ebe1ce17 825 if (buttonPressed && started) {
shimniok 0:9b27ebe1ce17 826 pc.printf(">>>>>>>>>>>>>>>>>>>>>>> HALT\n");
shimniok 0:9b27ebe1ce17 827 done = true;
shimniok 0:9b27ebe1ce17 828 goGoGo = false;
shimniok 0:9b27ebe1ce17 829 }
shimniok 0:9b27ebe1ce17 830 } else {
shimniok 0:9b27ebe1ce17 831 if (buttonPressed == true) {
shimniok 0:9b27ebe1ce17 832 pc.printf(">>>>>>>>>>>>>>>>>>>>>>> GO GO GO\n");
shimniok 0:9b27ebe1ce17 833 goGoGo = true;
shimniok 0:9b27ebe1ce17 834 buttonPressed = false;
shimniok 0:9b27ebe1ce17 835 }
shimniok 0:9b27ebe1ce17 836 }
shimniok 0:9b27ebe1ce17 837
shimniok 0:9b27ebe1ce17 838
shimniok 0:9b27ebe1ce17 839 // UPDATE EVERY UPDATE_PERIOD ms (50Hz)
shimniok 0:9b27ebe1ce17 840 // The compass updates only ever 50ms
shimniok 0:9b27ebe1ce17 841 // The gyro updates however fast we can read it
shimniok 0:9b27ebe1ce17 842 //
shimniok 0:9b27ebe1ce17 843
shimniok 0:9b27ebe1ce17 844 if ((millis % UPDATE_PERIOD) == 0) {
shimniok 0:9b27ebe1ce17 845 unsigned int adcResult[8];
shimniok 0:9b27ebe1ce17 846
shimniok 0:9b27ebe1ce17 847 //WHERE();
shimniok 0:9b27ebe1ce17 848
shimniok 0:9b27ebe1ce17 849 adc.setChannel(0);
shimniok 0:9b27ebe1ce17 850 for (int i=0; i < 8; i++) {
shimniok 0:9b27ebe1ce17 851 adcResult[i] = adc.read();
shimniok 0:9b27ebe1ce17 852 }
shimniok 0:9b27ebe1ce17 853
shimniok 0:9b27ebe1ce17 854 temp = adcResult[TEMP_CHAN];
shimniok 0:9b27ebe1ce17 855 // <== read/temp calibrate here
shimniok 0:9b27ebe1ce17 856 // Convert gyro adc reading to heading
shimniok 0:9b27ebe1ce17 857 // TODO:
shimniok 0:9b27ebe1ce17 858 // automatically calculate null at start
shimniok 0:9b27ebe1ce17 859 //gyro = gyroRate(adcResult[GYRO_CHAN]);
shimniok 0:9b27ebe1ce17 860
shimniok 0:9b27ebe1ce17 861 yaw = ahrsParser.readYaw(); // <-- this may have been causing trouble before. 20110420 1249
shimniok 0:9b27ebe1ce17 862 // 20110421
shimniok 0:9b27ebe1ce17 863 yawRate = yaw - lastYaw;
shimniok 0:9b27ebe1ce17 864 lastYaw = yaw;
shimniok 0:9b27ebe1ce17 865
shimniok 0:9b27ebe1ce17 866 // If we're stopped, set initialHdg to compass
shimniok 0:9b27ebe1ce17 867 // But do a running average / leaky integrator to reduce some noise
shimniok 0:9b27ebe1ce17 868 // 20110421
shimniok 0:9b27ebe1ce17 869 if (encSpeed == 0) {
shimniok 0:9b27ebe1ce17 870 findGPSBias(&gps2_off_brg, &gps2_off_dist, 1, wpt[0]);
shimniok 0:9b27ebe1ce17 871 initialHdgFilt += compassHdg - (initialHdgFilt * 0.25);
shimniok 0:9b27ebe1ce17 872 initialHdg = initialHdgFilt * 0.25;
shimniok 0:9b27ebe1ce17 873 while (initialHdg < 0.0) initialHdg += 360.0;
shimniok 0:9b27ebe1ce17 874 while (initialHdg >= 360.0) initialHdg -= 360.0;
shimniok 0:9b27ebe1ce17 875 }
shimniok 0:9b27ebe1ce17 876
shimniok 0:9b27ebe1ce17 877 // Substitute 6dof AHRS yaw for 1-axis gyro and 6dof compass
shimniok 0:9b27ebe1ce17 878 yawHdg = initialHdg + yaw;
shimniok 0:9b27ebe1ce17 879
shimniok 0:9b27ebe1ce17 880 while (yawHdg < 0) yawHdg += 360.0;
shimniok 0:9b27ebe1ce17 881 while (yawHdg >= 360.0) yawHdg -= 360.0;
shimniok 0:9b27ebe1ce17 882
shimniok 0:9b27ebe1ce17 883 compassHdg = compassHeading();
shimniok 0:9b27ebe1ce17 884
shimniok 0:9b27ebe1ce17 885 // pc.printf("Gyro: %d GyroHdg: %.1f\n", gyro, yaw);
shimniok 0:9b27ebe1ce17 886
shimniok 0:9b27ebe1ce17 887 // Need a better filtered heading output
shimniok 0:9b27ebe1ce17 888 // for now just use gyro heading as it seems to be most accurate
shimniok 0:9b27ebe1ce17 889 // provided the bias is set right.
shimniok 0:9b27ebe1ce17 890
shimniok 0:9b27ebe1ce17 891 // Odometry
shimniok 0:9b27ebe1ce17 892 unsigned int leftCount = left.read();
shimniok 0:9b27ebe1ce17 893 unsigned int rightCount = right.read();
shimniok 0:9b27ebe1ce17 894
shimniok 0:9b27ebe1ce17 895 // TODO--> sanity check on encoders; if difference between them
shimniok 0:9b27ebe1ce17 896 // is huge, what do we do? Slipping wheel? Skidding wheel?
shimniok 0:9b27ebe1ce17 897
shimniok 0:9b27ebe1ce17 898 double leftDist = (WHEEL_CIRC / 32) * (double) leftCount;
shimniok 0:9b27ebe1ce17 899 double rightDist = (WHEEL_CIRC / 32) * (double) rightCount;
shimniok 0:9b27ebe1ce17 900 encDistance = (leftDist + rightDist) / 2.0;
shimniok 0:9b27ebe1ce17 901 encSpeed = encDistance / (UPDATE_PERIOD * 0.001);
shimniok 0:9b27ebe1ce17 902
shimniok 0:9b27ebe1ce17 903 odo_heading += (leftDist - rightDist) / TRACK;
shimniok 0:9b27ebe1ce17 904
shimniok 0:9b27ebe1ce17 905 if ((millis % 500) == 0) {
shimniok 0:9b27ebe1ce17 906 pc.printf("Odo heading: %.1f\n", odo_heading);
shimniok 0:9b27ebe1ce17 907 }
shimniok 0:9b27ebe1ce17 908
shimniok 0:9b27ebe1ce17 909 // 20110421
shimniok 0:9b27ebe1ce17 910 if (encSpeed < GPS_MIN_SPEED) {
shimniok 0:9b27ebe1ce17 911 dr_heading = yawHdg;
shimniok 0:9b27ebe1ce17 912 } else {
shimniok 0:9b27ebe1ce17 913 started = true;
shimniok 0:9b27ebe1ce17 914 dr_heading = compassHdg;
shimniok 0:9b27ebe1ce17 915
shimniok 0:9b27ebe1ce17 916 // Yaw error
shimniok 0:9b27ebe1ce17 917 // Let yaw track closely behind compass which
shimniok 0:9b27ebe1ce17 918 // tracks closely behind GPS
shimniok 0:9b27ebe1ce17 919 err_yaw = clamp180(compassHdg - yawHdg);
shimniok 0:9b27ebe1ce17 920
shimniok 0:9b27ebe1ce17 921 // fix yaw by tweaking the initial heading
shimniok 0:9b27ebe1ce17 922 // which is a kludge but... hey it's 2 days before the competition
shimniok 0:9b27ebe1ce17 923 initialHdg += err_yaw * yawGain;
shimniok 0:9b27ebe1ce17 924 clamp360(initialHdg);
shimniok 0:9b27ebe1ce17 925
shimniok 0:9b27ebe1ce17 926 }
shimniok 0:9b27ebe1ce17 927 while (dr_heading < 0) dr_heading += 360.0;
shimniok 0:9b27ebe1ce17 928 while (dr_heading >= 360.0) dr_heading -= 360.0;
shimniok 0:9b27ebe1ce17 929
shimniok 0:9b27ebe1ce17 930 //pc.printf("%d dr_heading: %.2f\n", __LINE__, dr_heading);
shimniok 0:9b27ebe1ce17 931
shimniok 0:9b27ebe1ce17 932 // Dead Reckoning Position estimation, very simple stuff
shimniok 0:9b27ebe1ce17 933 // lat: north +y, south -y ; cos(hdg)
shimniok 0:9b27ebe1ce17 934 // long: west -x, east +x ; sin(hdg)
shimniok 0:9b27ebe1ce17 935 // 1852m = 1 nautical mile = 1 deg lat / long
shimniok 0:9b27ebe1ce17 936 dr_here.move(dr_heading, encDistance);
shimniok 0:9b27ebe1ce17 937
shimniok 0:9b27ebe1ce17 938 ////////////////////////////////
shimniok 0:9b27ebe1ce17 939 // Correct position here
shimniok 0:9b27ebe1ce17 940
shimniok 0:9b27ebe1ce17 941 //
shimniok 0:9b27ebe1ce17 942 //
shimniok 0:9b27ebe1ce17 943 ////////////////////////////////
shimniok 0:9b27ebe1ce17 944
shimniok 0:9b27ebe1ce17 945 dr_bearing = wpt[wptCurrent].bearing(dr_here);
shimniok 0:9b27ebe1ce17 946 dr_distance = wpt[wptCurrent].distance(dr_here);
shimniok 0:9b27ebe1ce17 947
shimniok 0:9b27ebe1ce17 948 //pc.printf("dr_heading: %.1f encDistance: %.3f\n", dr_heading, encDistance);
shimniok 0:9b27ebe1ce17 949
shimniok 0:9b27ebe1ce17 950 // Log dead reckoning info
shimniok 0:9b27ebe1ce17 951 logStatus = 0;
shimniok 0:9b27ebe1ce17 952 fprintf(fp, "%d, %.1f, %.1f, %.1f, %.8f, %.8f, , , , , , %.1f, %.1f, , %.1f, %.3f, %d, %d, %d, %.1f\n",
shimniok 0:9b27ebe1ce17 953 millis, yaw, yawHdg, compassHdg, dr_here.latitude(), dr_here.longitude(), dr_heading, encSpeed,
shimniok 0:9b27ebe1ce17 954 dr_bearing, dr_distance, left.readTotal(), right.readTotal(), temp, odo_heading);
shimniok 0:9b27ebe1ce17 955
shimniok 0:9b27ebe1ce17 956 //pc.printf("Yaw = %.1f\n", ahrsParser.readYaw());
shimniok 0:9b27ebe1ce17 957
shimniok 0:9b27ebe1ce17 958 // synchronize when RMC and GGA sentences received w/ AHRS
shimniok 0:9b27ebe1ce17 959 if (gps2Parse.rmc_ready() && gps2Parse.gga_ready()) {
shimniok 0:9b27ebe1ce17 960 char gps2date[32], gps2time[32];
shimniok 0:9b27ebe1ce17 961
shimniok 0:9b27ebe1ce17 962 // We synchronize on the first GPS packet that doesn't have GSV sentences
shimniok 0:9b27ebe1ce17 963 // then every 1000ms after, we will save DR data for error comparison
shimniok 0:9b27ebe1ce17 964 if (!gps2Parse.gsv_ready() && !gpsSync) {
shimniok 0:9b27ebe1ce17 965 syncTime = millis + 1000;
shimniok 0:9b27ebe1ce17 966 gpsSync = true;
shimniok 0:9b27ebe1ce17 967 }
shimniok 0:9b27ebe1ce17 968
shimniok 0:9b27ebe1ce17 969 doGPS(gps2Parse, here2, gps2Status, gps2date, gps2time);
shimniok 0:9b27ebe1ce17 970
shimniok 0:9b27ebe1ce17 971 if (gps2_hdop < GPS_MAX_HDOP) {
shimniok 0:9b27ebe1ce17 972
shimniok 0:9b27ebe1ce17 973 // Correct here2 for offset (bias)
shimniok 0:9b27ebe1ce17 974 here2.move(gps2_off_brg, gps2_off_dist);
shimniok 0:9b27ebe1ce17 975
shimniok 0:9b27ebe1ce17 976 //pc.printf("GPS2 %d HDOP: %.1f Compass: %.1f Gyro: %d Gyro Temp: %d\n", millis, gps1Parse.f_hdop(), compassHdg, gyro, temp);
shimniok 0:9b27ebe1ce17 977
shimniok 0:9b27ebe1ce17 978 gps2_bearing = wpt[wptCurrent].bearing(here2);
shimniok 0:9b27ebe1ce17 979 gps2_distance = wpt[wptCurrent].distance(here2);
shimniok 0:9b27ebe1ce17 980 gps2_heading = gps2Parse.f_course();
shimniok 0:9b27ebe1ce17 981
shimniok 0:9b27ebe1ce17 982 //pc.printf("GPS2: lat: %.6f lon: %.6f wpt[%d]: lat:%.6f lon:%.6f distance=%.2f\n",
shimniok 0:9b27ebe1ce17 983 // here2.latitude(), here2.longitude(), wptCurrent,
shimniok 0:9b27ebe1ce17 984 // wpt[wptCurrent].latitude(), wpt[wptCurrent].longitude(), distance);
shimniok 0:9b27ebe1ce17 985
shimniok 0:9b27ebe1ce17 986 // TODO --> Estimate new DR position here
shimniok 0:9b27ebe1ce17 987 // simple: take a weighted average midpoint
shimniok 0:9b27ebe1ce17 988 // TODO --> gradually move dr position by using same err distance/bearing but move a fraction of the distance
shimniok 0:9b27ebe1ce17 989 // over the next n samples between gps readings
shimniok 0:9b27ebe1ce17 990 // We have to address GPS lag. The easy (but probably incorrect way) is to use the DR info at the last time
shimniok 0:9b27ebe1ce17 991 // we received a GPS update. Because the GPS is spitting out CSV sentences every 5 seconds or so, that
shimniok 0:9b27ebe1ce17 992 // throws off the timing.
shimniok 0:9b27ebe1ce17 993
shimniok 0:9b27ebe1ce17 994 if (!firstGPS) {
shimniok 0:9b27ebe1ce17 995
shimniok 0:9b27ebe1ce17 996 // 20110421
shimniok 0:9b27ebe1ce17 997 // Position error
shimniok 0:9b27ebe1ce17 998 err_distance = here2.distance(dr_here_last);
shimniok 0:9b27ebe1ce17 999 err_bearing = here2.bearing(dr_here_last);
shimniok 0:9b27ebe1ce17 1000
shimniok 0:9b27ebe1ce17 1001 ////////////////////////////////////////////////////////////////////////////////////////////////////////////
shimniok 0:9b27ebe1ce17 1002 // Error Correction
shimniok 0:9b27ebe1ce17 1003 ////////////////////////////////////////////////////////////////////////////////////////////////////////////
shimniok 0:9b27ebe1ce17 1004 // The strategy is: rely on GPS when hdop < 2; corrected by distance from initial starting point. At
shimniok 0:9b27ebe1ce17 1005 // hdop > 2, gps position becomes unreliable, so fall back on compass and/or AHRS yaw and dead reckoning.
shimniok 0:9b27ebe1ce17 1006 // The latter is pretty reliable. But heading isn't. Compass err varies based on, apparently, mag field
shimniok 0:9b27ebe1ce17 1007 // from the motor/wires. We'll stick to a set speed and correct compass error while relying on gps. That
shimniok 0:9b27ebe1ce17 1008 // plus position correction should keep DR up to date with the last gps fix with hdop < GPS_MAX_HDOP
shimniok 0:9b27ebe1ce17 1009 // Meanwhile, AHRS yaw seems to be filtered to the point that bandwidth can't track even moderate turn
shimniok 0:9b27ebe1ce17 1010 // rates. The exact cause is yet undiscovered. The onboard compass has a distortion I've been unable
shimniok 0:9b27ebe1ce17 1011 // to resolve, so we're running only gyro+accelerometer
shimniok 0:9b27ebe1ce17 1012 //
shimniok 0:9b27ebe1ce17 1013 // At higher speeds, we can trust gps course, but at lower speeds we can't. Initially when we start, the
shimniok 0:9b27ebe1ce17 1014 // most reliable heading/DR information comes from encoders+AHRS. At speed = 0, yaw and compass are very
shimniok 0:9b27ebe1ce17 1015 // reliable, so yaw initial heading is set from compass. So we'll use dead reckoning until the vehicle
shimniok 0:9b27ebe1ce17 1016 // reaches a higher speed, at which point, position info comes from gps and is used for yaw/compass/position
shimniok 0:9b27ebe1ce17 1017 // correction.
shimniok 0:9b27ebe1ce17 1018 //
shimniok 0:9b27ebe1ce17 1019 // I wished I could figure out a mathematically fancy way to do this but I'm confounded by all the different
shimniok 0:9b27ebe1ce17 1020 // sensor errors. KF is the only thing I've sort-of learned so far and I don't yet see how I can use it
shimniok 0:9b27ebe1ce17 1021 // for the types of (apparently non-gaussian) noise/error I'm seeing.
shimniok 0:9b27ebe1ce17 1022 ////////////////////////////////////////////////////////////////////////////////////////////////////////////
shimniok 0:9b27ebe1ce17 1023 // 20110421
shimniok 0:9b27ebe1ce17 1024 if (encSpeed >= GPS_MIN_SPEED) {
shimniok 0:9b27ebe1ce17 1025 // Heading error
shimniok 0:9b27ebe1ce17 1026 err_compass = compassHdg_last - gps2_heading;
shimniok 0:9b27ebe1ce17 1027 if (err_compass < -180.0) err_compass += 360.0;
shimniok 0:9b27ebe1ce17 1028 if (err_compass > 180.0) err_compass -= 360.0;
shimniok 0:9b27ebe1ce17 1029 // we'll use err_compass when computing compass heading from now on
shimniok 0:9b27ebe1ce17 1030
shimniok 0:9b27ebe1ce17 1031 // fix compass by tweaking declination
shimniok 0:9b27ebe1ce17 1032 // again, no time to do anything elegant, just hacking here
shimniok 0:9b27ebe1ce17 1033 declination += err_compass * compassGain;
shimniok 0:9b27ebe1ce17 1034 clamp360(declination);
shimniok 0:9b27ebe1ce17 1035
shimniok 0:9b27ebe1ce17 1036 // 20110421
shimniok 0:9b27ebe1ce17 1037 fprintf(fp, "ERR: err_dist: %.5f err_brg: %.2f compassHdg_last: %.2f gps2_heading: %.2f err_compass: %.5f declination: %.2f\n",
shimniok 0:9b27ebe1ce17 1038 err_distance, err_bearing, compassHdg_last, gps2_heading, err_compass, declination);
shimniok 0:9b27ebe1ce17 1039 }
shimniok 0:9b27ebe1ce17 1040 } // if (!firstGPS)
shimniok 0:9b27ebe1ce17 1041
shimniok 0:9b27ebe1ce17 1042 // 20110421
shimniok 0:9b27ebe1ce17 1043 // Save currnt DR position / heading
shimniok 0:9b27ebe1ce17 1044 // change this so that after gpsSync == true, we only save this
shimniok 0:9b27ebe1ce17 1045 // when millis == syncTime, then syncTime += 1000
shimniok 0:9b27ebe1ce17 1046 dr_here_last.set( dr_here );
shimniok 0:9b27ebe1ce17 1047 compassHdg_last = compassHdg;
shimniok 0:9b27ebe1ce17 1048 firstGPS = false;
shimniok 0:9b27ebe1ce17 1049
shimniok 0:9b27ebe1ce17 1050 // less simple: find distance between & bearing, mult dist by weight, plot new position with move()
shimniok 0:9b27ebe1ce17 1051 // harder: kalman filter
shimniok 0:9b27ebe1ce17 1052 // ALSO---> need to do some kind of sanity check. If distance between each exceeds threshold, then one is bad
shimniok 0:9b27ebe1ce17 1053 // how do we guess which? Will gps ever be way off with low hdop?
shimniok 0:9b27ebe1ce17 1054 // maybe compare to predicted location based on last heading/speed?
shimniok 0:9b27ebe1ce17 1055 // should we convert to UTM?
shimniok 0:9b27ebe1ce17 1056 }
shimniok 0:9b27ebe1ce17 1057
shimniok 0:9b27ebe1ce17 1058 logStatus = 0;
shimniok 0:9b27ebe1ce17 1059 //WHERE();
shimniok 0:9b27ebe1ce17 1060 fprintf(fp, "%d, %.1f, %.1f, %.1f, %.8f, %.8f, GPS2, %s, %s, %.2f, %.1f, %.1f, %.1f, %.1f, %.3f, , , , %d, \n",
shimniok 0:9b27ebe1ce17 1061 millis, gyro, yawHdg, compassHdg, here2.latitude(), here2.longitude(), gps2date, gps2time,
shimniok 0:9b27ebe1ce17 1062 gps2Parse.f_altitude(), // <-- was hanging here... but probably due to goof up somewhere else. Stack corruption??
shimniok 0:9b27ebe1ce17 1063 gps2_heading, gps2Parse.f_speed_mph(), gps2Parse.f_hdop(), bearing, distance, temp);
shimniok 0:9b27ebe1ce17 1064 //WHERE();
shimniok 0:9b27ebe1ce17 1065
shimniok 0:9b27ebe1ce17 1066 }
shimniok 0:9b27ebe1ce17 1067
shimniok 0:9b27ebe1ce17 1068 //////////////////////////////////////////////////////////////////////////////
shimniok 0:9b27ebe1ce17 1069 // Update steering and throttle
shimniok 0:9b27ebe1ce17 1070 //////////////////////////////////////////////////////////////////////////////
shimniok 0:9b27ebe1ce17 1071 if ((millis % 100) == 0) {
shimniok 0:9b27ebe1ce17 1072
shimniok 0:9b27ebe1ce17 1073 // TODO --> Calculate bearing/distance from GPS and DR
shimniok 0:9b27ebe1ce17 1074
shimniok 0:9b27ebe1ce17 1075 // Might be able to do kalman filter here but... meanwhile:
shimniok 0:9b27ebe1ce17 1076 // 1) Weighted averaging of heading using GPS and Gyro to account for drift
shimniok 0:9b27ebe1ce17 1077 // 2) Correct GPS position for bias (brg/dist)
shimniok 0:9b27ebe1ce17 1078 // 3) Take corrected GPS position, go back 2 seconds, and find error brg/dist with DR at that time
shimniok 0:9b27ebe1ce17 1079 // 4) calculate a weighted average position based on DR and GPS, hdop and SV change
shimniok 0:9b27ebe1ce17 1080 // 5) apportion some of the error back into the GPS position bias (brg/dist) weighted on hdop and SV changes
shimniok 0:9b27ebe1ce17 1081
shimniok 0:9b27ebe1ce17 1082
shimniok 0:9b27ebe1ce17 1083 //////////////////////////////////////////////////////////////////////////////
shimniok 0:9b27ebe1ce17 1084 // Navigation -- see error correction above
shimniok 0:9b27ebe1ce17 1085 // We'll always navigate off DR but only because we're supposed to be
shimniok 0:9b27ebe1ce17 1086 // correcting it as we go via GPS
shimniok 0:9b27ebe1ce17 1087 //////////////////////////////////////////////////////////////////////////////
shimniok 0:9b27ebe1ce17 1088 // 20110421
shimniok 0:9b27ebe1ce17 1089 bearing = dr_bearing;
shimniok 0:9b27ebe1ce17 1090 distance = dr_distance;
shimniok 0:9b27ebe1ce17 1091 heading = dr_heading;
shimniok 0:9b27ebe1ce17 1092
shimniok 0:9b27ebe1ce17 1093 relativeBrg = bearing - heading; // 0 is desired heading
shimniok 0:9b27ebe1ce17 1094
shimniok 0:9b27ebe1ce17 1095 // limit steering angle based on object detection ?
shimniok 0:9b27ebe1ce17 1096 // or limit relative brg perhaps?
shimniok 0:9b27ebe1ce17 1097
shimniok 0:9b27ebe1ce17 1098 // if correction angle is < -180, express as negative degree
shimniok 0:9b27ebe1ce17 1099 if (relativeBrg < -180.0) relativeBrg += 360.0;
shimniok 0:9b27ebe1ce17 1100 if (relativeBrg > 180.0) relativeBrg -= 360.0;
shimniok 0:9b27ebe1ce17 1101
shimniok 0:9b27ebe1ce17 1102 float steerAngle = steerCalc.calcSA(relativeBrg);
shimniok 0:9b27ebe1ce17 1103
shimniok 0:9b27ebe1ce17 1104 // Convert steerAngle to servo value
shimniok 0:9b27ebe1ce17 1105 // Testing determined near linear conversion between servo ms setting and steering angle
shimniok 0:9b27ebe1ce17 1106 // up to 20*. Assumes a particular servo library with range = 0.005
shimniok 0:9b27ebe1ce17 1107 // In that case, f(SA) = servoPosition = 0.500 + SA/762.5
shimniok 0:9b27ebe1ce17 1108 // between 20 and 24* the slop is approximately 475
shimniok 0:9b27ebe1ce17 1109 // What if we ignore the linearity and just set to a max angle
shimniok 0:9b27ebe1ce17 1110 // also range is 0.535-0.460 --> slope = 800
shimniok 0:9b27ebe1ce17 1111 //steering = 0.500 + (double) steerAngle / 762.5;
shimniok 0:9b27ebe1ce17 1112
shimniok 0:9b27ebe1ce17 1113 if (encSpeed < GPS_MIN_SPEED) {
shimniok 0:9b27ebe1ce17 1114 steering = 0.495;
shimniok 0:9b27ebe1ce17 1115 } else {
shimniok 0:9b27ebe1ce17 1116 steering = 0.500 + (double) steerAngle / 808.0;
shimniok 0:9b27ebe1ce17 1117 }
shimniok 0:9b27ebe1ce17 1118 pc.printf("Wpt #%d: rel bearing: %.1f, bearing: %.1f, distance: %.5f\n", wptCurrent, relativeBrg, bearing, distance);
shimniok 0:9b27ebe1ce17 1119
shimniok 0:9b27ebe1ce17 1120 // if within 3m move to next waypoint
shimniok 0:9b27ebe1ce17 1121 // PARAMETERIZE DISTANCE
shimniok 0:9b27ebe1ce17 1122 if (distance < 3.0) {
shimniok 0:9b27ebe1ce17 1123 pc.printf("Arrived at wpt %d\n", wptCurrent);
shimniok 0:9b27ebe1ce17 1124 wptCurrent++;
shimniok 0:9b27ebe1ce17 1125 }
shimniok 0:9b27ebe1ce17 1126
shimniok 0:9b27ebe1ce17 1127 // Are we at the last waypoint?
shimniok 0:9b27ebe1ce17 1128 if (wptCurrent == wptCount) {
shimniok 0:9b27ebe1ce17 1129 pc.printf("Arrived at final destination. Done.\n");
shimniok 0:9b27ebe1ce17 1130 done = true;
shimniok 0:9b27ebe1ce17 1131 }
shimniok 0:9b27ebe1ce17 1132
shimniok 0:9b27ebe1ce17 1133 }
shimniok 0:9b27ebe1ce17 1134
shimniok 0:9b27ebe1ce17 1135
shimniok 0:9b27ebe1ce17 1136 /*
shimniok 0:9b27ebe1ce17 1137 if ((millis % 1000) == 0) {
shimniok 0:9b27ebe1ce17 1138 pc.printf("Wpt #%d: rel bearing: %.1f, bearing: %.1f, distance: %.5f\n", wptCurrent, relativeBrg, bearing, distance);
shimniok 0:9b27ebe1ce17 1139 pc.printf("dr_heading: %.1f encDistance: %.3f\n", dr_heading, encDistance);
shimniok 0:9b27ebe1ce17 1140 pc.printf("err_dist: %.5f\nerr_brg: %.1f\n\n", err_distance, err_bearing);
shimniok 0:9b27ebe1ce17 1141 }
shimniok 0:9b27ebe1ce17 1142 */
shimniok 0:9b27ebe1ce17 1143
shimniok 0:9b27ebe1ce17 1144 }
shimniok 0:9b27ebe1ce17 1145
shimniok 0:9b27ebe1ce17 1146 } // while
shimniok 0:9b27ebe1ce17 1147
shimniok 0:9b27ebe1ce17 1148 // shut 'er down!
shimniok 0:9b27ebe1ce17 1149 throttle = 0.500;
shimniok 0:9b27ebe1ce17 1150 steering = 0.500;
shimniok 0:9b27ebe1ce17 1151
shimniok 0:9b27ebe1ce17 1152 if (fp) {
shimniok 0:9b27ebe1ce17 1153 fclose(fp);
shimniok 0:9b27ebe1ce17 1154 logStatus = 0;
shimniok 0:9b27ebe1ce17 1155 pc.printf("closed log file.\n");
shimniok 0:9b27ebe1ce17 1156 }
shimniok 0:9b27ebe1ce17 1157
shimniok 0:9b27ebe1ce17 1158 //ahrsStatus = 0;
shimniok 0:9b27ebe1ce17 1159 gps2Status = 0;
shimniok 0:9b27ebe1ce17 1160 confStatus = 0;
shimniok 0:9b27ebe1ce17 1161 flasher = 0;
shimniok 0:9b27ebe1ce17 1162
shimniok 0:9b27ebe1ce17 1163 } // autonomousMode
shimniok 0:9b27ebe1ce17 1164
shimniok 0:9b27ebe1ce17 1165
shimniok 0:9b27ebe1ce17 1166 void idleMode()
shimniok 0:9b27ebe1ce17 1167 {
shimniok 0:9b27ebe1ce17 1168 while (1) {
shimniok 0:9b27ebe1ce17 1169 ahrsStatus = 0;
shimniok 0:9b27ebe1ce17 1170 wait(0.2);
shimniok 0:9b27ebe1ce17 1171 ahrsStatus = 1;
shimniok 0:9b27ebe1ce17 1172 wait(0.2);
shimniok 0:9b27ebe1ce17 1173 }
shimniok 0:9b27ebe1ce17 1174 }
shimniok 0:9b27ebe1ce17 1175
shimniok 0:9b27ebe1ce17 1176 ///////////////////////////////////////////////////////////////////////////////////////////////////////
shimniok 0:9b27ebe1ce17 1177 // UTILITIY FUNCTIONS
shimniok 0:9b27ebe1ce17 1178 ///////////////////////////////////////////////////////////////////////////////////////////////////////
shimniok 0:9b27ebe1ce17 1179
shimniok 0:9b27ebe1ce17 1180
shimniok 0:9b27ebe1ce17 1181 // Takes n position readings and averages the gps position
shimniok 0:9b27ebe1ce17 1182 // in a pretty rudimentary fashion, returning bearing and distance
shimniok 0:9b27ebe1ce17 1183 // from the specified place to the average position
shimniok 0:9b27ebe1ce17 1184 void findGPSBias(double *brg, double *dist, int n, GeoPosition place)
shimniok 0:9b27ebe1ce17 1185 {
shimniok 0:9b27ebe1ce17 1186 // GPS Position averaging
shimniok 0:9b27ebe1ce17 1187 double div = (double) n;
shimniok 0:9b27ebe1ce17 1188 double latavg = 0;
shimniok 0:9b27ebe1ce17 1189 double lonavg = 0;
shimniok 0:9b27ebe1ce17 1190 double lat = 0;
shimniok 0:9b27ebe1ce17 1191 double lon = 0;
shimniok 0:9b27ebe1ce17 1192 gps2Parse.reset_ready();
shimniok 0:9b27ebe1ce17 1193
shimniok 0:9b27ebe1ce17 1194 pc.printf("Calculating GPS offset...\n");
shimniok 0:9b27ebe1ce17 1195
shimniok 0:9b27ebe1ce17 1196 while (n > 0) {
shimniok 0:9b27ebe1ce17 1197 while (gps2Parse.rmc_ready() == false)
shimniok 0:9b27ebe1ce17 1198 wait(0.2);
shimniok 0:9b27ebe1ce17 1199
shimniok 0:9b27ebe1ce17 1200 gps2Parse.reset_ready();
shimniok 0:9b27ebe1ce17 1201 gps2Parse.f_get_position(&lat, &lon, &age);
shimniok 0:9b27ebe1ce17 1202
shimniok 0:9b27ebe1ce17 1203 if (gps2Parse.f_hdop() > 0 && gps2Parse.f_hdop() < 2.0) {
shimniok 0:9b27ebe1ce17 1204 pc.printf("GPS lat: %.7f lon %.7f\n", lat, lon);
shimniok 0:9b27ebe1ce17 1205 latavg += lat / div;
shimniok 0:9b27ebe1ce17 1206 lonavg += lon / div;
shimniok 0:9b27ebe1ce17 1207 n--;
shimniok 0:9b27ebe1ce17 1208 } else {
shimniok 0:9b27ebe1ce17 1209 pc.printf("Waiting for good GPS fix\n");
shimniok 0:9b27ebe1ce17 1210 gps2Status = 1;
shimniok 0:9b27ebe1ce17 1211 wait(0.1);
shimniok 0:9b27ebe1ce17 1212 gps2Status = 0;
shimniok 0:9b27ebe1ce17 1213 }
shimniok 0:9b27ebe1ce17 1214
shimniok 0:9b27ebe1ce17 1215 }
shimniok 0:9b27ebe1ce17 1216 pc.printf("AVG lat: %.7f lon %.7f\n", latavg, lonavg);
shimniok 0:9b27ebe1ce17 1217 GeoPosition avg(latavg, lonavg);
shimniok 0:9b27ebe1ce17 1218 *dist = place.distance( avg );
shimniok 0:9b27ebe1ce17 1219 *brg = place.bearing( avg );
shimniok 0:9b27ebe1ce17 1220 // Now that we've calculated a bearing and distance to a quickie offset/bias
shimniok 0:9b27ebe1ce17 1221 // we can subtract this from all future GPS readings by simply taking the
shimniok 0:9b27ebe1ce17 1222 // gps position and doing a move(gps2_off_brg, gps2_off_dist)
shimniok 0:9b27ebe1ce17 1223 // maybe do adjustments based on how far off predicted DR position is from
shimniok 0:9b27ebe1ce17 1224 // GPS reported pos. I'd love to be able to factor in SV and HDOP info too
shimniok 0:9b27ebe1ce17 1225 }
shimniok 0:9b27ebe1ce17 1226
shimniok 0:9b27ebe1ce17 1227
shimniok 0:9b27ebe1ce17 1228 void compass3dCalibrate()
shimniok 0:9b27ebe1ce17 1229 {
shimniok 0:9b27ebe1ce17 1230 int tick;
shimniok 0:9b27ebe1ce17 1231 FILE *fp;
shimniok 0:9b27ebe1ce17 1232
shimniok 0:9b27ebe1ce17 1233 pc.printf("Entering calibration mode for 30 seconds\nRotate compass thru 360 degree sphere\n");
shimniok 0:9b27ebe1ce17 1234 for (int i=10; i > 0; i--) {
shimniok 0:9b27ebe1ce17 1235 pc.printf("%d...", i);
shimniok 0:9b27ebe1ce17 1236 if (pc.readable()) {
shimniok 0:9b27ebe1ce17 1237 char cmd = pc.getc();
shimniok 0:9b27ebe1ce17 1238 }
shimniok 0:9b27ebe1ce17 1239 wait(1.0);
shimniok 0:9b27ebe1ce17 1240 }
shimniok 0:9b27ebe1ce17 1241 pc.printf("Begin calibration\n");
shimniok 0:9b27ebe1ce17 1242
shimniok 0:9b27ebe1ce17 1243 fp = fopen("/log/compass.txt", "w");
shimniok 0:9b27ebe1ce17 1244
shimniok 0:9b27ebe1ce17 1245 if (fp == 0) {
shimniok 0:9b27ebe1ce17 1246 pc.printf("Error opening file\n");
shimniok 0:9b27ebe1ce17 1247 } else {
shimniok 0:9b27ebe1ce17 1248 // printf(fp, "# magX, magY, magZ\n");
shimniok 0:9b27ebe1ce17 1249 timer.reset();
shimniok 0:9b27ebe1ce17 1250 timer.start();
shimniok 0:9b27ebe1ce17 1251 tick = 30;
shimniok 0:9b27ebe1ce17 1252 while (tick > 0) {
shimniok 0:9b27ebe1ce17 1253 // run every 20ms
shimniok 0:9b27ebe1ce17 1254 // get all three axes
shimniok 0:9b27ebe1ce17 1255 // printf(fp, "%d, %d, %d\n"
shimniok 0:9b27ebe1ce17 1256 if (timer.read_ms() > 1000) {
shimniok 0:9b27ebe1ce17 1257 pc.printf("%d\n", tick--);
shimniok 0:9b27ebe1ce17 1258 timer.reset();
shimniok 0:9b27ebe1ce17 1259 }
shimniok 0:9b27ebe1ce17 1260 wait(0.2);
shimniok 0:9b27ebe1ce17 1261 }
shimniok 0:9b27ebe1ce17 1262 fclose(fp);
shimniok 0:9b27ebe1ce17 1263 pc.printf("\nData saved to compass.txt\n\n");
shimniok 0:9b27ebe1ce17 1264 }
shimniok 0:9b27ebe1ce17 1265 }
shimniok 0:9b27ebe1ce17 1266
shimniok 0:9b27ebe1ce17 1267
shimniok 0:9b27ebe1ce17 1268 // HMC6352 compass calibration mode
shimniok 0:9b27ebe1ce17 1269 void compassCalibrate()
shimniok 0:9b27ebe1ce17 1270 {
shimniok 0:9b27ebe1ce17 1271 int tick;
shimniok 0:9b27ebe1ce17 1272
shimniok 0:9b27ebe1ce17 1273 pc.printf("Entering calibration mode for 60 seconds\nRotate compass at least once through 360 degrees\n");
shimniok 0:9b27ebe1ce17 1274 for (int i=10; i > 0; i--) {
shimniok 0:9b27ebe1ce17 1275 pc.printf("%d...", i);
shimniok 0:9b27ebe1ce17 1276 if (pc.readable()) {
shimniok 0:9b27ebe1ce17 1277 char cmd = pc.getc();
shimniok 0:9b27ebe1ce17 1278 }
shimniok 0:9b27ebe1ce17 1279 wait(1.0);
shimniok 0:9b27ebe1ce17 1280 }
shimniok 0:9b27ebe1ce17 1281 pc.printf("Begin calibration\n");
shimniok 0:9b27ebe1ce17 1282
shimniok 0:9b27ebe1ce17 1283 //compass.setCalibrationMode(HMC6352_ENTER_CALIB);
shimniok 0:9b27ebe1ce17 1284 timer.start();
shimniok 0:9b27ebe1ce17 1285 tick = 60;
shimniok 0:9b27ebe1ce17 1286 while (tick > 0) {
shimniok 0:9b27ebe1ce17 1287 if (timer.read_ms() > 1000) {
shimniok 0:9b27ebe1ce17 1288 pc.printf("%d\n", tick--);
shimniok 0:9b27ebe1ce17 1289 timer.reset();
shimniok 0:9b27ebe1ce17 1290 }
shimniok 0:9b27ebe1ce17 1291 wait(0.2);
shimniok 0:9b27ebe1ce17 1292 }
shimniok 0:9b27ebe1ce17 1293 //compass.setCalibrationMode(HMC6352_EXIT_CALIB);
shimniok 0:9b27ebe1ce17 1294 pc.printf("Calibration complete.\n");
shimniok 0:9b27ebe1ce17 1295 }
shimniok 0:9b27ebe1ce17 1296
shimniok 0:9b27ebe1ce17 1297 void servoCalibrate()
shimniok 0:9b27ebe1ce17 1298 {
shimniok 0:9b27ebe1ce17 1299 }
shimniok 0:9b27ebe1ce17 1300
shimniok 0:9b27ebe1ce17 1301 void bridgeRecv()
shimniok 0:9b27ebe1ce17 1302 {
shimniok 0:9b27ebe1ce17 1303 while (dev && dev->readable()) {
shimniok 0:9b27ebe1ce17 1304 pc.putc(dev->getc());
shimniok 0:9b27ebe1ce17 1305 }
shimniok 0:9b27ebe1ce17 1306 }
shimniok 0:9b27ebe1ce17 1307
shimniok 0:9b27ebe1ce17 1308 void serialBridge(Serial &gps)
shimniok 0:9b27ebe1ce17 1309 {
shimniok 0:9b27ebe1ce17 1310 char x;
shimniok 0:9b27ebe1ce17 1311 int count = 0;
shimniok 0:9b27ebe1ce17 1312 bool done=false;
shimniok 0:9b27ebe1ce17 1313
shimniok 0:9b27ebe1ce17 1314 pc.printf("\nEntering serial bridge in 2 seconds, +++ to escape\n\n");
shimniok 0:9b27ebe1ce17 1315 wait(2.0);
shimniok 0:9b27ebe1ce17 1316 //dev = &gps;
shimniok 0:9b27ebe1ce17 1317 gps.attach(NULL, Serial::RxIrq);
shimniok 0:9b27ebe1ce17 1318 while (!done) {
shimniok 0:9b27ebe1ce17 1319 if (pc.readable()) {
shimniok 0:9b27ebe1ce17 1320 x = pc.getc();
shimniok 0:9b27ebe1ce17 1321 // escape sequence
shimniok 0:9b27ebe1ce17 1322 if (x == '+') {
shimniok 0:9b27ebe1ce17 1323 if (++count >= 3) done=true;
shimniok 0:9b27ebe1ce17 1324 } else {
shimniok 0:9b27ebe1ce17 1325 count = 0;
shimniok 0:9b27ebe1ce17 1326 }
shimniok 0:9b27ebe1ce17 1327 gps.putc(x);
shimniok 0:9b27ebe1ce17 1328 }
shimniok 0:9b27ebe1ce17 1329 if (gps.readable()) {
shimniok 0:9b27ebe1ce17 1330 pc.putc(gps.getc());
shimniok 0:9b27ebe1ce17 1331 }
shimniok 0:9b27ebe1ce17 1332 }
shimniok 0:9b27ebe1ce17 1333 }
shimniok 0:9b27ebe1ce17 1334
shimniok 0:9b27ebe1ce17 1335
shimniok 0:9b27ebe1ce17 1336 void instrumentCheck()
shimniok 0:9b27ebe1ce17 1337 {
shimniok 0:9b27ebe1ce17 1338 bool done = false;
shimniok 0:9b27ebe1ce17 1339 float compassHdg = 0;
shimniok 0:9b27ebe1ce17 1340 float initialHdg = 0;
shimniok 0:9b27ebe1ce17 1341 float ahrsHdg = 0;
shimniok 0:9b27ebe1ce17 1342 unsigned int adcResult[8] = {0,0,0,0,0,0,0,0};
shimniok 0:9b27ebe1ce17 1343 //unsigned int sonar, gyro, temp;
shimniok 0:9b27ebe1ce17 1344 Timer timer;
shimniok 0:9b27ebe1ce17 1345 SimpleFilter ranger(8);
shimniok 0:9b27ebe1ce17 1346
shimniok 0:9b27ebe1ce17 1347 initialHdg = compassHeading();
shimniok 0:9b27ebe1ce17 1348 initAHRS();
shimniok 0:9b27ebe1ce17 1349
shimniok 0:9b27ebe1ce17 1350 timer.reset();
shimniok 0:9b27ebe1ce17 1351 timer.start();
shimniok 0:9b27ebe1ce17 1352
shimniok 0:9b27ebe1ce17 1353 buttonReleased = buttonPressed = false;
shimniok 0:9b27ebe1ce17 1354
shimniok 0:9b27ebe1ce17 1355 pc.printf("press e to exit\n");
shimniok 0:9b27ebe1ce17 1356 while (!done) {
shimniok 0:9b27ebe1ce17 1357 if (buttonPressed) done=true;
shimniok 0:9b27ebe1ce17 1358 while (pc.readable()) {
shimniok 0:9b27ebe1ce17 1359 if (pc.getc() == 'e') {
shimniok 0:9b27ebe1ce17 1360 done = true;
shimniok 0:9b27ebe1ce17 1361 break;
shimniok 0:9b27ebe1ce17 1362 }
shimniok 0:9b27ebe1ce17 1363 }
shimniok 0:9b27ebe1ce17 1364
shimniok 0:9b27ebe1ce17 1365 int millis = timer.read_ms();
shimniok 0:9b27ebe1ce17 1366
shimniok 0:9b27ebe1ce17 1367 if ((millis % 20) == 0) {
shimniok 0:9b27ebe1ce17 1368
shimniok 0:9b27ebe1ce17 1369 if ((millis % 100) == 0) {
shimniok 0:9b27ebe1ce17 1370 sonarStart = 1;
shimniok 0:9b27ebe1ce17 1371 wait_us(25);
shimniok 0:9b27ebe1ce17 1372 sonarStart = 0;
shimniok 0:9b27ebe1ce17 1373 wait_us(50);
shimniok 0:9b27ebe1ce17 1374 }
shimniok 0:9b27ebe1ce17 1375
shimniok 0:9b27ebe1ce17 1376 adc.setChannel(0);
shimniok 0:9b27ebe1ce17 1377 for (int i = 0; i < 8; i++) {
shimniok 0:9b27ebe1ce17 1378 adcResult[i] = adc.read();
shimniok 0:9b27ebe1ce17 1379 }
shimniok 0:9b27ebe1ce17 1380 ranger.filter(adcResult[0]);
shimniok 0:9b27ebe1ce17 1381
shimniok 0:9b27ebe1ce17 1382 if ((millis % 1000) == 0) {
shimniok 0:9b27ebe1ce17 1383 compassHdg = compassHeading();
shimniok 0:9b27ebe1ce17 1384
shimniok 0:9b27ebe1ce17 1385 ahrsHdg = initialHdg + ahrsParser.readYaw();
shimniok 0:9b27ebe1ce17 1386 while (ahrsHdg >= 360.0) ahrsHdg -= 360.0;
shimniok 0:9b27ebe1ce17 1387 while (ahrsHdg < 0) ahrsHdg += 360.0;
shimniok 0:9b27ebe1ce17 1388
shimniok 0:9b27ebe1ce17 1389 char data[4];
shimniok 0:9b27ebe1ce17 1390
shimniok 0:9b27ebe1ce17 1391 // CMUcam1 I2C code goes here
shimniok 0:9b27ebe1ce17 1392
shimniok 0:9b27ebe1ce17 1393 pc.printf("-----e to exit-----\nCompass: %.1f\nAHRS: %.1f\nGyro: %.3f\nGyro Temp: %d\nSonar Left: %.2f\nSonar Right: %.2f\nIR Left: %.5f\nIR Right: %.5f\nEncLeft: %d\nEncRight: %d\n\n",
shimniok 0:9b27ebe1ce17 1394 compassHdg, ahrsHdg, gyroRate(adcResult[GYRO_CHAN]), adcResult[TEMP_CHAN],
shimniok 0:9b27ebe1ce17 1395 sonarDistance(adcResult[SONARLEFT_CHAN]), sonarDistance(adcResult[SONARRIGHT_CHAN]),
shimniok 0:9b27ebe1ce17 1396 irDistance(adcResult[IRLEFT_CHAN]), irDistance(adcResult[IRRIGHT_CHAN]), left.readTotal(), right.readTotal());
shimniok 0:9b27ebe1ce17 1397
shimniok 0:9b27ebe1ce17 1398 pc.printf("IR filter: %d %.1f\n", ranger.value(), irDistance(ranger.value()));
shimniok 0:9b27ebe1ce17 1399
shimniok 0:9b27ebe1ce17 1400 pc.printf("\nCam Obj Box: (%d,%d) (%d,%d)\n\n", data[0], data[1], data[2], data[3]);
shimniok 0:9b27ebe1ce17 1401
shimniok 0:9b27ebe1ce17 1402 for (int i = 0; i < 8; i++) {
shimniok 0:9b27ebe1ce17 1403 pc.printf("ADC(%d)=%d\n", i, adcResult[i]);
shimniok 0:9b27ebe1ce17 1404 }
shimniok 0:9b27ebe1ce17 1405 pc.printf("\n");
shimniok 0:9b27ebe1ce17 1406 }
shimniok 0:9b27ebe1ce17 1407 wait_ms(2);
shimniok 0:9b27ebe1ce17 1408 }
shimniok 0:9b27ebe1ce17 1409 }
shimniok 0:9b27ebe1ce17 1410 // clear input buffer
shimniok 0:9b27ebe1ce17 1411 while (pc.readable()) pc.getc();
shimniok 0:9b27ebe1ce17 1412 }
shimniok 0:9b27ebe1ce17 1413
shimniok 0:9b27ebe1ce17 1414
shimniok 0:9b27ebe1ce17 1415 ///////////////////////////////////////////////////////////////////////////////////////////////////////
shimniok 0:9b27ebe1ce17 1416 // ADC CONVERSION FUNCTIONS
shimniok 0:9b27ebe1ce17 1417 ///////////////////////////////////////////////////////////////////////////////////////////////////////
shimniok 0:9b27ebe1ce17 1418 float compassHeading() {
shimniok 0:9b27ebe1ce17 1419 //Timer t;
shimniok 0:9b27ebe1ce17 1420 //int usec1, usec2;
shimniok 0:9b27ebe1ce17 1421 //t.reset();
shimniok 0:9b27ebe1ce17 1422 //t.start();
shimniok 0:9b27ebe1ce17 1423 //float compassHdg = compass.sample() / 10.0; // read compass
shimniok 0:9b27ebe1ce17 1424 //float compassHdg = compass3d.heading2d();
shimniok 0:9b27ebe1ce17 1425 ////WHERE();
shimniok 0:9b27ebe1ce17 1426 //__disable_irq(); // Disable Interrupts
shimniok 0:9b27ebe1ce17 1427 //usec1 = t.read_us();
shimniok 0:9b27ebe1ce17 1428 float compassHdg = compass3d.heading();
shimniok 0:9b27ebe1ce17 1429 //usec2 = t.read_us();
shimniok 0:9b27ebe1ce17 1430 //__enable_irq(); // Enable Interrupts
shimniok 0:9b27ebe1ce17 1431
shimniok 0:9b27ebe1ce17 1432 //pc.printf("usec1: %d, usec2: %d, usec: %d\n", usec1, usec2, usec2-usec1);
shimniok 0:9b27ebe1ce17 1433
shimniok 0:9b27ebe1ce17 1434 //WHERE();
shimniok 0:9b27ebe1ce17 1435 compassHdg -= declination; // Correct for local declination
shimniok 0:9b27ebe1ce17 1436 clamp360(compassHdg);
shimniok 0:9b27ebe1ce17 1437
shimniok 0:9b27ebe1ce17 1438 return compassHdg;
shimniok 0:9b27ebe1ce17 1439 }
shimniok 0:9b27ebe1ce17 1440
shimniok 0:9b27ebe1ce17 1441
shimniok 0:9b27ebe1ce17 1442 // returns rate in */sec
shimniok 0:9b27ebe1ce17 1443 //
shimniok 0:9b27ebe1ce17 1444 // Gyro is ratiometric and so is ADC, so we can cancel out
shimniok 0:9b27ebe1ce17 1445 // errors due to voltage supply by putting sensitivity in
shimniok 0:9b27ebe1ce17 1446 // terms of adc reading and calculate rate from that.
shimniok 0:9b27ebe1ce17 1447 // (adc - bias) / sens
shimniok 0:9b27ebe1ce17 1448 // UPDATE_PERIOD in ms, 5.91mV/degree/sec is scale factor
shimniok 0:9b27ebe1ce17 1449 //
shimniok 0:9b27ebe1ce17 1450 // TODO: use constants and defines
shimniok 0:9b27ebe1ce17 1451 //
shimniok 0:9b27ebe1ce17 1452 float gyroRate(unsigned int adc)
shimniok 0:9b27ebe1ce17 1453 {
shimniok 0:9b27ebe1ce17 1454 float rate = 0.0;
shimniok 0:9b27ebe1ce17 1455
shimniok 0:9b27ebe1ce17 1456 rate = ((float) (adc - gyroBias)) / gyroSens;
shimniok 0:9b27ebe1ce17 1457
shimniok 0:9b27ebe1ce17 1458 return rate;
shimniok 0:9b27ebe1ce17 1459 }
shimniok 0:9b27ebe1ce17 1460
shimniok 0:9b27ebe1ce17 1461 // returns distance in m for LV-EZ1 sonar
shimniok 0:9b27ebe1ce17 1462 //
shimniok 0:9b27ebe1ce17 1463 float sonarDistance(unsigned int adc)
shimniok 0:9b27ebe1ce17 1464 {
shimniok 0:9b27ebe1ce17 1465 float distance = 9999.9;
shimniok 0:9b27ebe1ce17 1466
shimniok 0:9b27ebe1ce17 1467 // EZ1 uses 9.8mV/inch @ 5V or scaling factor of Vcc / 512
shimniok 0:9b27ebe1ce17 1468 // so we can eliminate Vcc changes by simply converting the 0-512 inch range
shimniok 0:9b27ebe1ce17 1469 // to the ADC's 0-4096 range
shimniok 0:9b27ebe1ce17 1470 distance = ((float) adc) * (512 * 0.0254) / 4096; // distance converted to inch then meter
shimniok 0:9b27ebe1ce17 1471
shimniok 0:9b27ebe1ce17 1472 return distance;
shimniok 0:9b27ebe1ce17 1473 }
shimniok 0:9b27ebe1ce17 1474
shimniok 0:9b27ebe1ce17 1475 // returns distance in m for Sharp GP2YOA710K0F
shimniok 0:9b27ebe1ce17 1476 // to get m and b, I wrote down volt vs. dist by eyeballin the
shimniok 0:9b27ebe1ce17 1477 // datasheet chart plot. Then used Excel to do linear regression
shimniok 0:9b27ebe1ce17 1478 //
shimniok 0:9b27ebe1ce17 1479 float irDistance(unsigned int adc)
shimniok 0:9b27ebe1ce17 1480 {
shimniok 0:9b27ebe1ce17 1481 float b = 1.0934; // Intercept from Excel
shimniok 0:9b27ebe1ce17 1482 float m = 1.4088; // Slope from Excel
shimniok 0:9b27ebe1ce17 1483
shimniok 0:9b27ebe1ce17 1484 return m / (((float) adc) * 4.95/4096 - b);
shimniok 0:9b27ebe1ce17 1485 }