pohui_pohui/firmware
1
0
Fork 0
mirror of https://github.com/meshtastic/firmware.git synced 2025-02-03 19:29:57 +00:00
Code Issues Actions 6 Packages Projects Releases Wiki Activity

Merge pull request #896 from a-f-G-U-C/gps-use-pos-struct

Browse Source 
use Position struct for GPS data
This commit is contained in:
Sacha Weatherstone 2021-10-25 07:41:09 +11:00 committed by GitHub
commit 2f7b58abaf
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
6 changed files with 123 additions and 62 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

26
src/GPSStatus.h
View File

@ -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);
bool isDirty = matches(newStatus);
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);
}

11
src/gps/GPS.cpp
View File

@ -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;
}

13
src/gps/GPS.h
View File

@ -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") {}

48
src/gps/NMEAGPS.cpp
View File

@ -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());

54
src/gps/UBloxGPS.cpp
View File

@ -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",

27
src/mesh/MeshService.cpp
View File

@ -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);