sforkowany z mirror/meshtastic-firmware
Merge pull request #896 from a-f-G-U-C/gps-use-pos-struct
use Position struct for GPS data1.2-legacy
commit
2f7b58abaf
|
@ -99,6 +99,10 @@ class GPSStatus : public Status
|
|||
|
||||
bool matches(const GPSStatus *newStatus) const
|
||||
{
|
||||
#if GPS_EXTRAVERBOSE
|
||||
DEBUG_MSG("GPSStatus.match() new pos@%x to old pos@%x\n",
|
||||
newStatus->p.pos_timestamp, p.pos_timestamp);
|
||||
#endif
|
||||
return (newStatus->hasLock != hasLock ||
|
||||
newStatus->isConnected != isConnected ||
|
||||
newStatus->p.latitude_i != p.latitude_i ||
|
||||
|
@ -109,25 +113,33 @@ class GPSStatus : public Status
|
|||
newStatus->p.ground_track != p.ground_track ||
|
||||
newStatus->p.sats_in_view != p.sats_in_view);
|
||||
}
|
||||
|
||||
int updateStatus(const GPSStatus *newStatus)
|
||||
{
|
||||
// Only update the status if values have actually changed
|
||||
bool isDirty;
|
||||
{
|
||||
isDirty = matches(newStatus);
|
||||
initialized = true;
|
||||
hasLock = newStatus->hasLock;
|
||||
isConnected = newStatus->isConnected;
|
||||
bool isDirty = matches(newStatus);
|
||||
|
||||
p = newStatus->p;
|
||||
if (isDirty && p.pos_timestamp &&
|
||||
(newStatus->p.pos_timestamp == p.pos_timestamp)) {
|
||||
// We can NEVER be in two locations at the same time! (also PR #886)
|
||||
DEBUG_MSG("BUG!! positional timestamp unchanged from prev solution\n");
|
||||
}
|
||||
|
||||
initialized = true;
|
||||
hasLock = newStatus->hasLock;
|
||||
isConnected = newStatus->isConnected;
|
||||
|
||||
p = newStatus->p;
|
||||
|
||||
if (isDirty) {
|
||||
if (hasLock)
|
||||
DEBUG_MSG("New GPS pos lat=%f, lon=%f, alt=%d, pdop=%.2f, heading=%.2f, sats=%d\n",
|
||||
if (hasLock) {
|
||||
// In debug logs, identify position by @timestamp:stage (stage 3 = notify)
|
||||
DEBUG_MSG("New GPS pos@%x:3 lat=%f, lon=%f, alt=%d, pdop=%.2f, track=%.2f, sats=%d\n",
|
||||
p.pos_timestamp,
|
||||
p.latitude_i * 1e-7, p.longitude_i * 1e-7,
|
||||
p.altitude, p.PDOP * 1e-2, p.ground_track * 1e-5,
|
||||
p.sats_in_view);
|
||||
else
|
||||
} else
|
||||
DEBUG_MSG("No GPS lock\n");
|
||||
onNewStatus.notifyObservers(this);
|
||||
}
|
||||
|
|
|
@ -202,11 +202,13 @@ void GPS::publishUpdate()
|
|||
if (shouldPublish) {
|
||||
shouldPublish = false;
|
||||
|
||||
DEBUG_MSG("publishing GPS lock=%d\n", hasLock());
|
||||
// In debug logs, identify position by @timestamp:stage (stage 2 = publish)
|
||||
DEBUG_MSG("publishing pos@%x:2, hasVal=%d, GPSlock=%d\n",
|
||||
p.pos_timestamp, hasValidLocation, hasLock());
|
||||
|
||||
// Notify any status instances that are observing us
|
||||
const meshtastic::GPSStatus status =
|
||||
meshtastic::GPSStatus(hasValidLocation, isConnected(), latitude, longitude, altitude, dop, heading, numSatellites);
|
||||
meshtastic::GPSStatus(hasValidLocation, isConnected(), p);
|
||||
newStatus.notifyObservers(&status);
|
||||
}
|
||||
}
|
||||
|
@ -244,6 +246,7 @@ int32_t GPS::runOnce()
|
|||
|
||||
bool gotLoc = lookForLocation();
|
||||
if (gotLoc && !hasValidLocation) { // declare that we have location ASAP
|
||||
DEBUG_MSG("hasValidLocation RISING EDGE\n");
|
||||
hasValidLocation = true;
|
||||
shouldPublish = true;
|
||||
}
|
||||
|
@ -260,6 +263,10 @@ int32_t GPS::runOnce()
|
|||
|
||||
if (tooLong) {
|
||||
// we didn't get a location during this ack window, therefore declare loss of lock
|
||||
if (hasValidLocation) {
|
||||
DEBUG_MSG("hasValidLocation FALLING EDGE (last read: %d)\n", gotLoc);
|
||||
}
|
||||
p = Position_init_default;
|
||||
hasValidLocation = false;
|
||||
}
|
||||
|
||||
|
|
|
@ -17,6 +17,10 @@ class GPS : private concurrency::OSThread
|
|||
private:
|
||||
uint32_t lastWakeStartMsec = 0, lastSleepStartMsec = 0, lastWhileActiveMsec = 0;
|
||||
|
||||
/**
|
||||
* hasValidLocation - indicates that the position variables contain a complete
|
||||
* GPS location, valid and fresh (< gps_update_interval + gps_attempt_time)
|
||||
*/
|
||||
bool hasValidLocation = false; // default to false, until we complete our first read
|
||||
|
||||
bool isAwake = false; // true if we want a location right now
|
||||
|
@ -39,14 +43,7 @@ class GPS : private concurrency::OSThread
|
|||
/** If !0 we will attempt to connect to the GPS over I2C */
|
||||
static uint8_t i2cAddress;
|
||||
|
||||
int32_t latitude = 0, longitude = 0; // as an int mult by 1e-7 to get value as double
|
||||
int32_t altitude = 0;
|
||||
uint32_t dop = 0; // Diminution of position; PDOP where possible (UBlox), HDOP otherwise (TinyGPS) in 10^2 units (needs
|
||||
// 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
|
||||
Position p = Position_init_default;
|
||||
|
||||
GPS() : concurrency::OSThread("GPS") {}
|
||||
|
||||
|
|
|
@ -95,6 +95,17 @@ bool NMEAGPS::lookForLocation()
|
|||
if (! hasLock())
|
||||
return false;
|
||||
|
||||
#ifdef GPS_EXTRAVERBOSE
|
||||
DEBUG_MSG("AGE: LOC=%d FIX=%d DATE=%d TIME=%d\n",
|
||||
reader.location.age(),
|
||||
#ifndef TINYGPS_OPTION_NO_CUSTOM_FIELDS
|
||||
gsafixtype.age(),
|
||||
#else
|
||||
0,
|
||||
#endif
|
||||
reader.date.age(), reader.time.age());
|
||||
#endif // GPS_EXTRAVERBOSE
|
||||
|
||||
// check if a complete GPS solution set is available for reading
|
||||
// tinyGPSDatum::age() also includes isValid() test
|
||||
// FIXME
|
||||
|
@ -105,7 +116,7 @@ bool NMEAGPS::lookForLocation()
|
|||
(reader.time.age() < GPS_SOL_EXPIRY_MS) &&
|
||||
(reader.date.age() < GPS_SOL_EXPIRY_MS)))
|
||||
{
|
||||
// DEBUG_MSG("SOME data is TOO OLD\n");
|
||||
DEBUG_MSG("SOME data is TOO OLD\n");
|
||||
return false;
|
||||
}
|
||||
|
||||
|
@ -113,7 +124,7 @@ bool NMEAGPS::lookForLocation()
|
|||
if (! reader.location.isUpdated())
|
||||
return false;
|
||||
|
||||
// Start reading the data
|
||||
// We know the solution is fresh and valid, so just read the data
|
||||
auto loc = reader.location.value();
|
||||
|
||||
// Some GPSes (Air530) seem to send a zero longitude when the current fix is bogus
|
||||
|
@ -123,27 +134,34 @@ bool NMEAGPS::lookForLocation()
|
|||
return false;
|
||||
}
|
||||
|
||||
p.location_source = Position_LocSource_LOCSRC_GPS_INTERNAL;
|
||||
|
||||
// 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());
|
||||
p.HDOP = reader.hdop.value();
|
||||
p.PDOP = TinyGPSPlus::parseDecimal(gsapdop.value());
|
||||
DEBUG_MSG("PDOP=%d, HDOP=%d\n", dop, reader.hdop.value());
|
||||
#else
|
||||
// FIXME! naive PDOP emulation (assumes VDOP==HDOP)
|
||||
// correct formula is PDOP = SQRT(HDOP^2 + VDOP^2)
|
||||
dop = 1.41 * reader.hdop.value();
|
||||
p.HDOP = reader.hdop.value();
|
||||
p.PDOP = 1.41 * reader.hdop.value();
|
||||
#endif
|
||||
|
||||
// Discard incomplete or erroneous readings
|
||||
if (dop == 0)
|
||||
if (reader.hdop.value() == 0)
|
||||
return false;
|
||||
|
||||
latitude = toDegInt(loc.lat);
|
||||
longitude = toDegInt(loc.lng);
|
||||
p.latitude_i = toDegInt(loc.lat);
|
||||
p.longitude_i = toDegInt(loc.lng);
|
||||
|
||||
geoidal_height = reader.geoidHeight.meters();
|
||||
#ifdef GPS_ALTITUDE_HAE
|
||||
altitude = reader.altitude.meters() + geoidal_height;
|
||||
#else
|
||||
altitude = reader.altitude.meters();
|
||||
p.alt_geoid_sep = reader.geoidHeight.meters();
|
||||
p.altitude_hae = reader.altitude.meters() + p.alt_geoid_sep;
|
||||
p.altitude = reader.altitude.meters();
|
||||
|
||||
p.fix_quality = fixQual;
|
||||
#ifndef TINYGPS_OPTION_NO_CUSTOM_FIELDS
|
||||
p.fix_type = fixType;
|
||||
#endif
|
||||
|
||||
// positional timestamp
|
||||
|
@ -155,16 +173,16 @@ bool NMEAGPS::lookForLocation()
|
|||
t.tm_mon = reader.date.month() - 1;
|
||||
t.tm_year = reader.date.year() - 1900;
|
||||
t.tm_isdst = false;
|
||||
pos_timestamp = mktime(&t);
|
||||
p.pos_timestamp = mktime(&t);
|
||||
|
||||
// Nice to have, if available
|
||||
if (reader.satellites.isUpdated()) {
|
||||
setNumSatellites(reader.satellites.value());
|
||||
p.sats_in_view = reader.satellites.value();
|
||||
}
|
||||
|
||||
if (reader.course.isUpdated() && reader.course.isValid()) {
|
||||
if (reader.course.value() < 36000) { // sanity check
|
||||
heading = reader.course.value() * 1e3; // Scale the heading (in degrees * 10^-2) to match the expected degrees * 10^-5
|
||||
p.ground_track = reader.course.value() * 1e3; // Scale the heading (in degrees * 10^-2) to match the expected degrees * 10^-5
|
||||
} else {
|
||||
DEBUG_MSG("BOGUS course.value() REJECTED: %d\n",
|
||||
reader.course.value());
|
||||
|
|
|
@ -10,6 +10,8 @@
|
|||
|
||||
#define PDOP_INVALID 9999
|
||||
|
||||
// #define UBX_MODE_NMEA
|
||||
|
||||
extern RadioConfig radioConfig;
|
||||
|
||||
UBloxGPS::UBloxGPS() {}
|
||||
|
@ -48,12 +50,22 @@ bool UBloxGPS::setupGPS()
|
|||
delay(500);
|
||||
|
||||
if (isConnected()) {
|
||||
#ifdef UBX_MODE_NMEA
|
||||
DEBUG_MSG("Connected to UBLOX GPS, downgrading to NMEA mode\n");
|
||||
DEBUG_MSG("- GPS errors below are related and safe to ignore\n");
|
||||
#else
|
||||
DEBUG_MSG("Connected to UBLOX GPS successfully\n");
|
||||
#endif
|
||||
|
||||
if (!setUBXMode())
|
||||
RECORD_CRITICALERROR(CriticalErrorCode_UBloxInitFailed); // Don't halt the boot if saving the config fails, but do report the bug
|
||||
|
||||
#ifdef UBX_MODE_NMEA
|
||||
return false;
|
||||
#else
|
||||
return true;
|
||||
#endif
|
||||
|
||||
} else {
|
||||
return false;
|
||||
}
|
||||
|
@ -61,6 +73,17 @@ bool UBloxGPS::setupGPS()
|
|||
|
||||
bool UBloxGPS::setUBXMode()
|
||||
{
|
||||
#ifdef UBX_MODE_NMEA
|
||||
if (_serial_gps) {
|
||||
ublox.setUART1Output(COM_TYPE_NMEA, 1000);
|
||||
}
|
||||
if (i2cAddress) {
|
||||
ublox.setI2COutput(COM_TYPE_NMEA, 1000);
|
||||
}
|
||||
|
||||
return false; // pretend initialization failed to force NMEA mode
|
||||
#endif
|
||||
|
||||
if (_serial_gps) {
|
||||
if (!ublox.setUART1Output(COM_TYPE_UBX, 1000)) // Use native API
|
||||
return false;
|
||||
|
@ -119,7 +142,6 @@ bool UBloxGPS::factoryReset()
|
|||
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
|
||||
|
@ -177,6 +199,7 @@ bool UBloxGPS::lookForLocation()
|
|||
ublox.moduleQueried.longitude &&
|
||||
ublox.moduleQueried.altitude &&
|
||||
ublox.moduleQueried.pDOP &&
|
||||
ublox.moduleQueried.SIV &&
|
||||
ublox.moduleQueried.gpsDay))
|
||||
{
|
||||
// Not ready? No problem! We'll try again later.
|
||||
|
@ -188,6 +211,7 @@ bool UBloxGPS::lookForLocation()
|
|||
DEBUG_MSG("FixType=%d\n", fixType);
|
||||
#endif
|
||||
|
||||
|
||||
// check if GPS has an acceptable lock
|
||||
if (! hasLock()) {
|
||||
ublox.flushPVT(); // reset ALL freshness flags
|
||||
|
@ -217,11 +241,7 @@ bool UBloxGPS::lookForLocation()
|
|||
|
||||
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);
|
||||
}
|
||||
// FIXME - can opportunistically attempt to set RTC from GPS timestamp?
|
||||
|
||||
// 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!
|
||||
|
@ -232,16 +252,18 @@ bool UBloxGPS::lookForLocation()
|
|||
|
||||
// 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;
|
||||
p.location_source = Position_LocSource_LOCSRC_GPS_INTERNAL;
|
||||
p.longitude_i = tmp_lon;
|
||||
p.latitude_i = tmp_lat;
|
||||
p.altitude = tmp_alt_msl / 1000;
|
||||
p.altitude_hae = tmp_alt_hae / 1000;
|
||||
p.alt_geoid_sep = (tmp_alt_hae - tmp_alt_msl) / 1000;
|
||||
p.pos_timestamp = tmp_ts;
|
||||
p.PDOP = tmp_dop;
|
||||
p.fix_type = fixType;
|
||||
p.sats_in_view = ublox.getSIV(0);
|
||||
// In debug logs, identify position by @timestamp:stage (stage 1 = birth)
|
||||
DEBUG_MSG("lookForLocation() new pos@%x:1\n", tmp_ts);
|
||||
} else {
|
||||
// INVALID solution - should never happen
|
||||
DEBUG_MSG("Invalid location lat/lon/hae/dop %d/%d/%d/%d - discarded\n",
|
||||
|
|
|
@ -213,27 +213,32 @@ int MeshService::onGPSChanged(const meshtastic::GPSStatus *newStatus)
|
|||
{
|
||||
// Update our local node info with our position (even if we don't decide to update anyone else)
|
||||
NodeInfo *node = refreshMyNodeInfo();
|
||||
Position pos = node->position;
|
||||
Position pos = Position_init_default;
|
||||
|
||||
if (newStatus->getHasLock()) {
|
||||
if (gps->altitude != 0)
|
||||
pos.altitude = gps->altitude;
|
||||
pos.latitude_i = gps->latitude;
|
||||
pos.longitude_i = gps->longitude;
|
||||
// load data from GPS object, will add timestamp + battery further down
|
||||
pos = gps->p;
|
||||
} else {
|
||||
// The GPS has lost lock, if we are fixed position we should just keep using
|
||||
// the old position
|
||||
#if GPS_EXTRAVERBOSE
|
||||
DEBUG_MSG("onGPSchanged() - lost validLocation\n");
|
||||
#endif
|
||||
if (radioConfig.preferences.fixed_position) {
|
||||
DEBUG_MSG("WARNING: Using fixed position\n");
|
||||
} else {
|
||||
// throw away old position
|
||||
pos.latitude_i = 0;
|
||||
pos.longitude_i = 0;
|
||||
pos.altitude = 0;
|
||||
pos = node->position;
|
||||
}
|
||||
}
|
||||
|
||||
DEBUG_MSG("got gps notify time=%u, lat=%d, bat=%d\n", pos.time, pos.latitude_i, pos.battery_level);
|
||||
// Finally add a fresh timestamp and battery level reading
|
||||
// I KNOW this is redundant with refreshMyNodeInfo() above, but these are
|
||||
// inexpensive nonblocking calls and can be refactored in due course
|
||||
pos.time = getValidTime(RTCQualityGPS);
|
||||
pos.battery_level = powerStatus->getBatteryChargePercent();
|
||||
|
||||
// In debug logs, identify position by @timestamp:stage (stage 4 = nodeDB)
|
||||
DEBUG_MSG("onGPSChanged() pos@%x:4, time=%u, lat=%d, bat=%d\n",
|
||||
pos.pos_timestamp, pos.time, pos.latitude_i, pos.battery_level);
|
||||
|
||||
// Update our current position in the local DB
|
||||
nodeDB.updatePosition(nodeDB.getNodeNum(), pos, RX_SRC_LOCAL);
|
||||
|
|
Ładowanie…
Reference in New Issue