CaryCoders / AdafruitGPS

GPS code for Adafruit ultimate GPS

Dependents:   demo_gps_sdcard zeus

Fork of GPS by Simon Ford

GPS.cpp@1:35fcaa2209af, 2015-06-16 (annotated)

Committer:
ftagius
Date:
Tue Jun 16 12:03:20 2015 +0000
Revision:
1:35fcaa2209af
Parent:
0:15611c7938a3
Child:
2:509abe8eda59
GPS code for use with Adafruit ultimate gps

Who changed what in which revision?

UserRevisionLine numberNew 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);
ftagius 1:35fcaa2209af 154 return true;
ftagius 1:35fcaa2209af 155 }
ftagius 1:35fcaa2209af 156 if (strstr(nmea, "$GPRMC")) {
ftagius 1:35fcaa2209af 157 // found RMC
ftagius 1:35fcaa2209af 158 char *p = nmea;
ftagius 1:35fcaa2209af 159
ftagius 1:35fcaa2209af 160 // get time
ftagius 1:35fcaa2209af 161 p = strchr(p, ',')+1;
ftagius 1:35fcaa2209af 162 timef = atof(p);
ftagius 1:35fcaa2209af 163 uint32_t time = timef;
ftagius 1:35fcaa2209af 164 hour = time / 10000;
ftagius 1:35fcaa2209af 165 minute = (time % 10000) / 100;
ftagius 1:35fcaa2209af 166 seconds = (time % 100);
ftagius 1:35fcaa2209af 167
ftagius 1:35fcaa2209af 168 milliseconds = fmod((double) timef, 1.0) * 1000;
ftagius 1:35fcaa2209af 169
ftagius 1:35fcaa2209af 170 p = strchr(p, ',')+1;
ftagius 1:35fcaa2209af 171 // Serial.println(p);
ftagius 1:35fcaa2209af 172 if (p[0] == 'A')
ftagius 1:35fcaa2209af 173 fix = true;
ftagius 1:35fcaa2209af 174 else if (p[0] == 'V')
ftagius 1:35fcaa2209af 175 fix = false;
ftagius 1:35fcaa2209af 176 else
ftagius 1:35fcaa2209af 177 return false;
ftagius 1:35fcaa2209af 178
ftagius 1:35fcaa2209af 179 // parse out latitude
ftagius 1:35fcaa2209af 180 p = strchr(p, ',')+1;
ftagius 1:35fcaa2209af 181 latitude = atof(p);
ftagius 1:35fcaa2209af 182
ftagius 1:35fcaa2209af 183 p = strchr(p, ',')+1;
ftagius 1:35fcaa2209af 184 if (p[0] == 'N') lat = 'N';
ftagius 1:35fcaa2209af 185 else if (p[0] == 'S') lat = 'S';
ftagius 1:35fcaa2209af 186 else if (p[0] == ',') lat = 0;
ftagius 1:35fcaa2209af 187 else return false;
ftagius 1:35fcaa2209af 188
ftagius 1:35fcaa2209af 189 // parse out longitude
ftagius 1:35fcaa2209af 190 p = strchr(p, ',')+1;
ftagius 1:35fcaa2209af 191 longitude = atof(p);
ftagius 1:35fcaa2209af 192
ftagius 1:35fcaa2209af 193 p = strchr(p, ',')+1;
ftagius 1:35fcaa2209af 194 if (p[0] == 'W') lon = 'W';
ftagius 1:35fcaa2209af 195 else if (p[0] == 'E') lon = 'E';
ftagius 1:35fcaa2209af 196 else if (p[0] == ',') lon = 0;
ftagius 1:35fcaa2209af 197 else return false;
ftagius 1:35fcaa2209af 198 // convert to degrees
ftagius 1:35fcaa2209af 199 lat_deg = latitude;
ftagius 1:35fcaa2209af 200 lon_deg = longitude;
ftagius 1:35fcaa2209af 201
ftagius 1:35fcaa2209af 202 if(lat == 'S') { lat_deg *= -1.0; }
ftagius 1:35fcaa2209af 203 if(lon == 'W') { lon_deg *= -1.0; }
ftagius 1:35fcaa2209af 204 float degrees = trunc(lat_deg / 100.0f);
ftagius 1:35fcaa2209af 205 float minutes = lat_deg - (degrees * 100.0f);
ftagius 1:35fcaa2209af 206 lat_deg = degrees + minutes / 60.0f;
ftagius 1:35fcaa2209af 207 degrees = trunc(lon_deg / 100.0f);
ftagius 1:35fcaa2209af 208 minutes = lon_deg - (degrees * 100.0f);
ftagius 1:35fcaa2209af 209 lon_deg = degrees + minutes / 60.0f;
ftagius 1:35fcaa2209af 210
ftagius 1:35fcaa2209af 211 // speed
ftagius 1:35fcaa2209af 212 p = strchr(p, ',')+1;
ftagius 1:35fcaa2209af 213 speed = atof(p);
ftagius 1:35fcaa2209af 214
ftagius 1:35fcaa2209af 215 // angle
ftagius 1:35fcaa2209af 216 p = strchr(p, ',')+1;
ftagius 1:35fcaa2209af 217 angle = atof(p);
ftagius 1:35fcaa2209af 218
ftagius 1:35fcaa2209af 219 p = strchr(p, ',')+1;
ftagius 1:35fcaa2209af 220 uint32_t fulldate = atof(p);
ftagius 1:35fcaa2209af 221 day = fulldate / 10000;
ftagius 1:35fcaa2209af 222 month = (fulldate % 10000) / 100;
ftagius 1:35fcaa2209af 223 year = (fulldate % 100);
ftagius 1:35fcaa2209af 224
ftagius 1:35fcaa2209af 225 // we dont parse the remaining, yet!
ftagius 1:35fcaa2209af 226 return true;
ftagius 1:35fcaa2209af 227 }
ftagius 1:35fcaa2209af 228
ftagius 1:35fcaa2209af 229 return false;
simon 0:15611c7938a3 230 }
ftagius 1:35fcaa2209af 231
ftagius 1:35fcaa2209af 232
ftagius 1:35fcaa2209af 233 char GPS::read(void) {
ftagius 1:35fcaa2209af 234 char c = 0;
ftagius 1:35fcaa2209af 235
ftagius 1:35fcaa2209af 236 if (paused) return c;
ftagius 1:35fcaa2209af 237
ftagius 1:35fcaa2209af 238 if(!_gps.readable()) return c;
ftagius 1:35fcaa2209af 239 c = _gps.getc();
ftagius 1:35fcaa2209af 240
ftagius 1:35fcaa2209af 241 //Serial.print(c);
ftagius 1:35fcaa2209af 242
ftagius 1:35fcaa2209af 243 if (c == '$') {
ftagius 1:35fcaa2209af 244 currentline[lineidx] = 0;
ftagius 1:35fcaa2209af 245 lineidx = 0;
ftagius 1:35fcaa2209af 246 }
ftagius 1:35fcaa2209af 247 if (c == '\n') {
ftagius 1:35fcaa2209af 248 currentline[lineidx] = 0;
ftagius 1:35fcaa2209af 249
ftagius 1:35fcaa2209af 250 if (currentline == line1) {
ftagius 1:35fcaa2209af 251 currentline = line2;
ftagius 1:35fcaa2209af 252 lastline = line1;
ftagius 1:35fcaa2209af 253 } else {
ftagius 1:35fcaa2209af 254 currentline = line1;
ftagius 1:35fcaa2209af 255 lastline = line2;
ftagius 1:35fcaa2209af 256 }
ftagius 1:35fcaa2209af 257
ftagius 1:35fcaa2209af 258 lineidx = 0;
ftagius 1:35fcaa2209af 259 recvdflag = true;
ftagius 1:35fcaa2209af 260 }
ftagius 1:35fcaa2209af 261
ftagius 1:35fcaa2209af 262 currentline[lineidx++] = c;
ftagius 1:35fcaa2209af 263 if (lineidx >= MAXLINELENGTH)
ftagius 1:35fcaa2209af 264 lineidx = MAXLINELENGTH-1;
ftagius 1:35fcaa2209af 265
ftagius 1:35fcaa2209af 266 return c;
ftagius 1:35fcaa2209af 267 }
ftagius 1:35fcaa2209af 268
ftagius 1:35fcaa2209af 269 void GPS::setBaud(int baud)
ftagius 1:35fcaa2209af 270 {
ftagius 1:35fcaa2209af 271
ftagius 1:35fcaa2209af 272 switch (baud) {
ftagius 1:35fcaa2209af 273 case 9600:
ftagius 1:35fcaa2209af 274 _gps.baud(9600);
ftagius 1:35fcaa2209af 275 sendCommand(PMTK_SET_BAUD_9600);
ftagius 1:35fcaa2209af 276 wait_ms(100);
ftagius 1:35fcaa2209af 277 printf("baud (%d), set to 9600\r\n",baud);
ftagius 1:35fcaa2209af 278 break;
ftagius 1:35fcaa2209af 279 case 57600:
ftagius 1:35fcaa2209af 280 _gps.baud(9600); // assume device is at default 9600 baud
ftagius 1:35fcaa2209af 281 sendCommand(PMTK_SET_BAUD_57600);
ftagius 1:35fcaa2209af 282 _gps.baud(57600);
ftagius 1:35fcaa2209af 283 wait_ms(100);
ftagius 1:35fcaa2209af 284 printf("baud (%d), set to 57600\r\n",baud);
ftagius 1:35fcaa2209af 285 break;
ftagius 1:35fcaa2209af 286 default:
ftagius 1:35fcaa2209af 287 printf("unsupported baud (%d), set to 9600\r\n",baud);
ftagius 1:35fcaa2209af 288 _gps.baud(9600);
ftagius 1:35fcaa2209af 289 sendCommand(PMTK_SET_BAUD_9600);
ftagius 1:35fcaa2209af 290 break;
ftagius 1:35fcaa2209af 291 }
ftagius 1:35fcaa2209af 292
ftagius 1:35fcaa2209af 293 wait_ms(10);
ftagius 1:35fcaa2209af 294 }
ftagius 1:35fcaa2209af 295
ftagius 1:35fcaa2209af 296 void GPS::sendCommand(char *str) {
ftagius 1:35fcaa2209af 297 _gps.printf("%s\r\n",str);
ftagius 1:35fcaa2209af 298 }
ftagius 1:35fcaa2209af 299
ftagius 1:35fcaa2209af 300 void GPS::setSetup(void) {
ftagius 1:35fcaa2209af 301
ftagius 1:35fcaa2209af 302
ftagius 1:35fcaa2209af 303
ftagius 1:35fcaa2209af 304
ftagius 1:35fcaa2209af 305 }
ftagius 1:35fcaa2209af 306
ftagius 1:35fcaa2209af 307 // read a Hex value and return the decimal equivalent
ftagius 1:35fcaa2209af 308 uint8_t GPS::parseHex(char c) {
ftagius 1:35fcaa2209af 309 if (c < '0')
ftagius 1:35fcaa2209af 310 return 0;
ftagius 1:35fcaa2209af 311 if (c <= '9')
ftagius 1:35fcaa2209af 312 return c - '0';
ftagius 1:35fcaa2209af 313 if (c < 'A')
ftagius 1:35fcaa2209af 314 return 0;
ftagius 1:35fcaa2209af 315 if (c <= 'F')
ftagius 1:35fcaa2209af 316 return (c - 'A')+10;
ftagius 1:35fcaa2209af 317 return 0;
ftagius 1:35fcaa2209af 318 }
ftagius 1:35fcaa2209af 319
ftagius 1:35fcaa2209af 320 bool GPS::waitForSentence(char *wait4me, uint8_t max) {
ftagius 1:35fcaa2209af 321 char str[20];
ftagius 1:35fcaa2209af 322
ftagius 1:35fcaa2209af 323 uint8_t i=0;
ftagius 1:35fcaa2209af 324 while (i < max) {
ftagius 1:35fcaa2209af 325 if (newNMEAreceived()) {
ftagius 1:35fcaa2209af 326 char *nmea = lastNMEA();
ftagius 1:35fcaa2209af 327 strncpy(str, nmea, 20);
ftagius 1:35fcaa2209af 328 str[19] = 0;
ftagius 1:35fcaa2209af 329 i++;
ftagius 1:35fcaa2209af 330
ftagius 1:35fcaa2209af 331 if (strstr(str, wait4me))
ftagius 1:35fcaa2209af 332 return true;
ftagius 1:35fcaa2209af 333 }
ftagius 1:35fcaa2209af 334 }
ftagius 1:35fcaa2209af 335
ftagius 1:35fcaa2209af 336 return false;
ftagius 1:35fcaa2209af 337 }
ftagius 1:35fcaa2209af 338
ftagius 1:35fcaa2209af 339 bool GPS::LOCUS_StartLogger(void) {
ftagius 1:35fcaa2209af 340 sendCommand(PMTK_LOCUS_STARTLOG);
ftagius 1:35fcaa2209af 341 recvdflag = false;
ftagius 1:35fcaa2209af 342 return waitForSentence(PMTK_LOCUS_LOGSTARTED);
ftagius 1:35fcaa2209af 343 }
ftagius 1:35fcaa2209af 344
ftagius 1:35fcaa2209af 345 bool GPS::LOCUS_ReadStatus(void) {
ftagius 1:35fcaa2209af 346 sendCommand(PMTK_LOCUS_QUERY_STATUS);
ftagius 1:35fcaa2209af 347
ftagius 1:35fcaa2209af 348 if (! waitForSentence("$PMTKLOG"))
ftagius 1:35fcaa2209af 349 return false;
ftagius 1:35fcaa2209af 350
ftagius 1:35fcaa2209af 351 char *response = lastNMEA();
ftagius 1:35fcaa2209af 352 uint16_t parsed[10];
ftagius 1:35fcaa2209af 353 int8_t i;
ftagius 1:35fcaa2209af 354
ftagius 1:35fcaa2209af 355 for (i=0; i<10; i++) parsed[i] = -1;
ftagius 1:35fcaa2209af 356
ftagius 1:35fcaa2209af 357 response = strchr(response, ',');
ftagius 1:35fcaa2209af 358 for (i=0; i<10; i++) {
ftagius 1:35fcaa2209af 359 if (!response || (response[0] == 0) || (response[0] == '*'))
ftagius 1:35fcaa2209af 360 break;
ftagius 1:35fcaa2209af 361 response++;
ftagius 1:35fcaa2209af 362 parsed[i]=0;
ftagius 1:35fcaa2209af 363 while ((response[0] != ',') &&
ftagius 1:35fcaa2209af 364 (response[0] != '*') && (response[0] != 0)) {
ftagius 1:35fcaa2209af 365 parsed[i] *= 10;
ftagius 1:35fcaa2209af 366 char c = response[0];
ftagius 1:35fcaa2209af 367 if (isdigit(c))
ftagius 1:35fcaa2209af 368 parsed[i] += c - '0';
ftagius 1:35fcaa2209af 369 else
ftagius 1:35fcaa2209af 370 parsed[i] = c;
ftagius 1:35fcaa2209af 371 response++;
ftagius 1:35fcaa2209af 372 }
ftagius 1:35fcaa2209af 373 }
ftagius 1:35fcaa2209af 374 LOCUS_serial = parsed[0];
ftagius 1:35fcaa2209af 375 LOCUS_type = parsed[1];
ftagius 1:35fcaa2209af 376 if (isalpha(parsed[2])) {
ftagius 1:35fcaa2209af 377 parsed[2] = parsed[2] - 'a' + 10;
ftagius 1:35fcaa2209af 378 }
ftagius 1:35fcaa2209af 379 LOCUS_mode = parsed[2];
ftagius 1:35fcaa2209af 380 LOCUS_config = parsed[3];
ftagius 1:35fcaa2209af 381 LOCUS_interval = parsed[4];
ftagius 1:35fcaa2209af 382 LOCUS_distance = parsed[5];
ftagius 1:35fcaa2209af 383 LOCUS_speed = parsed[6];
ftagius 1:35fcaa2209af 384 LOCUS_status = !parsed[7];
ftagius 1:35fcaa2209af 385 LOCUS_records = parsed[8];
ftagius 1:35fcaa2209af 386 LOCUS_percent = parsed[9];
ftagius 1:35fcaa2209af 387
ftagius 1:35fcaa2209af 388 return true;
ftagius 1:35fcaa2209af 389 }
ftagius 1:35fcaa2209af 390
ftagius 1:35fcaa2209af 391 // Standby Mode Switches
ftagius 1:35fcaa2209af 392 bool GPS::standby(void) {
ftagius 1:35fcaa2209af 393 if (inStandbyMode) {
ftagius 1:35fcaa2209af 394 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 395 }
ftagius 1:35fcaa2209af 396 else {
ftagius 1:35fcaa2209af 397 inStandbyMode = true;
ftagius 1:35fcaa2209af 398 sendCommand(PMTK_STANDBY);
ftagius 1:35fcaa2209af 399 //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 400 return true;
ftagius 1:35fcaa2209af 401 }
ftagius 1:35fcaa2209af 402 }
ftagius 1:35fcaa2209af 403
ftagius 1:35fcaa2209af 404 bool GPS::wakeup(void) {
ftagius 1:35fcaa2209af 405 if (inStandbyMode) {
ftagius 1:35fcaa2209af 406 inStandbyMode = false;
ftagius 1:35fcaa2209af 407 sendCommand(""); // send byte to wake it up
ftagius 1:35fcaa2209af 408 return waitForSentence(PMTK_AWAKE);
ftagius 1:35fcaa2209af 409 }
ftagius 1:35fcaa2209af 410 else {
ftagius 1:35fcaa2209af 411 return false; // Returns false if not in standby mode, nothing to wakeup
ftagius 1:35fcaa2209af 412 }
ftagius 1:35fcaa2209af 413 }