GPS code for Adafruit ultimate GPS
Dependents: demo_gps_sdcard zeus
Fork of GPS by
GPS.cpp@2:509abe8eda59, 2015-09-22 (annotated)
- Committer:
- cmkachur
- Date:
- Tue Sep 22 23:05:13 2015 +0000
- Revision:
- 2:509abe8eda59
- Parent:
- 1:35fcaa2209af
Add ability to read and store analog input data for the phase detectors. ; Allow use of $GPGGA for GPS fix. ; Code refactoring.
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
simon | 0:15611c7938a3 | 1 | /* mbed EM-406 GPS Module Library |
simon | 0:15611c7938a3 | 2 | * Copyright (c) 2008-2010, sford |
simon | 0:15611c7938a3 | 3 | * |
simon | 0:15611c7938a3 | 4 | * Permission is hereby granted, free of charge, to any person obtaining a copy |
simon | 0:15611c7938a3 | 5 | * of this software and associated documentation files (the "Software"), to deal |
simon | 0:15611c7938a3 | 6 | * in the Software without restriction, including without limitation the rights |
simon | 0:15611c7938a3 | 7 | * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell |
simon | 0:15611c7938a3 | 8 | * copies of the Software, and to permit persons to whom the Software is |
simon | 0:15611c7938a3 | 9 | * furnished to do so, subject to the following conditions: |
simon | 0:15611c7938a3 | 10 | * |
simon | 0:15611c7938a3 | 11 | * The above copyright notice and this permission notice shall be included in |
simon | 0:15611c7938a3 | 12 | * all copies or substantial portions of the Software. |
simon | 0:15611c7938a3 | 13 | * |
simon | 0:15611c7938a3 | 14 | * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR |
simon | 0:15611c7938a3 | 15 | * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, |
simon | 0:15611c7938a3 | 16 | * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE |
simon | 0:15611c7938a3 | 17 | * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER |
simon | 0:15611c7938a3 | 18 | * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, |
simon | 0:15611c7938a3 | 19 | * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN |
simon | 0:15611c7938a3 | 20 | * THE SOFTWARE. |
simon | 0:15611c7938a3 | 21 | */ |
simon | 0:15611c7938a3 | 22 | |
simon | 0:15611c7938a3 | 23 | #include "GPS.h" |
ftagius | 1:35fcaa2209af | 24 | #include "main.h" |
ftagius | 1:35fcaa2209af | 25 | // how long are max NMEA lines to parse? |
ftagius | 1:35fcaa2209af | 26 | #define MAXLINELENGTH 120 |
ftagius | 1:35fcaa2209af | 27 | // we double buffer: read one line in and leave one for the main program |
ftagius | 1:35fcaa2209af | 28 | volatile char line1[MAXLINELENGTH]; |
ftagius | 1:35fcaa2209af | 29 | volatile char line2[MAXLINELENGTH]; |
ftagius | 1:35fcaa2209af | 30 | // our index into filling the current line |
ftagius | 1:35fcaa2209af | 31 | volatile uint16_t lineidx=0; |
ftagius | 1:35fcaa2209af | 32 | // pointers to the double buffers |
ftagius | 1:35fcaa2209af | 33 | volatile char *currentline; |
ftagius | 1:35fcaa2209af | 34 | volatile char *lastline; |
ftagius | 1:35fcaa2209af | 35 | volatile bool recvdflag; |
ftagius | 1:35fcaa2209af | 36 | volatile bool inStandbyMode; |
simon | 0:15611c7938a3 | 37 | |
simon | 0:15611c7938a3 | 38 | GPS::GPS(PinName tx, PinName rx) : _gps(tx, rx) { |
ftagius | 1:35fcaa2209af | 39 | _gps.baud(9600); |
ftagius | 1:35fcaa2209af | 40 | recvdflag = false; |
ftagius | 1:35fcaa2209af | 41 | paused = false; |
ftagius | 1:35fcaa2209af | 42 | lineidx = 0; |
ftagius | 1:35fcaa2209af | 43 | currentline = line1; |
ftagius | 1:35fcaa2209af | 44 | lastline = line2; |
simon | 0:15611c7938a3 | 45 | } |
simon | 0:15611c7938a3 | 46 | |
ftagius | 1:35fcaa2209af | 47 | bool GPS::newNMEAreceived(void) { |
ftagius | 1:35fcaa2209af | 48 | return recvdflag; |
simon | 0:15611c7938a3 | 49 | } |
simon | 0:15611c7938a3 | 50 | |
ftagius | 1:35fcaa2209af | 51 | void GPS::pause(bool p) { |
ftagius | 1:35fcaa2209af | 52 | paused = p; |
ftagius | 1:35fcaa2209af | 53 | } |
ftagius | 1:35fcaa2209af | 54 | |
ftagius | 1:35fcaa2209af | 55 | char *GPS::lastNMEA(void) { |
ftagius | 1:35fcaa2209af | 56 | recvdflag = false; |
ftagius | 1:35fcaa2209af | 57 | return (char *)lastline; |
ftagius | 1:35fcaa2209af | 58 | } |
ftagius | 1:35fcaa2209af | 59 | |
simon | 0:15611c7938a3 | 60 | float GPS::trunc(float v) { |
simon | 0:15611c7938a3 | 61 | if(v < 0.0) { |
simon | 0:15611c7938a3 | 62 | v*= -1.0; |
simon | 0:15611c7938a3 | 63 | v = floor(v); |
simon | 0:15611c7938a3 | 64 | v*=-1.0; |
simon | 0:15611c7938a3 | 65 | } else { |
simon | 0:15611c7938a3 | 66 | v = floor(v); |
simon | 0:15611c7938a3 | 67 | } |
simon | 0:15611c7938a3 | 68 | return v; |
simon | 0:15611c7938a3 | 69 | } |
simon | 0:15611c7938a3 | 70 | |
ftagius | 1:35fcaa2209af | 71 | |
ftagius | 1:35fcaa2209af | 72 | bool GPS::parse(char *nmea) { |
ftagius | 1:35fcaa2209af | 73 | // do checksum check |
ftagius | 1:35fcaa2209af | 74 | |
ftagius | 1:35fcaa2209af | 75 | // first look if we even have one |
ftagius | 1:35fcaa2209af | 76 | if (nmea[strlen(nmea)-4] == '*') { |
ftagius | 1:35fcaa2209af | 77 | uint16_t sum = parseHex(nmea[strlen(nmea)-3]) * 16; |
ftagius | 1:35fcaa2209af | 78 | sum += parseHex(nmea[strlen(nmea)-2]); |
ftagius | 1:35fcaa2209af | 79 | |
ftagius | 1:35fcaa2209af | 80 | // check checksum |
ftagius | 1:35fcaa2209af | 81 | for (uint8_t i=1; i < (strlen(nmea)-4); i++) { |
ftagius | 1:35fcaa2209af | 82 | sum ^= nmea[i]; |
ftagius | 1:35fcaa2209af | 83 | } |
ftagius | 1:35fcaa2209af | 84 | if (sum != 0) { |
ftagius | 1:35fcaa2209af | 85 | // bad checksum :( |
ftagius | 1:35fcaa2209af | 86 | //return false; |
simon | 0:15611c7938a3 | 87 | } |
ftagius | 1:35fcaa2209af | 88 | } |
ftagius | 1:35fcaa2209af | 89 | |
ftagius | 1:35fcaa2209af | 90 | // look for a few common sentences |
ftagius | 1:35fcaa2209af | 91 | if (strstr(nmea, "$GPGGA")) { |
ftagius | 1:35fcaa2209af | 92 | // found GGA |
ftagius | 1:35fcaa2209af | 93 | char *p = nmea; |
ftagius | 1:35fcaa2209af | 94 | // get time |
ftagius | 1:35fcaa2209af | 95 | p = strchr(p, ',')+1; |
ftagius | 1:35fcaa2209af | 96 | timef = atof(p); |
ftagius | 1:35fcaa2209af | 97 | uint32_t time = timef; |
ftagius | 1:35fcaa2209af | 98 | hour = time / 10000; |
ftagius | 1:35fcaa2209af | 99 | minute = (time % 10000) / 100; |
ftagius | 1:35fcaa2209af | 100 | seconds = (time % 100); |
ftagius | 1:35fcaa2209af | 101 | |
ftagius | 1:35fcaa2209af | 102 | milliseconds = fmod((double) timef, 1.0) * 1000; |
ftagius | 1:35fcaa2209af | 103 | |
ftagius | 1:35fcaa2209af | 104 | // parse out latitude |
ftagius | 1:35fcaa2209af | 105 | p = strchr(p, ',')+1; |
ftagius | 1:35fcaa2209af | 106 | latitude = atof(p); |
ftagius | 1:35fcaa2209af | 107 | |
ftagius | 1:35fcaa2209af | 108 | p = strchr(p, ',')+1; |
ftagius | 1:35fcaa2209af | 109 | if (p[0] == 'N') lat = 'N'; |
ftagius | 1:35fcaa2209af | 110 | else if (p[0] == 'S') lat = 'S'; |
ftagius | 1:35fcaa2209af | 111 | else if (p[0] == ',') lat = 0; |
ftagius | 1:35fcaa2209af | 112 | else return false; |
ftagius | 1:35fcaa2209af | 113 | |
ftagius | 1:35fcaa2209af | 114 | // parse out longitude |
ftagius | 1:35fcaa2209af | 115 | p = strchr(p, ',')+1; |
ftagius | 1:35fcaa2209af | 116 | longitude = atof(p); |
ftagius | 1:35fcaa2209af | 117 | |
ftagius | 1:35fcaa2209af | 118 | p = strchr(p, ',')+1; |
ftagius | 1:35fcaa2209af | 119 | if (p[0] == 'W') lon = 'W'; |
ftagius | 1:35fcaa2209af | 120 | else if (p[0] == 'E') lon = 'E'; |
ftagius | 1:35fcaa2209af | 121 | else if (p[0] == ',') lon = 0; |
ftagius | 1:35fcaa2209af | 122 | else return false; |
ftagius | 1:35fcaa2209af | 123 | // convert to degrees |
ftagius | 1:35fcaa2209af | 124 | lat_deg = latitude; |
ftagius | 1:35fcaa2209af | 125 | lon_deg = longitude; |
ftagius | 1:35fcaa2209af | 126 | |
ftagius | 1:35fcaa2209af | 127 | if(lat == 'S') { lat_deg *= -1.0; } |
ftagius | 1:35fcaa2209af | 128 | if(lon == 'W') { lon_deg *= -1.0; } |
ftagius | 1:35fcaa2209af | 129 | float degrees = trunc(lat_deg / 100.0f); |
ftagius | 1:35fcaa2209af | 130 | float minutes = lat_deg - (degrees * 100.0f); |
ftagius | 1:35fcaa2209af | 131 | lat_deg = degrees + minutes / 60.0f; |
ftagius | 1:35fcaa2209af | 132 | degrees = trunc(lon_deg / 100.0f); |
ftagius | 1:35fcaa2209af | 133 | //degrees = trunc(longitude / 100.0f * 0.01f); |
ftagius | 1:35fcaa2209af | 134 | //printf("my lon=%f deg=%f\r\n",longitude, degrees); |
ftagius | 1:35fcaa2209af | 135 | minutes = lon_deg - (degrees * 100.0f); |
ftagius | 1:35fcaa2209af | 136 | lon_deg = degrees + minutes / 60.0f; |
ftagius | 1:35fcaa2209af | 137 | //printf("local: lat=%f lon=%f\r\n",l_latitude, l_longitude); |
ftagius | 1:35fcaa2209af | 138 | //printf("gpgga rcvd\r\n"); |
ftagius | 1:35fcaa2209af | 139 | |
ftagius | 1:35fcaa2209af | 140 | p = strchr(p, ',')+1; |
ftagius | 1:35fcaa2209af | 141 | fixquality = atoi(p); |
ftagius | 1:35fcaa2209af | 142 | |
ftagius | 1:35fcaa2209af | 143 | p = strchr(p, ',')+1; |
ftagius | 1:35fcaa2209af | 144 | satellites = atoi(p); |
ftagius | 1:35fcaa2209af | 145 | |
ftagius | 1:35fcaa2209af | 146 | p = strchr(p, ',')+1; |
ftagius | 1:35fcaa2209af | 147 | HDOP = atof(p); |
ftagius | 1:35fcaa2209af | 148 | |
ftagius | 1:35fcaa2209af | 149 | p = strchr(p, ',')+1; |
ftagius | 1:35fcaa2209af | 150 | altitude = atof(p); |
ftagius | 1:35fcaa2209af | 151 | p = strchr(p, ',')+1; |
ftagius | 1:35fcaa2209af | 152 | p = strchr(p, ',')+1; |
ftagius | 1:35fcaa2209af | 153 | geoidheight = atof(p); |
cmkachur | 2:509abe8eda59 | 154 | fix = true; |
ftagius | 1:35fcaa2209af | 155 | return true; |
ftagius | 1:35fcaa2209af | 156 | } |
ftagius | 1:35fcaa2209af | 157 | if (strstr(nmea, "$GPRMC")) { |
ftagius | 1:35fcaa2209af | 158 | // found RMC |
ftagius | 1:35fcaa2209af | 159 | char *p = nmea; |
ftagius | 1:35fcaa2209af | 160 | |
ftagius | 1:35fcaa2209af | 161 | // get time |
ftagius | 1:35fcaa2209af | 162 | p = strchr(p, ',')+1; |
ftagius | 1:35fcaa2209af | 163 | timef = atof(p); |
ftagius | 1:35fcaa2209af | 164 | uint32_t time = timef; |
ftagius | 1:35fcaa2209af | 165 | hour = time / 10000; |
ftagius | 1:35fcaa2209af | 166 | minute = (time % 10000) / 100; |
ftagius | 1:35fcaa2209af | 167 | seconds = (time % 100); |
ftagius | 1:35fcaa2209af | 168 | |
ftagius | 1:35fcaa2209af | 169 | milliseconds = fmod((double) timef, 1.0) * 1000; |
ftagius | 1:35fcaa2209af | 170 | |
ftagius | 1:35fcaa2209af | 171 | p = strchr(p, ',')+1; |
ftagius | 1:35fcaa2209af | 172 | // Serial.println(p); |
ftagius | 1:35fcaa2209af | 173 | if (p[0] == 'A') |
ftagius | 1:35fcaa2209af | 174 | fix = true; |
ftagius | 1:35fcaa2209af | 175 | else if (p[0] == 'V') |
ftagius | 1:35fcaa2209af | 176 | fix = false; |
ftagius | 1:35fcaa2209af | 177 | else |
ftagius | 1:35fcaa2209af | 178 | return false; |
ftagius | 1:35fcaa2209af | 179 | |
ftagius | 1:35fcaa2209af | 180 | // parse out latitude |
ftagius | 1:35fcaa2209af | 181 | p = strchr(p, ',')+1; |
ftagius | 1:35fcaa2209af | 182 | latitude = atof(p); |
ftagius | 1:35fcaa2209af | 183 | |
ftagius | 1:35fcaa2209af | 184 | p = strchr(p, ',')+1; |
ftagius | 1:35fcaa2209af | 185 | if (p[0] == 'N') lat = 'N'; |
ftagius | 1:35fcaa2209af | 186 | else if (p[0] == 'S') lat = 'S'; |
ftagius | 1:35fcaa2209af | 187 | else if (p[0] == ',') lat = 0; |
ftagius | 1:35fcaa2209af | 188 | else return false; |
ftagius | 1:35fcaa2209af | 189 | |
ftagius | 1:35fcaa2209af | 190 | // parse out longitude |
ftagius | 1:35fcaa2209af | 191 | p = strchr(p, ',')+1; |
ftagius | 1:35fcaa2209af | 192 | longitude = atof(p); |
ftagius | 1:35fcaa2209af | 193 | |
ftagius | 1:35fcaa2209af | 194 | p = strchr(p, ',')+1; |
ftagius | 1:35fcaa2209af | 195 | if (p[0] == 'W') lon = 'W'; |
ftagius | 1:35fcaa2209af | 196 | else if (p[0] == 'E') lon = 'E'; |
ftagius | 1:35fcaa2209af | 197 | else if (p[0] == ',') lon = 0; |
ftagius | 1:35fcaa2209af | 198 | else return false; |
ftagius | 1:35fcaa2209af | 199 | // convert to degrees |
ftagius | 1:35fcaa2209af | 200 | lat_deg = latitude; |
ftagius | 1:35fcaa2209af | 201 | lon_deg = longitude; |
ftagius | 1:35fcaa2209af | 202 | |
ftagius | 1:35fcaa2209af | 203 | if(lat == 'S') { lat_deg *= -1.0; } |
ftagius | 1:35fcaa2209af | 204 | if(lon == 'W') { lon_deg *= -1.0; } |
ftagius | 1:35fcaa2209af | 205 | float degrees = trunc(lat_deg / 100.0f); |
ftagius | 1:35fcaa2209af | 206 | float minutes = lat_deg - (degrees * 100.0f); |
ftagius | 1:35fcaa2209af | 207 | lat_deg = degrees + minutes / 60.0f; |
ftagius | 1:35fcaa2209af | 208 | degrees = trunc(lon_deg / 100.0f); |
ftagius | 1:35fcaa2209af | 209 | minutes = lon_deg - (degrees * 100.0f); |
ftagius | 1:35fcaa2209af | 210 | lon_deg = degrees + minutes / 60.0f; |
ftagius | 1:35fcaa2209af | 211 | |
ftagius | 1:35fcaa2209af | 212 | // speed |
ftagius | 1:35fcaa2209af | 213 | p = strchr(p, ',')+1; |
ftagius | 1:35fcaa2209af | 214 | speed = atof(p); |
ftagius | 1:35fcaa2209af | 215 | |
ftagius | 1:35fcaa2209af | 216 | // angle |
ftagius | 1:35fcaa2209af | 217 | p = strchr(p, ',')+1; |
ftagius | 1:35fcaa2209af | 218 | angle = atof(p); |
ftagius | 1:35fcaa2209af | 219 | |
ftagius | 1:35fcaa2209af | 220 | p = strchr(p, ',')+1; |
ftagius | 1:35fcaa2209af | 221 | uint32_t fulldate = atof(p); |
ftagius | 1:35fcaa2209af | 222 | day = fulldate / 10000; |
ftagius | 1:35fcaa2209af | 223 | month = (fulldate % 10000) / 100; |
ftagius | 1:35fcaa2209af | 224 | year = (fulldate % 100); |
ftagius | 1:35fcaa2209af | 225 | |
ftagius | 1:35fcaa2209af | 226 | // we dont parse the remaining, yet! |
ftagius | 1:35fcaa2209af | 227 | return true; |
ftagius | 1:35fcaa2209af | 228 | } |
ftagius | 1:35fcaa2209af | 229 | |
ftagius | 1:35fcaa2209af | 230 | return false; |
simon | 0:15611c7938a3 | 231 | } |
ftagius | 1:35fcaa2209af | 232 | |
ftagius | 1:35fcaa2209af | 233 | |
ftagius | 1:35fcaa2209af | 234 | char GPS::read(void) { |
ftagius | 1:35fcaa2209af | 235 | char c = 0; |
ftagius | 1:35fcaa2209af | 236 | |
ftagius | 1:35fcaa2209af | 237 | if (paused) return c; |
ftagius | 1:35fcaa2209af | 238 | |
ftagius | 1:35fcaa2209af | 239 | if(!_gps.readable()) return c; |
ftagius | 1:35fcaa2209af | 240 | c = _gps.getc(); |
ftagius | 1:35fcaa2209af | 241 | |
ftagius | 1:35fcaa2209af | 242 | //Serial.print(c); |
ftagius | 1:35fcaa2209af | 243 | |
ftagius | 1:35fcaa2209af | 244 | if (c == '$') { |
ftagius | 1:35fcaa2209af | 245 | currentline[lineidx] = 0; |
ftagius | 1:35fcaa2209af | 246 | lineidx = 0; |
ftagius | 1:35fcaa2209af | 247 | } |
ftagius | 1:35fcaa2209af | 248 | if (c == '\n') { |
ftagius | 1:35fcaa2209af | 249 | currentline[lineidx] = 0; |
ftagius | 1:35fcaa2209af | 250 | |
ftagius | 1:35fcaa2209af | 251 | if (currentline == line1) { |
ftagius | 1:35fcaa2209af | 252 | currentline = line2; |
ftagius | 1:35fcaa2209af | 253 | lastline = line1; |
ftagius | 1:35fcaa2209af | 254 | } else { |
ftagius | 1:35fcaa2209af | 255 | currentline = line1; |
ftagius | 1:35fcaa2209af | 256 | lastline = line2; |
ftagius | 1:35fcaa2209af | 257 | } |
ftagius | 1:35fcaa2209af | 258 | |
ftagius | 1:35fcaa2209af | 259 | lineidx = 0; |
ftagius | 1:35fcaa2209af | 260 | recvdflag = true; |
ftagius | 1:35fcaa2209af | 261 | } |
ftagius | 1:35fcaa2209af | 262 | |
ftagius | 1:35fcaa2209af | 263 | currentline[lineidx++] = c; |
ftagius | 1:35fcaa2209af | 264 | if (lineidx >= MAXLINELENGTH) |
ftagius | 1:35fcaa2209af | 265 | lineidx = MAXLINELENGTH-1; |
ftagius | 1:35fcaa2209af | 266 | |
ftagius | 1:35fcaa2209af | 267 | return c; |
ftagius | 1:35fcaa2209af | 268 | } |
ftagius | 1:35fcaa2209af | 269 | |
ftagius | 1:35fcaa2209af | 270 | void GPS::setBaud(int baud) |
ftagius | 1:35fcaa2209af | 271 | { |
ftagius | 1:35fcaa2209af | 272 | |
ftagius | 1:35fcaa2209af | 273 | switch (baud) { |
ftagius | 1:35fcaa2209af | 274 | case 9600: |
ftagius | 1:35fcaa2209af | 275 | _gps.baud(9600); |
ftagius | 1:35fcaa2209af | 276 | sendCommand(PMTK_SET_BAUD_9600); |
ftagius | 1:35fcaa2209af | 277 | wait_ms(100); |
ftagius | 1:35fcaa2209af | 278 | printf("baud (%d), set to 9600\r\n",baud); |
ftagius | 1:35fcaa2209af | 279 | break; |
ftagius | 1:35fcaa2209af | 280 | case 57600: |
ftagius | 1:35fcaa2209af | 281 | _gps.baud(9600); // assume device is at default 9600 baud |
ftagius | 1:35fcaa2209af | 282 | sendCommand(PMTK_SET_BAUD_57600); |
ftagius | 1:35fcaa2209af | 283 | _gps.baud(57600); |
ftagius | 1:35fcaa2209af | 284 | wait_ms(100); |
ftagius | 1:35fcaa2209af | 285 | printf("baud (%d), set to 57600\r\n",baud); |
ftagius | 1:35fcaa2209af | 286 | break; |
ftagius | 1:35fcaa2209af | 287 | default: |
ftagius | 1:35fcaa2209af | 288 | printf("unsupported baud (%d), set to 9600\r\n",baud); |
ftagius | 1:35fcaa2209af | 289 | _gps.baud(9600); |
ftagius | 1:35fcaa2209af | 290 | sendCommand(PMTK_SET_BAUD_9600); |
ftagius | 1:35fcaa2209af | 291 | break; |
ftagius | 1:35fcaa2209af | 292 | } |
ftagius | 1:35fcaa2209af | 293 | |
ftagius | 1:35fcaa2209af | 294 | wait_ms(10); |
ftagius | 1:35fcaa2209af | 295 | } |
ftagius | 1:35fcaa2209af | 296 | |
ftagius | 1:35fcaa2209af | 297 | void GPS::sendCommand(char *str) { |
ftagius | 1:35fcaa2209af | 298 | _gps.printf("%s\r\n",str); |
ftagius | 1:35fcaa2209af | 299 | } |
ftagius | 1:35fcaa2209af | 300 | |
ftagius | 1:35fcaa2209af | 301 | void GPS::setSetup(void) { |
ftagius | 1:35fcaa2209af | 302 | |
ftagius | 1:35fcaa2209af | 303 | |
ftagius | 1:35fcaa2209af | 304 | |
ftagius | 1:35fcaa2209af | 305 | |
ftagius | 1:35fcaa2209af | 306 | } |
ftagius | 1:35fcaa2209af | 307 | |
ftagius | 1:35fcaa2209af | 308 | // read a Hex value and return the decimal equivalent |
ftagius | 1:35fcaa2209af | 309 | uint8_t GPS::parseHex(char c) { |
ftagius | 1:35fcaa2209af | 310 | if (c < '0') |
ftagius | 1:35fcaa2209af | 311 | return 0; |
ftagius | 1:35fcaa2209af | 312 | if (c <= '9') |
ftagius | 1:35fcaa2209af | 313 | return c - '0'; |
ftagius | 1:35fcaa2209af | 314 | if (c < 'A') |
ftagius | 1:35fcaa2209af | 315 | return 0; |
ftagius | 1:35fcaa2209af | 316 | if (c <= 'F') |
ftagius | 1:35fcaa2209af | 317 | return (c - 'A')+10; |
ftagius | 1:35fcaa2209af | 318 | return 0; |
ftagius | 1:35fcaa2209af | 319 | } |
ftagius | 1:35fcaa2209af | 320 | |
ftagius | 1:35fcaa2209af | 321 | bool GPS::waitForSentence(char *wait4me, uint8_t max) { |
ftagius | 1:35fcaa2209af | 322 | char str[20]; |
ftagius | 1:35fcaa2209af | 323 | |
ftagius | 1:35fcaa2209af | 324 | uint8_t i=0; |
ftagius | 1:35fcaa2209af | 325 | while (i < max) { |
ftagius | 1:35fcaa2209af | 326 | if (newNMEAreceived()) { |
ftagius | 1:35fcaa2209af | 327 | char *nmea = lastNMEA(); |
ftagius | 1:35fcaa2209af | 328 | strncpy(str, nmea, 20); |
ftagius | 1:35fcaa2209af | 329 | str[19] = 0; |
ftagius | 1:35fcaa2209af | 330 | i++; |
ftagius | 1:35fcaa2209af | 331 | |
ftagius | 1:35fcaa2209af | 332 | if (strstr(str, wait4me)) |
ftagius | 1:35fcaa2209af | 333 | return true; |
ftagius | 1:35fcaa2209af | 334 | } |
ftagius | 1:35fcaa2209af | 335 | } |
ftagius | 1:35fcaa2209af | 336 | |
ftagius | 1:35fcaa2209af | 337 | return false; |
ftagius | 1:35fcaa2209af | 338 | } |
ftagius | 1:35fcaa2209af | 339 | |
ftagius | 1:35fcaa2209af | 340 | bool GPS::LOCUS_StartLogger(void) { |
ftagius | 1:35fcaa2209af | 341 | sendCommand(PMTK_LOCUS_STARTLOG); |
ftagius | 1:35fcaa2209af | 342 | recvdflag = false; |
ftagius | 1:35fcaa2209af | 343 | return waitForSentence(PMTK_LOCUS_LOGSTARTED); |
ftagius | 1:35fcaa2209af | 344 | } |
ftagius | 1:35fcaa2209af | 345 | |
ftagius | 1:35fcaa2209af | 346 | bool GPS::LOCUS_ReadStatus(void) { |
ftagius | 1:35fcaa2209af | 347 | sendCommand(PMTK_LOCUS_QUERY_STATUS); |
ftagius | 1:35fcaa2209af | 348 | |
ftagius | 1:35fcaa2209af | 349 | if (! waitForSentence("$PMTKLOG")) |
ftagius | 1:35fcaa2209af | 350 | return false; |
ftagius | 1:35fcaa2209af | 351 | |
ftagius | 1:35fcaa2209af | 352 | char *response = lastNMEA(); |
ftagius | 1:35fcaa2209af | 353 | uint16_t parsed[10]; |
ftagius | 1:35fcaa2209af | 354 | int8_t i; |
ftagius | 1:35fcaa2209af | 355 | |
ftagius | 1:35fcaa2209af | 356 | for (i=0; i<10; i++) parsed[i] = -1; |
ftagius | 1:35fcaa2209af | 357 | |
ftagius | 1:35fcaa2209af | 358 | response = strchr(response, ','); |
ftagius | 1:35fcaa2209af | 359 | for (i=0; i<10; i++) { |
ftagius | 1:35fcaa2209af | 360 | if (!response || (response[0] == 0) || (response[0] == '*')) |
ftagius | 1:35fcaa2209af | 361 | break; |
ftagius | 1:35fcaa2209af | 362 | response++; |
ftagius | 1:35fcaa2209af | 363 | parsed[i]=0; |
ftagius | 1:35fcaa2209af | 364 | while ((response[0] != ',') && |
ftagius | 1:35fcaa2209af | 365 | (response[0] != '*') && (response[0] != 0)) { |
ftagius | 1:35fcaa2209af | 366 | parsed[i] *= 10; |
ftagius | 1:35fcaa2209af | 367 | char c = response[0]; |
ftagius | 1:35fcaa2209af | 368 | if (isdigit(c)) |
ftagius | 1:35fcaa2209af | 369 | parsed[i] += c - '0'; |
ftagius | 1:35fcaa2209af | 370 | else |
ftagius | 1:35fcaa2209af | 371 | parsed[i] = c; |
ftagius | 1:35fcaa2209af | 372 | response++; |
ftagius | 1:35fcaa2209af | 373 | } |
ftagius | 1:35fcaa2209af | 374 | } |
ftagius | 1:35fcaa2209af | 375 | LOCUS_serial = parsed[0]; |
ftagius | 1:35fcaa2209af | 376 | LOCUS_type = parsed[1]; |
ftagius | 1:35fcaa2209af | 377 | if (isalpha(parsed[2])) { |
ftagius | 1:35fcaa2209af | 378 | parsed[2] = parsed[2] - 'a' + 10; |
ftagius | 1:35fcaa2209af | 379 | } |
ftagius | 1:35fcaa2209af | 380 | LOCUS_mode = parsed[2]; |
ftagius | 1:35fcaa2209af | 381 | LOCUS_config = parsed[3]; |
ftagius | 1:35fcaa2209af | 382 | LOCUS_interval = parsed[4]; |
ftagius | 1:35fcaa2209af | 383 | LOCUS_distance = parsed[5]; |
ftagius | 1:35fcaa2209af | 384 | LOCUS_speed = parsed[6]; |
ftagius | 1:35fcaa2209af | 385 | LOCUS_status = !parsed[7]; |
ftagius | 1:35fcaa2209af | 386 | LOCUS_records = parsed[8]; |
ftagius | 1:35fcaa2209af | 387 | LOCUS_percent = parsed[9]; |
ftagius | 1:35fcaa2209af | 388 | |
ftagius | 1:35fcaa2209af | 389 | return true; |
ftagius | 1:35fcaa2209af | 390 | } |
ftagius | 1:35fcaa2209af | 391 | |
ftagius | 1:35fcaa2209af | 392 | // Standby Mode Switches |
ftagius | 1:35fcaa2209af | 393 | bool GPS::standby(void) { |
ftagius | 1:35fcaa2209af | 394 | if (inStandbyMode) { |
ftagius | 1:35fcaa2209af | 395 | return false; // Returns false if already in standby mode, so that you do not wake it up by sending commands to GPS |
ftagius | 1:35fcaa2209af | 396 | } |
ftagius | 1:35fcaa2209af | 397 | else { |
ftagius | 1:35fcaa2209af | 398 | inStandbyMode = true; |
ftagius | 1:35fcaa2209af | 399 | sendCommand(PMTK_STANDBY); |
ftagius | 1:35fcaa2209af | 400 | //return waitForSentence(PMTK_STANDBY_SUCCESS); // don't seem to be fast enough to catch the message, or something else just is not working |
ftagius | 1:35fcaa2209af | 401 | return true; |
ftagius | 1:35fcaa2209af | 402 | } |
ftagius | 1:35fcaa2209af | 403 | } |
ftagius | 1:35fcaa2209af | 404 | |
ftagius | 1:35fcaa2209af | 405 | bool GPS::wakeup(void) { |
ftagius | 1:35fcaa2209af | 406 | if (inStandbyMode) { |
ftagius | 1:35fcaa2209af | 407 | inStandbyMode = false; |
ftagius | 1:35fcaa2209af | 408 | sendCommand(""); // send byte to wake it up |
ftagius | 1:35fcaa2209af | 409 | return waitForSentence(PMTK_AWAKE); |
ftagius | 1:35fcaa2209af | 410 | } |
ftagius | 1:35fcaa2209af | 411 | else { |
ftagius | 1:35fcaa2209af | 412 | return false; // Returns false if not in standby mode, nothing to wakeup |
ftagius | 1:35fcaa2209af | 413 | } |
ftagius | 1:35fcaa2209af | 414 | } |