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

Sensors/GPS/GPS.cpp@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 // For SiRF III
shimniok 0:826c6171fc1b 2
shimniok 0:826c6171fc1b 3 #include "GPS.h"
shimniok 0:826c6171fc1b 4
shimniok 0:826c6171fc1b 5 extern Serial pc;
shimniok 0:826c6171fc1b 6
shimniok 0:826c6171fc1b 7
shimniok 0:826c6171fc1b 8
shimniok 0:826c6171fc1b 9
shimniok 0:826c6171fc1b 10 GPS::GPS(PinName tx, PinName rx, int type):
shimniok 0:826c6171fc1b 11 hdop(0.0),
shimniok 0:826c6171fc1b 12 serial(tx, rx)
shimniok 0:826c6171fc1b 13 {
shimniok 0:826c6171fc1b 14 setType(SIRF);
shimniok 0:826c6171fc1b 15 setBaud(4800);
shimniok 0:826c6171fc1b 16 }
shimniok 0:826c6171fc1b 17
shimniok 0:826c6171fc1b 18
shimniok 0:826c6171fc1b 19 void GPS::setType(int type)
shimniok 0:826c6171fc1b 20 {
shimniok 0:826c6171fc1b 21 if (type == VENUS || type == MTK || type == SIRF) {
shimniok 0:826c6171fc1b 22 _type = type;
shimniok 0:826c6171fc1b 23 }
shimniok 0:826c6171fc1b 24
shimniok 0:826c6171fc1b 25 return;
shimniok 0:826c6171fc1b 26 }
shimniok 0:826c6171fc1b 27
shimniok 0:826c6171fc1b 28 void GPS::setBaud(int baud)
shimniok 0:826c6171fc1b 29 {
shimniok 0:826c6171fc1b 30 serial.baud(baud);
shimniok 0:826c6171fc1b 31
shimniok 0:826c6171fc1b 32 return;
shimniok 0:826c6171fc1b 33 }
shimniok 0:826c6171fc1b 34
shimniok 0:826c6171fc1b 35 void GPS::setUpdateRate(int rate)
shimniok 0:826c6171fc1b 36 {
shimniok 0:826c6171fc1b 37 char msg[10] = { 0xA0, 0xA1, 0x00, 0x03,
shimniok 0:826c6171fc1b 38 0x0E, rate&0xFF, 01,
shimniok 0:826c6171fc1b 39 0, 0x0D, 0x0A };
shimniok 0:826c6171fc1b 40 for (int i=4; i < 7; i++) {
shimniok 0:826c6171fc1b 41 msg[7] ^= msg[i];
shimniok 0:826c6171fc1b 42 }
shimniok 0:826c6171fc1b 43 switch (rate) {
shimniok 0:826c6171fc1b 44 case 1 :
shimniok 0:826c6171fc1b 45 case 2 :
shimniok 0:826c6171fc1b 46 case 4 :
shimniok 0:826c6171fc1b 47 case 5 :
shimniok 0:826c6171fc1b 48 case 8 :
shimniok 0:826c6171fc1b 49 case 10 :
shimniok 0:826c6171fc1b 50 case 20 :
shimniok 0:826c6171fc1b 51 for (int i=0; i < 10; i++)
shimniok 0:826c6171fc1b 52 serial.putc(msg[i]);
shimniok 0:826c6171fc1b 53 break;
shimniok 0:826c6171fc1b 54 default :
shimniok 0:826c6171fc1b 55 break;
shimniok 0:826c6171fc1b 56 }
shimniok 0:826c6171fc1b 57 }
shimniok 0:826c6171fc1b 58
shimniok 0:826c6171fc1b 59 void GPS::setNmeaMessages(bool gga, bool gsa, bool gsv, bool gll, bool rmc, bool vtg)
shimniok 0:826c6171fc1b 60 {
shimniok 0:826c6171fc1b 61 if (_type == VENUS) {
shimniok 0:826c6171fc1b 62 // VENUS Binary MsgID=0x08
shimniok 0:826c6171fc1b 63 // GGA interval
shimniok 0:826c6171fc1b 64 // GSA interval
shimniok 0:826c6171fc1b 65 // GSV interval
shimniok 0:826c6171fc1b 66 // GLL interval
shimniok 0:826c6171fc1b 67 // RMC interval
shimniok 0:826c6171fc1b 68 // VTG interval
shimniok 0:826c6171fc1b 69 // ZDA interval -- hardcode off
shimniok 0:826c6171fc1b 70 char msg[15] = { 0xA0, 0xA1, 0x00, 0x09,
shimniok 0:826c6171fc1b 71 0x08, gga, gsa, gsv, gll, rmc, vtg, 0,
shimniok 0:826c6171fc1b 72 0, 0x0D, 0x0A };
shimniok 0:826c6171fc1b 73 for (int i=4; i < 12; i++) {
shimniok 0:826c6171fc1b 74 msg[12] ^= msg[i];
shimniok 0:826c6171fc1b 75 }
shimniok 0:826c6171fc1b 76 for (int i=0; i < 15; i++)
shimniok 0:826c6171fc1b 77 serial.putc(msg[i]);
shimniok 0:826c6171fc1b 78 }
shimniok 0:826c6171fc1b 79 }
shimniok 0:826c6171fc1b 80
shimniok 0:826c6171fc1b 81
shimniok 0:826c6171fc1b 82 void GPS::gsvMessage(bool enable)
shimniok 0:826c6171fc1b 83 {
shimniok 0:826c6171fc1b 84 if (enable) {
shimniok 0:826c6171fc1b 85 if (_type == MTK) {
shimniok 0:826c6171fc1b 86 serial.printf("$PSRF103,03,00,01,01*26\r\n"); // Enable GSV
shimniok 0:826c6171fc1b 87 } else if (_type == VENUS) {
shimniok 0:826c6171fc1b 88 }
shimniok 0:826c6171fc1b 89 } else {
shimniok 0:826c6171fc1b 90 if (_type == MTK) {
shimniok 0:826c6171fc1b 91 serial.printf("$PSRF103,03,00,00,01*27\r\n"); // Disable GSV
shimniok 0:826c6171fc1b 92 } else if (_type == VENUS) {
shimniok 0:826c6171fc1b 93 // ??
shimniok 0:826c6171fc1b 94 }
shimniok 0:826c6171fc1b 95 }
shimniok 0:826c6171fc1b 96
shimniok 0:826c6171fc1b 97 return;
shimniok 0:826c6171fc1b 98 }
shimniok 0:826c6171fc1b 99
shimniok 0:826c6171fc1b 100 void GPS::gsaMessage(bool enable)
shimniok 0:826c6171fc1b 101 {
shimniok 0:826c6171fc1b 102 if (enable) {
shimniok 0:826c6171fc1b 103 if (_type == SIRF) {
shimniok 0:826c6171fc1b 104 serial.printf("$PSRF103,02,00,01,01*27\r\n"); // Enable GSA
shimniok 0:826c6171fc1b 105 } else if (_type == VENUS) {
shimniok 0:826c6171fc1b 106 // ??
shimniok 0:826c6171fc1b 107 }
shimniok 0:826c6171fc1b 108 } else {
shimniok 0:826c6171fc1b 109 if (_type == SIRF) {
shimniok 0:826c6171fc1b 110 serial.printf("$PSRF103,02,00,00,01*26\r\n"); // Disable GSA
shimniok 0:826c6171fc1b 111 } else if (_type == VENUS) {
shimniok 0:826c6171fc1b 112 // ??
shimniok 0:826c6171fc1b 113 }
shimniok 0:826c6171fc1b 114 }
shimniok 0:826c6171fc1b 115
shimniok 0:826c6171fc1b 116 return;
shimniok 0:826c6171fc1b 117 }
shimniok 0:826c6171fc1b 118
shimniok 0:826c6171fc1b 119
shimniok 0:826c6171fc1b 120 // Handle data from a GPS (there may be two GPS's so needed to put the code in one routine
shimniok 0:826c6171fc1b 121 //
shimniok 0:826c6171fc1b 122 void GPS::process(GeoPosition &here, char *date, char *time)
shimniok 0:826c6171fc1b 123 {
shimniok 0:826c6171fc1b 124 double lat, lon;
shimniok 0:826c6171fc1b 125 unsigned long age;
shimniok 0:826c6171fc1b 126
shimniok 0:826c6171fc1b 127 nmea.reset_ready(); // reset the flags
shimniok 0:826c6171fc1b 128 //pc.printf("%d GPS RMC are ready\n", millis);
shimniok 0:826c6171fc1b 129 nmea.f_get_position(&lat, &lon, &age);
shimniok 0:826c6171fc1b 130 nmea.crack_datetime(&year, &month, &day, &hour, &minute, &second, &hundredths, &age);
shimniok 0:826c6171fc1b 131
shimniok 0:826c6171fc1b 132 sprintf(date, "%02d/%02d/%4d", month, day, year);
shimniok 0:826c6171fc1b 133 sprintf(time, "%02d:%02d:%02d.%03d", hour, minute, second, hundredths);
shimniok 0:826c6171fc1b 134
shimniok 0:826c6171fc1b 135 hdop = nmea.f_hdop();
shimniok 0:826c6171fc1b 136
shimniok 0:826c6171fc1b 137 // Bearing and distance to waypoint
shimniok 0:826c6171fc1b 138 here.set(lat, lon);
shimniok 0:826c6171fc1b 139
shimniok 0:826c6171fc1b 140 //pc.printf("HDOP: %.1f gyro: %d\n", gps1_hdop, gyro);
shimniok 0:826c6171fc1b 141
shimniok 0:826c6171fc1b 142 return;
shimniok 0:826c6171fc1b 143 }
shimniok 0:826c6171fc1b 144
shimniok 0:826c6171fc1b 145
shimniok 0:826c6171fc1b 146 void GPS::init()
shimniok 0:826c6171fc1b 147 {
shimniok 0:826c6171fc1b 148 // Initialize the GPS comm and handler
shimniok 0:826c6171fc1b 149 //serial.baud(57600); // LOCOSYS LS20031
shimniok 0:826c6171fc1b 150 //DigitalIn myRx(_rx);
shimniok 0:826c6171fc1b 151 //Timer tm;
shimniok 0:826c6171fc1b 152
shimniok 0:826c6171fc1b 153
shimniok 0:826c6171fc1b 154 /*
shimniok 0:826c6171fc1b 155 //pc.printf("gps.init()\n\n");
shimniok 0:826c6171fc1b 156 int rate = 99999999;
shimniok 0:826c6171fc1b 157 for (int i=0; i < 10; i++) {
shimniok 0:826c6171fc1b 158 //pc.printf("rate: %d\n", rate);
shimniok 0:826c6171fc1b 159 while( myRx ); // wait for low
shimniok 0:826c6171fc1b 160 tm.reset();
shimniok 0:826c6171fc1b 161 tm.start();
shimniok 0:826c6171fc1b 162 while ( !myRx ); // wait for high
shimniok 0:826c6171fc1b 163 if (tm.read_us() < rate) rate = tm.read_us();
shimniok 0:826c6171fc1b 164 }
shimniok 0:826c6171fc1b 165 */
shimniok 0:826c6171fc1b 166 if (_type == MTK) {
shimniok 0:826c6171fc1b 167 gsvMessage(false); // Disable GSV
shimniok 0:826c6171fc1b 168 gsaMessage(false); // Disable GSA
shimniok 0:826c6171fc1b 169 } else if (_type == VENUS) {
shimniok 0:826c6171fc1b 170 setNmeaMessages(true, true, false, false, true, false);
shimniok 0:826c6171fc1b 171 setUpdateRate(10);
shimniok 0:826c6171fc1b 172 }
shimniok 0:826c6171fc1b 173
shimniok 0:826c6171fc1b 174 // Synchronize with GPS
shimniok 0:826c6171fc1b 175 nmea.reset_ready();
shimniok 0:826c6171fc1b 176 }