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@23:a34af501ea89, 2018-11-30 (annotated)
- Committer:
- shimniok
- Date:
- Fri Nov 30 15:41:05 2018 +0000
- Revision:
- 23:a34af501ea89
- Parent:
- 2:fbc6e3cf3ed8
Disabled PinDetect code (for now); fixed attach() calls in various functions to use new callback class.
Who changed what in which revision?
User | Revision | Line number | New 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 | 23:a34af501ea89 | 31 | serial.attach(callback(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 | } |