From 134fc75b67acefca55732445dfb995a70feb4669 Mon Sep 17 00:00:00 2001
From: code8buster <20384924+code8buster@users.noreply.github.com>
Date: Tue, 5 Sep 2023 00:26:42 -0400
Subject: [PATCH] UBX-RXM-PMREQ soft-off implemented

---
 src/gps/GPS.cpp | 31 ++++++++++++++++++++++++++++++-
 src/gps/GPS.h   | 10 +++++++++-
 src/sleep.cpp   | 13 +++++++++++++
 3 files changed, 52 insertions(+), 2 deletions(-)

diff --git a/src/gps/GPS.cpp b/src/gps/GPS.cpp
index e0b3e2e01..d2ebb6894 100644
--- a/src/gps/GPS.cpp
+++ b/src/gps/GPS.cpp
@@ -28,11 +28,19 @@ GPS *gps;
 /// Multiple GPS instances might use the same serial port (in sequence), but we can
 /// only init that port once.
 static bool didSerialInit;
+uint8_t UBXscratch[250] = {0};
 
 struct uBloxGnssModelInfo info;
 uint8_t uBloxProtocolVersion;
 
-void GPS::UBXChecksum(byte *message, size_t length)
+const uint8_t GPS::_message_PMREQ[] PROGMEM = {
+    0x00, 0x00, // 4 bytes duration of request task
+    0x00, 0x00, // (milliseconds)
+    0x02, 0x00, // Task flag bitfield
+    0x00, 0x00, // byte index 1 = sleep mode
+};
+
+void GPS::UBXChecksum(uint8_t *message, size_t length)
 {
     uint8_t CK_A = 0, CK_B = 0;
 
@@ -47,6 +55,27 @@ void GPS::UBXChecksum(byte *message, size_t length)
     message[length - 1] = CK_B;
 }
 
+// Function to create a ublox packet for editing in memory
+uint8_t GPS::makeUBXPacket(uint8_t class_id, uint8_t msg_id, uint8_t payload_size, const uint8_t *msg)
+{
+    // Construct the UBX packet
+    UBXscratch[0] = 0xB5;         // header
+    UBXscratch[1] = 0x62;         // header
+    UBXscratch[2] = class_id;     // class
+    UBXscratch[3] = msg_id;       // id
+    UBXscratch[4] = payload_size; // length
+    UBXscratch[5] = 0x00;
+
+    UBXscratch[6 + payload_size] = 0x00; // CK_A
+    UBXscratch[7 + payload_size] = 0x00; // CK_B
+
+    for (int i = 0; i < payload_size; i++) {
+        UBXscratch[6 + i] = pgm_read_byte(&msg[i]);
+    }
+    UBXChecksum(UBXscratch, (payload_size + 8));
+    return (payload_size + 8);
+}
+
 GPS_RESPONSE GPS::getACK(const char *message, uint32_t waitMillis)
 {
     uint8_t buffer[768] = {0};
diff --git a/src/gps/GPS.h b/src/gps/GPS.h
index 89ae7a916..eb5459ff2 100644
--- a/src/gps/GPS.h
+++ b/src/gps/GPS.h
@@ -62,6 +62,8 @@ 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[8];
+
     meshtastic_Position p = meshtastic_Position_init_default;
 
     GPS() : concurrency::OSThread("GPS") {}
@@ -101,6 +103,9 @@ class GPS : private concurrency::OSThread
     // Empty the input buffer as quickly as possible
     void clearBuffer();
 
+    // Create a ublox packet for editing in memory
+    uint8_t makeUBXPacket(uint8_t class_id, uint8_t msg_id, uint8_t payload_size, const uint8_t *msg);
+
   protected:
     /// Do gps chipset specific init, return true for success
     virtual bool setupGPS();
@@ -141,6 +146,9 @@ class GPS : private concurrency::OSThread
 
     void setNumSatellites(uint8_t n);
 
+    // scratch space for creating ublox packets
+    uint8_t UBXscratch[250];
+
   private:
     /// Prepare the GPS for the cpu entering deep or light sleep, expect to be gone for at least 100s of msecs
     /// always returns 0 to indicate okay to sleep
@@ -151,7 +159,7 @@ class GPS : private concurrency::OSThread
     int prepareDeepSleep(void *unused);
 
     // Calculate checksum
-    void UBXChecksum(byte *message, size_t length);
+    void UBXChecksum(uint8_t *message, size_t length);
 
     /**
      * Switch the GPS into a mode where we are actively looking for a lock, or alternatively switch GPS into a low power mode
diff --git a/src/sleep.cpp b/src/sleep.cpp
index 3d6da7feb..891cce228 100644
--- a/src/sleep.cpp
+++ b/src/sleep.cpp
@@ -204,6 +204,19 @@ void doGPSpowersave(bool on)
         notifyGPSSleep.notifyObservers(NULL);
     }
 #endif
+#if !(defined(HAS_PMU) || defined(PIN_GPS_EN) || defined(PIN_GPS_WAKE))
+    if (!on) {
+        uint8_t msglen;
+        notifyGPSSleep.notifyObservers(NULL);
+        msglen = gps->makeUBXPacket(0x02, 0x41, 0x08, gps->_message_PMREQ);
+        for (int i = 0; i < msglen; i++) {
+            gps->_serial_gps->write(gps->UBXscratch, msglen);
+        }
+    } else {
+        gps->forceWake(1);
+        gps->_serial_gps->write(0xFF);
+    }
+#endif
 }
 
 void doDeepSleep(uint32_t msecToWake)