GPS code for Adafruit ultimate GPS
Dependents: demo_gps_sdcard zeus
Fork of GPS by
GPS.cpp
- Committer:
- ftagius
- Date:
- 2015-06-16
- Revision:
- 1:35fcaa2209af
- Parent:
- 0:15611c7938a3
- Child:
- 2:509abe8eda59
File content as of revision 1:35fcaa2209af:
/* mbed EM-406 GPS Module Library * Copyright (c) 2008-2010, sford * * Permission is hereby granted, free of charge, to any person obtaining a copy * of this software and associated documentation files (the "Software"), to deal * in the Software without restriction, including without limitation the rights * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell * copies of the Software, and to permit persons to whom the Software is * furnished to do so, subject to the following conditions: * * The above copyright notice and this permission notice shall be included in * all copies or substantial portions of the Software. * * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN * THE SOFTWARE. */ #include "GPS.h" #include "main.h" // how long are max NMEA lines to parse? #define MAXLINELENGTH 120 // we double buffer: read one line in and leave one for the main program volatile char line1[MAXLINELENGTH]; volatile char line2[MAXLINELENGTH]; // our index into filling the current line volatile uint16_t lineidx=0; // pointers to the double buffers volatile char *currentline; volatile char *lastline; volatile bool recvdflag; volatile bool inStandbyMode; GPS::GPS(PinName tx, PinName rx) : _gps(tx, rx) { _gps.baud(9600); recvdflag = false; paused = false; lineidx = 0; currentline = line1; lastline = line2; } bool GPS::newNMEAreceived(void) { return recvdflag; } void GPS::pause(bool p) { paused = p; } char *GPS::lastNMEA(void) { recvdflag = false; return (char *)lastline; } float GPS::trunc(float v) { if(v < 0.0) { v*= -1.0; v = floor(v); v*=-1.0; } else { v = floor(v); } return v; } bool GPS::parse(char *nmea) { // do checksum check // first look if we even have one if (nmea[strlen(nmea)-4] == '*') { uint16_t sum = parseHex(nmea[strlen(nmea)-3]) * 16; sum += parseHex(nmea[strlen(nmea)-2]); // check checksum for (uint8_t i=1; i < (strlen(nmea)-4); i++) { sum ^= nmea[i]; } if (sum != 0) { // bad checksum :( //return false; } } // look for a few common sentences if (strstr(nmea, "$GPGGA")) { // found GGA char *p = nmea; // get time p = strchr(p, ',')+1; timef = atof(p); uint32_t time = timef; hour = time / 10000; minute = (time % 10000) / 100; seconds = (time % 100); milliseconds = fmod((double) timef, 1.0) * 1000; // parse out latitude p = strchr(p, ',')+1; latitude = atof(p); p = strchr(p, ',')+1; if (p[0] == 'N') lat = 'N'; else if (p[0] == 'S') lat = 'S'; else if (p[0] == ',') lat = 0; else return false; // parse out longitude p = strchr(p, ',')+1; longitude = atof(p); p = strchr(p, ',')+1; if (p[0] == 'W') lon = 'W'; else if (p[0] == 'E') lon = 'E'; else if (p[0] == ',') lon = 0; else return false; // convert to degrees lat_deg = latitude; lon_deg = longitude; if(lat == 'S') { lat_deg *= -1.0; } if(lon == 'W') { lon_deg *= -1.0; } float degrees = trunc(lat_deg / 100.0f); float minutes = lat_deg - (degrees * 100.0f); lat_deg = degrees + minutes / 60.0f; degrees = trunc(lon_deg / 100.0f); //degrees = trunc(longitude / 100.0f * 0.01f); //printf("my lon=%f deg=%f\r\n",longitude, degrees); minutes = lon_deg - (degrees * 100.0f); lon_deg = degrees + minutes / 60.0f; //printf("local: lat=%f lon=%f\r\n",l_latitude, l_longitude); //printf("gpgga rcvd\r\n"); p = strchr(p, ',')+1; fixquality = atoi(p); p = strchr(p, ',')+1; satellites = atoi(p); p = strchr(p, ',')+1; HDOP = atof(p); p = strchr(p, ',')+1; altitude = atof(p); p = strchr(p, ',')+1; p = strchr(p, ',')+1; geoidheight = atof(p); return true; } if (strstr(nmea, "$GPRMC")) { // found RMC char *p = nmea; // get time p = strchr(p, ',')+1; timef = atof(p); uint32_t time = timef; hour = time / 10000; minute = (time % 10000) / 100; seconds = (time % 100); milliseconds = fmod((double) timef, 1.0) * 1000; p = strchr(p, ',')+1; // Serial.println(p); if (p[0] == 'A') fix = true; else if (p[0] == 'V') fix = false; else return false; // parse out latitude p = strchr(p, ',')+1; latitude = atof(p); p = strchr(p, ',')+1; if (p[0] == 'N') lat = 'N'; else if (p[0] == 'S') lat = 'S'; else if (p[0] == ',') lat = 0; else return false; // parse out longitude p = strchr(p, ',')+1; longitude = atof(p); p = strchr(p, ',')+1; if (p[0] == 'W') lon = 'W'; else if (p[0] == 'E') lon = 'E'; else if (p[0] == ',') lon = 0; else return false; // convert to degrees lat_deg = latitude; lon_deg = longitude; if(lat == 'S') { lat_deg *= -1.0; } if(lon == 'W') { lon_deg *= -1.0; } float degrees = trunc(lat_deg / 100.0f); float minutes = lat_deg - (degrees * 100.0f); lat_deg = degrees + minutes / 60.0f; degrees = trunc(lon_deg / 100.0f); minutes = lon_deg - (degrees * 100.0f); lon_deg = degrees + minutes / 60.0f; // speed p = strchr(p, ',')+1; speed = atof(p); // angle p = strchr(p, ',')+1; angle = atof(p); p = strchr(p, ',')+1; uint32_t fulldate = atof(p); day = fulldate / 10000; month = (fulldate % 10000) / 100; year = (fulldate % 100); // we dont parse the remaining, yet! return true; } return false; } char GPS::read(void) { char c = 0; if (paused) return c; if(!_gps.readable()) return c; c = _gps.getc(); //Serial.print(c); if (c == '$') { currentline[lineidx] = 0; lineidx = 0; } if (c == '\n') { currentline[lineidx] = 0; if (currentline == line1) { currentline = line2; lastline = line1; } else { currentline = line1; lastline = line2; } lineidx = 0; recvdflag = true; } currentline[lineidx++] = c; if (lineidx >= MAXLINELENGTH) lineidx = MAXLINELENGTH-1; return c; } void GPS::setBaud(int baud) { switch (baud) { case 9600: _gps.baud(9600); sendCommand(PMTK_SET_BAUD_9600); wait_ms(100); printf("baud (%d), set to 9600\r\n",baud); break; case 57600: _gps.baud(9600); // assume device is at default 9600 baud sendCommand(PMTK_SET_BAUD_57600); _gps.baud(57600); wait_ms(100); printf("baud (%d), set to 57600\r\n",baud); break; default: printf("unsupported baud (%d), set to 9600\r\n",baud); _gps.baud(9600); sendCommand(PMTK_SET_BAUD_9600); break; } wait_ms(10); } void GPS::sendCommand(char *str) { _gps.printf("%s\r\n",str); } void GPS::setSetup(void) { } // read a Hex value and return the decimal equivalent uint8_t GPS::parseHex(char c) { if (c < '0') return 0; if (c <= '9') return c - '0'; if (c < 'A') return 0; if (c <= 'F') return (c - 'A')+10; return 0; } bool GPS::waitForSentence(char *wait4me, uint8_t max) { char str[20]; uint8_t i=0; while (i < max) { if (newNMEAreceived()) { char *nmea = lastNMEA(); strncpy(str, nmea, 20); str[19] = 0; i++; if (strstr(str, wait4me)) return true; } } return false; } bool GPS::LOCUS_StartLogger(void) { sendCommand(PMTK_LOCUS_STARTLOG); recvdflag = false; return waitForSentence(PMTK_LOCUS_LOGSTARTED); } bool GPS::LOCUS_ReadStatus(void) { sendCommand(PMTK_LOCUS_QUERY_STATUS); if (! waitForSentence("$PMTKLOG")) return false; char *response = lastNMEA(); uint16_t parsed[10]; int8_t i; for (i=0; i<10; i++) parsed[i] = -1; response = strchr(response, ','); for (i=0; i<10; i++) { if (!response || (response[0] == 0) || (response[0] == '*')) break; response++; parsed[i]=0; while ((response[0] != ',') && (response[0] != '*') && (response[0] != 0)) { parsed[i] *= 10; char c = response[0]; if (isdigit(c)) parsed[i] += c - '0'; else parsed[i] = c; response++; } } LOCUS_serial = parsed[0]; LOCUS_type = parsed[1]; if (isalpha(parsed[2])) { parsed[2] = parsed[2] - 'a' + 10; } LOCUS_mode = parsed[2]; LOCUS_config = parsed[3]; LOCUS_interval = parsed[4]; LOCUS_distance = parsed[5]; LOCUS_speed = parsed[6]; LOCUS_status = !parsed[7]; LOCUS_records = parsed[8]; LOCUS_percent = parsed[9]; return true; } // Standby Mode Switches bool GPS::standby(void) { if (inStandbyMode) { return false; // Returns false if already in standby mode, so that you do not wake it up by sending commands to GPS } else { inStandbyMode = true; sendCommand(PMTK_STANDBY); //return waitForSentence(PMTK_STANDBY_SUCCESS); // don't seem to be fast enough to catch the message, or something else just is not working return true; } } bool GPS::wakeup(void) { if (inStandbyMode) { inStandbyMode = false; sendCommand(""); // send byte to wake it up return waitForSentence(PMTK_AWAKE); } else { return false; // Returns false if not in standby mode, nothing to wakeup } }