Ok

Dependencies:   mbed_rtos_types Mutex mbed_rtos_storage mbed Semaphore

Embed: (wiki syntax)

« Back to documentation index

Show/hide line numbers Mbed_Adafruit_GPS.cpp Source File

Mbed_Adafruit_GPS.cpp

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