MBed_Adafruit_GPS.cpp

00001 /***********************************
00002 This is our GPS library
00003 
00004 Adafruit invests time and resources providing this open source code,
00005 please support Adafruit and open-source hardware by purchasing
00006 products from Adafruit!
00007 
00008 Written by Limor Fried/Ladyada for Adafruit Industries.
00009 BSD license, check license.txt for more information
00010 All text above must be included in any redistribution
00011 ****************************************/
00012 
00013 #include "MBed_Adafruit_GPS.h"
00014 
00015 // how long are max NMEA lines to parse?
00016 #define MAXLINELENGTH 120
00017 
00018 // we double buffer: read one line in and leave one for the main program
00019 volatile char line1[MAXLINELENGTH];
00020 volatile char line2[MAXLINELENGTH];
00021 // our index into filling the current line
00022 volatile uint16_t lineidx=0;
00023 // pointers to the double buffers
00024 volatile char *currentline;
00025 volatile char *lastline;
00026 volatile bool recvdflag;
00027 volatile bool inStandbyMode;
00028 
00029 
00030 bool Adafruit_GPS::parse(char *nmea) {
00031   // do checksum check
00032 
00033   // first look if we even have one
00034   if (nmea[strlen(nmea)-4] == '*') {
00035     uint16_t sum = parseHex(nmea[strlen(nmea)-3]) * 16;
00036     sum += parseHex(nmea[strlen(nmea)-2]);
00037     
00038     // check checksum 
00039     for (uint8_t i=1; i < (strlen(nmea)-4); i++) {
00040       sum ^= nmea[i];
00041     }
00042     if (sum != 0) {
00043       // bad checksum :(
00044       //return false;
00045     }
00046   }
00047 
00048   // look for a few common sentences
00049   if (strstr(nmea, "$GPGGA")) {
00050     // found GGA
00051     char *p = nmea;
00052     // get time
00053     p = strchr(p, ',')+1;
00054     float timef = atof(p);
00055     uint32_t time = timef;
00056     hour = time / 10000;
00057     minute = (time % 10000) / 100;
00058     seconds = (time % 100);
00059 
00060     milliseconds = fmod((double) timef, 1.0) * 1000;
00061 
00062     // parse out latitude
00063     /*
00064     p = strchr(p, ',')+1;
00065     latitudeH = atol(p);
00066     p = strchr(p, '.')+1;
00067     latitudeL = atol(p);
00068     */
00069     
00070     p = strchr(p, ',')+1;
00071     long sub_latitudeH = atol(p);
00072     p = strchr(p, '.')+1;
00073     long sub_latitudeL = atol(p);
00074     
00075     latitudeH = sub_latitudeH / 100;
00076     latitudeL = ((sub_latitudeH % 100) *10000) + sub_latitudeL; 
00077     
00078 
00079     p = strchr(p, ',')+1;
00080     if (p[0] == 'N') lat = 'N';
00081     else if (p[0] == 'S') lat = 'S';
00082     else if (p[0] == ',') lat = 0;
00083     else return false;
00084 
00085     // parse out longitude
00086     /*p = strchr(p, ',')+1;
00087     longitudeH = atol(p);
00088     p = strchr(p, '.')+1;
00089     longitudeL = atol(p);
00090     */
00091 
00092     p = strchr(p, ',')+1;
00093     long sub_longitudeH = atol(p);
00094     p = strchr(p, '.')+1;
00095     long sub_longitudeL = atol(p);
00096     
00097     longitudeH = sub_longitudeH / 100;
00098     longitudeL = ((sub_longitudeH % 100) *10000) + sub_longitudeL; 
00099 
00100 
00101     p = strchr(p, ',')+1;
00102     if (p[0] == 'W') lon = 'W';
00103     else if (p[0] == 'E') lon = 'E';
00104     else if (p[0] == ',') lon = 0;
00105     else return false;
00106 
00107     p = strchr(p, ',')+1;
00108     fixquality = atoi(p);
00109 
00110     p = strchr(p, ',')+1;
00111     satellites = atoi(p);
00112 
00113     p = strchr(p, ',')+1;
00114     HDOP = atof(p);
00115 
00116     p = strchr(p, ',')+1;
00117     altitude = atof(p);
00118     p = strchr(p, ',')+1;
00119     p = strchr(p, ',')+1;
00120     geoidheight = atof(p);
00121     return true;
00122   }
00123   if (strstr(nmea, "$GPRMC")) {
00124    // found RMC
00125     char *p = nmea;
00126 
00127     // get time
00128     p = strchr(p, ',')+1;
00129     float timef = atof(p);
00130     uint32_t time = timef;
00131     hour = time / 10000;
00132     minute = (time % 10000) / 100;
00133     seconds = (time % 100);
00134 
00135     milliseconds = fmod((double) timef, 1.0) * 1000;
00136 
00137     p = strchr(p, ',')+1;
00138     // Serial.println(p);
00139     if (p[0] == 'A') 
00140       fix = true;
00141     else if (p[0] == 'V')
00142       fix = false;
00143     else
00144       return false;
00145 
00146     // parse out latitude
00147     /*
00148     p = strchr(p, ',')+1;
00149     latitudeH = atol(p);
00150     p = strchr(p, '.')+1;
00151     latitudeL = atol(p);
00152     */
00153 
00154     p = strchr(p, ',')+1;
00155     long sub_latitudeH = atol(p);
00156     p = strchr(p, '.')+1;
00157     long sub_latitudeL = atol(p);
00158     
00159     latitudeH = sub_latitudeH / 100;
00160     latitudeL = ((sub_latitudeH % 100) *10000) + sub_latitudeL; 
00161 
00162 
00163     p = strchr(p, ',')+1;
00164     if (p[0] == 'N') lat = 'N';
00165     else if (p[0] == 'S') lat = 'S';
00166     else if (p[0] == ',') lat = 0;
00167     else return false;
00168 
00169     // parse out longitude
00170     /*
00171     p = strchr(p, ',')+1;
00172     longitudeH = atol(p);
00173     p = strchr(p, '.')+1;
00174     longitudeL = atol(p);
00175     */
00176     
00177     p = strchr(p, ',')+1;
00178     long sub_longitudeH = atol(p);
00179     p = strchr(p, '.')+1;
00180     long sub_longitudeL = atol(p);
00181     
00182     longitudeH = sub_longitudeH / 100;
00183     longitudeL = ((sub_longitudeH % 100) *10000) + sub_longitudeL; 
00184     
00185     
00186     
00187     p = strchr(p, ',')+1;
00188     if (p[0] == 'W') lon = 'W';
00189     else if (p[0] == 'E') lon = 'E';
00190     else if (p[0] == ',') lon = 0;
00191     else return false;
00192 
00193     // speed
00194     p = strchr(p, ',')+1;
00195     speed = atof(p);
00196 
00197     // angle
00198     p = strchr(p, ',')+1;
00199     angle = atof(p);
00200 
00201     p = strchr(p, ',')+1;
00202     uint32_t fulldate = atof(p);
00203     day = fulldate / 10000;
00204     month = (fulldate % 10000) / 100;
00205     year = (fulldate % 100);
00206 
00207     // we dont parse the remaining, yet!
00208     return true;
00209   }
00210 
00211   return false;
00212 }
00213 
00214 char Adafruit_GPS::read(void) {
00215   char c = 0;
00216   
00217   if (paused) return c;
00218 
00219     if(!gpsSerial->readable()) return c;
00220     c = gpsSerial->getc();
00221 
00222   //Serial.print(c);
00223 
00224   if (c == '$') {
00225     currentline[lineidx] = 0;
00226     lineidx = 0;
00227   }
00228   if (c == '\n') {
00229     currentline[lineidx] = 0;
00230 
00231     if (currentline == line1) {
00232       currentline = line2;
00233       lastline = line1;
00234     } else {
00235       currentline = line1;
00236       lastline = line2;
00237     }
00238 
00239     lineidx = 0;
00240     recvdflag = true;
00241   }
00242 
00243   currentline[lineidx++] = c;
00244   if (lineidx >= MAXLINELENGTH)
00245     lineidx = MAXLINELENGTH-1;
00246 
00247   return c;
00248 }
00249 
00250 Adafruit_GPS::Adafruit_GPS (Serial *ser)
00251 {
00252   common_init();     // Set everything to common state, then...
00253   gpsSerial = ser; // ...override gpsSwSerial with value passed.
00254   latitudeH=0;latitudeL=0;longitudeH=0;longitudeL=0; 
00255   latitudeKH=0;latitudeKL=0;longitudeKH=0;longitudeKL=0; 
00256 }
00257 
00258 // Initialization code used by all constructor types
00259 void Adafruit_GPS::common_init(void) {
00260   gpsSerial = NULL;
00261   recvdflag   = false;
00262   paused      = false;
00263   lineidx     = 0;
00264   currentline = line1;
00265   lastline    = line2;
00266 
00267   hour = minute = seconds = year = month = day =
00268     fixquality = satellites = 0; // uint8_t
00269   lat = lon = mag = 0; // char
00270   fix = false; // bool
00271   milliseconds = 0; // uint16_t
00272   geoidheight = altitude = speed = angle = magvariation = HDOP = 0.0; // float
00273   latitudeH = latitudeL = longitudeH = longitudeL = 0;
00274     
00275 }
00276 
00277 void Adafruit_GPS::begin(int baud)
00278 {
00279   gpsSerial->baud(baud);
00280   wait_ms(10);
00281 }
00282 
00283 void Adafruit_GPS::sendCommand(char *str) {
00284   gpsSerial->printf("%s",str);
00285 }
00286 
00287 bool Adafruit_GPS::newNMEAreceived(void) {
00288   return recvdflag;
00289 }
00290 
00291 void Adafruit_GPS::pause(bool p) {
00292   paused = p;
00293 }
00294 
00295 char *Adafruit_GPS::lastNMEA(void) {
00296   recvdflag = false;
00297   return (char *)lastline;
00298 }
00299 
00300 // read a Hex value and return the decimal equivalent
00301 uint8_t Adafruit_GPS::parseHex(char c) {
00302     if (c < '0')
00303       return 0;
00304     if (c <= '9')
00305       return c - '0';
00306     if (c < 'A')
00307        return 0;
00308     if (c <= 'F')
00309        return (c - 'A')+10;
00310 }
00311 
00312 bool Adafruit_GPS::waitForSentence(char *wait4me, uint8_t max) {
00313   char str[20];
00314 
00315   uint8_t i=0;
00316   while (i < max) {
00317     if (newNMEAreceived()) { 
00318       char *nmea = lastNMEA();
00319       strncpy(str, nmea, 20);
00320       str[19] = 0;
00321       i++;
00322 
00323       if (strstr(str, wait4me))
00324     return true;
00325     }
00326   }
00327 
00328   return false;
00329 }
00330 
00331 bool Adafruit_GPS::LOCUS_StartLogger(void) {
00332   sendCommand(PMTK_LOCUS_STARTLOG);
00333   recvdflag = false;
00334   return waitForSentence(PMTK_LOCUS_LOGSTARTED);
00335 }
00336 
00337 bool Adafruit_GPS::LOCUS_ReadStatus(void) {
00338   sendCommand(PMTK_LOCUS_QUERY_STATUS);
00339   
00340   if (! waitForSentence("$PMTKLOG"))
00341     return false;
00342 
00343   char *response = lastNMEA();
00344   uint16_t parsed[10];
00345   uint8_t i;
00346   
00347   for (i=0; i<10; i++) parsed[i] = -1;
00348   
00349   response = strchr(response, ',');
00350   for (i=0; i<10; i++) {
00351     if (!response || (response[0] == 0) || (response[0] == '*')) 
00352       break;
00353     response++;
00354     parsed[i]=0;
00355     while ((response[0] != ',') && 
00356        (response[0] != '*') && (response[0] != 0)) {
00357       parsed[i] *= 10;
00358       char c = response[0];
00359       if (isdigit(c))
00360         parsed[i] += c - '0';
00361       else
00362         parsed[i] = c;
00363       response++;
00364     }
00365   }
00366   LOCUS_serial = parsed[0];
00367   LOCUS_type = parsed[1];
00368   if (isalpha(parsed[2])) {
00369     parsed[2] = parsed[2] - 'a' + 10; 
00370   }
00371   LOCUS_mode = parsed[2];
00372   LOCUS_config = parsed[3];
00373   LOCUS_interval = parsed[4];
00374   LOCUS_distance = parsed[5];
00375   LOCUS_speed = parsed[6];
00376   LOCUS_status = !parsed[7];
00377   LOCUS_records = parsed[8];
00378   LOCUS_percent = parsed[9];
00379 
00380   return true;
00381 }
00382 
00383 // Standby Mode Switches
00384 bool Adafruit_GPS::standby(void) {
00385   if (inStandbyMode) {
00386     return false;  // Returns false if already in standby mode, so that you do not wake it up by sending commands to GPS
00387   }
00388   else {
00389     inStandbyMode = true;
00390     sendCommand(PMTK_STANDBY);
00391     //return waitForSentence(PMTK_STANDBY_SUCCESS);  // don't seem to be fast enough to catch the message, or something else just is not working
00392     return true;
00393   }
00394 }
00395 
00396 bool Adafruit_GPS::wakeup(void) {
00397   if (inStandbyMode) {
00398    inStandbyMode = false;
00399     sendCommand("");  // send byte to wake it up
00400     return waitForSentence(PMTK_AWAKE);
00401   }
00402   else {
00403       return false;  // Returns false if not in standby mode, nothing to wakeup
00404   }
00405 }