Merge pull request #107 from sellensr/master

Improve NMEA sentence checking

Adafruit_GPS.cpp

@@ -41,34 +41,16 @@ static boolean strStartsWith(const char* str, const char* prefix);
 /**************************************************************************/
 boolean Adafruit_GPS::parse(char *nmea) {
   // do checksum check
+  if(!check(nmea)) return false;
+  // passed the check, so there's a valid source in thisSource and a valid sentence in thisSentence
 
-  // first look if we even have one
-  char *ast = strchr(nmea,'*');
-  if (ast != NULL) {
-    uint16_t sum = parseHex(*(ast+1)) * 16;
-    sum += parseHex(*(ast+2));
-    // check checksum
-    char *p = strchr(nmea,'$');
-    if(p == NULL) return false;
-    else{
-      for (char *p1 = p+1; p1 < ast; p1++) {
-        sum ^= *p1;
-      }
-      if (sum != 0) {
-        // bad checksum :(
-        return false;
-      }
-    }
-  } else {
-    return false;
-  }
   // look for a few common sentences
-  char *p = nmea;
+  char *p = nmea;         // Pointer to move through the sentence -- good parsers are non-destructive
+  p = strchr(p, ',')+1;   // Skip to the character after the next comma, then check sentence.
 
-  if (strStartsWith(nmea, "$GPGGA") || strStartsWith(nmea, "$GNGGA")) {
+  if (!strcmp(thisSentence,"GGA")) {
     // found GGA
     // get time
-    p = strchr(p, ',')+1;
     parseTime(p);
 
     // parse out latitude
@@ -118,13 +100,11 @@ boolean Adafruit_GPS::parse(char *nmea) {
     {
       geoidheight = atof(p);
     }
-    return true;
+  } 
-  }
 
-  if (strStartsWith(nmea, "$GPRMC") || strStartsWith(nmea, "$GNRMC")) {
+  else if (!strcmp(thisSentence,"RMC")) {
     // found RMC
     // get time
-    p = strchr(p, ',')+1;
     parseTime(p);
 
     // fix or no fix
@@ -166,13 +146,11 @@ boolean Adafruit_GPS::parse(char *nmea) {
       year = (fulldate % 100);
       lastDate = sentTime;
     }
-    return true;
   }
 
-  if (strStartsWith(nmea, "$GPGLL") || strStartsWith(nmea, "$GNGLL")) {
+  else if (!strcmp(thisSentence,"GLL")) {
     // found GLL
     // parse out latitude
-    p = strchr(p, ',')+1;
     parseLat(p);
     p = strchr(p, ',')+1;
     if(!parseLatDir(p)) return false;
@@ -190,69 +168,138 @@ boolean Adafruit_GPS::parse(char *nmea) {
     // fix or no fix
     p = strchr(p, ',')+1;
     if(!parseFix(p)) return false;
-
-    return true;
   }
 
-if (strStartsWith(nmea, "$GPGSA")) {
+  else if (!strcmp(thisSentence,"GSA")) {
-  // found GSA
+    // found GSA
-  // parse out Auto selection, but ignore them
+    // parse out Auto selection, but ignore them
-  p = strchr(p, ',')+1;
+    // parse out 3d fixquality
-  // parse out 3d fixquality
+    p = strchr(p, ',')+1;
-  p = strchr(p, ',')+1;
+    if (',' != *p)
-  if (',' != *p)
+    {
-  {
+      fixquality_3d = atoi(p);
-    fixquality_3d = atoi(p);
+    }
+    // skip 12 Satellite PDNs without interpreting them
+    for(int i = 0;i < 12;i++) p = strchr(p, ',')+1;
+
+    //parse out PDOP
+    p = strchr(p, ',')+1;
+    if (',' != *p)
+    {
+      PDOP = atof(p);
+    }
+    // parse out HDOP, we also parse this from the GGA sentence. Chipset should report the same for both
+    p = strchr(p, ',')+1;
+    if (',' != *p)
+    {
+      HDOP = atof(p);
+    }
+    // parse out VDOP
+    p = strchr(p, ',')+1;
+    if (',' != *p)
+    {
+      VDOP = atof(p);
+    }
   }
-  // parse out Satellite PDNs, but ignore them
-  p = strchr(p, ',')+1;
-
-  p = strchr(p, ',')+1;
-
-  p = strchr(p, ',')+1;
-
-  p = strchr(p, ',')+1;
-
-  p = strchr(p, ',')+1;
-
-  p = strchr(p, ',')+1;
-
-  p = strchr(p, ',')+1;
-
-  p = strchr(p, ',')+1;
-
-  p = strchr(p, ',')+1;
-
-  p = strchr(p, ',')+1;
-
-  p = strchr(p, ',')+1;
-
-  p = strchr(p, ',')+1;
-
-  //parse out PDOP
-  p = strchr(p, ',')+1;
-  if (',' != *p)
-  {
-    PDOP = atof(p);
-  }
-  // parse out HDOP, we also parse this from the GGA sentence. Chipset should report the same for both
-  p = strchr(p, ',')+1;
-  if (',' != *p)
-  {
-    HDOP = atof(p);
-  }
-  // parse out VDOP
-  p = strchr(p, ',')+1;
-  if (',' != *p)
-  {
-    VDOP = atof(p);
-  }
-  return true;
-}
 
 
   // we dont parse the remaining, yet!
-  return false;
+  else return false;
+  
+  // Record the successful parsing of where the last data came from and when
+  strcpy(lastSource,thisSource);
+  strcpy(lastSentence,thisSentence);
+  lastUpdate = millis();
+  return true;  
+}
+
+/**************************************************************************/
+/*!
+    @brief Check an NMEA string for basic format, valid source ID and valid
+    and valid sentence ID. Update the values of thisCheck, thisSource and 
+    thisSentence. 
+    @param nmea Pointer to the NMEA string
+    @return True if well formed, false if it has problems
+*/
+/**************************************************************************/
+boolean Adafruit_GPS::check(char *nmea) {
+  thisCheck = 0;                            // new check
+  if(*nmea != '$') return false;            // doesn't start with $
+  else thisCheck += NMEA_HAS_DOLLAR;
+  // do checksum check -- first look if we even have one -- ignore all but last *
+  char *ast = nmea;                         // not strchr(nmea,'*'); for first *
+  while(*ast) ast++;                        // go to the end
+  while(*ast != '*' && ast > nmea) ast--;   // then back to * if it's there
+  if (*ast != '*') return false;            // there is no asterisk
+  else {
+    uint16_t sum = parseHex(*(ast+1)) * 16; // extract checksum
+    sum += parseHex(*(ast+2));
+    char *p = nmea;                         // check checksum
+    for (char *p1 = p+1; p1 < ast; p1++) sum ^= *p1;
+    if (sum != 0) return false;             // bad checksum :(
+    else thisCheck += NMEA_HAS_CHECKSUM;
+  }
+  // extract source of variable length
+  char *p = nmea +1;
+  const char *src = tokenOnList(p,sources);
+  if(src){
+    strcpy(thisSource,src);
+    thisCheck += NMEA_HAS_SOURCE;
+  } else return false;
+  p += strlen(src);
+  // extract sentence id and check if parsed
+  const char *snc = tokenOnList(p,sentences_parsed);
+  if(snc){
+    strcpy(thisSentence,snc);
+    thisCheck += NMEA_HAS_SENTENCE_P + NMEA_HAS_SENTENCE;
+  } else {      // check if known
+    snc = tokenOnList(p,sentences_known);
+    if(snc){
+      strcpy(thisSentence,snc);
+      thisCheck += NMEA_HAS_SENTENCE;
+      return false;
+    }    
+  }
+  return true;                              // passed all the tests
+}
+
+/**************************************************************************/
+/*!
+    @brief Check if a token at the start of a string is on a list.
+    @param token Pointer to the string
+    @param list A list of strings, with the final entry starting "ZZ"
+    @return Pointer to the found token, or NULL if it fails
+*/
+/**************************************************************************/
+const char * Adafruit_GPS::tokenOnList(char *token, const char **list) {
+  int i = 0;                                    // index in the list
+  while(strncmp(list[i],"ZZ",2) && i < 1000){   // stop at terminator and don't crash without it
+    // test for a match on the sentence name
+    if(!strncmp((const char *)list[i],(const char *)token,strlen(list[i]))) return list[i];
+    i++;
+  }
+  return NULL;                               // couldn't find a match
+}
+
+/**************************************************************************/
+/*!
+    @brief Add *CS where CS is the two character hex checksum for all but 
+    the first character in the string. The checksum is the result of an
+    exclusive or of all the characters in the string. Also useful if you 
+    are creating new PMTK strings for controlling a GPS module and need a
+    checksum added.
+    @param buff Pointer to the string, which must be long enough 
+    @return none
+*/
+/**************************************************************************/
+void Adafruit_GPS::addChecksum(char *buff){
+  char cs = 0;
+  int i = 1;
+  while(buff[i]){
+    cs ^= buff[i];
+    i++;
+  }
+  sprintf(buff,"%s*%02X",buff,cs);
 }
 
 /**************************************************************************/
@@ -927,4 +974,4 @@ static boolean strStartsWith(const char* str, const char* prefix)
       return false;
   }
   return true;
-}
+}

Adafruit_GPS.h

@@ -31,6 +31,8 @@
 #define GPS_MAX_I2C_TRANSFER 32  ///< The max number of bytes we'll try to read at once
 #define GPS_MAX_SPI_TRANSFER 100  ///< The max number of bytes we'll try to read at once
 #define MAXLINELENGTH 120 ///< how long are max NMEA lines to parse?
+#define NMEA_MAX_SENTENCE_ID  20    ///< maximum length of a sentence ID name, including terminating 0
+#define NMEA_MAX_SOURCE_ID     3    ///< maximum length of a source ID name, including terminating 0
 
 
 #include "Arduino.h"
@@ -101,6 +103,16 @@
 #define MAXWAITSENTENCE 10   ///< how long to wait when we're looking for a response
 /**************************************************************************/
 
+/// type for resulting code from running check()
+typedef enum {
+    NMEA_BAD                  =  0,   ///< passed none of the checks
+    NMEA_HAS_DOLLAR           =  1,   ///< has a dollar sign in the first position
+    NMEA_HAS_CHECKSUM         =  2,   ///< has a valid checksum at the end
+    NMEA_HAS_NAME             =  4,   ///< there is a token after the $ followed by a comma
+    NMEA_HAS_SOURCE           = 10,   ///< has a recognized source ID
+    NMEA_HAS_SENTENCE         = 20,   ///< has a recognized sentence ID
+    NMEA_HAS_SENTENCE_P       = 40    ///< has a recognized parseable sentence ID
+} nmea_check_t;                       
 
 /**************************************************************************/
 /*!
@@ -131,7 +143,9 @@ class Adafruit_GPS : public Print{
   size_t write(uint8_t);
   size_t available(void);
 
+  boolean check(char *nmea); 
   boolean parse(char *);
+  void addChecksum(char *buff);
   float secondsSinceFix();
   float secondsSinceTime();
   float secondsSinceDate();
@@ -139,6 +153,12 @@ class Adafruit_GPS : public Print{
   boolean wakeup(void);
   boolean standby(void);
 
+  int thisCheck = 0;                                ///< the results of the check on the current sentence
+  char thisSource[NMEA_MAX_SOURCE_ID] = {0};        ///< the first two letters of the current sentence, e.g. WI, GP
+  char thisSentence[NMEA_MAX_SENTENCE_ID] = {0};    ///< the next three letters of the current sentence, e.g. GLL, RMC
+  char lastSource[NMEA_MAX_SOURCE_ID] = {0};        ///< the results of the check on the most recent successfully parsed sentence
+  char lastSentence[NMEA_MAX_SENTENCE_ID] = {0};    ///< the next three letters of the most recent successfully parsed sentence, e.g. GLL, RMC
+
   uint8_t hour;                                     ///< GMT hours
   uint8_t minute;                                   ///< GMT minutes
   uint8_t seconds;                                  ///< GMT seconds
@@ -191,19 +211,26 @@ class Adafruit_GPS : public Print{
   uint8_t LOCUS_percent;    ///< Log life used percentage
 
  private:
+  const char * tokenOnList(char *token, const char **list);
   void parseTime(char *);
   void parseLat(char *);
   boolean parseLatDir(char *);
   void parseLon(char *);
   boolean parseLonDir(char *);
   boolean parseFix(char *);
+  // used by check() for validity tests, room for future expansion
+  const char *sources[5] =          {"II",  "WI",  "GP",  "GN",  "ZZZ"};  ///< valid source ids
+  const char *sentences_parsed[5] = {"GGA", "GLL", "GSA", "RMC", "ZZZ"};  ///< parseable sentence ids
+  const char *sentences_known[1] =  {"ZZZ"};        ///< known, but not parseable sentence ids
+  
   // Make all of these times far in the past by setting them near the middle of the
   // millis() range. Timing assumes that sentences are parsed promptly.
-  uint32_t lastFix = 2000000000L;		// millis() when last fix received
+  uint32_t lastUpdate = 2000000000L;  ///< millis() when last full sentence successfully parsed
-  uint32_t lastTime = 2000000000L;    // millis() when last time received
+  uint32_t lastFix = 2000000000L;     ///< millis() when last fix received
-  uint32_t lastDate = 2000000000L;    // millis() when last date received
+  uint32_t lastTime = 2000000000L;    ///< millis() when last time received
-  uint32_t recvdTime = 2000000000L;   // millis() when last full sentence received
+  uint32_t lastDate = 2000000000L;    ///< millis() when last date received
-  uint32_t sentTime = 2000000000L;   // millis() when first character of last full sentence received
+  uint32_t recvdTime = 2000000000L;   ///< millis() when last full sentence received
+  uint32_t sentTime = 2000000000L;    ///< millis() when first character of last full sentence received
   boolean paused;
 
   uint8_t parseResponse(char *response);