Michael Shimniok / Mbed 2 deprecated DataBus

Code for autonomous rover for Sparkfun AVC. DataBus won 3rd in 2012 and the same code was used on Troubled Child, a 1986 Jeep Grand Wagoneer to win 1st in 2014.

Dependencies:   mbed Watchdog SDFileSystem DigoleSerialDisp

Sensors/GPS/Sirf3.cpp@2:fbc6e3cf3ed8, 2013-06-06 (annotated)

Committer:
shimniok
Date:
Thu Jun 06 13:40:23 2013 +0000
Revision:
2:fbc6e3cf3ed8
Parent:
0:a6a169de725f
Child:
23:a34af501ea89
Sort-of working version, still some errors with estimation. Added clamp() function.

Who changed what in which revision?

UserRevisionLine numberNew contents of line
shimniok 0:a6a169de725f 1 #include "mbed.h"
shimniok 0:a6a169de725f 2 #include "Sirf3.h"
shimniok 0:a6a169de725f 3
shimniok 0:a6a169de725f 4 // TODO: parameterize LED
shimniok 0:a6a169de725f 5
shimniok 0:a6a169de725f 6 Sirf3::Sirf3(PinName tx, PinName rx):
shimniok 0:a6a169de725f 7 serial(tx, rx)
shimniok 0:a6a169de725f 8 {
shimniok 0:a6a169de725f 9 enable();
shimniok 0:a6a169de725f 10 }
shimniok 0:a6a169de725f 11
shimniok 0:a6a169de725f 12 void Sirf3::init(void)
shimniok 0:a6a169de725f 13 {
shimniok 0:a6a169de725f 14 disableVerbose();
shimniok 0:a6a169de725f 15 }
shimniok 0:a6a169de725f 16
shimniok 0:a6a169de725f 17 void Sirf3::setBaud(int baud)
shimniok 0:a6a169de725f 18 {
shimniok 0:a6a169de725f 19 serial.baud(baud);
shimniok 0:a6a169de725f 20 }
shimniok 0:a6a169de725f 21
shimniok 0:a6a169de725f 22 Serial *Sirf3::getSerial(void)
shimniok 0:a6a169de725f 23 {
shimniok 0:a6a169de725f 24 return &serial;
shimniok 0:a6a169de725f 25 }
shimniok 0:a6a169de725f 26
shimniok 0:a6a169de725f 27 void Sirf3::enable(void)
shimniok 0:a6a169de725f 28 {
shimniok 0:a6a169de725f 29 reset_available();
shimniok 2:fbc6e3cf3ed8 30 setBaud(4800);
shimniok 0:a6a169de725f 31 serial.attach(this, &Sirf3::recv_handler, Serial::RxIrq);
shimniok 0:a6a169de725f 32 }
shimniok 0:a6a169de725f 33
shimniok 0:a6a169de725f 34 void Sirf3::disable(void)
shimniok 0:a6a169de725f 35 {
shimniok 2:fbc6e3cf3ed8 36 setBaud(4800);
shimniok 0:a6a169de725f 37 serial.attach(NULL, Serial::RxIrq);
shimniok 0:a6a169de725f 38 }
shimniok 0:a6a169de725f 39
shimniok 0:a6a169de725f 40 /**
shimniok 0:a6a169de725f 41 * Enable verbose messages for debugging
shimniok 0:a6a169de725f 42 */
shimniok 0:a6a169de725f 43 void Sirf3::enableVerbose(void)
shimniok 0:a6a169de725f 44 {
shimniok 2:fbc6e3cf3ed8 45 setBaud(4800);
shimniok 0:a6a169de725f 46 gsaMessage(true);
shimniok 2:fbc6e3cf3ed8 47 ggaMessage(true);
shimniok 2:fbc6e3cf3ed8 48 gllMessage(false);
shimniok 0:a6a169de725f 49 gsvMessage(true);
shimniok 2:fbc6e3cf3ed8 50 rmcMessage(true);
shimniok 2:fbc6e3cf3ed8 51 vtgMessage(false);
shimniok 0:a6a169de725f 52 }
shimniok 0:a6a169de725f 53
shimniok 0:a6a169de725f 54 /**
shimniok 0:a6a169de725f 55 * Disable verbose messages for debugging
shimniok 0:a6a169de725f 56 */
shimniok 0:a6a169de725f 57 void Sirf3::disableVerbose(void)
shimniok 0:a6a169de725f 58 {
shimniok 2:fbc6e3cf3ed8 59 setBaud(4800);
shimniok 2:fbc6e3cf3ed8 60 gsaMessage(true);
shimniok 2:fbc6e3cf3ed8 61 ggaMessage(true);
shimniok 2:fbc6e3cf3ed8 62 gllMessage(false);
shimniok 0:a6a169de725f 63 gsvMessage(false);
shimniok 2:fbc6e3cf3ed8 64 rmcMessage(true);
shimniok 2:fbc6e3cf3ed8 65 vtgMessage(false);
shimniok 2:fbc6e3cf3ed8 66 }
shimniok 2:fbc6e3cf3ed8 67
shimniok 2:fbc6e3cf3ed8 68 void Sirf3::setUpdateRate(int rate)
shimniok 2:fbc6e3cf3ed8 69 {
shimniok 2:fbc6e3cf3ed8 70 // We're stuck at 1Hz
shimniok 2:fbc6e3cf3ed8 71 return;
shimniok 2:fbc6e3cf3ed8 72 }
shimniok 2:fbc6e3cf3ed8 73
shimniok 2:fbc6e3cf3ed8 74 int Sirf3::getAvailable(void)
shimniok 2:fbc6e3cf3ed8 75 {
shimniok 2:fbc6e3cf3ed8 76 int answer = 0x00;
shimniok 2:fbc6e3cf3ed8 77 if (nmea.gga_ready()) answer |= 0x01;
shimniok 2:fbc6e3cf3ed8 78 if (nmea.rmc_ready()) answer |= 0x02;
shimniok 2:fbc6e3cf3ed8 79 return answer;
shimniok 0:a6a169de725f 80 }
shimniok 0:a6a169de725f 81
shimniok 0:a6a169de725f 82 double Sirf3::latitude(void)
shimniok 0:a6a169de725f 83 {
shimniok 0:a6a169de725f 84 double latitude, longitude;
shimniok 0:a6a169de725f 85 unsigned long age;
shimniok 0:a6a169de725f 86 nmea.f_get_position(&latitude, &longitude, &age);
shimniok 0:a6a169de725f 87 return latitude;
shimniok 0:a6a169de725f 88 }
shimniok 0:a6a169de725f 89
shimniok 0:a6a169de725f 90 double Sirf3::longitude(void)
shimniok 0:a6a169de725f 91 {
shimniok 0:a6a169de725f 92 double latitude, longitude;
shimniok 0:a6a169de725f 93 unsigned long age;
shimniok 0:a6a169de725f 94 nmea.f_get_position(&latitude, &longitude, &age);
shimniok 0:a6a169de725f 95 return longitude;
shimniok 0:a6a169de725f 96 }
shimniok 0:a6a169de725f 97
shimniok 0:a6a169de725f 98 float Sirf3::hdop(void)
shimniok 0:a6a169de725f 99 {
shimniok 0:a6a169de725f 100 return nmea.f_hdop();
shimniok 0:a6a169de725f 101 }
shimniok 0:a6a169de725f 102
shimniok 0:a6a169de725f 103 int Sirf3::sat_count(void)
shimniok 0:a6a169de725f 104 {
shimniok 0:a6a169de725f 105 return nmea.sat_count();
shimniok 0:a6a169de725f 106 }
shimniok 0:a6a169de725f 107
shimniok 0:a6a169de725f 108 float Sirf3::speed_mps(void)
shimniok 0:a6a169de725f 109 {
shimniok 0:a6a169de725f 110 return nmea.f_speed_mps();
shimniok 0:a6a169de725f 111 }
shimniok 0:a6a169de725f 112
shimniok 0:a6a169de725f 113 float Sirf3::heading_deg(void)
shimniok 0:a6a169de725f 114 {
shimniok 0:a6a169de725f 115 return nmea.f_course();
shimniok 0:a6a169de725f 116 }
shimniok 0:a6a169de725f 117
shimniok 0:a6a169de725f 118 bool Sirf3::available(void)
shimniok 0:a6a169de725f 119 {
shimniok 0:a6a169de725f 120 return nmea.ready();
shimniok 0:a6a169de725f 121 }
shimniok 0:a6a169de725f 122
shimniok 0:a6a169de725f 123 void Sirf3::reset_available(void)
shimniok 0:a6a169de725f 124 {
shimniok 0:a6a169de725f 125 nmea.reset_ready();
shimniok 0:a6a169de725f 126 }
shimniok 0:a6a169de725f 127
shimniok 0:a6a169de725f 128 void Sirf3::recv_handler(void)
shimniok 0:a6a169de725f 129 {
shimniok 0:a6a169de725f 130 while (serial.readable()) {
shimniok 0:a6a169de725f 131 nmea.encode(serial.getc());
shimniok 0:a6a169de725f 132 }
shimniok 0:a6a169de725f 133 }
shimniok 0:a6a169de725f 134
shimniok 2:fbc6e3cf3ed8 135 void Sirf3::ggaMessage(bool enable)
shimniok 2:fbc6e3cf3ed8 136 {
shimniok 2:fbc6e3cf3ed8 137 if (enable) {
shimniok 2:fbc6e3cf3ed8 138 serial.printf("$PSRF103,00,00,01,01*25\r\n"); // Enable GGA
shimniok 2:fbc6e3cf3ed8 139 } else {
shimniok 2:fbc6e3cf3ed8 140 serial.printf("$PSRF103,00,00,00,01*24\r\n"); // Disable GGA
shimniok 2:fbc6e3cf3ed8 141 }
shimniok 2:fbc6e3cf3ed8 142
shimniok 2:fbc6e3cf3ed8 143 return;
shimniok 2:fbc6e3cf3ed8 144 }
shimniok 2:fbc6e3cf3ed8 145
shimniok 2:fbc6e3cf3ed8 146 void Sirf3::gllMessage(bool enable)
shimniok 2:fbc6e3cf3ed8 147 {
shimniok 2:fbc6e3cf3ed8 148 if (enable) {
shimniok 2:fbc6e3cf3ed8 149 serial.printf("$PSRF103,01,00,01,01*24\r\n"); // Enable GLL
shimniok 2:fbc6e3cf3ed8 150 } else {
shimniok 2:fbc6e3cf3ed8 151 serial.printf("$PSRF103,01,00,00,01*25\r\n"); // Disable GLL
shimniok 2:fbc6e3cf3ed8 152 }
shimniok 2:fbc6e3cf3ed8 153
shimniok 2:fbc6e3cf3ed8 154 return;
shimniok 2:fbc6e3cf3ed8 155 }
shimniok 2:fbc6e3cf3ed8 156
shimniok 0:a6a169de725f 157 void Sirf3::gsaMessage(bool enable)
shimniok 0:a6a169de725f 158 {
shimniok 0:a6a169de725f 159 if (enable) {
shimniok 0:a6a169de725f 160 serial.printf("$PSRF103,02,00,01,01*27\r\n"); // Enable GSA
shimniok 0:a6a169de725f 161 } else {
shimniok 0:a6a169de725f 162 serial.printf("$PSRF103,02,00,00,01*26\r\n"); // Disable GSA
shimniok 0:a6a169de725f 163 }
shimniok 0:a6a169de725f 164
shimniok 0:a6a169de725f 165 return;
shimniok 0:a6a169de725f 166 }
shimniok 0:a6a169de725f 167
shimniok 0:a6a169de725f 168 void Sirf3::gsvMessage(bool enable)
shimniok 0:a6a169de725f 169 {
shimniok 0:a6a169de725f 170 if (enable) {
shimniok 0:a6a169de725f 171 serial.printf("$PSRF103,03,00,01,01*26\r\n"); // Enable GSV
shimniok 0:a6a169de725f 172 } else {
shimniok 0:a6a169de725f 173 serial.printf("$PSRF103,03,00,00,01*27\r\n"); // Disable GSV
shimniok 0:a6a169de725f 174 }
shimniok 0:a6a169de725f 175
shimniok 0:a6a169de725f 176 return;
shimniok 0:a6a169de725f 177 }
shimniok 0:a6a169de725f 178
shimniok 2:fbc6e3cf3ed8 179 void Sirf3::rmcMessage(bool enable)
shimniok 2:fbc6e3cf3ed8 180 {
shimniok 2:fbc6e3cf3ed8 181 if (enable) {
shimniok 2:fbc6e3cf3ed8 182 serial.printf("$PSRF103,04,00,01,01*21\r\n"); // Enable RMC
shimniok 2:fbc6e3cf3ed8 183 } else {
shimniok 2:fbc6e3cf3ed8 184 serial.printf("$PSRF103,04,00,00,01*20\r\n"); // Disable RMC
shimniok 2:fbc6e3cf3ed8 185 }
shimniok 2:fbc6e3cf3ed8 186
shimniok 2:fbc6e3cf3ed8 187 return;
shimniok 2:fbc6e3cf3ed8 188 }
shimniok 2:fbc6e3cf3ed8 189
shimniok 2:fbc6e3cf3ed8 190 void Sirf3::vtgMessage(bool enable)
shimniok 2:fbc6e3cf3ed8 191 {
shimniok 2:fbc6e3cf3ed8 192 if (enable) {
shimniok 2:fbc6e3cf3ed8 193 serial.printf("$PSRF103,05,00,01,01*20\r\n"); // Enable VTG
shimniok 2:fbc6e3cf3ed8 194 } else {
shimniok 2:fbc6e3cf3ed8 195 serial.printf("$PSRF103,05,00,00,01*21\r\n"); // Disable VTG
shimniok 2:fbc6e3cf3ed8 196 }
shimniok 2:fbc6e3cf3ed8 197
shimniok 2:fbc6e3cf3ed8 198 return;
shimniok 2:fbc6e3cf3ed8 199 }