From 50db2d0e9b2a119e5c359c87b3474c023e13a847 Mon Sep 17 00:00:00 2001
From: Jonathan Bennett <jbennett@incomsystems.biz>
Date: Sun, 1 Oct 2023 20:39:18 -0500
Subject: [PATCH] Add timeout to ublox PMREQ command (#2851)

---
 src/gps/GPS.cpp | 16 ++++++++++------
 src/gps/GPS.h   |  4 ++--
 src/gps/ubx.h   |  2 +-
 src/sleep.cpp   |  2 +-
 4 files changed, 14 insertions(+), 10 deletions(-)

diff --git a/src/gps/GPS.cpp b/src/gps/GPS.cpp
index 6a0b5e3dd..87cda5c4b 100644
--- a/src/gps/GPS.cpp
+++ b/src/gps/GPS.cpp
@@ -433,7 +433,7 @@ GPS::~GPS()
     notifyGPSSleepObserver.observe(&notifyGPSSleep);
 }
 
-void GPS::setGPSPower(bool on, bool standbyOnly)
+void GPS::setGPSPower(bool on, bool standbyOnly, uint32_t sleepTime)
 {
     LOG_INFO("Setting GPS power=%d\n", on);
     if (on) {
@@ -483,6 +483,10 @@ void GPS::setGPSPower(bool on, bool standbyOnly)
     if (!on) {
         if (gnssModel == GNSS_MODEL_UBLOX) {
             uint8_t msglen;
+            LOG_DEBUG("Sleep Time: %i\n", sleepTime);
+            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, 0x08, gps->_message_PMREQ);
             gps->_serial_gps->write(gps->UBXscratch, msglen);
         }
@@ -514,7 +518,7 @@ void GPS::setAwake(bool on)
         LOG_DEBUG("WANT GPS=%d\n", on);
         isAwake = on;
         if (!enabled) { // short circuit if the user has disabled GPS
-            setGPSPower(false, false);
+            setGPSPower(false, false, 0);
             return;
         }
 
@@ -532,12 +536,12 @@ void GPS::setAwake(bool on)
         }
         if ((int32_t)getSleepTime() - averageLockTime >
             15 * 60 * 1000) { // 15 minutes is probably long enough to make a complete poweroff worth it.
-            setGPSPower(on, false);
+            setGPSPower(on, false, getSleepTime() - averageLockTime);
         } else if ((int32_t)getSleepTime() - averageLockTime > 10000) { // 10 seconds is enough for standby
 #ifdef GPS_UC6580
-            setGPSPower(on, false);
+            setGPSPower(on, false, getSleepTime() - averageLockTime);
 #else
-            setGPSPower(on, true);
+            setGPSPower(on, true, getSleepTime() - averageLockTime);
 #endif
         } else if (averageLockTime > 20000) {
             averageLockTime -= 1000; // eventually want to sleep again.
@@ -899,7 +903,7 @@ GPS *GPS::createGps()
     LOG_DEBUG("Using " NMEA_MSG_GXGSA " for 3DFIX and PDOP\n");
 #endif
 
-    new_gps->setGPSPower(true, false);
+    new_gps->setGPSPower(true, false, 0);
 
 #ifdef PIN_GPS_RESET
     digitalWrite(PIN_GPS_RESET, GPS_RESET_MODE); // assert for 10ms
diff --git a/src/gps/GPS.h b/src/gps/GPS.h
index 4269acf2d..bdd0b31a7 100644
--- a/src/gps/GPS.h
+++ b/src/gps/GPS.h
@@ -94,7 +94,7 @@ class GPS : private concurrency::OSThread
     /** If !NULL we will use this serial port to construct our GPS */
     static HardwareSerial *_serial_gps;
 
-    static const uint8_t _message_PMREQ[];
+    static uint8_t _message_PMREQ[];
     static const uint8_t _message_CFG_RXM_PSM[];
     static const uint8_t _message_CFG_RXM_ECO[];
     static const uint8_t _message_CFG_PM2[];
@@ -132,7 +132,7 @@ class GPS : private concurrency::OSThread
     // Disable the thread
     int32_t disable() override;
 
-    void setGPSPower(bool on, bool standbyOnly);
+    void setGPSPower(bool on, bool standbyOnly, uint32_t sleepTime);
 
     /// Returns true if we have acquired GPS lock.
     virtual bool hasLock();
diff --git a/src/gps/ubx.h b/src/gps/ubx.h
index 6493b2ce7..2be2d1d33 100644
--- a/src/gps/ubx.h
+++ b/src/gps/ubx.h
@@ -1,4 +1,4 @@
-const uint8_t GPS::_message_PMREQ[] PROGMEM = {
+uint8_t GPS::_message_PMREQ[] PROGMEM = {
     0x00, 0x00, // 4 bytes duration of request task
     0x00, 0x00, // (milliseconds)
     0x02, 0x00, // Task flag bitfield
diff --git a/src/sleep.cpp b/src/sleep.cpp
index d9fb1ba50..9d0468e70 100644
--- a/src/sleep.cpp
+++ b/src/sleep.cpp
@@ -175,7 +175,7 @@ void doDeepSleep(uint32_t msecToWake, bool skipPreflight = false)
     nodeDB.saveToDisk();
 
     // Kill GPS power completely (even if previously we just had it in sleep mode)
-    gps->setGPSPower(false, false);
+    gps->setGPSPower(false, false, 0);
 
     setLed(false);