GPS.cpp

00001 /* mbed EM-406 GPS Module Library
00002  * Copyright (c) 2008-2010, sford
00003  *
00004  * Permission is hereby granted, free of charge, to any person obtaining a copy
00005  * of this software and associated documentation files (the "Software"), to deal
00006  * in the Software without restriction, including without limitation the rights
00007  * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
00008  * copies of the Software, and to permit persons to whom the Software is
00009  * furnished to do so, subject to the following conditions:
00010  *
00011  * The above copyright notice and this permission notice shall be included in
00012  * all copies or substantial portions of the Software.
00013  *
00014  * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
00015  * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
00016  * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
00017  * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
00018  * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
00019  * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
00020  * THE SOFTWARE.
00021  */
00022  
00023 #include "GPS.h"
00024 #include "main.h"
00025 // how long are max NMEA lines to parse?
00026 #define MAXLINELENGTH 120
00027 // we double buffer: read one line in and leave one for the main program
00028 volatile char line1[MAXLINELENGTH];
00029 volatile char line2[MAXLINELENGTH];
00030 // our index into filling the current line
00031 volatile uint16_t lineidx=0;
00032 // pointers to the double buffers
00033 volatile char *currentline;
00034 volatile char *lastline;
00035 volatile bool recvdflag;
00036 volatile bool inStandbyMode;
00037 
00038 GPS::GPS(PinName tx, PinName rx) : _gps(tx, rx) {
00039     _gps.baud(9600);    
00040     recvdflag   = false;
00041     paused      = false;
00042     lineidx     = 0;
00043     currentline = line1;
00044     lastline    = line2;
00045 }
00046 
00047 bool GPS::newNMEAreceived(void) {
00048   return recvdflag;
00049 }
00050 
00051 void GPS::pause(bool p) {
00052   paused = p;
00053 }
00054 
00055 char *GPS::lastNMEA(void) {
00056   recvdflag = false;
00057   return (char *)lastline;
00058 }
00059   
00060 float GPS::trunc(float v) {
00061     if(v < 0.0) {
00062         v*= -1.0;
00063         v = floor(v);
00064         v*=-1.0;
00065     } else {
00066         v = floor(v);
00067     }
00068     return v;
00069 }
00070 
00071 
00072 bool GPS::parse(char *nmea) {
00073   // do checksum check
00074 
00075   // first look if we even have one
00076   if (nmea[strlen(nmea)-4] == '*') {
00077     uint16_t sum = parseHex(nmea[strlen(nmea)-3]) * 16;
00078     sum += parseHex(nmea[strlen(nmea)-2]);
00079     
00080     // check checksum 
00081     for (uint8_t i=1; i < (strlen(nmea)-4); i++) {
00082       sum ^= nmea[i];
00083     }
00084     if (sum != 0) {
00085       // bad checksum :(
00086       //return false;
00087     }
00088   }
00089 
00090   // look for a few common sentences
00091   if (strstr(nmea, "$GPGGA")) {
00092     // found GGA
00093     char *p = nmea;
00094     // get time
00095     p = strchr(p, ',')+1;
00096     timef = atof(p);
00097     uint32_t time = timef;
00098     hour = time / 10000;
00099     minute = (time % 10000) / 100;
00100     seconds = (time % 100);
00101 
00102     milliseconds = fmod((double) timef, 1.0) * 1000;
00103 
00104     // parse out latitude
00105     p = strchr(p, ',')+1;
00106     latitude = atof(p);
00107 
00108     p = strchr(p, ',')+1;
00109     if (p[0] == 'N') lat = 'N';
00110     else if (p[0] == 'S') lat = 'S';
00111     else if (p[0] == ',') lat = 0;
00112     else return false;
00113 
00114     // parse out longitude
00115     p = strchr(p, ',')+1;
00116     longitude = atof(p);
00117 
00118     p = strchr(p, ',')+1;
00119     if (p[0] == 'W') lon = 'W';
00120     else if (p[0] == 'E') lon = 'E';
00121     else if (p[0] == ',') lon = 0;
00122     else return false;
00123    // convert to degrees
00124     lat_deg = latitude;
00125     lon_deg = longitude;
00126     
00127     if(lat == 'S') {    lat_deg  *= -1.0; }
00128     if(lon == 'W') {    lon_deg *= -1.0; }
00129     float degrees = trunc(lat_deg / 100.0f);
00130     float minutes = lat_deg - (degrees * 100.0f);
00131     lat_deg = degrees + minutes / 60.0f;    
00132     degrees = trunc(lon_deg / 100.0f);
00133     //degrees = trunc(longitude / 100.0f * 0.01f);
00134     //printf("my lon=%f deg=%f\r\n",longitude, degrees);
00135     minutes = lon_deg - (degrees * 100.0f);
00136     lon_deg = degrees + minutes / 60.0f;
00137     //printf("local: lat=%f lon=%f\r\n",l_latitude, l_longitude);
00138     //printf("gpgga rcvd\r\n");
00139 
00140     p = strchr(p, ',')+1;
00141     fixquality = atoi(p);
00142 
00143     p = strchr(p, ',')+1;
00144     satellites = atoi(p);
00145 
00146     p = strchr(p, ',')+1;
00147     HDOP = atof(p);
00148 
00149     p = strchr(p, ',')+1;
00150     altitude = atof(p);
00151     p = strchr(p, ',')+1;
00152     p = strchr(p, ',')+1;
00153     geoidheight = atof(p);
00154     fix = true; 
00155     return true;
00156   }
00157   if (strstr(nmea, "$GPRMC")) {
00158    // found RMC
00159     char *p = nmea;
00160 
00161     // get time
00162     p = strchr(p, ',')+1;
00163     timef = atof(p);
00164     uint32_t time = timef;
00165     hour = time / 10000;
00166     minute = (time % 10000) / 100;
00167     seconds = (time % 100);
00168 
00169     milliseconds = fmod((double) timef, 1.0) * 1000;
00170 
00171     p = strchr(p, ',')+1;
00172     // Serial.println(p);
00173     if (p[0] == 'A') 
00174       fix = true;
00175     else if (p[0] == 'V')
00176       fix = false;
00177     else
00178       return false;
00179 
00180     // parse out latitude
00181     p = strchr(p, ',')+1;
00182     latitude = atof(p);
00183 
00184     p = strchr(p, ',')+1;
00185     if (p[0] == 'N') lat = 'N';
00186     else if (p[0] == 'S') lat = 'S';
00187     else if (p[0] == ',') lat = 0;
00188     else return false;
00189 
00190     // parse out longitude
00191     p = strchr(p, ',')+1;
00192     longitude = atof(p);
00193 
00194     p = strchr(p, ',')+1;
00195     if (p[0] == 'W') lon = 'W';
00196     else if (p[0] == 'E') lon = 'E';
00197     else if (p[0] == ',') lon = 0;
00198     else return false;
00199    // convert to degrees
00200     lat_deg = latitude;
00201     lon_deg = longitude;
00202     
00203     if(lat == 'S') {    lat_deg  *= -1.0; }
00204     if(lon == 'W') {    lon_deg *= -1.0; }
00205     float degrees = trunc(lat_deg / 100.0f);
00206     float minutes = lat_deg - (degrees * 100.0f);
00207     lat_deg = degrees + minutes / 60.0f;    
00208     degrees = trunc(lon_deg / 100.0f);
00209     minutes = lon_deg - (degrees * 100.0f);
00210     lon_deg = degrees + minutes / 60.0f;
00211      
00212     // speed
00213     p = strchr(p, ',')+1;
00214     speed = atof(p);
00215 
00216     // angle
00217     p = strchr(p, ',')+1;
00218     angle = atof(p);
00219 
00220     p = strchr(p, ',')+1;
00221     uint32_t fulldate = atof(p);
00222     day = fulldate / 10000;
00223     month = (fulldate % 10000) / 100;
00224     year = (fulldate % 100);
00225 
00226     // we dont parse the remaining, yet!
00227     return true;
00228   }
00229 
00230   return false;
00231 }
00232 
00233 
00234 char GPS::read(void) {
00235   char c = 0;
00236   
00237   if (paused) return c;
00238 
00239     if(!_gps.readable()) return c;
00240     c = _gps.getc();
00241 
00242   //Serial.print(c);
00243 
00244   if (c == '$') {
00245     currentline[lineidx] = 0;
00246     lineidx = 0;
00247   }
00248   if (c == '\n') {
00249     currentline[lineidx] = 0;
00250 
00251     if (currentline == line1) {
00252       currentline = line2;
00253       lastline = line1;
00254     } else {
00255       currentline = line1;
00256       lastline = line2;
00257     }
00258 
00259     lineidx = 0;
00260     recvdflag = true;
00261   }
00262 
00263   currentline[lineidx++] = c;
00264   if (lineidx >= MAXLINELENGTH)
00265     lineidx = MAXLINELENGTH-1;
00266 
00267   return c;
00268 }
00269 
00270 void GPS::setBaud(int baud)
00271 {
00272    
00273      switch (baud) {
00274         case 9600:
00275             _gps.baud(9600);
00276             sendCommand(PMTK_SET_BAUD_9600);
00277             wait_ms(100);
00278             printf("baud (%d), set to 9600\r\n",baud);
00279             break;  
00280         case 57600:
00281             _gps.baud(9600);  // assume device is at default 9600 baud
00282             sendCommand(PMTK_SET_BAUD_57600);
00283             _gps.baud(57600);
00284             wait_ms(100);
00285             printf("baud (%d), set to 57600\r\n",baud);
00286             break;        
00287         default:
00288             printf("unsupported baud (%d), set to 9600\r\n",baud);
00289             _gps.baud(9600);
00290             sendCommand(PMTK_SET_BAUD_9600);
00291             break;    
00292     }
00293   
00294   wait_ms(10);
00295 }
00296 
00297 void GPS::sendCommand(char *str) {
00298   _gps.printf("%s\r\n",str);
00299 }
00300 
00301 void GPS::setSetup(void) {
00302   
00303  
00304 
00305   
00306 }
00307 
00308 // read a Hex value and return the decimal equivalent
00309 uint8_t GPS::parseHex(char c) {
00310     if (c < '0')
00311       return 0;
00312     if (c <= '9')
00313       return c - '0';
00314     if (c < 'A')
00315        return 0;
00316     if (c <= 'F')
00317        return (c - 'A')+10;
00318     return 0;
00319 }
00320 
00321 bool GPS::waitForSentence(char *wait4me, uint8_t max) {
00322   char str[20];
00323 
00324   uint8_t i=0;
00325   while (i < max) {
00326     if (newNMEAreceived()) { 
00327       char *nmea = lastNMEA();
00328       strncpy(str, nmea, 20);
00329       str[19] = 0;
00330       i++;
00331 
00332       if (strstr(str, wait4me))
00333     return true;
00334     }
00335   }
00336 
00337   return false;
00338 }
00339 
00340 bool GPS::LOCUS_StartLogger(void) {
00341   sendCommand(PMTK_LOCUS_STARTLOG);
00342   recvdflag = false;
00343   return waitForSentence(PMTK_LOCUS_LOGSTARTED);
00344 }
00345 
00346 bool GPS::LOCUS_ReadStatus(void) {
00347   sendCommand(PMTK_LOCUS_QUERY_STATUS);
00348   
00349   if (! waitForSentence("$PMTKLOG"))
00350     return false;
00351 
00352   char *response = lastNMEA();
00353   uint16_t parsed[10];
00354   int8_t i;
00355   
00356   for (i=0; i<10; i++) parsed[i] = -1;
00357   
00358   response = strchr(response, ',');
00359   for (i=0; i<10; i++) {
00360     if (!response || (response[0] == 0) || (response[0] == '*')) 
00361       break;
00362     response++;
00363     parsed[i]=0;
00364     while ((response[0] != ',') && 
00365        (response[0] != '*') && (response[0] != 0)) {
00366       parsed[i] *= 10;
00367       char c = response[0];
00368       if (isdigit(c))
00369         parsed[i] += c - '0';
00370       else
00371         parsed[i] = c;
00372       response++;
00373     }
00374   }
00375   LOCUS_serial = parsed[0];
00376   LOCUS_type = parsed[1];
00377   if (isalpha(parsed[2])) {
00378     parsed[2] = parsed[2] - 'a' + 10; 
00379   }
00380   LOCUS_mode = parsed[2];
00381   LOCUS_config = parsed[3];
00382   LOCUS_interval = parsed[4];
00383   LOCUS_distance = parsed[5];
00384   LOCUS_speed = parsed[6];
00385   LOCUS_status = !parsed[7];
00386   LOCUS_records = parsed[8];
00387   LOCUS_percent = parsed[9];
00388 
00389   return true;
00390 }
00391 
00392 // Standby Mode Switches
00393 bool GPS::standby(void) {
00394   if (inStandbyMode) {
00395     return false;  // Returns false if already in standby mode, so that you do not wake it up by sending commands to GPS
00396   }
00397   else {
00398     inStandbyMode = true;
00399     sendCommand(PMTK_STANDBY);
00400     //return waitForSentence(PMTK_STANDBY_SUCCESS);  // don't seem to be fast enough to catch the message, or something else just is not working
00401     return true;
00402   }
00403 }
00404 
00405 bool GPS::wakeup(void) {
00406   if (inStandbyMode) {
00407    inStandbyMode = false;
00408     sendCommand("");  // send byte to wake it up
00409     return waitForSentence(PMTK_AWAKE);
00410   }
00411   else {
00412       return false;  // Returns false if not in standby mode, nothing to wakeup
00413   }
00414 }