Merge pull request #197 from ladyada-eagleclaw/gps-shared-position-integration

Use shared GNSS position results in the GPS decoder

extras/tests/navigation_state/navigation_state.ino

@@ -73,6 +73,44 @@ void setup() {
     return;
   }
   Serial.println(F("PASS: valid no-fix messages preserve absent measurements"));
+
+  uint32_t dateTime = GPS.lastDate;
+  GPS.sentTime = 1000;
+  if (!parseBody("$GPGGA,123519,4807.038,N,01131.000,E,4,08,0.9,545.4,M,46.9") ||
+      !GPS.fix || GPS.fixquality != 4 || GPS.lastFix != 1000 ||
+      GPS.lastTime != 1000 || GPS.lastDate != dateTime) {
+    Serial.println(F("FAIL: GGA position update or independent date timestamp"));
+    return;
+  }
+  latitude = GPS.latitude_fixed;
+  longitude = GPS.longitude_fixed;
+  GPS.sentTime = 2000;
+  if (!parseBody("$GPRMC,,V,,,,,,,") || GPS.fix || GPS.fixquality != 4 ||
+      GPS.latitude_fixed != latitude || GPS.longitude_fixed != longitude ||
+      GPS.lastFix != 1000 || GPS.lastTime != 1000 || GPS.lastDate != dateTime) {
+    Serial.println(F("FAIL: RMC no-fix changed GGA quality or absent data"));
+    return;
+  }
+  GPS.sentTime = 3000;
+  if (!parseBody("$GPGLL,,,,,123520,A") || !GPS.fix || GPS.fixquality != 4 ||
+      GPS.seconds != 20 || GPS.lastFix != 3000 || GPS.lastTime != 3000 ||
+      GPS.lastDate != dateTime || GPS.latitude_fixed != latitude ||
+      GPS.longitude_fixed != longitude) {
+    Serial.println(F("FAIL: GLL fix/time update replaced absent position or date"));
+    return;
+  }
+  GPS.sentTime = 4000;
+  if (!parseBody("$GPGGA,,,,,,,,,,,") || !GPS.fix || GPS.fixquality != 4 ||
+      GPS.lastFix != 3000 || GPS.lastTime != 3000 || GPS.lastDate != dateTime) {
+    Serial.println(F("FAIL: empty GGA changed stored fix or timestamps"));
+    return;
+  }
+  if (!parseBody("$GPGGA,,,,,,0,00,99.99,,M,") || GPS.fix || GPS.fixquality ||
+      GPS.lastFix != 3000 || GPS.lastTime != 3000) {
+    Serial.println(F("FAIL: GGA no-fix refreshed last fix or time"));
+    return;
+  }
+  Serial.println(F("PASS: shared position results preserve legacy merging and timestamps"));
   testsPassed = true;
 }
 

src/Adafruit_GNSS.cpp

@@ -301,10 +301,24 @@ static nmea_number_status_t parseSixDigits(nmea_span_t field, uint8_t *pairs) {
 /**************************************************************************/
 gnss_validation_t Adafruit_GNSS::validateNavigation(nmea_span_t type,
                                                     nmea_span_t fields) {
-  gnss_validation_t result = {GNSS_SENTENCE_UNSUPPORTED, 0};
   if (!type.data || type.length != 3)
-    return result;
-  uint8_t sentence = sentenceType(type);
+    return {GNSS_SENTENCE_UNSUPPORTED, 0};
+  return decodeNavigation(sentenceType(type), fields, NULL);
+}
+
+/**************************************************************************/
+/*!
+    @brief Validate navigation and optionally collect exact position fields.
+    @param sentence Private sentence kind.
+    @param fields Bounded sentence fields.
+    @param position Optional scratch result; discard it if validation fails.
+    @return Status and first failing field.
+*/
+/**************************************************************************/
+gnss_validation_t Adafruit_GNSS::decodeNavigation(uint8_t sentence,
+                                                  nmea_span_t fields,
+                                                  gnss_position_t *position) {
+  gnss_validation_t result = {GNSS_SENTENCE_UNSUPPORTED, 0};
   if (sentence == UNSUPPORTED)
     return result;
   uint8_t required = 6, latitudeField = 1, timeField = 5;
@@ -333,9 +347,9 @@ gnss_validation_t Adafruit_GNSS::validateNavigation(nmea_span_t type,
         return result;
       }
       result.status = GNSS_SENTENCE_INVALID_FIELD;
+      gnss_coordinate_t coordinate = parseCoordinate(field, hemisphere);
+      bool latitude = i == latitudeField;
       if (field.length || hemisphere.length) {
-        gnss_coordinate_t coordinate = parseCoordinate(field, hemisphere);
-        bool latitude = i == latitudeField;
         if (coordinate.status != NMEA_NUMBER_VALID ||
             (latitude && coordinate.hemisphere != 'N' &&
              coordinate.hemisphere != 'S') ||
@@ -343,27 +357,62 @@ gnss_validation_t Adafruit_GNSS::validateNavigation(nmea_span_t type,
              coordinate.hemisphere != 'W'))
           return result;
       }
+      if (position) {
+        if (latitude)
+          position->latitude = coordinate;
+        else
+          position->longitude = coordinate;
+      }
       i++;
       continue;
     }
-    if (!field.length)
+    if (!field.length) {
+      if (position) {
+        if (i == timeField)
+          position->time.status = NMEA_NUMBER_EMPTY;
+        else if (sentence == RMC && i == 9)
+          position->date.status = NMEA_NUMBER_EMPTY;
+        else if ((sentence == RMC && i == 2) || (sentence != RMC && i == 6)) {
+          position->fixStatus = NMEA_NUMBER_EMPTY;
+          if (sentence == GGA)
+            position->fixQualityStatus = NMEA_NUMBER_EMPTY;
+        }
+      }
       continue;
+    }
     result.status = GNSS_SENTENCE_INVALID_FIELD;
     if (sentence == GSA && (i == 1 || (i >= 3 && i <= 14))) {
       continue; // Selection mode and satellite IDs are not decoded by GPS.
     } else if (i == timeField) {
-      if (parseTime(field).status != NMEA_NUMBER_VALID)
+      gnss_time_t time = parseTime(field);
+      if (time.status != NMEA_NUMBER_VALID)
         return result;
+      if (position)
+        position->time = time;
     } else if (sentence == RMC && i == 9) {
-      if (parseDate(field).status != NMEA_NUMBER_VALID)
+      gnss_date_t date = parseDate(field);
+      if (date.status != NMEA_NUMBER_VALID)
         return result;
+      if (position)
+        position->date = date;
     } else if ((sentence == RMC && i == 2) || (sentence == GLL && i == 6)) {
       if (field.length != 1 || (field.data[0] != 'A' && field.data[0] != 'V'))
         return result;
+      if (position) {
+        position->fixStatus = NMEA_NUMBER_VALID;
+        position->fix = field.data[0] == 'A';
+      }
     } else if ((sentence == GGA && (i == 6 || i == 7)) ||
                (sentence == GSA && i == 2)) {
       if (!validUnsignedByte(field))
         return result;
+      if (position && sentence == GGA && i == 6) {
+        position->fixStatus = position->fixQualityStatus = NMEA_NUMBER_VALID;
+        for (size_t j = 0; j < field.length; j++)
+          position->fixQuality =
+              position->fixQuality * 10 + field.data[j] - '0';
+        position->fix = position->fixQuality > 0;
+      }
     } else if (sentence == GGA && i == 10) {
       if (field.length != 1 || field.data[0] != 'M')
         return result;
@@ -409,8 +458,8 @@ uint8_t Adafruit_GNSS::sentenceType(nmea_span_t type) {
 
     Call after validating the enclosing frame with Adafruit_NMEA::validate().
     Input storage must remain readable and unchanged throughout the call.
-    All navigation fields consumed by the GPS decoder are validated first,
-    including fields this position result does not expose (such as altitude).
+    Validate all consumed navigation fields before returning measurements,
+    including fields this result does not expose (such as altitude).
     Optional tails retain validateNavigation()'s existing behavior.
 
     Populated coordinates retain all nine fractional-minute digits and can be
@@ -436,46 +485,15 @@ gnss_position_t Adafruit_GNSS::parsePosition(nmea_span_t type,
   uint8_t sentence = sentenceType(type);
   if (sentence != GGA && sentence != RMC && sentence != GLL)
     return result;
-  result.validation = validateNavigation(type, fields);
-  if (result.validation.status != GNSS_SENTENCE_VALID)
-    return result;
-
-  uint8_t latitudeField = 1, timeField = 5, fixField = 6, lastField = 6;
-  if (sentence == GGA) {
-    latitudeField = 2;
-    timeField = 1;
-  } else if (sentence == RMC) {
-    latitudeField = 3;
-    timeField = 1;
-    fixField = 2;
-    lastField = 9;
-  }
-  for (uint8_t i = 1; i <= lastField; i++) {
-    nmea_span_t field = Adafruit_NMEA::nextField(fields);
-    if (i == latitudeField || i == latitudeField + 2) {
-      nmea_span_t hemisphere = Adafruit_NMEA::nextField(fields);
-      if (i == latitudeField)
-        result.latitude = parseCoordinate(field, hemisphere);
-      else
-        result.longitude = parseCoordinate(field, hemisphere);
-      i++;
-    } else if (i == timeField) {
-      result.time = parseTime(field);
-    } else if (i == fixField) {
-      result.fixStatus = field.length ? NMEA_NUMBER_VALID : NMEA_NUMBER_EMPTY;
-      if (sentence == GGA) {
-        result.fixQualityStatus = result.fixStatus;
-        // Validation already proved that this integer fits uint8_t.
-        for (size_t j = 0; j < field.length; j++)
-          result.fixQuality = result.fixQuality * 10 + field.data[j] - '0';
-        result.fix = result.fixQuality > 0;
-      } else {
-        result.fix = field.length && field.data[0] == 'A';
-      }
-    } else if (sentence == RMC && i == 9) {
-      result.date = parseDate(field);
-    }
+  gnss_validation_t validation = decodeNavigation(sentence, fields, &result);
+  if (validation.status != GNSS_SENTENCE_VALID) {
+    // Discard every tentative value, including fields before the error.
+    result = {};
+    result.latitude.status = result.longitude.status = NMEA_NUMBER_MISSING;
+    result.time.status = result.date.status = NMEA_NUMBER_MISSING;
+    result.fixStatus = result.fixQualityStatus = NMEA_NUMBER_MISSING;
   }
+  result.validation = validation;
   return result;
 }
 

src/Adafruit_GNSS.h

@@ -93,6 +93,9 @@ public:
 
 private:
   static uint8_t sentenceType(nmea_span_t type);
+  static gnss_validation_t decodeNavigation(uint8_t sentence,
+                                            nmea_span_t fields,
+                                            gnss_position_t *position);
   /// Sentence kinds handled by the navigation validator.
   enum {
     UNSUPPORTED, ///< Sentence has no standard navigation validation.

src/Adafruit_GPS.h

@@ -60,7 +60,7 @@
 #ifdef USE_SW_SERIAL
 #include <SoftwareSerial.h>
 #endif
-#include <Adafruit_NMEA.h>
+#include <Adafruit_GNSS.h>
 #include <Adafruit_PMTK.h>
 #include <NMEA_data.h>
 #include <SPI.h>
@@ -250,9 +250,10 @@ private:
                   nmea_float_t *angle = NULL, int32_t *angle_fixed = NULL,
                   char *dir = NULL);
   char *parseStr(char *buff, char *p, int n);
-  char *parseCoordinates(char *p);
-  bool parseTime(char *);
-  bool parseFix(char *);
+  void setCoordinate(const gnss_coordinate_t &coordinate,
+                     nmea_float_t *angleDegrees, nmea_float_t *angle,
+                     int32_t *angle_fixed, char *dir);
+  gnss_sentence_status_t updatePosition(nmea_span_t type, nmea_span_t fields);
   bool parseAntenna(char *);
   bool isEmpty(char *pStart);
 

src/NMEA_parse.cpp

@@ -91,10 +91,10 @@ bool Adafruit_GPS::parse(char *nmea) {
   // changing any fix data. The enclosing frame has already passed check().
   nmea_span_t type = {thisSentence, strlen(thisSentence)};
   nmea_span_t dataFields = {p, (size_t)(strchr(p, '*') - p)};
-  gnss_validation_t validation =
-      Adafruit_GNSS::validateNavigation(type, dataFields);
-  if (validation.status != GNSS_SENTENCE_VALID &&
-      validation.status != GNSS_SENTENCE_UNSUPPORTED)
+  gnss_sentence_status_t status = updatePosition(type, dataFields);
+  if (status == GNSS_SENTENCE_UNSUPPORTED)
+    status = Adafruit_GNSS::validateNavigation(type, dataFields).status;
+  if (status != GNSS_SENTENCE_VALID && status != GNSS_SENTENCE_UNSUPPORTED)
     return false;
 
   // This may look inefficient, but an M0 will get down the list in about 1 us /
@@ -102,20 +102,9 @@ bool Adafruit_GPS::parse(char *nmea) {
   // pruning excess code easier. Otherwise, keep them alphabetical for ease of
   // reading.
   if (!strcmp(thisSentence, "GGA")) { //************************************GGA
-    // Adafruit from Actisense NGW-1 from SH CP150C
-    parseTime(p);
-    p = strchr(p, ',') + 1; // parse time with specialized function
-    p = parseCoordinates(p);
-    if (!isEmpty(p)) { // if it's a , (or a * at end of sentence) the value is
-                       // not included
-      fixquality = atoi(p); // needs additional processing
-      if (fixquality > 0) {
-        fix = true;
-        lastFix = sentTime;
-      } else
-        fix = false;
-    }
-    p = strchr(p, ',') + 1; // then move on to the next
+    // Position, time, and fix are already decoded by the shared core.
+    for (uint8_t i = 0; i < 6; i++)
+      p = strchr(p, ',') + 1;
     // Most can just be parsed with atoi() or atof(), then move on to the next.
     if (!isEmpty(p))
       satellites = atoi(p);
@@ -131,33 +120,16 @@ bool Adafruit_GPS::parse(char *nmea) {
       geoidheight = atof(p); // skip the rest
 
   } else if (!strcmp(thisSentence, "RMC")) { //*****************************RMC
-    // in Adafruit from Actisense NGW-1 from SH CP150C
-    parseTime(p);
-    p = strchr(p, ',') + 1;
-    parseFix(p);
-    p = strchr(p, ',') + 1;
-    p = parseCoordinates(p);
+    for (uint8_t i = 0; i < 6; i++)
+      p = strchr(p, ',') + 1;
     if (!isEmpty(p))
       newDataValue(NMEA_SOG, speed = atof(p));
     p = strchr(p, ',') + 1;
     if (!isEmpty(p))
       newDataValue(NMEA_COG, angle = atof(p));
-    p = strchr(p, ',') + 1;
-    if (!isEmpty(p)) {
-      // RMC dates have already passed full-field validation above.
-      gnss_date_t date = Adafruit_GNSS::parseDate({p, 6});
-      day = date.day;
-      month = date.month;
-      year = date.year;
-      lastDate = sentTime;
-    } // skip the rest
 
   } else if (!strcmp(thisSentence, "GLL")) { //*****************************GLL
-    // in Adafruit from Actisense NGW-1 from SH CP150C
-    p = parseCoordinates(p);
-    parseTime(p);
-    p = strchr(p, ',') + 1;
-    parseFix(p); // skip the rest
+    // All GLL fields are handled by the shared position decoder.
 
   } else if (!strcmp(thisSentence, "GSA")) { //*****************************GSA
     // in Adafruit from Actisense NGW-1
@@ -739,6 +711,24 @@ bool Adafruit_GPS::parseCoord(char *pStart, nmea_float_t *angleDegrees,
   if (coordinate.status != NMEA_NUMBER_VALID)
     return false;
 
+  setCoordinate(coordinate, angleDegrees, angle, angle_fixed, dir);
+  return true;
+}
+
+/**************************************************************************/
+/*!
+    @brief Convert exact components into the legacy coordinate representations.
+    @param coordinate Validated coordinate components.
+    @param angleDegrees Optional signed decimal degrees output.
+    @param angle Optional unsigned degrees/minutes output.
+    @param angle_fixed Optional signed E7 output.
+    @param dir Optional hemisphere output.
+*/
+/**************************************************************************/
+void Adafruit_GPS::setCoordinate(const gnss_coordinate_t &coordinate,
+                                 nmea_float_t *angleDegrees,
+                                 nmea_float_t *angle, int32_t *angle_fixed,
+                                 char *dir) {
   // Derive convenience floats only after exact fixed-point conversion. Keep
   // their available precision instead of rebuilding them from the E7 value.
   nmea_float_t minutes = coordinate.minutes + coordinate.fractionalMinutes /
@@ -754,29 +744,56 @@ bool Adafruit_GPS::parseCoord(char *pStart, nmea_float_t *angleDegrees,
     *angleDegrees = degrees;
   if (dir)
     *dir = coordinate.hemisphere;
-  return true;
 }
 
 /**************************************************************************/
 /*!
-    @brief Update the adjacent latitude and longitude fields of a sentence.
-    @param p First coordinate field in an already validated GGA, RMC, or GLL.
-    @return Field following longitude and its hemisphere.
+    @brief Apply one validated shared position result to legacy GPS state.
+    @param type Sentence type without its talker prefix.
+    @param fields Bounded fields from the already checked frame.
+    @return Shared validation status; rejected sentences leave data unchanged.
 
-    The caller must validate all four fields before calling. Empty coordinate
-    pairs leave their previous values unchanged. Use the same exact conversion
-    and data update path for each navigation sentence.
+    Empty fields preserve previous values and timestamps. GGA quality and
+    RMC/GLL validity remain independent, and only a positive fix refreshes
+    lastFix. Exact components are converted only at the legacy float boundary.
 */
 /**************************************************************************/
-char *Adafruit_GPS::parseCoordinates(char *p) {
-  if (parseCoord(p, &latitudeDegrees, &latitude, &latitude_fixed, &lat))
+gnss_sentence_status_t Adafruit_GPS::updatePosition(nmea_span_t type,
+                                                    nmea_span_t fields) {
+  gnss_position_t position = Adafruit_GNSS::parsePosition(type, fields);
+  if (position.validation.status != GNSS_SENTENCE_VALID)
+    return position.validation.status;
+  if (position.latitude.status == NMEA_NUMBER_VALID) {
+    setCoordinate(position.latitude, &latitudeDegrees, &latitude,
+                  &latitude_fixed, &lat);
     newDataValue(NMEA_LAT, latitudeDegrees);
-  p = strchr(p, ',') + 1;
-  p = strchr(p, ',') + 1;
-  if (parseCoord(p, &longitudeDegrees, &longitude, &longitude_fixed, &lon))
+  }
+  if (position.longitude.status == NMEA_NUMBER_VALID) {
+    setCoordinate(position.longitude, &longitudeDegrees, &longitude,
+                  &longitude_fixed, &lon);
     newDataValue(NMEA_LON, longitudeDegrees);
-  p = strchr(p, ',') + 1;
-  return strchr(p, ',') + 1;
+  }
+  if (position.time.status == NMEA_NUMBER_VALID) {
+    hour = position.time.hour;
+    minute = position.time.minute;
+    seconds = position.time.second;
+    milliseconds = position.time.millisecond;
+    lastTime = sentTime;
+  }
+  if (position.date.status == NMEA_NUMBER_VALID) {
+    day = position.date.day;
+    month = position.date.month;
+    year = position.date.year;
+    lastDate = sentTime;
+  }
+  if (position.fixStatus == NMEA_NUMBER_VALID) {
+    fix = position.fix;
+    if (fix)
+      lastFix = sentTime;
+  }
+  if (position.fixQualityStatus == NMEA_NUMBER_VALID)
+    fixquality = position.fixQuality;
+  return GNSS_SENTENCE_VALID;
 }
 
 /**************************************************************************/
@@ -805,52 +822,6 @@ char *Adafruit_GPS::parseStr(char *buff, char *p, int n) {
   return buff;
 }
 
-/**************************************************************************/
-/*!
-    @brief Parse a part of an NMEA string for time. Independent of number
-    of decimal places after the '.'
-    @param p Pointer to the location of the token in the NMEA string
-    @return true if successful, false otherwise
-*/
-/**************************************************************************/
-bool Adafruit_GPS::parseTime(char *p) {
-  if (!p)
-    return false;
-  char *end = p;
-  while (*end && *end != ',' && *end != '*')
-    end++;
-  gnss_time_t time = Adafruit_GNSS::parseTime({p, (size_t)(end - p)});
-  if (time.status != NMEA_NUMBER_VALID)
-    return false;
-  hour = time.hour;
-  minute = time.minute;
-  seconds = time.second;
-  milliseconds = time.millisecond;
-  lastTime = sentTime;
-  return true;
-}
-
-/**************************************************************************/
-/*!
-    @brief Parse a part of an NMEA string for whether there is a fix
-    @param p Pointer to the location of the token in the NMEA string
-    @return True if we parsed it, false if it has invalid data
-*/
-/**************************************************************************/
-bool Adafruit_GPS::parseFix(char *p) {
-  if (!isEmpty(p)) {
-    if (p[0] == 'A') {
-      fix = true;
-      lastFix = sentTime;
-    } else if (p[0] == 'V')
-      fix = false;
-    else
-      return false;
-    return true;
-  }
-  return false;
-}
-
 /**************************************************************************/
 /*!
     @brief Parse a part of an NMEA string for antenna that is used