diff --git a/src/gps/GPS.cpp b/src/gps/GPS.cpp index ec7d725b8..8eb7fef27 100644 --- a/src/gps/GPS.cpp +++ b/src/gps/GPS.cpp @@ -9,6 +9,7 @@ #include "main.h" // pmu_found #include "sleep.h" +#include "GPSUpdateScheduling.h" #include "cas.h" #include "ubx.h" @@ -22,19 +23,6 @@ #define GPS_RESET_MODE HIGH #endif -// How many minutes of sleep make it worthwhile to power-off the GPS -// Shorter than this, and GPS will only enter standby -// Affected by lock-time, and config.position.gps_update_interval -#ifndef GPS_STANDBY_THRESHOLD_MINUTES -#define GPS_STANDBY_THRESHOLD_MINUTES 15 -#endif - -// How many seconds of sleep make it worthwhile for the GPS to use powered-on standby -// Shorter than this, and we'll just wait instead -#ifndef GPS_IDLE_THRESHOLD_SECONDS -#define GPS_IDLE_THRESHOLD_SECONDS 10 -#endif - #if defined(NRF52840_XXAA) || defined(NRF52833_XXAA) || defined(ARCH_ESP32) || defined(ARCH_PORTDUINO) HardwareSerial *GPS::_serial_gps = &Serial1; #else @@ -43,6 +31,8 @@ HardwareSerial *GPS::_serial_gps = NULL; GPS *gps = nullptr; +GPSUpdateScheduling scheduling; + /// Multiple GPS instances might use the same serial port (in sequence), but we can /// only init that port once. static bool didSerialInit; @@ -52,6 +42,25 @@ uint8_t uBloxProtocolVersion; #define GPS_SOL_EXPIRY_MS 5000 // in millis. give 1 second time to combine different sentences. NMEA Frequency isn't higher anyway #define NMEA_MSG_GXGSA "GNGSA" // GSA message (GPGSA, GNGSA etc) +// For logging +const char *getGPSPowerStateString(GPSPowerState state) +{ + switch (state) { + case GPS_ACTIVE: + return "ACTIVE"; + case GPS_IDLE: + return "IDLE"; + case GPS_SOFTSLEEP: + return "SOFTSLEEP"; + case GPS_HARDSLEEP: + return "HARDSLEEP"; + case GPS_OFF: + return "OFF"; + default: + assert(false); // Unhandled enum value.. + } +} + void GPS::UBXChecksum(uint8_t *message, size_t length) { uint8_t CK_A = 0, CK_B = 0; @@ -767,7 +776,6 @@ bool GPS::setup() } notifyDeepSleepObserver.observe(¬ifyDeepSleep); - notifyGPSSleepObserver.observe(¬ifyGPSSleep); return true; } @@ -776,124 +784,185 @@ GPS::~GPS() { // we really should unregister our sleep observer notifyDeepSleepObserver.unobserve(¬ifyDeepSleep); - notifyGPSSleepObserver.observe(¬ifyGPSSleep); } -const char *GPS::powerStateToString() +// Put the GPS hardware into a specified state +void GPS::setPowerState(GPSPowerState newState, uint32_t sleepTime) { - switch (powerState) { - case GPS_OFF: - return "OFF"; - case GPS_IDLE: - return "IDLE"; - case GPS_STANDBY: - return "STANDBY"; + // Update the stored GPSPowerstate, and create local copies + GPSPowerState oldState = powerState; + powerState = newState; + LOG_INFO("GPS power state moving from %s to %s\n", getGPSPowerStateString(oldState), getGPSPowerStateString(newState)); + + switch (newState) { case GPS_ACTIVE: - return "ACTIVE"; - default: - return "UNKNOWN"; + case GPS_IDLE: + if (oldState == GPS_ACTIVE || oldState == GPS_IDLE) // If hardware already awake, no changes needed + break; + if (oldState != GPS_ACTIVE && oldState != GPS_IDLE) // If hardware just waking now, clear buffer + clearBuffer(); + powerMon->setState(meshtastic_PowerMon_State_GPS_Active); // Report change for power monitoring (during testing) + writePinEN(true); // Power (EN pin): on + setPowerPMU(true); // Power (PMU): on + writePinStandby(false); // Standby (pin): awake (not standby) + setPowerUBLOX(true); // Standby (UBLOX): awake + break; + + case GPS_SOFTSLEEP: + powerMon->clearState(meshtastic_PowerMon_State_GPS_Active); // Report change for power monitoring (during testing) + writePinEN(true); // Power (EN pin): on + setPowerPMU(true); // Power (PMU): on + writePinStandby(true); // Standby (pin): asleep (not awake) + setPowerUBLOX(false, sleepTime); // Standby (UBLOX): asleep, timed + break; + + case GPS_HARDSLEEP: + powerMon->clearState(meshtastic_PowerMon_State_GPS_Active); // Report change for power monitoring (during testing) + writePinEN(false); // Power (EN pin): off + setPowerPMU(false); // Power (PMU): off + writePinStandby(true); // Standby (pin): asleep (not awake) + setPowerUBLOX(false, sleepTime); // Standby (UBLOX): asleep, timed + break; + + case GPS_OFF: + assert(sleepTime == 0); // This is an indefinite sleep + powerMon->clearState(meshtastic_PowerMon_State_GPS_Active); // Report change for power monitoring (during testing) + writePinEN(false); // Power (EN pin): off + setPowerPMU(false); // Power (PMU): off + writePinStandby(true); // Standby (pin): asleep + setPowerUBLOX(false, 0); // Standby (UBLOX): asleep, indefinitely + break; } } -void GPS::setGPSPower(bool on, bool standbyOnly, uint32_t sleepTime) +// Set power with EN pin, if relevant +void GPS::writePinEN(bool on) { - // Record the current powerState - if (on) - powerState = GPS_ACTIVE; - else if (!enabled) // User has disabled with triple press - powerState = GPS_OFF; - else if (sleepTime <= GPS_IDLE_THRESHOLD_SECONDS * 1000UL) - powerState = GPS_IDLE; - else if (standbyOnly) - powerState = GPS_STANDBY; - else - powerState = GPS_OFF; - - LOG_DEBUG("GPS::powerState=%s\n", powerStateToString()); - - // If the next update is due *really soon*, don't actually power off or enter standby. Just wait it out. - if (!on && powerState == GPS_IDLE) + // Abort: if conflict with Canned Messages when using Wisblock(?) + if (HW_VENDOR == meshtastic_HardwareModel_RAK4631 && (rotaryEncoderInterruptImpl1 || upDownInterruptImpl1)) return; - if (on) { - powerMon->setState(meshtastic_PowerMon_State_GPS_Active); - clearBuffer(); // drop any old data waiting in the buffer before re-enabling - if (en_gpio) - digitalWrite(en_gpio, on ? GPS_EN_ACTIVE : !GPS_EN_ACTIVE); // turn this on if defined, every time - } else { - powerMon->clearState(meshtastic_PowerMon_State_GPS_Active); - } - isInPowersave = !on; - if (!standbyOnly && en_gpio != 0 && - !(HW_VENDOR == meshtastic_HardwareModel_RAK4631 && (rotaryEncoderInterruptImpl1 || upDownInterruptImpl1))) { - LOG_DEBUG("GPS powerdown using GPS_EN_ACTIVE\n"); - digitalWrite(en_gpio, on ? GPS_EN_ACTIVE : !GPS_EN_ACTIVE); + // Abort: if pin unset + if (!en_gpio) return; - } -#ifdef HAS_PMU // We only have PMUs on the T-Beam, and that board has a tiny battery to save GPS ephemera, so treat as a standby. - if (pmu_found && PMU) { - uint8_t model = PMU->getChipModel(); - if (model == XPOWERS_AXP2101) { - if (HW_VENDOR == meshtastic_HardwareModel_TBEAM) { - // t-beam v1.2 GNSS power channel - on ? PMU->enablePowerOutput(XPOWERS_ALDO3) : PMU->disablePowerOutput(XPOWERS_ALDO3); - } else if (HW_VENDOR == meshtastic_HardwareModel_LILYGO_TBEAM_S3_CORE) { - // t-beam-s3-core GNSS power channel - on ? PMU->enablePowerOutput(XPOWERS_ALDO4) : PMU->disablePowerOutput(XPOWERS_ALDO4); - } - } else if (model == XPOWERS_AXP192) { - // t-beam v1.1 GNSS power channel - on ? PMU->enablePowerOutput(XPOWERS_LDO3) : PMU->disablePowerOutput(XPOWERS_LDO3); - } - return; - } + + // Determine new value for the pin + bool val = GPS_EN_ACTIVE ? on : !on; + + // Write and log + pinMode(en_gpio, OUTPUT); + digitalWrite(en_gpio, val); +#ifdef GPS_EXTRAVERBOSE + LOG_DEBUG("Pin EN %s\n", val == HIGH ? "HIGH" : "LOW"); #endif +} + +// Set the value of the STANDBY pin, if relevant +// true for standby state, false for awake +void GPS::writePinStandby(bool standby) +{ #ifdef PIN_GPS_STANDBY // Specifically the standby pin for L76B, L76K and clones - if (on) { - LOG_INFO("Waking GPS\n"); - pinMode(PIN_GPS_STANDBY, OUTPUT); - // Some PCB's use an inverse logic due to a transistor driver - // Example for this is the Pico-Waveshare Lora+GPS HAT -#ifdef PIN_GPS_STANDBY_INVERTED - digitalWrite(PIN_GPS_STANDBY, 0); + +// Determine the new value for the pin +// Normally: active HIGH for awake +#if PIN_GPS_STANDBY_INVERTED + bool val = standby; #else - digitalWrite(PIN_GPS_STANDBY, 1); + bool val = !standby; #endif - return; - } else { - LOG_INFO("GPS entering sleep\n"); - // notifyGPSSleep.notifyObservers(NULL); - pinMode(PIN_GPS_STANDBY, OUTPUT); -#ifdef PIN_GPS_STANDBY_INVERTED - digitalWrite(PIN_GPS_STANDBY, 1); -#else - digitalWrite(PIN_GPS_STANDBY, 0); + + // Write and log + pinMode(PIN_GPS_STANDBY, OUTPUT); + digitalWrite(PIN_GPS_STANDBY, val); +#ifdef GPS_EXTRAVERBOSE + LOG_DEBUG("Pin STANDBY %s\n", val == HIGH ? "HIGH" : "LOW"); #endif +#endif +} + +// Enable / Disable GPS with PMU, if present +void GPS::setPowerPMU(bool on) +{ + // We only have PMUs on the T-Beam, and that board has a tiny battery to save GPS ephemera, + // so treat as a standby. +#ifdef HAS_PMU + // Abort: if no PMU + if (!pmu_found) return; + + // Abort: if PMU not initialized + if (!PMU) + return; + + uint8_t model = PMU->getChipModel(); + if (model == XPOWERS_AXP2101) { + if (HW_VENDOR == meshtastic_HardwareModel_TBEAM) { + // t-beam v1.2 GNSS power channel + on ? PMU->enablePowerOutput(XPOWERS_ALDO3) : PMU->disablePowerOutput(XPOWERS_ALDO3); + } else if (HW_VENDOR == meshtastic_HardwareModel_LILYGO_TBEAM_S3_CORE) { + // t-beam-s3-core GNSS power channel + on ? PMU->enablePowerOutput(XPOWERS_ALDO4) : PMU->disablePowerOutput(XPOWERS_ALDO4); + } + } else if (model == XPOWERS_AXP192) { + // t-beam v1.1 GNSS power channel + on ? PMU->enablePowerOutput(XPOWERS_LDO3) : PMU->disablePowerOutput(XPOWERS_LDO3); } + +#ifdef GPS_EXTRAVERBOSE + LOG_DEBUG("PMU %s\n", on ? "on" : "off"); #endif - if (!on) { - if (gnssModel == GNSS_MODEL_UBLOX) { - uint8_t msglen; - LOG_DEBUG("Sleep Time: %i\n", sleepTime); - if (strncmp(info.hwVersion, "000A0000", 8) != 0) { - for (int i = 0; i < 4; i++) { - gps->_message_PMREQ[0 + i] = sleepTime >> (i * 8); // Encode the sleep time in millis into the packet - } - msglen = gps->makeUBXPacket(0x02, 0x41, sizeof(_message_PMREQ), gps->_message_PMREQ); - } else { - for (int i = 0; i < 4; i++) { - gps->_message_PMREQ_10[4 + i] = sleepTime >> (i * 8); // Encode the sleep time in millis into the packet - } - msglen = gps->makeUBXPacket(0x02, 0x41, sizeof(_message_PMREQ_10), gps->_message_PMREQ_10); - } - gps->_serial_gps->write(gps->UBXscratch, msglen); +#endif +} + +// Set UBLOX power, if relevant +void GPS::setPowerUBLOX(bool on, uint32_t sleepMs) +{ + // Abort: if not UBLOX hardware + if (gnssModel != GNSS_MODEL_UBLOX) + return; + + // If waking + if (on) { + gps->_serial_gps->write(0xFF); + clearBuffer(); // This often returns old data, so drop it +#ifdef GPS_EXTRAVERBOSE + LOG_DEBUG("UBLOX: wake\n"); +#endif + } + + // If putting to sleep + else { + uint8_t msglen; + + // If we're being asked to sleep indefinitely, make *sure* we're awake first, to process the new sleep command + if (sleepMs == 0) { + setPowerUBLOX(true); + delay(500); } - } else { - if (gnssModel == GNSS_MODEL_UBLOX) { - gps->_serial_gps->write(0xFF); - clearBuffer(); // This often returns old data, so drop it + + // Determine hardware version + if (strncmp(info.hwVersion, "000A0000", 8) != 0) { + // Encode the sleep time in millis into the packet + for (int i = 0; i < 4; i++) + gps->_message_PMREQ[0 + i] = sleepMs >> (i * 8); + + // Record the message length + msglen = gps->makeUBXPacket(0x02, 0x41, sizeof(_message_PMREQ), gps->_message_PMREQ); + } else { + // Encode the sleep time in millis into the packet + for (int i = 0; i < 4; i++) + gps->_message_PMREQ_10[4 + i] = sleepMs >> (i * 8); + + // Record the message length + msglen = gps->makeUBXPacket(0x02, 0x41, sizeof(_message_PMREQ_10), gps->_message_PMREQ_10); } + + // Send the UBX packet + gps->_serial_gps->write(gps->UBXscratch, msglen); + +#ifdef GPS_EXTRAVERBOSE + LOG_DEBUG("UBLOX: sleep for %dmS\n", sleepMs); +#endif } } @@ -906,108 +975,54 @@ void GPS::setConnected() } } -/** - * Switch the GPS into a mode where we are actively looking for a lock, or alternatively switch GPS into a low power mode - * - * calls sleep/wake - */ -void GPS::setAwake(bool wantAwake) +// We want a GPS lock. Wake the hardware +void GPS::up() { + scheduling.informSearching(); + setPowerState(GPS_ACTIVE); +} - // If user has disabled GPS, make sure it is off, not just in standby or idle - if (!wantAwake && !enabled && powerState != GPS_OFF) { - setGPSPower(false, false, 0); - return; - } +// We've got a GPS lock. Enter a low power state, potentially. +void GPS::down() +{ + scheduling.informGotLock(); + uint32_t predictedSearchDuration = scheduling.predictedSearchDurationMs(); + uint32_t sleepTime = scheduling.msUntilNextSearch(); + uint32_t updateInterval = Default::getConfiguredOrDefaultMs(config.position.gps_update_interval); - // If GPS power state needs to change - if ((wantAwake && powerState != GPS_ACTIVE) || (!wantAwake && powerState == GPS_ACTIVE)) { - LOG_DEBUG("WANT GPS=%d\n", wantAwake); + LOG_DEBUG("%us until next search\n", sleepTime / 1000); - // Calculate how long it takes to get a GPS lock - if (wantAwake) { - // Record the time we start looking for a lock - lastWakeStartMsec = millis(); - } else { - // Record by how much we missed our ideal target postion.gps_update_interval (for logging only) - // Need to calculate this before we update lastSleepStartMsec, to make the new prediction - int32_t lateByMsec = (int32_t)(millis() - lastSleepStartMsec) - (int32_t)getSleepTime(); + // If update interval less than 10 seconds, no attempt to sleep + if (updateInterval <= 10 * 1000UL) + setPowerState(GPS_IDLE); - // Record the time we finish looking for a lock - lastSleepStartMsec = millis(); - - // How long did it take to get GPS lock this time? - uint32_t lockTime = lastSleepStartMsec - lastWakeStartMsec; - - // Update the lock-time prediction - // Used pre-emptively, attempting to hit target of gps.position_update_interval - switch (GPSCycles) { - case 0: - LOG_DEBUG("Initial GPS lock took %ds\n", lockTime / 1000); - break; - case 1: - predictedLockTime = lockTime; // Avoid slow ramp-up - start with a real value - LOG_DEBUG("GPS Lock took %ds\n", lockTime / 1000); - break; - default: - // Predict lock-time using exponential smoothing: respond slowly to changes - predictedLockTime = (lockTime * 0.2) + (predictedLockTime * 0.8); // Latest lock time has 20% weight on prediction - LOG_INFO("GPS Lock took %ds. %s by %ds. Next lock predicted to take %ds.\n", lockTime / 1000, - (lateByMsec > 0) ? "Late" : "Early", abs(lateByMsec) / 1000, predictedLockTime / 1000); - } - GPSCycles++; - } - - // How long to wait before attempting next GPS update - // Aims to hit position.gps_update_interval by using the lock-time prediction - uint32_t compensatedSleepTime = (getSleepTime() > predictedLockTime) ? (getSleepTime() - predictedLockTime) : 0; - - // If long interval between updates: power off between updates - if (compensatedSleepTime > GPS_STANDBY_THRESHOLD_MINUTES * MS_IN_MINUTE) { - setGPSPower(wantAwake, false, getSleepTime() - predictedLockTime); - } - - // If waking relatively frequently: don't power off. Would use more energy trying to reacquire lock each time - // We'll either use a "powered-on" standby, or just wait it out, depending on how soon the next update is due - // Will decide which inside setGPSPower method - else { -#ifdef GPS_UC6580 - setGPSPower(wantAwake, false, compensatedSleepTime); -#else - setGPSPower(wantAwake, true, compensatedSleepTime); + else { + // Check whether the GPS hardware is capable of GPS_SOFTSLEEP + // If not, fallback to GPS_HARDSLEEP instead + bool softsleepSupported = false; + if (gnssModel == GNSS_MODEL_UBLOX) // U-blox is supported via PMREQ + softsleepSupported = true; +#ifdef PIN_GPS_STANDBY // L76B, L76K and clones have a standby pin + softsleepSupported = true; #endif - } + + // How long does gps_update_interval need to be, for GPS_HARDSLEEP to become more efficient than GPS_SOFTSLEEP? + // Heuristic equation. A compromise manually fitted to power observations from U-blox NEO-6M and M10050 + // https://www.desmos.com/calculator/6gvjghoumr + // This is not particularly accurate, but probably an impromevement over a single, fixed threshold + uint32_t hardsleepThreshold = (2750 * pow(predictedSearchDuration / 1000, 1.22)); + LOG_DEBUG("gps_update_interval >= %us needed to justify hardsleep\n", hardsleepThreshold / 1000); + + // If update interval too short: softsleep (if supported by hardware) + if (softsleepSupported && updateInterval < hardsleepThreshold) + setPowerState(GPS_SOFTSLEEP, sleepTime); + + // If update interval long enough (or softsleep unsupported): hardsleep instead + else + setPowerState(GPS_HARDSLEEP, sleepTime); } } -/** Get how long we should stay looking for each acquisition in msecs - */ -uint32_t GPS::getWakeTime() const -{ - uint32_t t = config.position.position_broadcast_secs; - - if (t == UINT32_MAX) - return t; // already maxint - - return Default::getConfiguredOrDefaultMs(t, default_broadcast_interval_secs); -} - -/** Get how long we should sleep between aqusition attempts in msecs - */ -uint32_t GPS::getSleepTime() const -{ - uint32_t t = config.position.gps_update_interval; - - // We'll not need the GPS thread to wake up again after first acq. with fixed position. - if (config.position.gps_mode != meshtastic_Config_PositionConfig_GpsMode_ENABLED || config.position.fixed_position) - t = UINT32_MAX; // Sleep forever now - - if (t == UINT32_MAX) - return t; // already maxint - - return Default::getConfiguredOrDefaultMs(t, default_gps_update_interval); -} - void GPS::publishUpdate() { if (shouldPublish) { @@ -1056,13 +1071,13 @@ int32_t GPS::runOnce() return disable(); } - if (whileIdle()) { + if (whileActive()) { // if we have received valid NMEA claim we are connected setConnected(); } else { if ((config.position.gps_mode == meshtastic_Config_PositionConfig_GpsMode_ENABLED) && (gnssModel == GNSS_MODEL_UBLOX)) { // reset the GPS on next bootup - if (devicestate.did_gps_reset && (millis() - lastWakeStartMsec > 60000) && !hasFlow()) { + if (devicestate.did_gps_reset && scheduling.elapsedSearchMs() > 60 * 1000UL && !hasFlow()) { LOG_DEBUG("GPS is not communicating, trying factory reset on next bootup.\n"); devicestate.did_gps_reset = false; nodeDB->saveDeviceStateToDisk(); @@ -1077,54 +1092,43 @@ int32_t GPS::runOnce() // gps->factoryReset(); } - // If we are overdue for an update, turn on the GPS and at least publish the current status - uint32_t now = millis(); - uint32_t timeAsleep = now - lastSleepStartMsec; + // If we're due for an update, wake the GPS + if (!config.position.fixed_position && powerState != GPS_ACTIVE && scheduling.isUpdateDue()) + up(); - auto sleepTime = getSleepTime(); - if (powerState != GPS_ACTIVE && (sleepTime != UINT32_MAX) && - ((timeAsleep > sleepTime) || (isInPowersave && timeAsleep > (sleepTime - predictedLockTime)))) { - // We now want to be awake - so wake up the GPS - setAwake(true); + // If we've already set time from the GPS, no need to ask the GPS + bool gotTime = (getRTCQuality() >= RTCQualityGPS); + if (!gotTime && lookForTime()) { // Note: we count on this && short-circuiting and not resetting the RTC time + gotTime = true; + shouldPublish = true; } - // While we are awake - if (powerState == GPS_ACTIVE) { - // LOG_DEBUG("looking for location\n"); - // If we've already set time from the GPS, no need to ask the GPS - bool gotTime = (getRTCQuality() >= RTCQualityGPS); - if (!gotTime && lookForTime()) { // Note: we count on this && short-circuiting and not resetting the RTC time - gotTime = true; - shouldPublish = true; - } + bool gotLoc = lookForLocation(); + if (gotLoc && !hasValidLocation) { // declare that we have location ASAP + LOG_DEBUG("hasValidLocation RISING EDGE\n"); + hasValidLocation = true; + shouldPublish = true; + } - bool gotLoc = lookForLocation(); - if (gotLoc && !hasValidLocation) { // declare that we have location ASAP - LOG_DEBUG("hasValidLocation RISING EDGE\n"); - hasValidLocation = true; - shouldPublish = true; - } + bool tooLong = scheduling.searchedTooLong(); + if (tooLong) + LOG_WARN("Couldn't publish a valid location: didn't get a GPS lock in time.\n"); - now = millis(); - auto wakeTime = getWakeTime(); - bool tooLong = wakeTime != UINT32_MAX && (now - lastWakeStartMsec) > wakeTime; + // Once we get a location we no longer desperately want an update + // LOG_DEBUG("gotLoc %d, tooLong %d, gotTime %d\n", gotLoc, tooLong, gotTime); + if ((gotLoc && gotTime) || tooLong) { - // Once we get a location we no longer desperately want an update - // LOG_DEBUG("gotLoc %d, tooLong %d, gotTime %d\n", gotLoc, tooLong, gotTime); - if ((gotLoc && gotTime) || tooLong) { - - if (tooLong) { - // we didn't get a location during this ack window, therefore declare loss of lock - if (hasValidLocation) { - LOG_DEBUG("hasValidLocation FALLING EDGE (last read: %d)\n", gotLoc); - } - p = meshtastic_Position_init_default; - hasValidLocation = false; + if (tooLong) { + // we didn't get a location during this ack window, therefore declare loss of lock + if (hasValidLocation) { + LOG_DEBUG("hasValidLocation FALLING EDGE\n"); } - - setAwake(false); - shouldPublish = true; // publish our update for this just finished acquisition window + p = meshtastic_Position_init_default; + hasValidLocation = false; } + + down(); + shouldPublish = true; // publish our update for this just finished acquisition window } // If state has changed do a publish @@ -1150,9 +1154,7 @@ void GPS::clearBuffer() int GPS::prepareDeepSleep(void *unused) { LOG_INFO("GPS deep sleep!\n"); - - setAwake(false); - + disable(); return 0; } @@ -1348,12 +1350,6 @@ GPS *GPS::createGps() new_gps->tx_gpio = _tx_gpio; new_gps->en_gpio = _en_gpio; - if (_en_gpio != 0) { - LOG_DEBUG("Setting %d to output.\n", _en_gpio); - pinMode(_en_gpio, OUTPUT); - digitalWrite(_en_gpio, !GPS_EN_ACTIVE); - } - #ifdef PIN_GPS_PPS // pulse per second pinMode(PIN_GPS_PPS, INPUT); @@ -1368,7 +1364,8 @@ GPS *GPS::createGps() LOG_DEBUG("Using " NMEA_MSG_GXGSA " for 3DFIX and PDOP\n"); #endif - new_gps->setGPSPower(true, false, 0); + // Make sure the GPS is awake before performing any init. + new_gps->up(); #ifdef PIN_GPS_RESET pinMode(PIN_GPS_RESET, OUTPUT); @@ -1376,7 +1373,6 @@ GPS *GPS::createGps() delay(10); digitalWrite(PIN_GPS_RESET, !GPS_RESET_MODE); #endif - new_gps->setAwake(true); // Wake GPS power before doing any init if (_serial_gps) { #ifdef ARCH_ESP32 @@ -1662,13 +1658,13 @@ bool GPS::hasFlow() return reader.passedChecksum() > 0; } -bool GPS::whileIdle() +bool GPS::whileActive() { unsigned int charsInBuf = 0; bool isValid = false; if (powerState != GPS_ACTIVE) { clearBuffer(); - return (powerState == GPS_ACTIVE); + return false; } #ifdef SERIAL_BUFFER_SIZE if (_serial_gps->available() >= SERIAL_BUFFER_SIZE - 1) { @@ -1699,20 +1695,21 @@ bool GPS::whileIdle() } void GPS::enable() { - // Clear the old lock-time prediction - GPSCycles = 0; - predictedLockTime = 0; + // Clear the old scheduling info (reset the lock-time prediction) + scheduling.reset(); enabled = true; setInterval(GPS_THREAD_INTERVAL); - setAwake(true); + + scheduling.informSearching(); + setPowerState(GPS_ACTIVE); } int32_t GPS::disable() { enabled = false; setInterval(INT32_MAX); - setAwake(false); + setPowerState(GPS_OFF); return INT32_MAX; } @@ -1721,11 +1718,11 @@ void GPS::toggleGpsMode() { if (config.position.gps_mode == meshtastic_Config_PositionConfig_GpsMode_ENABLED) { config.position.gps_mode = meshtastic_Config_PositionConfig_GpsMode_DISABLED; - LOG_DEBUG("Flag set to false for gps power. GpsMode: DISABLED\n"); + LOG_INFO("User toggled GpsMode. Now DISABLED.\n"); disable(); } else if (config.position.gps_mode == meshtastic_Config_PositionConfig_GpsMode_DISABLED) { config.position.gps_mode = meshtastic_Config_PositionConfig_GpsMode_ENABLED; - LOG_DEBUG("Flag set to true to restore power. GpsMode: ENABLED\n"); + LOG_INFO("User toggled GpsMode. Now ENABLED\n"); enable(); } } diff --git a/src/gps/GPS.h b/src/gps/GPS.h index 6afbd4fab..7cbf771bc 100644 --- a/src/gps/GPS.h +++ b/src/gps/GPS.h @@ -39,10 +39,11 @@ typedef enum { } GPS_RESPONSE; enum GPSPowerState : uint8_t { - GPS_OFF = 0, // Physically powered off - GPS_ACTIVE = 1, // Awake and want a position - GPS_STANDBY = 2, // Physically powered on, but soft-sleeping - GPS_IDLE = 3, // Awake, but not wanting another position yet + GPS_ACTIVE, // Awake and want a position + GPS_IDLE, // Awake, but not wanting another position yet + GPS_SOFTSLEEP, // Physically powered on, but soft-sleeping + GPS_HARDSLEEP, // Physically powered off, but scheduled to wake + GPS_OFF // Powered off indefinitely }; // Generate a string representation of DOP @@ -67,14 +68,11 @@ class GPS : private concurrency::OSThread uint8_t fixType = 0; // fix type from GPGSA #endif private: - uint32_t lastWakeStartMsec = 0, lastSleepStartMsec = 0; const int serialSpeeds[6] = {9600, 4800, 38400, 57600, 115200, 9600}; uint32_t rx_gpio = 0; uint32_t tx_gpio = 0; uint32_t en_gpio = 0; - uint32_t predictedLockTime = 0; - uint32_t GPSCycles = 0; int speedSelect = 0; int probeTries = 2; @@ -99,7 +97,6 @@ class GPS : private concurrency::OSThread uint8_t numSatellites = 0; CallbackObserver notifyDeepSleepObserver = CallbackObserver(this, &GPS::prepareDeepSleep); - CallbackObserver notifyGPSSleepObserver = CallbackObserver(this, &GPS::prepareDeepSleep); public: /** If !NULL we will use this serial port to construct our GPS */ @@ -175,7 +172,8 @@ class GPS : private concurrency::OSThread // toggle between enabled/disabled void toggleGpsMode(); - void setGPSPower(bool on, bool standbyOnly, uint32_t sleepTime); + // Change the power state of the GPS - for power saving / shutdown + void setPowerState(GPSPowerState newState, uint32_t sleepMs = 0); /// Returns true if we have acquired GPS lock. virtual bool hasLock(); @@ -206,18 +204,18 @@ class GPS : private concurrency::OSThread GPS_RESPONSE getACKCas(uint8_t class_id, uint8_t msg_id, uint32_t waitMillis); - /** - * Switch the GPS into a mode where we are actively looking for a lock, or alternatively switch GPS into a low power mode - * - * calls sleep/wake - */ - void setAwake(bool on); virtual bool factoryReset(); // Creates an instance of the GPS class. // Returns the new instance or null if the GPS is not present. static GPS *createGps(); + // Wake the GPS hardware - ready for an update + void up(); + + // Let the GPS hardware save power between updates + void down(); + protected: /** * Perform any processing that should be done only while the GPS is awake and looking for a fix. @@ -240,7 +238,7 @@ class GPS : private concurrency::OSThread * * Return true if we received a valid message from the GPS */ - virtual bool whileIdle(); + virtual bool whileActive(); /** * Perform any processing that should be done only while the GPS is awake and looking for a fix. @@ -267,13 +265,21 @@ class GPS : private concurrency::OSThread void UBXChecksum(uint8_t *message, size_t length); void CASChecksum(uint8_t *message, size_t length); - /** Get how long we should stay looking for each aquisition + /** Set power with EN pin, if relevant */ - uint32_t getWakeTime() const; + void writePinEN(bool on); - /** Get how long we should sleep between aqusition attempts + /** Set the value of the STANDBY pin, if relevant */ - uint32_t getSleepTime() const; + void writePinStandby(bool standby); + + /** Set GPS power with PMU, if relevant + */ + void setPowerPMU(bool on); + + /** Set UBLOX power, if relevant + */ + void setPowerUBLOX(bool on, uint32_t sleepMs = 0); /** * Tell users we have new GPS readings diff --git a/src/gps/GPSUpdateScheduling.cpp b/src/gps/GPSUpdateScheduling.cpp new file mode 100644 index 000000000..949ef6039 --- /dev/null +++ b/src/gps/GPSUpdateScheduling.cpp @@ -0,0 +1,118 @@ +#include "GPSUpdateScheduling.h" + +#include "Default.h" + +// Mark the time when searching for GPS position begins +void GPSUpdateScheduling::informSearching() +{ + searchStartedMs = millis(); +} + +// Mark the time when searching for GPS is complete, +// then update the predicted lock-time +void GPSUpdateScheduling::informGotLock() +{ + searchEndedMs = millis(); + LOG_DEBUG("Took %us to get lock\n", (searchEndedMs - searchStartedMs) / 1000); + updateLockTimePrediction(); +} + +// Clear old lock-time prediction data. +// When re-enabling GPS with user button. +void GPSUpdateScheduling::reset() +{ + searchStartedMs = 0; + searchEndedMs = 0; + searchCount = 0; + predictedMsToGetLock = 0; +} + +// How many milliseconds before we should next search for GPS position +// Used by GPS hardware directly, to enter timed hardware sleep +uint32_t GPSUpdateScheduling::msUntilNextSearch() +{ + uint32_t now = millis(); + + // Target interval (seconds), between GPS updates + uint32_t updateInterval = Default::getConfiguredOrDefaultMs(config.position.gps_update_interval, default_gps_update_interval); + + // Check how long until we should start searching, to hopefully hit our target interval + uint32_t dueAtMs = searchEndedMs + updateInterval; + uint32_t compensatedStart = dueAtMs - predictedMsToGetLock; + int32_t remainingMs = compensatedStart - now; + + // If we should have already started (negative value), start ASAP + if (remainingMs < 0) + remainingMs = 0; + + return (uint32_t)remainingMs; +} + +// How long have we already been searching? +// Used to abort a search in progress, if it runs unnaceptably long +uint32_t GPSUpdateScheduling::elapsedSearchMs() +{ + // If searching + if (searchStartedMs > searchEndedMs) + return millis() - searchStartedMs; + + // If not searching - 0ms. We shouldn't really consume this value + else + return 0; +} + +// Is it now time to begin searching for a GPS position? +bool GPSUpdateScheduling::isUpdateDue() +{ + return (msUntilNextSearch() == 0); +} + +// Have we been searching for a GPS position for too long? +bool GPSUpdateScheduling::searchedTooLong() +{ + uint32_t maxSearchMs = + Default::getConfiguredOrDefaultMs(config.position.position_broadcast_secs, default_broadcast_interval_secs); + + // If broadcast interval set to max, no such thing as "too long" + if (maxSearchMs == UINT32_MAX) + return false; + + // If we've been searching longer than our position broadcast interval: that's too long + else if (elapsedSearchMs() > maxSearchMs) + return true; + + // Otherwise, not too long yet! + else + return false; +} + +// Updates the predicted time-to-get-lock, by exponentially smoothing the latest observation +void GPSUpdateScheduling::updateLockTimePrediction() +{ + + // How long did it take to get GPS lock this time? + // Duration between down() calls + int32_t lockTime = searchEndedMs - searchStartedMs; + if (lockTime < 0) + lockTime = 0; + + // Ignore the first lock-time: likely to be long, will skew data + + // Second locktime: likely stable. Use to intialize the smoothing filter + if (searchCount == 1) + predictedMsToGetLock = lockTime; + + // Third locktime and after: predict using exponential smoothing. Respond slowly to changes + else if (searchCount > 1) + predictedMsToGetLock = (lockTime * weighting) + (predictedMsToGetLock * (1 - weighting)); + + searchCount++; // Only tracked so we can diregard initial lock-times + + LOG_DEBUG("Predicting %us to get next lock\n", predictedMsToGetLock / 1000); +} + +// How long do we expect to spend searching for a lock? +uint32_t GPSUpdateScheduling::predictedSearchDurationMs() +{ + return GPSUpdateScheduling::predictedMsToGetLock; +} \ No newline at end of file diff --git a/src/gps/GPSUpdateScheduling.h b/src/gps/GPSUpdateScheduling.h new file mode 100644 index 000000000..7e121c9b6 --- /dev/null +++ b/src/gps/GPSUpdateScheduling.h @@ -0,0 +1,29 @@ +#pragma once + +#include "configuration.h" + +// Encapsulates code responsible for the timing of GPS updates +class GPSUpdateScheduling +{ + public: + // Marks the time of these events, for calculation use + void informSearching(); + void informGotLock(); // Predicted lock-time is recalculated here + + void reset(); // Reset the prediction - after GPS::disable() / GPS::enable() + bool isUpdateDue(); // Is it time to begin searching for a GPS position? + bool searchedTooLong(); // Have we been searching for too long? + + uint32_t msUntilNextSearch(); // How long until we need to begin searching for a GPS? Info provided to GPS hardware for sleep + uint32_t elapsedSearchMs(); // How long have we been searching so far? + uint32_t predictedSearchDurationMs(); // How long do we expect to spend searching for a lock? + + private: + void updateLockTimePrediction(); // Called from informGotLock + uint32_t searchStartedMs = 0; + uint32_t searchEndedMs = 0; + uint32_t searchCount = 0; + uint32_t predictedMsToGetLock = 0; + + const float weighting = 0.2; // Controls exponential smoothing of lock-times prediction. 20% weighting of "latest lock-time". +}; \ No newline at end of file diff --git a/src/sleep.cpp b/src/sleep.cpp index 721c7d188..3793ee0cf 100644 --- a/src/sleep.cpp +++ b/src/sleep.cpp @@ -37,10 +37,7 @@ Observable preflightSleep; /// Called to tell observers we are now entering sleep and you should prepare. Must return 0 /// notifySleep will be called for light or deep sleep, notifyDeepSleep is only called for deep sleep -/// notifyGPSSleep will be called when config.position.gps_enabled is set to 0 or from buttonthread when GPS_POWER_TOGGLE is -/// enabled. Observable notifySleep, notifyDeepSleep; -Observable notifyGPSSleep; // deep sleep support RTC_DATA_ATTR int bootCount = 0; @@ -240,11 +237,6 @@ void doDeepSleep(uint32_t msecToWake, bool skipPreflight = false) #ifdef PIN_POWER_EN pinMode(PIN_POWER_EN, INPUT); // power off peripherals // pinMode(PIN_POWER_EN1, INPUT_PULLDOWN); -#endif -#if HAS_GPS - // Kill GPS power completely (even if previously we just had it in sleep mode) - if (gps) - gps->setGPSPower(false, false, 0); #endif setLed(false); diff --git a/src/sleep.h b/src/sleep.h index 8d5b9a94f..f154b8d44 100644 --- a/src/sleep.h +++ b/src/sleep.h @@ -41,8 +41,6 @@ extern Observable notifySleep; /// Called to tell observers we are now entering (deep) sleep and you should prepare. Must return 0 extern Observable notifyDeepSleep; -/// Called to tell GPS thread to enter deep sleep independently of LoRa/MCU sleep, prior to full poweroff. Must return 0 -extern Observable notifyGPSSleep; void enableModemSleep(); #ifdef ARCH_ESP32 void enableLoraInterrupt();