firmware/src/gps/GPS.cpp

#include <cstring> // Include for strstr
#include <vector>

#include "configuration.h"
#if !MESHTASTIC_EXCLUDE_GPS
#include "Default.h"
#include "GPS.h"
#include "GpioLogic.h"
#include "NodeDB.h"
#include "PowerMon.h"
#include "RTC.h"
#include "Throttle.h"
#include "buzz.h"
#include "concurrency/Periodic.h"
#include "meshUtils.h"

#include "main.h" // pmu_found
#include "sleep.h"

#include "GPSUpdateScheduling.h"
#include "cas.h"
#include "ubx.h"

#ifdef ARCH_PORTDUINO
#include "PortduinoGlue.h"
#include "meshUtils.h"
#include <algorithm>
#include <ctime>
#endif

#ifndef GPS_RESET_MODE
#define GPS_RESET_MODE HIGH
#endif

// Not all platforms have std::size().
template <typename T, std::size_t N> std::size_t array_count(const T (&)[N])
{
    return N;
}

#ifndef GPS_SERIAL_PORT
#define GPS_SERIAL_PORT Serial1
#endif

#if defined(ARCH_NRF52)
Uart *GPS::_serial_gps = &GPS_SERIAL_PORT;
#elif defined(ARCH_ESP32) || defined(ARCH_PORTDUINO) || defined(ARCH_STM32WL)
HardwareSerial *GPS::_serial_gps = &GPS_SERIAL_PORT;
#elif defined(ARCH_RP2040)
SerialUART *GPS::_serial_gps = &GPS_SERIAL_PORT;
#else
HardwareSerial *GPS::_serial_gps = nullptr;
#endif

std::unique_ptr<GPS> gps = nullptr;

static GPSUpdateScheduling scheduling;

/// Multiple GPS instances might use the same serial port (in sequence), but we can
/// only init that port once.
static bool didSerialInit;

static struct uBloxGnssModelInfo {
    char swVersion[30];
    char hwVersion[10];
    uint8_t extensionNo;
    char extension[10][30];
    uint8_t protocol_version;
} ublox_info;

#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
static 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..
        return "FALSE"; // to make new ESP-IDF happy
    }
}

#ifdef PIN_GPS_SWITCH
// If we have a hardware switch, define a periodic watcher outside of the GPS runOnce thread, since this can be sleeping
// idefinitely

int lastState = LOW;
bool firstrun = true;

static int32_t gpsSwitch()
{
    if (gps) {
        int currentState = digitalRead(PIN_GPS_SWITCH);

        // if the switch is set to zero, disable the GPS Thread
        if (firstrun)
            if (currentState == LOW)
                lastState = HIGH;

        if (currentState != lastState) {
            if (currentState == LOW) {
                config.position.gps_mode = meshtastic_Config_PositionConfig_GpsMode_DISABLED;
                if (!firstrun)
                    playGPSDisableBeep();
                gps->disable();
            } else {
                config.position.gps_mode = meshtastic_Config_PositionConfig_GpsMode_ENABLED;
                if (!firstrun)
                    playGPSEnableBeep();
                gps->enable();
            }
            lastState = currentState;
        }
        firstrun = false;
    }
    return 1000;
}

static std::unique_ptr<concurrency::Periodic> gpsPeriodic;
#endif

static void UBXChecksum(uint8_t *message, size_t length)
{
    uint8_t CK_A = 0, CK_B = 0;

    // Calculate the checksum, starting from the CLASS field (which is message[2])
    for (size_t i = 2; i < length - 2; i++) {
        CK_A = (CK_A + message[i]) & 0xFF;
        CK_B = (CK_B + CK_A) & 0xFF;
    }

    // Place the calculated checksum values in the message
    message[length - 2] = CK_A;
    message[length - 1] = CK_B;
}

// Calculate the checksum for a CAS packet
static void CASChecksum(uint8_t *message, size_t length)
{
    uint32_t cksum = ((uint32_t)message[5] << 24); // Message ID
    cksum += ((uint32_t)message[4]) << 16;         // Class
    cksum += message[2];                           // Payload Len

    // Iterate over the payload as a series of uint32_t's and
    // accumulate the cksum
    for (size_t i = 0; i < (length - 10) / 4; i++) {
        uint32_t pl = 0;
        memcpy(&pl, (message + 6) + (i * sizeof(uint32_t)), sizeof(uint32_t)); // avoid pointer dereference
        cksum += pl;
    }

    // Place the checksum values in the message
    message[length - 4] = (cksum & 0xFF);
    message[length - 3] = (cksum & (0xFF << 8)) >> 8;
    message[length - 2] = (cksum & (0xFF << 16)) >> 16;
    message[length - 1] = (cksum & (0xFF << 24)) >> 24;
}

// 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);
}

// Function to create a CAS packet for editing in memory
uint8_t GPS::makeCASPacket(uint8_t class_id, uint8_t msg_id, uint8_t payload_size, const uint8_t *msg)
{
    // General CAS structure
    //        | H1   | H2   | payload_len | cls  | msg  | Payload       ...   | Checksum                  |
    // Size:  | 1    | 1    | 2           | 1    | 1    | payload_len         | 4                         |
    // Pos:   | 0    | 1    | 2    | 3    | 4    | 5    | 6    | 7      ...   | 6 + payload_len ...       |
    //        |------|------|-------------|------|------|------|--------------|---------------------------|
    //        | 0xBA | 0xCE | 0xXX | 0xXX | 0xXX | 0xXX | 0xXX | 0xXX   ...   | 0xXX | 0xXX | 0xXX | 0xXX |

    // Construct the CAS packet
    UBXscratch[0] = 0xBA;         // header 1 (0xBA)
    UBXscratch[1] = 0xCE;         // header 2 (0xCE)
    UBXscratch[2] = payload_size; // length 1
    UBXscratch[3] = 0;            // length 2
    UBXscratch[4] = class_id;     // class
    UBXscratch[5] = msg_id;       // id

    UBXscratch[6 + payload_size] = 0x00; // Checksum
    UBXscratch[7 + payload_size] = 0x00;
    UBXscratch[8 + payload_size] = 0x00;
    UBXscratch[9 + payload_size] = 0x00;

    for (int i = 0; i < payload_size; i++) {
        UBXscratch[6 + i] = pgm_read_byte(&msg[i]);
    }
    CASChecksum(UBXscratch, (payload_size + 10));

#if defined(GPS_DEBUG) && defined(DEBUG_PORT)
    LOG_DEBUG("CAS packet: ");
    DEBUG_PORT.hexDump(MESHTASTIC_LOG_LEVEL_DEBUG, UBXscratch, payload_size + 10);
#endif
    return (payload_size + 10);
}

GPS_RESPONSE GPS::getACK(const char *message, uint32_t waitMillis)
{
    uint8_t buffer[768] = {0};
    uint8_t b;
    int bytesRead = 0;
    uint32_t startTimeout = millis() + waitMillis;
#ifdef GPS_DEBUG
    std::string debugmsg = "";
#endif
    while (millis() < startTimeout) {
        if (_serial_gps->available()) {
            b = _serial_gps->read();

#ifdef GPS_DEBUG
            debugmsg += vformat("%c", (b >= 32 && b <= 126) ? b : '.');
#endif
            buffer[bytesRead] = b;
            bytesRead++;
            if ((bytesRead == 767) || (b == '\r')) {
#ifdef GPS_DEBUG
                LOG_DEBUG(debugmsg.c_str());
#endif
                if (strnstr((char *)buffer, message, bytesRead) != nullptr) {
#ifdef GPS_DEBUG
                    LOG_DEBUG("Found: %s", message); // Log the found message
#endif
                    return GNSS_RESPONSE_OK;
                } else {
                    bytesRead = 0;
                }
            }
        }
    }
    return GNSS_RESPONSE_NONE;
}

GPS_RESPONSE GPS::getACKCas(uint8_t class_id, uint8_t msg_id, uint32_t waitMillis)
{
    uint32_t startTime = millis();
    uint8_t buffer[CAS_ACK_NACK_MSG_SIZE] = {0};
    uint8_t bufferPos = 0;

    // CAS-ACK-(N)ACK structure
    //         | H1   | H2   | Payload Len | cls  | msg  | Payload                   | Checksum (4)              |
    //         |      |      |             |      |      | Cls  | Msg  | Reserved    |                           |
    //         |------|------|-------------|------|------|------|------|-------------|---------------------------|
    // ACK-NACK| 0xBA | 0xCE | 0x04 | 0x00 | 0x05 | 0x00 | 0xXX | 0xXX | 0x00 | 0x00 | 0xXX | 0xXX | 0xXX | 0xXX |
    // ACK-ACK | 0xBA | 0xCE | 0x04 | 0x00 | 0x05 | 0x01 | 0xXX | 0xXX | 0x00 | 0x00 | 0xXX | 0xXX | 0xXX | 0xXX |

    while (Throttle::isWithinTimespanMs(startTime, waitMillis)) {
        if (_serial_gps->available()) {
            buffer[bufferPos++] = _serial_gps->read();

            // keep looking at the first two bytes of buffer until
            // we have found the CAS frame header (0xBA, 0xCE), if not
            // keep reading bytes until we find a frame header or we run
            // out of time.
            if ((bufferPos == 2) && !(buffer[0] == 0xBA && buffer[1] == 0xCE)) {
                buffer[0] = buffer[1];
                buffer[1] = 0;
                bufferPos = 1;
            }
        }

        // we have read all the bytes required for the Ack/Nack (14-bytes)
        // and we must have found a frame to get this far
        if (bufferPos == sizeof(buffer) - 1) {
            uint8_t msg_cls = buffer[4];     // message class should be 0x05
            uint8_t msg_msg_id = buffer[5];  // message id should be 0x00 or 0x01
            uint8_t payload_cls = buffer[6]; // payload class id
            uint8_t payload_msg = buffer[7]; // payload message id

            // Check for an ACK-ACK for the specified class and message id
            if ((msg_cls == 0x05) && (msg_msg_id == 0x01) && payload_cls == class_id && payload_msg == msg_id) {
#ifdef GPS_DEBUG
                LOG_INFO("Got ACK for class %02X message %02X in %dms", class_id, msg_id, millis() - startTime);
#endif
                return GNSS_RESPONSE_OK;
            }

            // Check for an ACK-NACK for the specified class and message id
            if ((msg_cls == 0x05) && (msg_msg_id == 0x00) && payload_cls == class_id && payload_msg == msg_id) {
#ifdef GPS_DEBUG
                LOG_WARN("Got NACK for class %02X message %02X in %dms", class_id, msg_id, millis() - startTime);
#endif
                return GNSS_RESPONSE_NAK;
            }

            // This isn't the frame we are looking for, clear the buffer
            // and try again until we run out of time.
            memset(buffer, 0x0, sizeof(buffer));
            bufferPos = 0;
        }
    }
    return GNSS_RESPONSE_NONE;
}

GPS_RESPONSE GPS::getACK(uint8_t class_id, uint8_t msg_id, uint32_t waitMillis)
{
    uint8_t b;
    uint8_t ack = 0;
    const uint8_t ackP[2] = {class_id, msg_id};
    uint8_t buf[10] = {0xB5, 0x62, 0x05, 0x01, 0x02, 0x00, 0x00, 0x00, 0x00, 0x00};
    uint32_t startTime = millis();
    const char frame_errors[] = "More than 100 frame errors";
    int sCounter = 0;
#ifdef GPS_DEBUG
    std::string debugmsg = "";
#endif

    for (int j = 2; j < 6; j++) {
        buf[8] += buf[j];
        buf[9] += buf[8];
    }

    for (int j = 0; j < 2; j++) {
        buf[6 + j] = ackP[j];
        buf[8] += buf[6 + j];
        buf[9] += buf[8];
    }

    while (Throttle::isWithinTimespanMs(startTime, waitMillis)) {
        if (ack > 9) {
#ifdef GPS_DEBUG
            LOG_INFO("Got ACK for class %02X message %02X in %dms", class_id, msg_id, millis() - startTime);
#endif
            return GNSS_RESPONSE_OK; // ACK received
        }
        if (_serial_gps->available()) {
            b = _serial_gps->read();
            if (b == frame_errors[sCounter]) {
                sCounter++;
                if (sCounter == 26) {
#ifdef GPS_DEBUG

                    LOG_DEBUG(debugmsg.c_str());
#endif
                    return GNSS_RESPONSE_FRAME_ERRORS;
                }
            } else {
                sCounter = 0;
            }
#ifdef GPS_DEBUG
            debugmsg += vformat("%02X", b);
#endif
            if (b == buf[ack]) {
                ack++;
            } else {
                if (ack == 3 && b == 0x00) { // UBX-ACK-NAK message
#ifdef GPS_DEBUG
                    LOG_DEBUG(debugmsg.c_str());
#endif
                    LOG_WARN("Got NAK for class %02X message %02X", class_id, msg_id);
                    return GNSS_RESPONSE_NAK; // NAK received
                }
                ack = 0; // Reset the acknowledgement counter
            }
        }
    }
#ifdef GPS_DEBUG
    LOG_DEBUG(debugmsg.c_str());
    LOG_WARN("No response for class %02X message %02X", class_id, msg_id);
#endif
    return GNSS_RESPONSE_NONE; // No response received within timeout
}

/**
 * @brief
 * @note   New method, this method can wait for the specified class and message ID, and return the payload
 * @param  *buffer: The message buffer, if there is a response payload message, it will be returned through the buffer parameter
 * @param  size:    size of buffer
 * @param  requestedClass:  request class constant
 * @param  requestedID:     request message ID constant
 * @retval length of payload message
 */
int GPS::getACK(uint8_t *buffer, uint16_t size, uint8_t requestedClass, uint8_t requestedID, uint32_t waitMillis)
{
    uint16_t ubxFrameCounter = 0;
    uint32_t startTime = millis();
    uint16_t needRead = 0;

    while (Throttle::isWithinTimespanMs(startTime, waitMillis)) {
        if (_serial_gps->available()) {
            int c = _serial_gps->read();
            switch (ubxFrameCounter) {
            case 0:
                // ubxFrame 'μ'
                if (c == 0xB5) {
                    ubxFrameCounter++;
                }
                break;
            case 1:
                // ubxFrame 'b'
                if (c == 0x62) {
                    ubxFrameCounter++;
                } else {
                    ubxFrameCounter = 0;
                }
                break;
            case 2:
                // Class
                if (c == requestedClass) {
                    ubxFrameCounter++;
                } else {
                    ubxFrameCounter = 0;
                }
                break;
            case 3:
                // Message ID
                if (c == requestedID) {
                    ubxFrameCounter++;
                } else {
                    ubxFrameCounter = 0;
                }
                break;
            case 4:
                // Payload length lsb
                needRead = c;
                ubxFrameCounter++;
                break;
            case 5:
                // Payload length msb
                needRead |= (c << 8);
                ubxFrameCounter++;
                // Check for buffer overflow
                if (needRead >= size) {
                    ubxFrameCounter = 0;
                    break;
                }
                if (_serial_gps->readBytes(buffer, needRead) != needRead) {
                    ubxFrameCounter = 0;
                } else {
                    // return payload length
#ifdef GPS_DEBUG
                    LOG_INFO("Got ACK for class %02X message %02X in %dms", requestedClass, requestedID, millis() - startTime);
#endif
                    return needRead;
                }
                break;

            default:
                break;
            }
        }
    }
    return 0;
}

#if GPS_BAUDRATE_FIXED
// if GPS_BAUDRATE is specified in variant, only try that.
static const int serialSpeeds[1] = {GPS_BAUDRATE};
static const int rareSerialSpeeds[1] = {GPS_BAUDRATE};
#else
static const int serialSpeeds[3] = {9600, 115200, 38400};
static const int rareSerialSpeeds[3] = {4800, 57600, GPS_BAUDRATE};
#endif

#ifndef GPS_PROBETRIES
#define GPS_PROBETRIES 2
#endif

/**
 * @brief  Setup the GPS based on the model detected.
 *  We detect the GPS by cycling through a set of baud rates, first common then rare.
 *  For each baud rate, we run GPS::Probe to send commands and match the responses
 *  to known GPS responses.
 * @retval Whether setup reached the end of its potential to configure the GPS.
 */
bool GPS::setup()
{
    if (!didSerialInit) {
        int msglen = 0;
        if (tx_gpio && gnssModel == GNSS_MODEL_UNKNOWN) {
            if (probeTries < GPS_PROBETRIES) {
                gnssModel = probe(serialSpeeds[speedSelect]);
                if (gnssModel == GNSS_MODEL_UNKNOWN) {
                    if (currentStep == 0 && ++speedSelect == array_count(serialSpeeds)) {
                        speedSelect = 0;
                        ++probeTries;
                    }
                }
            }
            // Rare Serial Speeds
#ifndef CONFIG_IDF_TARGET_ESP32C6
            if (probeTries == GPS_PROBETRIES) {
                gnssModel = probe(rareSerialSpeeds[speedSelect]);
                if (gnssModel == GNSS_MODEL_UNKNOWN) {
                    if (currentStep == 0 && ++speedSelect == array_count(rareSerialSpeeds)) {
                        LOG_WARN("Give up on GPS probe and set to %d", GPS_BAUDRATE);
                        return true;
                    }
                }
            }
#endif
        }

        if (gnssModel != GNSS_MODEL_UNKNOWN) {
            setConnected();
        } else {
            return false;
        }

        if (gnssModel == GNSS_MODEL_MTK) {
            /*
             * t-beam-s3-core uses the same L76K GNSS module as t-echo.
             * Unlike t-echo, L76K uses 9600 baud rate for communication by default.
             * */

            // Initialize the L76K Chip, use GPS + GLONASS + BEIDOU
            _serial_gps->write("$PCAS04,7*1E\r\n");
            delay(250);
            // only ask for RMC and GGA
            _serial_gps->write("$PCAS03,1,0,0,0,1,0,0,0,0,0,,,0,0*02\r\n");
            delay(250);
            // Switch to Vehicle Mode, since SoftRF enables Aviation < 2g
            _serial_gps->write("$PCAS11,3*1E\r\n");
            delay(250);
        } else if (gnssModel == GNSS_MODEL_MTK_L76B) {
            // Waveshare Pico-GPS hat uses the L76B with 9600 baud
            // Initialize the L76B Chip, use GPS + GLONASS
            // See note in L76_Series_GNSS_Protocol_Specification, chapter 3.29
            _serial_gps->write("$PMTK353,1,1,0,0,0*2B\r\n");
            // Above command will reset the GPS and takes longer before it will accept new commands
            delay(1000);
            // only ask for RMC and GGA (GNRMC and GNGGA)
            // See note in L76_Series_GNSS_Protocol_Specification, chapter 2.1
            _serial_gps->write("$PMTK314,0,1,0,1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0*28\r\n");
            delay(250);
            // Enable SBAS
            _serial_gps->write("$PMTK301,2*2E\r\n");
            delay(250);
            // Enable PPS for 2D/3D fix only
            _serial_gps->write("$PMTK285,3,100*3F\r\n");
            delay(250);
            // Switch to Fitness Mode, for running and walking purpose with low speed (<5 m/s)
            _serial_gps->write("$PMTK886,1*29\r\n");
            delay(250);
        } else if (gnssModel == GNSS_MODEL_MTK_PA1010D) {
            // PA1010D is used in the Pimoroni GPS board.

            // Enable all constellations.
            _serial_gps->write("$PMTK353,1,1,1,1,1*2A\r\n");
            // Above command will reset the GPS and takes longer before it will accept new commands
            delay(1000);
            // Only ask for RMC and GGA (GNRMC and GNGGA)
            _serial_gps->write("$PMTK314,0,1,0,1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0*28\r\n");
            delay(250);
            // Enable SBAS / WAAS
            _serial_gps->write("$PMTK301,2*2E\r\n");
            delay(250);
        } else if (gnssModel == GNSS_MODEL_MTK_PA1616S) {
            // PA1616S is used in some GPS breakout boards from Adafruit
            // PA1616S does not have GLONASS capability. PA1616D does, but is not implemented here.
            _serial_gps->write("$PMTK353,1,0,0,0,0*2A\r\n");
            // Above command will reset the GPS and takes longer before it will accept new commands
            delay(1000);
            // Only ask for RMC and GGA (GNRMC and GNGGA)
            _serial_gps->write("$PMTK314,0,1,0,1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0*28\r\n");
            delay(250);
            // Enable SBAS / WAAS
            _serial_gps->write("$PMTK301,2*2E\r\n");
            delay(250);
        } else if (gnssModel == GNSS_MODEL_ATGM336H) {
            // Set the intial configuration of the device - these _should_ work for most AT6558 devices
            msglen = makeCASPacket(0x06, 0x07, sizeof(_message_CAS_CFG_NAVX_CONF), _message_CAS_CFG_NAVX_CONF);
            _serial_gps->write(UBXscratch, msglen);
            if (getACKCas(0x06, 0x07, 250) != GNSS_RESPONSE_OK) {
                LOG_WARN("ATGM336H: Could not set Config");
            }

            // Set the update frequence to 1Hz
            msglen = makeCASPacket(0x06, 0x04, sizeof(_message_CAS_CFG_RATE_1HZ), _message_CAS_CFG_RATE_1HZ);
            _serial_gps->write(UBXscratch, msglen);
            if (getACKCas(0x06, 0x04, 250) != GNSS_RESPONSE_OK) {
                LOG_WARN("ATGM336H: Could not set Update Frequency");
            }

            // Set the NEMA output messages
            // Ask for only RMC and GGA
            uint8_t fields[] = {CAS_NEMA_RMC, CAS_NEMA_GGA};
            for (unsigned int i = 0; i < sizeof(fields); i++) {
                // Construct a CAS-CFG-MSG packet
                uint8_t cas_cfg_msg_packet[] = {0x4e, fields[i], 0x01, 0x00};
                msglen = makeCASPacket(0x06, 0x01, sizeof(cas_cfg_msg_packet), cas_cfg_msg_packet);
                _serial_gps->write(UBXscratch, msglen);
                if (getACKCas(0x06, 0x01, 250) != GNSS_RESPONSE_OK) {
                    LOG_WARN("ATGM336H: Could not enable NMEA MSG: %d", fields[i]);
                }
            }
        } else if (gnssModel == GNSS_MODEL_UC6580) {
            // The Unicore UC6580 can use a lot of sat systems, enable it to
            // use GPS L1 & L5 + BDS B1I & B2a + GLONASS L1 + GALILEO E1 & E5a + SBAS + QZSS
            // This will reset the receiver, so wait a bit afterwards
            // The paranoid will wait for the OK*04 confirmation response after each command.
            _serial_gps->write("$CFGSYS,h35155\r\n");
            delay(750);
            // Must be done after the CFGSYS command
            // Turn off GSV messages, we don't really care about which and where the sats are, maybe someday.
            _serial_gps->write("$CFGMSG,0,3,0\r\n");
            delay(250);
            // Turn off GSA messages, TinyGPS++ doesn't use this message.
            _serial_gps->write("$CFGMSG,0,2,0\r\n");
            delay(250);
            // Turn off NOTICE __TXT messages, these may provide Unicore some info but we don't care.
            _serial_gps->write("$CFGMSG,6,0,0\r\n");
            delay(250);
            _serial_gps->write("$CFGMSG,6,1,0\r\n");
            delay(250);
        } else if (IS_ONE_OF(gnssModel, GNSS_MODEL_AG3335, GNSS_MODEL_AG3352)) {

            if (config.lora.region == meshtastic_Config_LoRaConfig_RegionCode_IN ||
                config.lora.region == meshtastic_Config_LoRaConfig_RegionCode_NP_865) {
                _serial_gps->write("$PAIR066,1,0,1,0,0,1*3B\r\n"); // Enable GPS+GALILEO+NAVIC
                // GPS GLONASS GALILEO BDS QZSS NAVIC
                //  1    0       1      0   0    1
            } else {
                _serial_gps->write("$PAIR066,1,1,1,1,0,0*3A\r\n"); // Enable GPS+GLONASS+GALILEO+BDS
                // GPS GLONASS GALILEO BDS QZSS NAVIC
                //  1    1       1      1   0    0
            }
            // Configure NMEA (sentences will output once per fix)
            _serial_gps->write("$PAIR062,0,1*3F\r\n"); // GGA ON
            _serial_gps->write("$PAIR062,1,0*3F\r\n"); // GLL OFF
            _serial_gps->write("$PAIR062,2,0*3C\r\n"); // GSA OFF
            _serial_gps->write("$PAIR062,3,0*3D\r\n"); // GSV OFF
            _serial_gps->write("$PAIR062,4,1*3B\r\n"); // RMC ON
            _serial_gps->write("$PAIR062,5,0*3B\r\n"); // VTG OFF
            _serial_gps->write("$PAIR062,6,0*38\r\n"); // ZDA ON

            delay(250);
            _serial_gps->write("$PAIR513*3D\r\n"); // save configuration
        } else if (gnssModel == GNSS_MODEL_UBLOX6) {
            clearBuffer();
            SEND_UBX_PACKET(0x06, 0x02, _message_DISABLE_TXT_INFO, "disable text info messages", 500);
            SEND_UBX_PACKET(0x06, 0x39, _message_JAM_6_7, "enable interference resistance", 500);
            SEND_UBX_PACKET(0x06, 0x23, _message_NAVX5, "configure NAVX5 settings", 500);

            // Turn off unwanted NMEA messages, set update rate
            SEND_UBX_PACKET(0x06, 0x08, _message_1HZ, "set GPS update rate", 500);
            SEND_UBX_PACKET(0x06, 0x01, _message_GLL, "disable NMEA GLL", 500);
            SEND_UBX_PACKET(0x06, 0x01, _message_GSA, "enable NMEA GSA", 500);
            SEND_UBX_PACKET(0x06, 0x01, _message_GSV, "disable NMEA GSV", 500);
            SEND_UBX_PACKET(0x06, 0x01, _message_VTG, "disable NMEA VTG", 500);
            SEND_UBX_PACKET(0x06, 0x01, _message_RMC, "enable NMEA RMC", 500);
            SEND_UBX_PACKET(0x06, 0x01, _message_GGA, "enable NMEA GGA", 500);

            clearBuffer();
            SEND_UBX_PACKET(0x06, 0x11, _message_CFG_RXM_ECO, "enable powersave ECO mode for Neo-6", 500);
            SEND_UBX_PACKET(0x06, 0x3B, _message_CFG_PM2, "enable powersave details for GPS", 500);
            SEND_UBX_PACKET(0x06, 0x01, _message_AID, "disable UBX-AID", 500);

            msglen = makeUBXPacket(0x06, 0x09, sizeof(_message_SAVE), _message_SAVE);
            _serial_gps->write(UBXscratch, msglen);
            if (getACK(0x06, 0x09, 2000) != GNSS_RESPONSE_OK) {
                LOG_WARN("Unable to save GNSS module config");
            } else {
                LOG_INFO("GNSS module config saved!");
            }
        } else if (IS_ONE_OF(gnssModel, GNSS_MODEL_UBLOX7, GNSS_MODEL_UBLOX8, GNSS_MODEL_UBLOX9)) {
            if (gnssModel == GNSS_MODEL_UBLOX7) {
                LOG_DEBUG("Set GPS+SBAS");
                msglen = makeUBXPacket(0x06, 0x3e, sizeof(_message_GNSS_7), _message_GNSS_7);
                _serial_gps->write(UBXscratch, msglen);
            } else { // 8,9
                msglen = makeUBXPacket(0x06, 0x3e, sizeof(_message_GNSS_8), _message_GNSS_8);
                _serial_gps->write(UBXscratch, msglen);
            }

            if (getACK(0x06, 0x3e, 800) == GNSS_RESPONSE_NAK) {
                // It's not critical if the module doesn't acknowledge this configuration.
                LOG_DEBUG("reconfigure GNSS - defaults maintained. Is this module GPS-only?");
            } else {
                if (gnssModel == GNSS_MODEL_UBLOX7) {
                    LOG_INFO("GPS+SBAS configured");
                } else { // 8,9
                    LOG_INFO("GPS+SBAS+GLONASS+Galileo configured");
                }
                // Documentation say, we need wait atleast 0.5s after reconfiguration of GNSS module, before sending next
                // commands for the M8 it tends to be more... 1 sec should be enough ;>)
                delay(1000);
            }

            // Disable Text Info messages //6,7,8,9
            clearBuffer();
            SEND_UBX_PACKET(0x06, 0x02, _message_DISABLE_TXT_INFO, "disable text info messages", 500);

            if (gnssModel == GNSS_MODEL_UBLOX8) { // 8
                clearBuffer();
                SEND_UBX_PACKET(0x06, 0x39, _message_JAM_8, "enable interference resistance", 500);

                clearBuffer();
                SEND_UBX_PACKET(0x06, 0x23, _message_NAVX5_8, "configure NAVX5_8 settings", 500);
            } else { // 6,7,9
                SEND_UBX_PACKET(0x06, 0x39, _message_JAM_6_7, "enable interference resistance", 500);
                SEND_UBX_PACKET(0x06, 0x23, _message_NAVX5, "configure NAVX5 settings", 500);
            }
            // Turn off unwanted NMEA messages, set update rate
            SEND_UBX_PACKET(0x06, 0x08, _message_1HZ, "set GPS update rate", 500);
            SEND_UBX_PACKET(0x06, 0x01, _message_GLL, "disable NMEA GLL", 500);
            SEND_UBX_PACKET(0x06, 0x01, _message_GSA, "enable NMEA GSA", 500);
            SEND_UBX_PACKET(0x06, 0x01, _message_GSV, "disable NMEA GSV", 500);
            SEND_UBX_PACKET(0x06, 0x01, _message_VTG, "disable NMEA VTG", 500);
            SEND_UBX_PACKET(0x06, 0x01, _message_RMC, "enable NMEA RMC", 500);
            SEND_UBX_PACKET(0x06, 0x01, _message_GGA, "enable NMEA GGA", 500);

            if (ublox_info.protocol_version >= 18) {
                clearBuffer();
                SEND_UBX_PACKET(0x06, 0x86, _message_PMS, "enable powersave for GPS", 500);
                SEND_UBX_PACKET(0x06, 0x3B, _message_CFG_PM2, "enable powersave details for GPS", 500);

                // For M8 we want to enable NMEA vserion 4.10 so we can see the additional sats.
                if (gnssModel == GNSS_MODEL_UBLOX8) {
                    clearBuffer();
                    SEND_UBX_PACKET(0x06, 0x17, _message_NMEA, "enable NMEA 4.10", 500);
                }
            } else {
                SEND_UBX_PACKET(0x06, 0x11, _message_CFG_RXM_PSM, "enable powersave mode for GPS", 500);
                SEND_UBX_PACKET(0x06, 0x3B, _message_CFG_PM2, "enable powersave details for GPS", 500);
            }

            msglen = makeUBXPacket(0x06, 0x09, sizeof(_message_SAVE), _message_SAVE);
            _serial_gps->write(UBXscratch, msglen);
            if (getACK(0x06, 0x09, 2000) != GNSS_RESPONSE_OK) {
                LOG_WARN("Unable to save GNSS module config");
            } else {
                LOG_INFO("GNSS module configuration saved!");
            }
        } else if (gnssModel == GNSS_MODEL_UBLOX10) {
            delay(1000);
            clearBuffer();
            SEND_UBX_PACKET(0x06, 0x8A, _message_VALSET_DISABLE_NMEA_RAM, "disable NMEA messages in M10 RAM", 300);
            delay(750);
            clearBuffer();
            SEND_UBX_PACKET(0x06, 0x8A, _message_VALSET_DISABLE_NMEA_BBR, "disable NMEA messages in M10 BBR", 300);
            delay(750);
            clearBuffer();
            SEND_UBX_PACKET(0x06, 0x8A, _message_VALSET_DISABLE_TXT_INFO_RAM, "disable Info messages for M10 GPS RAM", 300);
            delay(750);
            // Next disable Info txt messages in BBR layer
            clearBuffer();
            SEND_UBX_PACKET(0x06, 0x8A, _message_VALSET_DISABLE_TXT_INFO_BBR, "disable Info messages for M10 GPS BBR", 300);
            delay(750);
            // Do M10 configuration for Power Management.
            SEND_UBX_PACKET(0x06, 0x8A, _message_VALSET_PM_RAM, "enable powersave for M10 GPS RAM", 300);
            delay(750);
            SEND_UBX_PACKET(0x06, 0x8A, _message_VALSET_PM_BBR, "enable powersave for M10 GPS BBR", 300);
            delay(750);
            SEND_UBX_PACKET(0x06, 0x8A, _message_VALSET_ITFM_RAM, "enable jam detection M10 GPS RAM", 300);
            delay(750);
            SEND_UBX_PACKET(0x06, 0x8A, _message_VALSET_ITFM_BBR, "enable jam detection M10 GPS BBR", 300);
            delay(750);
            // Here is where the init commands should go to do further M10 initialization.
            SEND_UBX_PACKET(0x06, 0x8A, _message_VALSET_DISABLE_SBAS_RAM, "disable SBAS M10 GPS RAM", 300);
            delay(750); // will cause a receiver restart so wait a bit
            SEND_UBX_PACKET(0x06, 0x8A, _message_VALSET_DISABLE_SBAS_BBR, "disable SBAS M10 GPS BBR", 300);
            delay(750); // will cause a receiver restart so wait a bit

            // Done with initialization, Now enable wanted NMEA messages in BBR layer so they will survive a periodic
            // sleep.
            SEND_UBX_PACKET(0x06, 0x8A, _message_VALSET_ENABLE_NMEA_BBR, "enable messages for M10 GPS BBR", 300);
            delay(750);
            // Next enable wanted NMEA messages in RAM layer
            SEND_UBX_PACKET(0x06, 0x8A, _message_VALSET_ENABLE_NMEA_RAM, "enable messages for M10 GPS RAM", 500);
            delay(750);

            // As the M10 has no flash, the best we can do to preserve the config is to set it in RAM and BBR.
            // BBR will survive a restart, and power off for a while, but modules with small backup
            // batteries or super caps will not retain the config for a long power off time.
            msglen = makeUBXPacket(0x06, 0x09, sizeof(_message_SAVE_10), _message_SAVE_10);
            _serial_gps->write(UBXscratch, msglen);
            if (getACK(0x06, 0x09, 2000) != GNSS_RESPONSE_OK) {
                LOG_WARN("Unable to save GNSS module config");
            } else {
                LOG_INFO("GNSS module configuration saved!");
            }
        } else if (gnssModel == GNSS_MODEL_CM121) {
            // only ask for RMC and GGA
            // enable GGA
            _serial_gps->write("$CFGMSG,0,0,1,1*1B\r\n");
            delay(250);
            // enable RMC
            _serial_gps->write("$CFGMSG,0,4,1,1*1F\r\n");
            delay(250);
        }
        didSerialInit = true;
    }

    notifyDeepSleepObserver.observe(&notifyDeepSleep);

    return true;
}

GPS::~GPS()
{
    // we really should unregister our sleep observer
    notifyDeepSleepObserver.unobserve(&notifyDeepSleep);
}

// Put the GPS hardware into a specified state
void GPS::setPowerState(GPSPowerState newState, uint32_t sleepTime)
{
    // Update the stored GPSPowerstate, and create local copies
    GPSPowerState oldState = powerState;
    powerState = newState;
    LOG_INFO("GPS power state move from %s to %s", getGPSPowerStateString(oldState), getGPSPowerStateString(newState));

    switch (newState) {
    case GPS_ACTIVE:
    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
#ifdef GNSS_AIROHA
        digitalWrite(PIN_GPS_EN, LOW);
#endif
        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
#ifdef GNSS_AIROHA
        digitalWrite(PIN_GPS_EN, LOW);
#endif
        break;
    }
}

// Set power with EN pin, if relevant
void GPS::writePinEN(bool on)
{
    // Abort: if conflict with Canned Messages when using Wisblock(?)
    if ((HW_VENDOR == meshtastic_HardwareModel_RAK4631 || HW_VENDOR == meshtastic_HardwareModel_WISMESH_TAP) &&
        (rotaryEncoderInterruptImpl1 || upDownInterruptImpl1))
        return;

    // Write and log
    enablePin->set(on);
#ifdef GPS_DEBUG
    LOG_DEBUG("Pin EN %s", on == HIGH ? "HI" : "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
    bool val;
    if (standby)
        val = GPS_STANDBY_ACTIVE;
    else
        val = !GPS_STANDBY_ACTIVE;

    // Write and log
    pinMode(PIN_GPS_STANDBY, OUTPUT);
    digitalWrite(PIN_GPS_STANDBY, val);

    // Enter backup mode on PA1010D; TODO: may be applicable to other MTK GPS too
    if (IS_ONE_OF(gnssModel, GNSS_MODEL_MTK_PA1010D)) {
        _serial_gps->write("$PMTK225,4*2F\r\n");
    }

#ifdef GPS_DEBUG
    LOG_DEBUG("Pin STANDBY %s", val == HIGH ? "HI" : "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 (HW_VENDOR == meshtastic_HardwareModel_T_WATCH_S3) {
            // t-watch-s3-plus GNSS power channel
            on ? PMU->enablePowerOutput(XPOWERS_BLDO1) : PMU->disablePowerOutput(XPOWERS_BLDO1);
        }
    } else if (model == XPOWERS_AXP192) {
        // t-beam v1.1 GNSS  power channel
        on ? PMU->enablePowerOutput(XPOWERS_LDO3) : PMU->disablePowerOutput(XPOWERS_LDO3);
    }
#ifdef GPS_DEBUG
    LOG_DEBUG("PMU %s", on ? "on" : "off");
#endif
#endif
}

// Set UBLOX power, if relevant
void GPS::setPowerUBLOX(bool on, uint32_t sleepMs)
{
    // Abort: if not UBLOX hardware
    if (!IS_ONE_OF(gnssModel, GNSS_MODEL_UBLOX6, GNSS_MODEL_UBLOX7, GNSS_MODEL_UBLOX8, GNSS_MODEL_UBLOX9, GNSS_MODEL_UBLOX10))
        return;

    // If waking
    if (on) {
        gps->_serial_gps->write(0xFF);
        clearBuffer(); // This often returns old data, so drop it
    }

    // 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);
        }

        // Determine hardware version
        if (gnssModel != GNSS_MODEL_UBLOX10) {
            // Encode the sleep time in millis into the packet
            for (int i = 0; i < 4; i++)
                _message_PMREQ[0 + i] = sleepMs >> (i * 8);

            // Record the message length
            msglen = gps->makeUBXPacket(0x02, 0x41, sizeof(_message_PMREQ), _message_PMREQ);
        } else {
            // Encode the sleep time in millis into the packet
            for (int i = 0; i < 4; i++)
                _message_PMREQ_10[4 + i] = sleepMs >> (i * 8);

            // Record the message length
            msglen = gps->makeUBXPacket(0x02, 0x41, sizeof(_message_PMREQ_10), _message_PMREQ_10);
        }

        // Send the UBX packet
        gps->_serial_gps->write(gps->UBXscratch, msglen);
#ifdef GPS_DEBUG
        LOG_DEBUG("UBLOX: sleep for %dmS", sleepMs);
#endif
    }
}

/// Record that we have a GPS
void GPS::setConnected()
{
    if (!hasGPS) {
        hasGPS = true;
        shouldPublish = true;
    }
}

// We want a GPS lock. Wake the hardware
void GPS::up()
{
    scheduling.informSearching();
    setPowerState(GPS_ACTIVE);
}

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

    LOG_DEBUG("%us until next search", sleepTime / 1000);

    // If update interval less than 10 seconds, no attempt to sleep
    if (updateInterval <= GPS_UPDATE_ALWAYS_ON_THRESHOLD_MS || sleepTime == 0)
        setPowerState(GPS_IDLE);

    else {
// Check whether the GPS hardware is capable of GPS_SOFTSLEEP
// If not, fallback to GPS_HARDSLEEP instead
#ifdef PIN_GPS_STANDBY // L76B, L76K and clones have a standby pin
        bool softsleepSupported = true;
#else
        bool softsleepSupported = false;
#endif
        // U-blox is supported via PMREQ
        if (IS_ONE_OF(gnssModel, GNSS_MODEL_UBLOX6, GNSS_MODEL_UBLOX7, GNSS_MODEL_UBLOX8, GNSS_MODEL_UBLOX9, GNSS_MODEL_UBLOX10))
            softsleepSupported = true;

        if (softsleepSupported) {
            // 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
            // improvement over a single, fixed threshold
            uint32_t hardsleepThreshold = (2750 * pow(predictedSearchDuration / 1000, 1.22));
            LOG_DEBUG("gps_update_interval >= %us needed to justify hardsleep", hardsleepThreshold / 1000);

            // If update interval too short: softsleep (if supported by hardware)
            if (updateInterval < hardsleepThreshold) {
                setPowerState(GPS_SOFTSLEEP, sleepTime);
                return;
            }
        }
        // If update interval long enough (or softsleep unsupported): hardsleep instead
        setPowerState(GPS_HARDSLEEP, sleepTime);
        // Reset the fix quality to 0, since we're off.
        fixQual = 0;
    }
}

void GPS::publishUpdate()
{
    if (shouldPublish) {
        shouldPublish = false;

        // In debug logs, identify position by @timestamp:stage (stage 2 = publish)
        LOG_DEBUG("Publish pos@%x:2, hasVal=%d, Sats=%d, GPSlock=%d", p.timestamp, hasValidLocation, p.sats_in_view, hasLock());

        // Notify any status instances that are observing us
        const meshtastic::GPSStatus status = meshtastic::GPSStatus(hasValidLocation, isConnected(), isPowerSaving(), p);
        newStatus.notifyObservers(&status);
        if (config.position.gps_mode == meshtastic_Config_PositionConfig_GpsMode_ENABLED) {
            positionModule->handleNewPosition();
        }
    }
}

int32_t GPS::runOnce()
{
    if (!GPSInitFinished) {
        if (!_serial_gps || config.position.gps_mode == meshtastic_Config_PositionConfig_GpsMode_NOT_PRESENT) {
            LOG_INFO("GPS set to not-present. Skip probe");
            return disable();
        }
        if (!setup())
            return currentDelay; // Setup failed, re-run in two seconds

        // We have now loaded our saved preferences from flash
        if (config.position.gps_mode != meshtastic_Config_PositionConfig_GpsMode_ENABLED) {
            return disable();
        }
        GPSInitFinished = true;
        publishUpdate();
    }

    // ======================== GPS_ACTIVE state ========================
    // In GPS_ACTIVE state, GPS is powered on and we're receiving NMEA messages.
    // We use the following logic to determine when to update the local position
    // or time by running GPS::publishUpdate.
    // Note: Local position update is asynchronous to position broadcast. We
    // generally run this state every gps_update_interval seconds, and in most cases
    // gps_update_interval is faster than the position broadcast interval so there's a
    // fresh position ready when the device wants to broadcast one on the mesh.
    //
    // 1. Got a time for the first time --> set the time, don't publish.
    // 2. Got a lock for the first time
    //   --> If gps_update_interval is <= 10s --> publishUpdate
    //   --> Otherwise, hold for MIN(gps_update_interval - GPS_UPDATE_ALWAYS_ON_THRESHOLD_MS, 20s)
    //  3. Got a lock after turning back on
    //   --> If gps_update_interval is <= 10s --> publishUpdate
    //   --> Otherwise, hold for MIN(gps_update_interval - GPS_UPDATE_ALWAYS_ON_THRESHOLD_MS, 20s)
    // 4. Hold has expired
    //   --> If we have a time and a location --> publishUpdate
    //   --> down()
    // 5. Search time has expired
    //   --> If we have a time and a location --> publishUpdate
    //   --> If we had a location before but don't now --> publishUpdate
    //   --> down()
    if (whileActive()) {
        // if we have received valid NMEA claim we are connected
        setConnected();
    }

    // If we're due for an update, wake the GPS
    if (!config.position.fixed_position && powerState != GPS_ACTIVE && scheduling.isUpdateDue())
        up();

    // quality of the previous fix. We set it to 0 when we go down, so it's a way
    // to check if we're getting a lock after being GPS_OFF.
    uint8_t prev_fixQual = fixQual;

    if (powerState == GPS_ACTIVE) {
        // if gps_update_interval is <=10s, GPS never goes off, so we treat that differently
        uint32_t updateInterval = Default::getConfiguredOrDefaultMs(config.position.gps_update_interval);

        // 1. Got a time for the first time
        bool gotTime = (getRTCQuality() >= RTCQualityGPS);
        if (!gotTime && lookForTime()) { // Note: we count on this && short-circuiting and not resetting the RTC time
            gotTime = true;
        }

        // 2. Got a lock for the first time, or 3. Got a lock after turning back on
        bool gotLoc = lookForLocation();
        if (gotLoc) {
#ifdef GPS_DEBUG
            if (!hasValidLocation) { // declare that we have location ASAP
                LOG_DEBUG("hasValidLocation RISING EDGE");
            }
#endif
            if (updateInterval <= GPS_UPDATE_ALWAYS_ON_THRESHOLD_MS) {
                hasValidLocation = true;
                shouldPublish = true;
            } else if (!hasValidLocation || prev_fixQual == 0 || (fixHoldEnds + GPS_THREAD_INTERVAL) < millis()) {
                hasValidLocation = true;
                // Hold for up to 20secs after getting a lock to download ephemeris etc
                uint32_t holdTime = updateInterval - GPS_UPDATE_ALWAYS_ON_THRESHOLD_MS;
                if (holdTime > GPS_FIX_HOLD_MAX_MS)
                    holdTime = GPS_FIX_HOLD_MAX_MS;
                fixHoldEnds = millis() + holdTime;
#ifdef GPS_DEBUG
                LOG_DEBUG("Holding for %ums after lock", holdTime);
#endif
            }
        }

        bool tooLong = scheduling.searchedTooLong();
        if (tooLong && !gotLoc) {
            LOG_WARN("Couldn't publish a valid location: didn't get a GPS lock in time");
            // we didn't get a location during this ack window, therefore declare loss of lock
            if (hasValidLocation) {
                p = meshtastic_Position_init_default;
                hasValidLocation = false;
                shouldPublish = true;
#ifdef GPS_DEBUG
                LOG_DEBUG("hasValidLocation FALLING EDGE");
#endif
            }
        }

        // Hold has expired , Search time has expired, we got a time only, or we never needed to hold.
        bool holdExpired = (fixHoldEnds != 0 && millis() > fixHoldEnds);
        if (shouldPublish || tooLong || holdExpired) {
            if (gotTime && hasValidLocation) {
                shouldPublish = true;
            }
            if (shouldPublish) {
                fixHoldEnds = 0;
                publishUpdate();
            }

            // There's a chance we just got a time, so keep going to see if we can get a location too
            if (tooLong || holdExpired) {
                down();
            }

#ifdef GPS_DEBUG
        } else if (fixHoldEnds != 0) {
            LOG_DEBUG("Holding for GPS data download: %d ms (numSats=%d)", fixHoldEnds - millis(), p.sats_in_view);
#endif
        }
    }
    // ===================== end GPS_ACTIVE state ========================

    if (config.position.fixed_position == true && hasValidLocation)
        return disable(); // This should trigger when we have a fixed position, and get that first position

    // 9600bps is approx 1 byte per msec, so considering our buffer size we never need to wake more often than 200ms
    // if not awake we can run super infrquently (once every 5 secs?) to see if we need to wake.
    return (powerState == GPS_ACTIVE) ? GPS_THREAD_INTERVAL : 5000;
}

// clear the GPS rx/tx buffer as quickly as possible
void GPS::clearBuffer()
{
#ifdef ARCH_ESP32
    _serial_gps->flush(false);
#else
    int x = _serial_gps->available();
    while (x--)
        _serial_gps->read();
#endif
}

/// Prepare the GPS for the cpu entering deep or light sleep, expect to be gone for at least 100s of msecs
int GPS::prepareDeepSleep(void *unused)
{
    LOG_INFO("GPS deep sleep!");
    disable();
    return 0;
}

static const char *PROBE_MESSAGE = "Trying %s (%s)...";
static const char *DETECTED_MESSAGE = "%s detected";

#define PROBE_SIMPLE(CHIP, TOWRITE, RESPONSE, DRIVER, TIMEOUT, ...)                                                              \
    do {                                                                                                                         \
        LOG_DEBUG(PROBE_MESSAGE, TOWRITE, CHIP);                                                                                 \
        clearBuffer();                                                                                                           \
        _serial_gps->write(TOWRITE "\r\n");                                                                                      \
        if (getACK(RESPONSE, TIMEOUT) == GNSS_RESPONSE_OK) {                                                                     \
            LOG_INFO(DETECTED_MESSAGE, CHIP);                                                                                    \
            return DRIVER;                                                                                                       \
        }                                                                                                                        \
    } while (0)

#define PROBE_FAMILY(FAMILY_NAME, COMMAND, RESPONSE_MAP, TIMEOUT)                                                                \
    do {                                                                                                                         \
        LOG_DEBUG(PROBE_MESSAGE, COMMAND, FAMILY_NAME);                                                                          \
        clearBuffer();                                                                                                           \
        _serial_gps->write(COMMAND "\r\n");                                                                                      \
        GnssModel_t detectedDriver = getProbeResponse(TIMEOUT, RESPONSE_MAP, serialSpeed);                                       \
        if (detectedDriver != GNSS_MODEL_UNKNOWN) {                                                                              \
            return detectedDriver;                                                                                               \
        }                                                                                                                        \
    } while (0)

GnssModel_t GPS::probe(int serialSpeed)
{
    uint8_t buffer[768] = {0};

    switch (currentStep) {
    case 0: {
#if defined(ARCH_NRF52) || defined(ARCH_PORTDUINO) || defined(ARCH_STM32WL)
        _serial_gps->end();
        _serial_gps->begin(serialSpeed);
#elif defined(ARCH_RP2040)
        _serial_gps->end();
        _serial_gps->setFIFOSize(256);
        _serial_gps->begin(serialSpeed);
#else
        if (_serial_gps->baudRate() != serialSpeed) {
            LOG_DEBUG("Set GPS Baud to %i", serialSpeed);
            _serial_gps->updateBaudRate(serialSpeed);
        }
#endif

        memset(&ublox_info, 0, sizeof(ublox_info));
        delay(100);

#if defined(PIN_GPS_RESET) && PIN_GPS_RESET != -1
        digitalWrite(PIN_GPS_RESET, GPS_RESET_MODE); // assert for 10ms
        delay(10);
        digitalWrite(PIN_GPS_RESET, !GPS_RESET_MODE);

        // attempt to detect the chip based on boot messages
        std::vector<ChipInfo> passive_detect = {
            {"AG3335", "$PAIR021,AG3335", GNSS_MODEL_AG3335},
            {"AG3352", "$PAIR021,AG3352", GNSS_MODEL_AG3352},
            {"RYS3520", "$PAIR021,REYAX_RYS3520_V2", GNSS_MODEL_AG3352},
            {"UC6580", "UC6580", GNSS_MODEL_UC6580},
            // as L76K is sort of a last ditch effort, we won't attempt to detect it by startup messages for now.
            /*{"L76K", "SW=URANUS", GNSS_MODEL_MTK}*/};
        GnssModel_t detectedDriver = getProbeResponse(500, passive_detect, serialSpeed);
        if (detectedDriver != GNSS_MODEL_UNKNOWN) {
            return detectedDriver;
        }
#endif
        // Close all NMEA sentences, valid for L76K, ATGM336H (and likely other AT6558 devices)
        _serial_gps->write("$PCAS03,0,0,0,0,0,0,0,0,0,0,,,0,0*02\r\n");
        delay(20);
        // Close NMEA sequences on Ublox
        _serial_gps->write("$PUBX,40,GLL,0,0,0,0,0,0*5C\r\n");
        _serial_gps->write("$PUBX,40,GSV,0,0,0,0,0,0*59\r\n");
        _serial_gps->write("$PUBX,40,VTG,0,0,0,0,0,0*5E\r\n");
        delay(20);
        // Close NMEA sequences on CM121
        _serial_gps->write("$CFGMSG,0,1,0,1*1B\r\n");
        _serial_gps->write("$CFGMSG,0,2,0,1*18\r\n");
        _serial_gps->write("$CFGMSG,0,3,0,1*19\r\n");
        currentDelay = 20;
        currentStep = 1;
        return GNSS_MODEL_UNKNOWN;
    }
    case 1: {

        // Unicore UFirebirdII Series: UC6580, UM620, UM621, UM670A, UM680A, or UM681A,or CM121
        std::vector<ChipInfo> unicore = {
            {"UC6580", "UC6580", GNSS_MODEL_UC6580}, {"UM600", "UM600", GNSS_MODEL_UC6580}, {"CM121", "CM121", GNSS_MODEL_CM121}};
        PROBE_FAMILY("Unicore Family", "$PDTINFO", unicore, 500);
        currentDelay = 20;
        currentStep = 2;
        return GNSS_MODEL_UNKNOWN;
    }
    case 2: {
        std::vector<ChipInfo> atgm = {
            {"ATGM336H", "$GPTXT,01,01,02,HW=ATGM336H", GNSS_MODEL_ATGM336H},
            /* ATGM332D series (-11(GPS), -21(BDS), -31(GPS+BDS), -51(GPS+GLONASS), -71-0(GPS+BDS+GLONASS)) based on AT6558 */
            {"ATGM332D", "$GPTXT,01,01,02,HW=ATGM332D", GNSS_MODEL_ATGM336H}};
        PROBE_FAMILY("ATGM33xx Family", "$PCAS06,1*1A", atgm, 500);
        currentDelay = 20;
        currentStep = 3;
        return GNSS_MODEL_UNKNOWN;
    }
    case 3: {
        /* Airoha (Mediatek) AG3335A/M/S, A3352Q, Quectel L89 2.0, SimCom SIM65M */
        _serial_gps->write("$PAIR062,2,0*3C\r\n"); // GSA OFF to reduce volume
        _serial_gps->write("$PAIR062,3,0*3D\r\n"); // GSV OFF to reduce volume
        _serial_gps->write("$PAIR513*3D\r\n");     // save configuration
        std::vector<ChipInfo> airoha = {{"AG3335", "$PAIR021,AG3335", GNSS_MODEL_AG3335},
                                        {"AG3352", "$PAIR021,AG3352", GNSS_MODEL_AG3352},
                                        {"RYS3520", "$PAIR021,REYAX_RYS3520_V2", GNSS_MODEL_AG3352}};
        PROBE_FAMILY("Airoha Family", "$PAIR021*39", airoha, 1000);
        currentDelay = 20;
        currentStep = 4;
        return GNSS_MODEL_UNKNOWN;
    }
    case 4: {
        PROBE_SIMPLE("LC86", "$PQTMVERNO*58", "$PQTMVERNO,LC86", GNSS_MODEL_AG3352, 500);
        PROBE_SIMPLE("L76K", "$PCAS06,0*1B", "$GPTXT,01,01,02,SW=", GNSS_MODEL_MTK, 500);
        currentDelay = 20;
        currentStep = 5;
        return GNSS_MODEL_UNKNOWN;
    }
    case 5: {

        // Close all NMEA sentences, valid for MTK3333 and MTK3339 platforms
        _serial_gps->write("$PMTK514,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0*2E\r\n");
        delay(20);
        std::vector<ChipInfo> mtk = {{"L76B", "Quectel-L76B", GNSS_MODEL_MTK_L76B}, {"PA1010D", "1010D", GNSS_MODEL_MTK_PA1010D},
                                     {"PA1616S", "1616S", GNSS_MODEL_MTK_PA1616S},  {"LS20031", "MC-1513", GNSS_MODEL_MTK_L76B},
                                     {"L96", "Quectel-L96", GNSS_MODEL_MTK_L76B},   {"L80-R", "_3337_", GNSS_MODEL_MTK_L76B},
                                     {"L80", "_3339_", GNSS_MODEL_MTK_L76B}};

        PROBE_FAMILY("MTK Family", "$PMTK605*31", mtk, 500);
        currentDelay = 20;
        currentStep = 6;
        return GNSS_MODEL_UNKNOWN;
    }
    case 6: {
        uint8_t cfg_rate[] = {0xB5, 0x62, 0x06, 0x08, 0x00, 0x00, 0x00, 0x00};
        UBXChecksum(cfg_rate, sizeof(cfg_rate));
        clearBuffer();
        _serial_gps->write(cfg_rate, sizeof(cfg_rate));
        // Check that the returned response class and message ID are correct
        GPS_RESPONSE response = getACK(0x06, 0x08, 750);
        if (response == GNSS_RESPONSE_NONE) {
            LOG_WARN("No GNSS Module (baudrate %d)", serialSpeed);
            currentDelay = 2000;
            currentStep = 0;
            return GNSS_MODEL_UNKNOWN;
        } else if (response == GNSS_RESPONSE_FRAME_ERRORS) {
            LOG_INFO("UBlox Frame Errors (baudrate %d)", serialSpeed);
        }

        memset(buffer, 0, sizeof(buffer));
        uint8_t _message_MONVER[8] = {
            0xB5, 0x62, // Sync message for UBX protocol
            0x0A, 0x04, // Message class and ID (UBX-MON-VER)
            0x00, 0x00, // Length of payload (we're asking for an answer, so no payload)
            0x00, 0x00  // Checksum
        };
        //  Get Ublox gnss module hardware and software info
        UBXChecksum(_message_MONVER, sizeof(_message_MONVER));
        clearBuffer();
        _serial_gps->write(_message_MONVER, sizeof(_message_MONVER));

        uint16_t len = getACK(buffer, sizeof(buffer), 0x0A, 0x04, 1200);
        if (len) {
            uint16_t position = 0;
            for (int i = 0; i < 30; i++) {
                ublox_info.swVersion[i] = buffer[position];
                position++;
            }
            for (int i = 0; i < 10; i++) {
                ublox_info.hwVersion[i] = buffer[position];
                position++;
            }

            while (len >= position + 30) {
                for (int i = 0; i < 30; i++) {
                    ublox_info.extension[ublox_info.extensionNo][i] = buffer[position];
                    position++;
                }
                ublox_info.extensionNo++;
                if (ublox_info.extensionNo > 9)
                    break;
            }

            LOG_DEBUG("Module Info : ");
            LOG_DEBUG("Soft version: %s", ublox_info.swVersion);
            LOG_DEBUG("Hard version: %s", ublox_info.hwVersion);
            LOG_DEBUG("Extensions:%d", ublox_info.extensionNo);
            for (int i = 0; i < ublox_info.extensionNo; i++) {
                LOG_DEBUG("  %s", ublox_info.extension[i]);
            }

            memset(buffer, 0, sizeof(buffer));

            // tips: extensionNo field is 0 on some 6M GNSS modules
            for (int i = 0; i < ublox_info.extensionNo; ++i) {
                if (!strncmp(ublox_info.extension[i], "MOD=", 4)) {
                    strncpy((char *)buffer, &(ublox_info.extension[i][4]), sizeof(buffer));
                } else if (!strncmp(ublox_info.extension[i], "PROTVER", 7)) {
                    char *ptr = nullptr;
                    memset(buffer, 0, sizeof(buffer));
                    strncpy((char *)buffer, &(ublox_info.extension[i][8]), sizeof(buffer));
                    LOG_DEBUG("Protocol Version:%s", (char *)buffer);
                    if (strlen((char *)buffer)) {
                        ublox_info.protocol_version = strtoul((char *)buffer, &ptr, 10);
                        LOG_DEBUG("ProtVer=%d", ublox_info.protocol_version);
                    } else {
                        ublox_info.protocol_version = 0;
                    }
                }
            }
            if (strncmp(ublox_info.hwVersion, "00040007", 8) == 0) {
                LOG_INFO(DETECTED_MESSAGE, "U-blox 6", "6");
                return GNSS_MODEL_UBLOX6;
            } else if (strncmp(ublox_info.hwVersion, "00070000", 8) == 0) {
                LOG_INFO(DETECTED_MESSAGE, "U-blox 7", "7");
                return GNSS_MODEL_UBLOX7;
            } else if (strncmp(ublox_info.hwVersion, "00080000", 8) == 0) {
                LOG_INFO(DETECTED_MESSAGE, "U-blox 8", "8");
                return GNSS_MODEL_UBLOX8;
            } else if (strncmp(ublox_info.hwVersion, "00190000", 8) == 0) {
                LOG_INFO(DETECTED_MESSAGE, "U-blox 9", "9");
                return GNSS_MODEL_UBLOX9;
            } else if (strncmp(ublox_info.hwVersion, "000A0000", 8) == 0) {
                LOG_INFO(DETECTED_MESSAGE, "U-blox 10", "10");
                return GNSS_MODEL_UBLOX10;
            }
        }
    }
    }
    LOG_WARN("No GNSS Module (baudrate %d)", serialSpeed);
    currentDelay = 2000;
    currentStep = 0;
    return GNSS_MODEL_UNKNOWN;
}

GnssModel_t GPS::getProbeResponse(unsigned long timeout, const std::vector<ChipInfo> &responseMap, int serialSpeed)
{
    // Calculate buffer size based on baud rate - 256 bytes for 9600 baud as baseline
    // Higher baud rates get proportionally larger buffers to handle more data
    int bufferSize = (serialSpeed * 256) / 9600;
    // Clamp buffer size between reasonable limits
    if (bufferSize < 128)
        bufferSize = 128;
    if (bufferSize > 2048)
        bufferSize = 2048;

    auto response = std::unique_ptr<char[]>(new char[bufferSize]); // Dynamically allocate based on baud rate
    uint16_t responseLen = 0;
    unsigned long start = millis();
    while (millis() - start < timeout) {
        if (_serial_gps->available()) {
            char c = _serial_gps->read();

            // Add char to buffer if there's space
            if (responseLen < bufferSize - 1) {
                response[responseLen++] = c;
                response[responseLen] = '\0';
            }

            if (c == ',' || (responseLen >= 2 && response[responseLen - 2] == '\r' && response[responseLen - 1] == '\n')) {
                // check if we can see our chips
                for (const auto &chipInfo : responseMap) {
                    if (strstr(response.get(), chipInfo.detectionString.c_str()) != nullptr) {
#ifdef GPS_DEBUG
                        LOG_DEBUG(response.get());
#endif
                        LOG_INFO("%s detected", chipInfo.chipName.c_str());
                        return chipInfo.driver;
                    }
                }
            }
            if (responseLen >= 2 && response[responseLen - 2] == '\r' && response[responseLen - 1] == '\n') {
#ifdef GPS_DEBUG
                LOG_DEBUG(response.get());
#endif
                // Reset the response buffer for the next potential message
                responseLen = 0;
                response[0] = '\0';
            }
        }
    }
#ifdef GPS_DEBUG
    LOG_DEBUG(response.get());
#endif
    return GNSS_MODEL_UNKNOWN; // Return unknown on timeout
}

std::unique_ptr<GPS> GPS::createGps()
{
    int8_t _rx_gpio = config.position.rx_gpio;
    int8_t _tx_gpio = config.position.tx_gpio;
    int8_t _en_gpio = config.position.gps_en_gpio;

#if defined(GPS_RX_PIN)
    if (!_rx_gpio)
        _rx_gpio = GPS_RX_PIN;
#endif
#if defined(GPS_TX_PIN)
    if (!_tx_gpio)
        _tx_gpio = GPS_TX_PIN;
#endif
#if defined(PIN_GPS_EN)
    if (!_en_gpio)
        _en_gpio = PIN_GPS_EN;
#endif
#ifdef ARCH_PORTDUINO
    if (!portduino_config.has_gps)
        return nullptr;
#endif
    if (!_rx_gpio || !_serial_gps) // Configured to have no GPS at all
        return nullptr;

    auto new_gps = std::unique_ptr<GPS>(new GPS());
    new_gps->rx_gpio = _rx_gpio;
    new_gps->tx_gpio = _tx_gpio;

    GpioVirtPin *virtPin = new GpioVirtPin();
    new_gps->enablePin = virtPin; // Always at least populate a virtual pin
    if (_en_gpio) {
        GpioPin *p = new GpioHwPin(_en_gpio);

        if (!GPS_EN_ACTIVE) { // Need to invert the pin before hardware
            new GpioNotTransformer(
                virtPin,
                p); // We just leave this created object on the heap so it can stay watching virtPin and driving en_gpio
        } else {
            new GpioUnaryTransformer(
                virtPin,
                p); // We just leave this created object on the heap so it can stay watching virtPin and driving en_gpio
        }
    }

#ifdef PIN_GPS_PPS
    // pulse per second
    pinMode(PIN_GPS_PPS, INPUT);
#endif

#ifdef PIN_GPS_SWITCH
    // toggle GPS via external GPIO switch
    pinMode(PIN_GPS_SWITCH, INPUT);
    gpsPeriodic = std::unique_ptr<concurrency::Periodic>(new concurrency::Periodic("GPSSwitch", gpsSwitch));
#endif

// Currently disabled per issue #525 (TinyGPS++ crash bug)
// when fixed upstream, can be un-disabled to enable 3D FixType and PDOP
#ifndef TINYGPS_OPTION_NO_CUSTOM_FIELDS
    // see NMEAGPS.h
    gsafixtype.begin(reader, NMEA_MSG_GXGSA, 2);
    gsapdop.begin(reader, NMEA_MSG_GXGSA, 15);
    LOG_DEBUG("Use " NMEA_MSG_GXGSA " for 3DFIX and PDOP");
#endif

    // Make sure the GPS is awake before performing any init.
    new_gps->up();

#ifdef PIN_GPS_RESET
    pinMode(PIN_GPS_RESET, OUTPUT);
    digitalWrite(PIN_GPS_RESET, !GPS_RESET_MODE);
#endif

    if (_serial_gps) {
#ifdef ARCH_ESP32
        // In esp32 framework, setRxBufferSize needs to be initialized before Serial
        _serial_gps->setRxBufferSize(SERIAL_BUFFER_SIZE); // the default is 256
#endif

        LOG_DEBUG("Use GPIO%d for GPS RX", new_gps->rx_gpio);
        LOG_DEBUG("Use GPIO%d for GPS TX", new_gps->tx_gpio);

//  ESP32 has a special set of parameters vs other arduino ports
#if defined(ARCH_ESP32)
        _serial_gps->begin(GPS_BAUDRATE, SERIAL_8N1, new_gps->rx_gpio, new_gps->tx_gpio);
#elif defined(ARCH_RP2040)
        _serial_gps->setPinout(new_gps->tx_gpio, new_gps->rx_gpio);
        _serial_gps->setFIFOSize(256);
        _serial_gps->begin(GPS_BAUDRATE);
#elif defined(ARCH_NRF52)
        _serial_gps->setPins(new_gps->rx_gpio, new_gps->tx_gpio);
        _serial_gps->begin(GPS_BAUDRATE);
#elif defined(ARCH_STM32WL)
        _serial_gps->setTx(new_gps->tx_gpio);
        _serial_gps->setRx(new_gps->rx_gpio);
        _serial_gps->begin(GPS_BAUDRATE);
#elif defined(ARCH_PORTDUINO)
        // Portduino can't set the GPS pins directly.
        _serial_gps->begin(GPS_BAUDRATE);
#else
#error Unsupported architecture!
#endif
    }
    return new_gps;
}

static int32_t toDegInt(RawDegrees d)
{
    int32_t degMult = 10000000; // 1e7
    int32_t r = d.deg * degMult + d.billionths / 100;
    if (d.negative)
        r *= -1;
    return r;
}

/**
 * Perform any processing that should be done only while the GPS is awake and looking for a fix.
 * Override this method to check for new locations
 *
 * @return true if we've set a new time
 */
bool GPS::lookForTime()
{
    auto ti = reader.time;
    auto d = reader.date;
    if (ti.isValid() && d.isValid()) { // Note: we don't check for updated, because we'll only be called if needed
        /* Convert to unix time
The Unix epoch (or Unix time or POSIX time or Unix timestamp) is the number of seconds that have elapsed since January 1,
1970 (midnight UTC/GMT), not counting leap seconds (in ISO 8601: 1970-01-01T00:00:00Z).
*/
        struct tm t;
        t.tm_sec = ti.second() + round(ti.age() / 1000);
        t.tm_min = ti.minute();
        t.tm_hour = ti.hour();
        t.tm_mday = d.day();
        t.tm_mon = d.month() - 1;
        t.tm_year = d.year() - 1900;
        t.tm_isdst = false;
        if (t.tm_mon > -1) {
            if (perhapsSetRTC(RTCQualityGPS, t) == RTCSetResultSuccess) {
                LOG_DEBUG("NMEA GPS time set %02d-%02d-%02d %02d:%02d:%02d age %d", d.year(), d.month(), t.tm_mday, t.tm_hour,
                          t.tm_min, t.tm_sec, ti.age());
                return true;
            } else {
                return false;
            }
        } else
            return false;
    } else
        return false;
}

/**
 * Perform any processing that should be done only while the GPS is awake and looking for a fix.
 * Override this method to check for new locations
 *
 * @return true if we've acquired a new location
 */
bool GPS::lookForLocation()
{
    // By default, TinyGPS++ does not parse GPGSA lines, which give us
    //   the 2D/3D fixType (see NMEAGPS.h)
    // At a minimum, use the fixQuality indicator in GPGGA (FIXME?)
    fixQual = reader.fixQuality();

#ifndef TINYGPS_OPTION_NO_STATISTICS
    if (reader.failedChecksum() > lastChecksumFailCount) {
// In a GPS_DEBUG build we want to log all of these. In production, we only care if there are many of them.
#ifndef GPS_DEBUG
        if (reader.failedChecksum() > 4)
#endif
            LOG_WARN("%u new GPS checksum failures, for a total of %u", reader.failedChecksum() - lastChecksumFailCount,
                     reader.failedChecksum());
        lastChecksumFailCount = reader.failedChecksum();
    }
#endif

#ifndef TINYGPS_OPTION_NO_CUSTOM_FIELDS
    fixType = atoi(gsafixtype.value()); // will set to zero if no data
#endif

    // check if GPS has an acceptable lock
    if (!hasLock())
        return false;

#ifdef GPS_DEBUG
    LOG_DEBUG("AGE: LOC=%d FIX=%d DATE=%d TIME=%d", reader.location.age(),
#ifndef TINYGPS_OPTION_NO_CUSTOM_FIELDS
              gsafixtype.age(),
#else
              0,
#endif
              reader.date.age(), reader.time.age());
#endif // GPS_DEBUG

    // Is this a new point or are we re-reading the previous one?
    if (!reader.location.isUpdated() && !reader.altitude.isUpdated())
        return false;

    // check if a complete GPS solution set is available for reading
    //   tinyGPSDatum::age() also includes isValid() test
    // FIXME
    if (!((reader.location.age() < GPS_SOL_EXPIRY_MS) &&
#ifndef TINYGPS_OPTION_NO_CUSTOM_FIELDS
          (gsafixtype.age() < GPS_SOL_EXPIRY_MS) &&
#endif
          (reader.time.age() < GPS_SOL_EXPIRY_MS) && (reader.date.age() < GPS_SOL_EXPIRY_MS))) {
        LOG_WARN("SOME data is TOO OLD: LOC %u, TIME %u, DATE %u", reader.location.age(), reader.time.age(), reader.date.age());
        return false;
    }

    // We know the solution is fresh and valid, so just read the data
    auto loc = reader.location.value();

    // Bail out EARLY to avoid overwriting previous good data (like #857)
    if (toDegInt(loc.lat) > 900000000) {
#ifdef GPS_DEBUG
        LOG_DEBUG("Bail out EARLY on LAT %i", toDegInt(loc.lat));
#endif
        return false;
    }
    if (toDegInt(loc.lng) > 1800000000) {
#ifdef GPS_DEBUG
        LOG_DEBUG("Bail out EARLY on LNG %i", toDegInt(loc.lng));
#endif
        return false;
    }

    p.location_source = meshtastic_Position_LocSource_LOC_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
    p.HDOP = reader.hdop.value();
    p.PDOP = TinyGPSPlus::parseDecimal(gsapdop.value());
#else
    // FIXME! naive PDOP emulation (assumes VDOP==HDOP)
    // correct formula is PDOP = SQRT(HDOP^2 + VDOP^2)
    p.HDOP = reader.hdop.value();
    p.PDOP = 1.41 * reader.hdop.value();
#endif

    // Discard incomplete or erroneous readings
    if (reader.hdop.value() == 0) {
        LOG_WARN("BOGUS hdop.value() REJECTED: %d", reader.hdop.value());
        return false;
    }

    p.latitude_i = toDegInt(loc.lat);
    p.longitude_i = toDegInt(loc.lng);

    p.altitude_geoidal_separation = reader.geoidHeight.meters();
    p.altitude_hae = reader.altitude.meters() + p.altitude_geoidal_separation;
    p.altitude = reader.altitude.meters();

    p.fix_quality = fixQual;
#ifndef TINYGPS_OPTION_NO_CUSTOM_FIELDS
    p.fix_type = fixType;
#endif

    // positional timestamp
    struct tm t;
    t.tm_sec = reader.time.second();
    t.tm_min = reader.time.minute();
    t.tm_hour = reader.time.hour();
    t.tm_mday = reader.date.day();
    t.tm_mon = reader.date.month() - 1;
    t.tm_year = reader.date.year() - 1900;
    t.tm_isdst = false;
    p.timestamp = gm_mktime(&t);

    // Nice to have, if available
    if (reader.satellites.isUpdated()) {
        p.sats_in_view = reader.satellites.value();
    }

    if (reader.course.isUpdated() && reader.course.isValid()) {
        if (reader.course.value() < 36000) { // sanity check
            p.ground_track =
                reader.course.value() * 1e3; // Scale the heading (in degrees * 10^-2) to match the expected degrees * 10^-5
        } else {
            LOG_WARN("BOGUS course.value() REJECTED: %d", reader.course.value());
        }
    }

    if (reader.speed.isUpdated() && reader.speed.isValid()) {
        p.ground_speed = reader.speed.kmph();
    }

    return true;
}

bool GPS::hasLock()
{
    // Using GPGGA fix quality indicator
    if (fixQual >= 1 && fixQual <= 5) {
#ifndef TINYGPS_OPTION_NO_CUSTOM_FIELDS
        // Use GPGSA fix type 2D/3D (better) if available
        if (fixType == 3 || fixType == 0) // zero means "no data received"
#endif
            return true;
    }

    return false;
}

bool GPS::hasFlow()
{
    return reader.passedChecksum() > 0;
}

bool GPS::whileActive()
{
    unsigned int charsInBuf = 0;
    bool isValid = false;
#ifdef GPS_DEBUG
    std::string debugmsg = "";
#endif
    if (powerState != GPS_ACTIVE) {
        clearBuffer();
        return false;
    }
#ifdef SERIAL_BUFFER_SIZE
    if (_serial_gps->available() >= SERIAL_BUFFER_SIZE - 1) {
        LOG_WARN("GPS Buffer full with %u bytes waiting. Flush to avoid corruption", _serial_gps->available());
        clearBuffer();
    }
#endif
    // First consume any chars that have piled up at the receiver
    while (_serial_gps->available() > 0) {
        int c = _serial_gps->read();
        UBXscratch[charsInBuf] = c;
#ifdef GPS_DEBUG
        debugmsg += vformat("%c", (c >= 32 && c <= 126) ? c : '.');
#endif
        isValid |= reader.encode(c);
        if (charsInBuf > sizeof(UBXscratch) - 10 || c == '\r') {
            if (strnstr((char *)UBXscratch, "$GPTXT,01,01,02,u-blox ag - www.u-blox.com*50", charsInBuf)) {
                rebootsSeen++;
            }
            charsInBuf = 0;
        } else {
            charsInBuf++;
        }
    }
#ifdef GPS_DEBUG
    if (debugmsg != "") {
        LOG_DEBUG(debugmsg.c_str());
    }
#endif
    return isValid;
}
void GPS::enable()
{
    // Clear the old scheduling info (reset the lock-time prediction)
    scheduling.reset();

    enabled = true;
    setInterval(GPS_THREAD_INTERVAL);

    scheduling.informSearching();
    setPowerState(GPS_ACTIVE);
}

int32_t GPS::disable()
{
    enabled = false;
    setInterval(INT32_MAX);
    setPowerState(GPS_OFF);

    return INT32_MAX;
}

void GPS::toggleGpsMode()
{
    if (config.position.gps_mode == meshtastic_Config_PositionConfig_GpsMode_ENABLED) {
        config.position.gps_mode = meshtastic_Config_PositionConfig_GpsMode_DISABLED;
        LOG_INFO("User toggled GpsMode. Now DISABLED");
        playGPSDisableBeep();
#ifdef GNSS_AIROHA
        if (powerState == GPS_ACTIVE) {
            LOG_DEBUG("User power Off GPS");
            digitalWrite(PIN_GPS_EN, LOW);
        }
#endif
        disable();
    } else if (config.position.gps_mode == meshtastic_Config_PositionConfig_GpsMode_DISABLED) {
        config.position.gps_mode = meshtastic_Config_PositionConfig_GpsMode_ENABLED;
        LOG_INFO("User toggled GpsMode. Now ENABLED");
        playGPSEnableBeep();
        enable();
    }
}
#endif // Exclude GPS