mirror of
https://github.com/meshtastic/firmware.git
synced 2025-02-03 03:09:59 +00:00
Merge branch 'master' into radiolib-sync-upstream
This commit is contained in:
Kevin Hester
GitHub
commit
e351f35cf2
@ -45,6 +45,9 @@ class GPS : private concurrency::OSThread
|
||||
// scaling before use)
|
||||
uint32_t heading = 0; // Heading of motion, in degrees * 10^-5
|
||||
|
||||
int32_t geoidal_height = 0; // geoidal separation, in meters!
|
||||
time_t pos_timestamp = 0; // positional timestamp from GPS solution
|
||||
|
||||
GPS() : concurrency::OSThread("GPS") {}
|
||||
|
||||
virtual ~GPS();
|
||||
|
||||
@ -2,6 +2,12 @@
|
||||
#include "NMEAGPS.h"
|
||||
#include "RTC.h"
|
||||
|
||||
#include <TinyGPS++.h>
|
||||
|
||||
// GPS solutions older than this will be rejected - see TinyGPSDatum::age()
|
||||
#define GPS_SOL_EXPIRY_MS 300 // in millis
|
||||
#define NMEA_MSG_GXGSA "GNGSA" // GSA message (GPGSA, GNGSA etc)
|
||||
|
||||
static int32_t toDegInt(RawDegrees d)
|
||||
{
|
||||
int32_t degMult = 10000000; // 1e7
|
||||
@ -21,6 +27,17 @@ bool NMEAGPS::setupGPS()
|
||||
pinMode(PIN_GPS_PPS, INPUT);
|
||||
#endif
|
||||
|
||||
// Currently disabled per issue #525 (TinyGPS++ crash bug)
|
||||
// when fixed upstream, can be un-disabled to enable 3D FixType and PDOP
|
||||
#ifndef TINYGPS_OPTION_NO_CUSTOM_FIELDS
|
||||
// see NMEAGPS.h
|
||||
gsafixtype.begin(reader, NMEA_MSG_GXGSA, 2);
|
||||
gsapdop.begin(reader, NMEA_MSG_GXGSA, 15);
|
||||
DEBUG_MSG("Using " NMEA_MSG_GXGSA " for 3DFIX and PDOP\n");
|
||||
#else
|
||||
DEBUG_MSG("GxGSA NOT available\n");
|
||||
#endif
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
@ -64,47 +81,117 @@ The Unix epoch (or Unix time or POSIX time or Unix timestamp) is the number of s
|
||||
*/
|
||||
bool NMEAGPS::lookForLocation()
|
||||
{
|
||||
bool foundLocation = false;
|
||||
// By default, TinyGPS++ does not parse GPGSA lines, which give us
|
||||
// the 2D/3D fixType (see NMEAGPS.h)
|
||||
// At a minimum, use the fixQuality indicator in GPGGA (FIXME?)
|
||||
fixQual = reader.fixQuality();
|
||||
|
||||
// uint8_t fixtype = reader.fixQuality();
|
||||
// hasValidLocation = ((fixtype >= 1) && (fixtype <= 5));
|
||||
#ifndef TINYGPS_OPTION_NO_CUSTOM_FIELDS
|
||||
fixType = atoi(gsafixtype.value()); // will set to zero if no data
|
||||
DEBUG_MSG("FIX QUAL=%d, TYPE=%d\n", fixQual, fixType);
|
||||
#endif
|
||||
|
||||
// check if GPS has an acceptable lock
|
||||
if (! hasLock())
|
||||
return false;
|
||||
|
||||
// check if a complete GPS solution set is available for reading
|
||||
// tinyGPSDatum::age() also includes isValid() test
|
||||
// FIXME
|
||||
if (! ((reader.location.age() < GPS_SOL_EXPIRY_MS) &&
|
||||
#ifndef TINYGPS_OPTION_NO_CUSTOM_FIELDS
|
||||
(gsafixtype.age() < GPS_SOL_EXPIRY_MS) &&
|
||||
#endif
|
||||
(reader.time.age() < GPS_SOL_EXPIRY_MS) &&
|
||||
(reader.date.age() < GPS_SOL_EXPIRY_MS)))
|
||||
{
|
||||
// DEBUG_MSG("SOME data is TOO OLD\n");
|
||||
return false;
|
||||
}
|
||||
|
||||
// Is this a new point or are we re-reading the previous one?
|
||||
if (! reader.location.isUpdated())
|
||||
return false;
|
||||
|
||||
// Start reading the data
|
||||
auto loc = reader.location.value();
|
||||
|
||||
// Some GPSes (Air530) seem to send a zero longitude when the current fix is bogus
|
||||
// Bail out EARLY to avoid overwriting previous good data (like #857)
|
||||
if(toDegInt(loc.lat) == 0) {
|
||||
DEBUG_MSG("Ignoring bogus NMEA position\n");
|
||||
return false;
|
||||
}
|
||||
|
||||
// Dilution of precision (an accuracy metric) is reported in 10^2 units, so we need to scale down when we use it
|
||||
#ifndef TINYGPS_OPTION_NO_CUSTOM_FIELDS
|
||||
dop = TinyGPSPlus::parseDecimal(gsapdop.value());
|
||||
#else
|
||||
// FIXME! naive PDOP emulation (assumes VDOP==HDOP)
|
||||
// correct formula is PDOP = SQRT(HDOP^2 + VDOP^2)
|
||||
dop = 1.41 * reader.hdop.value();
|
||||
#endif
|
||||
|
||||
// Discard incomplete or erroneous readings
|
||||
if (dop == 0)
|
||||
return false;
|
||||
|
||||
latitude = toDegInt(loc.lat);
|
||||
longitude = toDegInt(loc.lng);
|
||||
|
||||
geoidal_height = reader.geoidHeight.meters();
|
||||
#ifdef GPS_ALTITUDE_HAE
|
||||
altitude = reader.altitude.meters() + geoidal_height;
|
||||
#else
|
||||
altitude = reader.altitude.meters();
|
||||
#endif
|
||||
|
||||
// positional timestamp
|
||||
struct tm t;
|
||||
t.tm_sec = reader.time.second();
|
||||
t.tm_min = reader.time.minute();
|
||||
t.tm_hour = reader.time.hour();
|
||||
t.tm_mday = reader.date.day();
|
||||
t.tm_mon = reader.date.month() - 1;
|
||||
t.tm_year = reader.date.year() - 1900;
|
||||
t.tm_isdst = false;
|
||||
pos_timestamp = mktime(&t);
|
||||
|
||||
// Nice to have, if available
|
||||
if (reader.satellites.isUpdated()) {
|
||||
setNumSatellites(reader.satellites.value());
|
||||
}
|
||||
|
||||
// Diminution of precision (an accuracy metric) is reported in 10^2 units, so we need to scale down when we use it
|
||||
if (reader.hdop.isUpdated()) {
|
||||
dop = reader.hdop.value();
|
||||
}
|
||||
if (reader.course.isUpdated()) {
|
||||
if (reader.course.isUpdated() && reader.course.isValid()) {
|
||||
heading = reader.course.value() * 1e3; // Scale the heading (in degrees * 10^-2) to match the expected degrees * 10^-5
|
||||
}
|
||||
|
||||
if (reader.altitude.isUpdated())
|
||||
altitude = reader.altitude.meters();
|
||||
/*
|
||||
// REDUNDANT?
|
||||
// expect gps pos lat=37.520825, lon=-122.309162, alt=158
|
||||
DEBUG_MSG("new NMEA GPS pos lat=%f, lon=%f, alt=%d, dop=%g, heading=%f\n",
|
||||
latitude * 1e-7, longitude * 1e-7, altitude, dop * 1e-2,
|
||||
heading * 1e-5);
|
||||
*/
|
||||
return true;
|
||||
}
|
||||
|
||||
if (reader.location.isUpdated()) {
|
||||
|
||||
auto loc = reader.location.value();
|
||||
latitude = toDegInt(loc.lat);
|
||||
longitude = toDegInt(loc.lng);
|
||||
|
||||
// Some GPSes (Air530) seem to send a zero longitude when the current fix is bogus
|
||||
if(longitude == 0)
|
||||
DEBUG_MSG("Ignoring bogus NMEA position\n");
|
||||
else {
|
||||
foundLocation = true;
|
||||
|
||||
// expect gps pos lat=37.520825, lon=-122.309162, alt=158
|
||||
DEBUG_MSG("new NMEA GPS pos lat=%f, lon=%f, alt=%d, hdop=%g, heading=%f\n", latitude * 1e-7, longitude * 1e-7, altitude,
|
||||
dop * 1e-2, heading * 1e-5);
|
||||
}
|
||||
bool NMEAGPS::hasLock()
|
||||
{
|
||||
// Using GPGGA fix quality indicator
|
||||
if (fixQual >= 1 && fixQual <= 5) {
|
||||
#ifndef TINYGPS_OPTION_NO_CUSTOM_FIELDS
|
||||
// Use GPGSA fix type 2D/3D (better) if available
|
||||
if (fixType == 3 || fixType == 0) // zero means "no data received"
|
||||
#endif
|
||||
return true;
|
||||
}
|
||||
|
||||
return foundLocation;
|
||||
return false;
|
||||
}
|
||||
|
||||
|
||||
bool NMEAGPS::whileIdle()
|
||||
{
|
||||
bool isValid = false;
|
||||
|
||||
@ -12,6 +12,15 @@
|
||||
class NMEAGPS : public GPS
|
||||
{
|
||||
TinyGPSPlus reader;
|
||||
uint8_t fixQual = 0; // fix quality from GPGGA
|
||||
|
||||
#ifndef TINYGPS_OPTION_NO_CUSTOM_FIELDS
|
||||
// (20210908) TinyGps++ can only read the GPGSA "FIX TYPE" field
|
||||
// via optional feature "custom fields", currently disabled (bug #525)
|
||||
TinyGPSCustom gsafixtype; // custom extract fix type from GPGSA
|
||||
TinyGPSCustom gsapdop; // custom extract PDOP from GPGSA
|
||||
uint8_t fixType = 0; // fix type from GPGSA
|
||||
#endif
|
||||
|
||||
public:
|
||||
virtual bool setupGPS();
|
||||
@ -38,4 +47,6 @@ class NMEAGPS : public GPS
|
||||
* @return true if we've acquired a new location
|
||||
*/
|
||||
virtual bool lookForLocation();
|
||||
|
||||
virtual bool hasLock();
|
||||
};
|
||||
|
||||
@ -8,6 +8,8 @@
|
||||
// if gps_update_interval below this value, do not powercycle the GPS
|
||||
#define UBLOX_POWEROFF_THRESHOLD 90
|
||||
|
||||
#define PDOP_INVALID 9999
|
||||
|
||||
extern RadioConfig radioConfig;
|
||||
|
||||
UBloxGPS::UBloxGPS() {}
|
||||
@ -116,19 +118,18 @@ bool UBloxGPS::factoryReset()
|
||||
/** Idle processing while GPS is looking for lock */
|
||||
void UBloxGPS::whileActive()
|
||||
{
|
||||
ublox.flushPVT(); // reset ALL freshness flags first
|
||||
|
||||
ublox.getT(maxWait()); // ask for new time data - hopefully ready when we come back
|
||||
|
||||
// Ask for a new position fix - hopefully it will have results ready by next time
|
||||
// the order here is important, because we only check for has latitude when reading
|
||||
ublox.getSIV(maxWait());
|
||||
ublox.getPDOP(maxWait());
|
||||
ublox.getP(maxWait());
|
||||
|
||||
// Update fixtype
|
||||
if (ublox.moduleQueried.fixType) {
|
||||
fixType = ublox.getFixType(0);
|
||||
// DEBUG_MSG("GPS fix type %d, numSats %d\n", fixType, numSatellites);
|
||||
}
|
||||
//ublox.getSIV(maxWait()); // redundant with getPDOP below
|
||||
ublox.getPDOP(maxWait()); // will trigger getSOL on NEO6, getP on others
|
||||
ublox.getP(maxWait()); // will trigger getPosLLH on NEO6, getP on others
|
||||
|
||||
// the fixType flag will be checked and updated in lookForLocation()
|
||||
}
|
||||
|
||||
/**
|
||||
@ -169,28 +170,78 @@ bool UBloxGPS::lookForLocation()
|
||||
{
|
||||
bool foundLocation = false;
|
||||
|
||||
if (ublox.moduleQueried.SIV)
|
||||
setNumSatellites(ublox.getSIV(0));
|
||||
// catch fixType changes here, instead of whileActive()
|
||||
if (ublox.moduleQueried.fixType) {
|
||||
fixType = ublox.getFixType();
|
||||
}
|
||||
|
||||
if (ublox.moduleQueried.pDOP)
|
||||
dop = ublox.getPDOP(0); // PDOP (an accuracy metric) is reported in 10^2 units so we have to scale down when we use it
|
||||
// check if GPS has an acceptable lock
|
||||
if (! hasLock()) {
|
||||
return false;
|
||||
}
|
||||
|
||||
// we only notify if position has changed due to a new fix
|
||||
if ((fixType >= 3 && fixType <= 4)) {
|
||||
if (ublox.moduleQueried.latitude) // rd fixes only
|
||||
{
|
||||
latitude = ublox.getLatitude(0);
|
||||
longitude = ublox.getLongitude(0);
|
||||
altitude = ublox.getAltitudeMSL(0) / 1000; // in mm convert to meters
|
||||
// check if a complete GPS solution set is available for reading
|
||||
// (some of these, like lat/lon are redundant and can be removed)
|
||||
if ( ! (ublox.moduleQueried.latitude &&
|
||||
ublox.moduleQueried.longitude &&
|
||||
ublox.moduleQueried.altitude &&
|
||||
ublox.moduleQueried.pDOP &&
|
||||
ublox.moduleQueried.gpsiTOW))
|
||||
{
|
||||
// Not ready? No problem! We'll try again later.
|
||||
return false;
|
||||
}
|
||||
|
||||
// Note: heading is only currently implmented in the ublox for the 8m chipset - therefore
|
||||
// don't read it here - it will generate an ignored getPVT command on the 6ms
|
||||
// heading = ublox.getHeading(0);
|
||||
// read lat/lon/alt/dop data into temporary variables to avoid
|
||||
// overwriting global variables with potentially invalid data
|
||||
int32_t tmp_dop = ublox.getPDOP(0); // PDOP (an accuracy metric) is reported in 10^2 units so we have to scale down when we use it
|
||||
int32_t tmp_lat = ublox.getLatitude(0);
|
||||
int32_t tmp_lon = ublox.getLongitude(0);
|
||||
int32_t tmp_alt_msl = ublox.getAltitudeMSL(0);
|
||||
int32_t tmp_alt_hae = ublox.getAltitude(0);
|
||||
// Note: heading is only currently implmented in the ublox for the 8m chipset - therefore
|
||||
// don't read it here - it will generate an ignored getPVT command on the 6ms
|
||||
// heading = ublox.getHeading(0);
|
||||
|
||||
// bogus lat lon is reported as 0 or 0 (can be bogus just for one)
|
||||
// Also: apparently when the GPS is initially reporting lock it can output a bogus latitude > 90 deg!
|
||||
foundLocation = (latitude != 0) && (longitude != 0) && (latitude <= 900000000 && latitude >= -900000000);
|
||||
}
|
||||
// read positional timestamp
|
||||
struct tm t;
|
||||
t.tm_sec = ublox.getSecond(0);
|
||||
t.tm_min = ublox.getMinute(0);
|
||||
t.tm_hour = ublox.getHour(0);
|
||||
t.tm_mday = ublox.getDay(0);
|
||||
t.tm_mon = ublox.getMonth(0) - 1;
|
||||
t.tm_year = ublox.getYear(0) - 1900;
|
||||
t.tm_isdst = false;
|
||||
|
||||
time_t tmp_ts = mktime(&t);
|
||||
|
||||
// SIV number is nice-to-have if it's available
|
||||
if (ublox.moduleQueried.SIV) {
|
||||
uint16_t gSIV = ublox.getSIV(0);
|
||||
setNumSatellites(gSIV);
|
||||
}
|
||||
|
||||
// bogus lat lon is reported as 0 or 0 (can be bogus just for one)
|
||||
// Also: apparently when the GPS is initially reporting lock it can output a bogus latitude > 90 deg!
|
||||
// FIXME - NULL ISLAND is a real location on Earth!
|
||||
foundLocation = (tmp_lat != 0) && (tmp_lon != 0) &&
|
||||
(tmp_lat <= 900000000) && (tmp_lat >= -900000000) &&
|
||||
(tmp_dop < PDOP_INVALID);
|
||||
|
||||
// only if entire dataset is valid, update globals from temp vars
|
||||
if (foundLocation) {
|
||||
longitude = tmp_lon;
|
||||
latitude = tmp_lat;
|
||||
#ifdef GPS_ALTITUDE_HAE
|
||||
altitude = tmp_alt_hae / 1000;
|
||||
#else
|
||||
altitude = tmp_alt_msl / 1000;
|
||||
#endif
|
||||
geoidal_height = (tmp_alt_hae - tmp_alt_msl) / 1000;
|
||||
pos_timestamp = tmp_ts;
|
||||
dop = tmp_dop;
|
||||
} else {
|
||||
DEBUG_MSG("Invalid location discarded\n");
|
||||
}
|
||||
|
||||
return foundLocation;
|
||||
|
||||
Loading…
Reference in New Issue
Block a user