Skip to content

Add trip distance. Simplify parseCoords(). parse RMC only if valid fix. #145

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Open
wants to merge 1 commit into
base: master
Choose a base branch
from
Open

Add trip distance. Simplify parseCoords(). parse RMC only if valid fix. #145

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
12 changes: 12 additions & 0 deletions src/Adafruit_GPS.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -441,6 +441,18 @@ bool Adafruit_GPS::waitForSentence(const char *wait4me, uint8_t max,
return false;
}

/**************************************************************************/
/*!
@brief Set the tolerance for accepting distance travelled is real
@brief movement not just drift
@param meters float expressing lowest valid distance
*/
/**************************************************************************/
void Adafruit_GPS::setDriftTolerance(nmea_float_t meters) {
driftTolerance = meters;
}


/**************************************************************************/
/*!
@brief Start the LOCUS logger
Expand Down
14 changes: 14 additions & 0 deletions src/Adafruit_GPS.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -105,6 +105,7 @@ class Adafruit_GPS : public Print {
char *lastNMEA(void);
bool waitForSentence(const char *wait, uint8_t max = MAXWAITSENTENCE,
bool usingInterrupts = false);
void setDriftTolerance(nmea_float_t meters = 10.0);
bool LOCUS_StartLogger(void);
bool LOCUS_StopLogger(void);
bool LOCUS_ReadStatus(void);
Expand Down Expand Up @@ -182,6 +183,17 @@ class Adafruit_GPS : public Print {

nmea_float_t latitudeDegrees; ///< Latitude in decimal degrees
nmea_float_t longitudeDegrees; ///< Longitude in decimal degrees
nmea_float_t driftTolerance = 10.0; ///< Threshold for updating lat/long...DegreesLocked (below)
///< and tripDistance
nmea_float_t latitudeDegreesLocked; ///
///< Only updated if move more than drift tolerance
///< Floating point latitude value in degrees/minutes
///< as received from the GPS (DDMM.MMMM)
nmea_float_t longitudeDegreesLocked; ///
///< Only updated if move more than drift tolerance
///< Floating point longitude value in degrees/minutes
///< as received from the GPS (DDDMM.MMMM)
nmea_float_t tripDistance = 0.0; ///< cumulative distance travelled using lat/long...DegreesLocked
nmea_float_t geoidheight; ///< Diff between geoid height and WGS84 height
nmea_float_t altitude; ///< Altitude in meters above MSL
nmea_float_t speed; ///< Current speed over ground in knots
Expand Down Expand Up @@ -251,6 +263,8 @@ class Adafruit_GPS : public Print {
bool parseFix(char *);
bool parseAntenna(char *);
bool isEmpty(char *pStart);
nmea_float_t degreesToRadians(nmea_float_t deg);
void updateTripMeter(nmea_float_t);

// used by check() for validity tests, room for future expansion
const char *sources[7] = {"II", "WI", "GP", "PG",
Expand Down
111 changes: 76 additions & 35 deletions src/NMEA_parse.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -104,30 +104,67 @@ bool Adafruit_GPS::parse(char *nmea) {
parseTime(p);
p = strchr(p, ',') + 1;
parseFix(p);
p = strchr(p, ',') + 1;
// parse out both latitude and direction, then go to next field, or fail
if (parseCoord(p, &latitudeDegrees, &latitude, &latitude_fixed, &lat))
newDataValue(NMEA_LAT, latitudeDegrees);
p = strchr(p, ',') + 1;
p = strchr(p, ',') + 1;
// parse out both longitude and direction, then go to next field, or fail
if (parseCoord(p, &longitudeDegrees, &longitude, &longitude_fixed, &lon))
newDataValue(NMEA_LON, longitudeDegrees);
p = strchr(p, ',') + 1;
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)) {
uint32_t fulldate = atof(p);
day = fulldate / 10000;
month = (fulldate % 10000) / 100;
year = (fulldate % 100);
lastDate = sentTime;
} // skip the rest
// Skip rest unless valid fix
if (fix) {
p = strchr(p, ',') + 1;
// parse out both latitude and direction, then go to next field, or fail
if (parseCoord(p, &latitudeDegrees, &latitude, &latitude_fixed, &lat)) {
newDataValue(NMEA_LAT, latitudeDegrees);
//Valid latitude so move on to longitiude
p = strchr(p, ',') + 1;
p = strchr(p, ',') + 1;
// parse out both longitude and direction, then go to next field, or fail
if (parseCoord(p, &longitudeDegrees, &longitude, &longitude_fixed, &lon)) {
newDataValue(NMEA_LON, longitudeDegrees);
//Valid coords
if (latitudeDegreesLocked == NULL) {
//Initialize locked coords if NULL
latitudeDegreesLocked = latitudeDegrees;
longitudeDegreesLocked = longitudeDegrees;
}
else
{
// Do distance calc here
nmea_float_t lat1Radians = degreesToRadians(latitudeDegreesLocked);
nmea_float_t lat2Radians = degreesToRadians(latitudeDegrees);
nmea_float_t lon1Radians = degreesToRadians(longitudeDegreesLocked);
nmea_float_t lon2Radians = degreesToRadians(longitudeDegrees);
nmea_float_t d=6378100 * acos(sin(lat1Radians)*sin(lat2Radians)+cos(lat1Radians)*cos(lat2Radians)*cos(lon1Radians-lon2Radians));
if (d >= driftTolerance) {
tripDistance += d;
latitudeDegreesLocked = latitudeDegrees;
longitudeDegreesLocked = longitudeDegrees;
} // Only update locked position if distance travelled 3 meters or greater
}
}
//Good or bad, Skip to SOG
p = strchr(p, ',') + 1;
p = strchr(p, ',') + 1;
}
else
{
//No Latitude so skip to SOG
p = strchr(p, ',') + 1;
p = strchr(p, ',') + 1;
p = strchr(p, ',') + 1;
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)) {
uint32_t fulldate = atof(p);
day = fulldate / 10000;
month = (fulldate % 10000) / 100;
year = (fulldate % 100);
lastDate = sentTime;
} // skip the rest
}


} else if (!strcmp(thisSentence, "GLL")) { //*****************************GLL
// in Adafruit from Actisense NGW-1 from SH CP150C
Expand Down Expand Up @@ -684,15 +721,15 @@ bool Adafruit_GPS::parseCoord(char *pStart, nmea_float_t *angleDegrees,
if (!isEmpty(p)) {
// get the number in DDDMM.mmmm format and break into components
char degreebuff[10] = {0}; // Ensure string is terminated after strncpy
char *e = strchr(p, '.');
if (e == NULL || e - p > 6)
char *e = strchr(p, '.') - 2;
if (e == NULL || e - p > 4)
return false; // no decimal point in range
strncpy(degreebuff, p, e - p); // get DDDMM
long dddmm = atol(degreebuff);
long degrees = (dddmm / 100); // truncate the minutes
long minutes = dddmm - degrees * 100; // remove the degrees
p = e; // start from the decimal point
nmea_float_t decminutes = atof(e); // the fraction after the decimal point
strncpy(degreebuff, p, e - p); // get DDD
long degrees = atol(degreebuff);
// long degrees = (dddmm / 100); // truncate the minutes
// long minutes = dddmm - degrees * 100; // remove the degrees
p = e; // start from the start of minutes (decimal point - 2)
nmea_float_t minutes = atof(e); // the integer AND the fraction after the decimal point
p = strchr(p, ',') + 1; // go to the next field

// get the NSEW direction as a character
Expand All @@ -703,9 +740,8 @@ bool Adafruit_GPS::parseCoord(char *pStart, nmea_float_t *angleDegrees,
return false; // no direction provided

// set the various numerical formats to their values
long fixed = degrees * 10000000 + (minutes * 10000000) / 60 +
(decminutes * 10000000) / 60;
nmea_float_t ang = degrees * 100 + minutes + decminutes;
long fixed = degrees * 10000000 + (minutes * 10000000) / 60;
nmea_float_t ang = degrees * 100 + minutes;
nmea_float_t deg = fixed / (nmea_float_t)10000000.;
if (nsew == 'S' ||
nsew == 'W') { // fixed and deg are signed, but DDDMM.mmmm is not
Expand Down Expand Up @@ -816,6 +852,11 @@ bool Adafruit_GPS::parseFix(char *p) {
return false;
}

nmea_float_t Adafruit_GPS::degreesToRadians(nmea_float_t deg)
{
return deg*0.01745329251994329576923690768489;
}

/**************************************************************************/
/*!
@brief Parse a part of an NMEA string for antenna that is used
Expand Down