From 903f61960993c751de7334072649423f185b7bc6 Mon Sep 17 00:00:00 2001 From: Jonathan Bennett Date: Mon, 28 Aug 2023 11:13:17 -0500 Subject: [PATCH 01/12] Add GPS serial speed scan --- src/gps/GPS.cpp | 64 +++++++++++++++++++++-------------------------- src/gps/GPS.h | 2 +- src/meshUtils.cpp | 58 ++++++++++++++++++++++++++++++++++++++++++ src/meshUtils.h | 7 +++++- 4 files changed, 94 insertions(+), 37 deletions(-) create mode 100644 src/meshUtils.cpp diff --git a/src/gps/GPS.cpp b/src/gps/GPS.cpp index ab5156fab..170304a61 100644 --- a/src/gps/GPS.cpp +++ b/src/gps/GPS.cpp @@ -4,6 +4,10 @@ #include "configuration.h" #include "sleep.h" +#ifdef ARCH_PORTDUINO +#include "meshUtils.h" +#endif + #ifndef GPS_RESET_MODE #define GPS_RESET_MODE HIGH #endif @@ -203,7 +207,17 @@ bool GPS::setupGPS() /* * T-Beam-S3-Core will be preset to use gps Probe here, and other boards will not be changed first */ - gnssModel = probe(); +#if defined(GPS_UC6580) + _serial_gps->updateBaudRate(115200); + gnssModel = GNSS_MODEL_UC6850; +#else + for (int serialSpeed : {9600, 4800, 38400, 57600, 115200}) { + LOG_DEBUG("Probing for GPS at %d \n", serialSpeed); + gnssModel = probe(serialSpeed); + if (gnssModel != GNSS_MODEL_UNKNOWN) + break; + } +#endif if (gnssModel == GNSS_MODEL_MTK) { /* @@ -262,7 +276,6 @@ bool GPS::setupGPS() // what is specified in the Ublox documentation. // There is also a possibility that the module may be GPS-only. LOG_INFO("Unable to reconfigure GNSS - defaults maintained. Is this module GPS-only?\n"); - return true; } else { LOG_INFO("GNSS configured for GPS+SBAS+GLONASS. Pause for 0.75s before sending next command.\n"); // Documentation say, we need wait atleast 0.5s after reconfiguration of GNSS module, before sending next commands @@ -297,7 +310,6 @@ bool GPS::setupGPS() if (!getACK(0x06, 0x39)) { LOG_WARN("Unable to enable interference resistance.\n"); - return true; } // Configure navigation engine expert settings: @@ -343,7 +355,6 @@ bool GPS::setupGPS() if (!getACK(0x06, 0x23)) { LOG_WARN("Unable to configure extra settings.\n"); - return true; } /* @@ -384,7 +395,6 @@ bool GPS::setupGPS() if (!getACK(0x06, 0x08)) { LOG_WARN("Unable to set GPS update rate.\n"); - return true; } // Disable GGL. GGL - Geographic position (latitude and longitude), which provides the current geographical @@ -408,7 +418,6 @@ bool GPS::setupGPS() if (!getACK(0x06, 0x01)) { LOG_WARN("Unable to disable NMEA GGL.\n"); - return true; } // Enable GSA. GSA - GPS DOP and active satellites, used for detailing the satellites used in the positioning and @@ -427,7 +436,6 @@ bool GPS::setupGPS() _serial_gps->write(_message_GSA, sizeof(_message_GSA)); if (!getACK(0x06, 0x01)) { LOG_WARN("Unable to Enable NMEA GSA.\n"); - return true; } // Disable GSV. GSV - Satellites in view, details the number and location of satellites in view. @@ -445,7 +453,6 @@ bool GPS::setupGPS() _serial_gps->write(_message_GSV, sizeof(_message_GSV)); if (!getACK(0x06, 0x01)) { LOG_WARN("Unable to disable NMEA GSV.\n"); - return true; } // Disable VTG. VTG - Track made good and ground speed, which provides course and speed information relative to @@ -464,7 +471,6 @@ bool GPS::setupGPS() _serial_gps->write(_message_VTG, sizeof(_message_VTG)); if (!getACK(0x06, 0x01)) { LOG_WARN("Unable to disable NMEA VTG.\n"); - return true; } // Enable RMC. RMC - Recommended Minimum data, the essential gps pvt (position, velocity, time) data. @@ -482,7 +488,6 @@ bool GPS::setupGPS() _serial_gps->write(_message_RMC, sizeof(_message_RMC)); if (!getACK(0x06, 0x01)) { LOG_WARN("Unable to enable NMEA RMC.\n"); - return true; } // Enable GGA. GGA - Global Positioning System Fix Data, which provides 3D location and accuracy data. @@ -500,7 +505,6 @@ bool GPS::setupGPS() _serial_gps->write(_message_GGA, sizeof(_message_GGA)); if (!getACK(0x06, 0x01)) { LOG_WARN("Unable to enable NMEA GGA.\n"); - return true; } // The Power Management configuration allows the GPS module to operate in different power modes for optimized power @@ -534,8 +538,6 @@ bool GPS::setupGPS() _serial_gps->write(UBX_CFG_PMS, sizeof(UBX_CFG_PMS)); if (!getACK(0x06, 0x86)) { LOG_WARN("Unable to enable powersaving for GPS.\n"); - // T-beam doesn't support this mode. - // Don't bail from function early. } // We need save configuration to flash to make our config changes persistent @@ -558,10 +560,8 @@ bool GPS::setupGPS() if (!getACK(0x06, 0x09)) { LOG_WARN("Unable to save GNSS module configuration.\n"); - return true; } else { LOG_INFO("GNSS module configuration saved!\n"); - return true; } } } @@ -855,18 +855,15 @@ int GPS::prepareDeepSleep(void *unused) return 0; } -GnssModel_t GPS::probe() +GnssModel_t GPS::probe(int serialSpeed) { - memset(&info, 0, sizeof(struct uBloxGnssModelInfo)); -// return immediately if the model is set by the variant.h file -// #ifdef GPS_UBLOX (unless it's a ublox, because we might want to know the module info! -// return GNSS_MODEL_UBLOX; think about removing this macro and return) -#if defined(GPS_L76K) - return GNSS_MODEL_MTK; -#elif defined(GPS_UC6580) - _serial_gps->updateBaudRate(115200); - return GNSS_MODEL_UC6850; +#if defined(ARCH_NRF52) || defined(ARCH_PORTDUINO) || defined(ARCH_RP2040) + _serial_gps->end(); + _serial_gps->begin(serialSpeed); #else + _serial_gps->updateBaudRate(serialSpeed); +#endif + memset(&info, 0, sizeof(struct uBloxGnssModelInfo)); uint8_t buffer[384] = {0}; // Close all NMEA sentences , Only valid for MTK platform @@ -874,20 +871,17 @@ GnssModel_t GPS::probe() delay(20); // Get version information + clearBuffer(); _serial_gps->write("$PCAS06,0*1B\r\n"); uint32_t startTimeout = millis() + 500; while (millis() < startTimeout) { if (_serial_gps->available()) { - String ver = _serial_gps->readStringUntil('\r'); // Get module info , If the correct header is returned, // it can be determined that it is the MTK chip - int index = ver.indexOf("$"); - if (index != -1) { - ver = ver.substring(index); - if (ver.startsWith("$GPTXT,01,01,02,SW=")) { - LOG_INFO("L76K GNSS init succeeded, using L76K GNSS Module\n"); - return GNSS_MODEL_MTK; - } + int bytesRead = _serial_gps->readBytesUntil('\r', buffer, sizeof(buffer)); + if (strnstr((char *)buffer, "$GPTXT,01,01,02,SW=", bytesRead) != nullptr) { + LOG_INFO("L76K GNSS init succeeded, using L76K GNSS Module\n"); + return GNSS_MODEL_MTK; } } } @@ -897,9 +891,10 @@ GnssModel_t GPS::probe() _serial_gps->write(cfg_rate, sizeof(cfg_rate)); // Check that the returned response class and message ID are correct if (!getAck(buffer, 384, 0x06, 0x08)) { - LOG_WARN("Failed to find UBlox & MTK GNSS Module\n"); + LOG_WARN("Failed to find UBlox & MTK GNSS Module using baudrate %d\n", serialSpeed); return GNSS_MODEL_UNKNOWN; } + LOG_INFO("Found a UBlox Module using baudrate %d\n", serialSpeed); memset(buffer, 0, sizeof(buffer)); byte _message_MONVER[8] = { 0xB5, 0x62, // Sync message for UBX protocol @@ -971,7 +966,6 @@ GnssModel_t GPS::probe() } return GNSS_MODEL_UBLOX; -#endif } #if HAS_GPS diff --git a/src/gps/GPS.h b/src/gps/GPS.h index 89f6c491d..9ad4a9c3e 100644 --- a/src/gps/GPS.h +++ b/src/gps/GPS.h @@ -172,7 +172,7 @@ class GPS : private concurrency::OSThread // Get GNSS model String getNMEA(); - GnssModel_t probe(); + GnssModel_t probe(int serialSpeed); int getAck(uint8_t *buffer, uint16_t size, uint8_t requestedClass, uint8_t requestedID); diff --git a/src/meshUtils.cpp b/src/meshUtils.cpp new file mode 100644 index 000000000..cab05e54b --- /dev/null +++ b/src/meshUtils.cpp @@ -0,0 +1,58 @@ +#include "meshUtils.h" +#include + +/* + * Find the first occurrence of find in s, where the search is limited to the + * first slen characters of s. + * - + * Copyright (c) 2001 Mike Barcroft + * Copyright (c) 1990, 1993 + * The Regents of the University of California. All rights reserved. + * + * This code is derived from software contributed to Berkeley by + * Chris Torek. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * 3. Neither the name of the University nor the names of its contributors + * may be used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE REGENTS AND CONTRIBUTORS ``AS IS'' AND + * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE REGENTS OR CONTRIBUTORS BE LIABLE + * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL + * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS + * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) + * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY + * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF + * SUCH DAMAGE. + * + */ +char *strnstr(const char *s, const char *find, size_t slen) +{ + char c, sc; + size_t len; + + if ((c = *find++) != '\0') { + len = strlen(find); + do { + do { + if (slen-- < 1 || (sc = *s++) == '\0') + return (NULL); + } while (sc != c); + if (len > slen) + return (NULL); + } while (strncmp(s, find, len) != 0); + s--; + } + return ((char *)s); +} \ No newline at end of file diff --git a/src/meshUtils.h b/src/meshUtils.h index a725bf13c..a6436a8d5 100644 --- a/src/meshUtils.h +++ b/src/meshUtils.h @@ -4,4 +4,9 @@ template constexpr const T &clamp(const T &v, const T &lo, const T &hi) { return (v < lo) ? lo : (hi < v) ? hi : v; -} \ No newline at end of file +} + +#if (defined(ARCH_PORTDUINO) && !defined(STRNSTR)) +#define STRNSTR +char *strnstr(const char *s, const char *find, size_t slen); +#endif \ No newline at end of file From 3d2c419d0df59c298bbca4eb493785486a450861 Mon Sep 17 00:00:00 2001 From: code8buster <20384924+code8buster@users.noreply.github.com> Date: Wed, 30 Aug 2023 16:24:56 -0400 Subject: [PATCH 02/12] Remove leftover debug msg --- src/gps/GPS.cpp | 1 - 1 file changed, 1 deletion(-) diff --git a/src/gps/GPS.cpp b/src/gps/GPS.cpp index 170304a61..281f32065 100644 --- a/src/gps/GPS.cpp +++ b/src/gps/GPS.cpp @@ -931,7 +931,6 @@ GnssModel_t GPS::probe(int serialSpeed) LOG_DEBUG("Module Info : \n"); LOG_DEBUG("Soft version: %s\n", info.swVersion); - LOG_DEBUG("first char is %c\n", (char)info.swVersion[0]); LOG_DEBUG("Hard version: %s\n", info.hwVersion); LOG_DEBUG("Extensions:%d\n", info.extensionNo); for (int i = 0; i < info.extensionNo; i++) { From cf762bbd42ed7bf37f382a61fbcb763d1b17a4a6 Mon Sep 17 00:00:00 2001 From: Jonathan Bennett Date: Thu, 31 Aug 2023 20:39:23 -0500 Subject: [PATCH 03/12] Cut down delay times for GPS probe and init --- src/gps/GPS.cpp | 55 ++++++++++++++++++++++++++----------------------- src/gps/GPS.h | 4 ++-- 2 files changed, 31 insertions(+), 28 deletions(-) diff --git a/src/gps/GPS.cpp b/src/gps/GPS.cpp index 281f32065..8d80f8d42 100644 --- a/src/gps/GPS.cpp +++ b/src/gps/GPS.cpp @@ -47,7 +47,7 @@ void GPS::UBXChecksum(byte *message, size_t length) message[length - 1] = CK_B; } -bool GPS::getACK(uint8_t class_id, uint8_t msg_id) +bool GPS::getACK(uint8_t class_id, uint8_t msg_id, int waitMillis) { uint8_t b; uint8_t ack = 0; @@ -66,15 +66,11 @@ bool GPS::getACK(uint8_t class_id, uint8_t msg_id) buf[9] += buf[8]; } - while (1) { + while (millis() - startTime < waitMillis) { if (ack > 9) { - // LOG_INFO("Got ACK for class %02X message %02X\n", class_id, msg_id); + // LOG_INFO("Got ACK for class %02X message %02X in %d millis.\n", class_id, msg_id, millis() - startTime); return true; // ACK received } - if (millis() - startTime > 3000) { - LOG_WARN("No response for class %02X message %02X\n", class_id, msg_id); - return false; // No response received within 3 seconds - } if (_serial_gps->available()) { b = _serial_gps->read(); if (b == buf[ack]) { @@ -88,6 +84,8 @@ bool GPS::getACK(uint8_t class_id, uint8_t msg_id) } } } + // LOG_WARN("No response for class %02X message %02X\n", class_id, msg_id); + return false; // No response received within timeout } /** @@ -99,14 +97,14 @@ bool GPS::getACK(uint8_t class_id, uint8_t msg_id) * @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) +int GPS::getAck(uint8_t *buffer, uint16_t size, uint8_t requestedClass, uint8_t requestedID, int waitMillis) { uint16_t ubxFrameCounter = 0; uint32_t startTime = millis(); uint16_t needRead; - while (millis() - startTime < 1200) { - while (_serial_gps->available()) { + while (millis() - startTime < waitMillis) { + if (_serial_gps->available()) { int c = _serial_gps->read(); switch (ubxFrameCounter) { case 0: @@ -159,6 +157,8 @@ int GPS::getAck(uint8_t *buffer, uint16_t size, uint8_t requestedClass, uint8_t ubxFrameCounter = 0; } else { // return payload length + // LOG_INFO("Got ACK for class %02X message %02X in %d millis.\n", requestedClass, requestedID, millis() - + // startTime); return needRead; } break; @@ -168,6 +168,7 @@ int GPS::getAck(uint8_t *buffer, uint16_t size, uint8_t requestedClass, uint8_t } } } + // LOG_WARN("No response for class %02X message %02X\n", requestedClass, requestedID); return 0; } @@ -268,7 +269,7 @@ bool GPS::setupGPS() // Send the message to the module _serial_gps->write(_message_GNSS, sizeof(_message_GNSS)); - if (!getACK(0x06, 0x3e)) { + if (!getACK(0x06, 0x3e, 300)) { // It's not critical if the module doesn't acknowledge this configuration. // The module should operate adequately with its factory or previously saved settings. // It appears that there is a firmware bug in some GPS modules: When an attempt is made @@ -308,7 +309,7 @@ bool GPS::setupGPS() // Send the message to the module _serial_gps->write(_message_JAM, sizeof(_message_JAM)); - if (!getACK(0x06, 0x39)) { + if (!getACK(0x06, 0x39, 300)) { LOG_WARN("Unable to enable interference resistance.\n"); } @@ -353,7 +354,7 @@ bool GPS::setupGPS() // Send the message to the module _serial_gps->write(_message_NAVX5, sizeof(_message_NAVX5)); - if (!getACK(0x06, 0x23)) { + if (!getACK(0x06, 0x23, 300)) { LOG_WARN("Unable to configure extra settings.\n"); } @@ -365,7 +366,7 @@ bool GPS::setupGPS() byte _message_nmea[] = {0xB5, 0x62, 0x06, 0x00, 0x14, 0x00, 0x01, 0x00, 0x00, 0x00, 0xC0, 0x08, 0x00, 0x00, 0x80, 0x25, 0x00, 0x00, 0x07, 0x00, 0x02, 0x00, 0x00, 0x00, 0x00, 0x00, 0x91, 0xAF}; _serial_gps->write(_message_nmea, sizeof(_message_nmea)); - if (!getACK(0x06, 0x00)) { + if (!getACK(0x06, 0x00, 300)) { LOG_WARN("Unable to enable NMEA Mode.\n"); return true; } @@ -393,7 +394,7 @@ bool GPS::setupGPS() // Send the message to the module _serial_gps->write(_message_1Hz, sizeof(_message_1Hz)); - if (!getACK(0x06, 0x08)) { + if (!getACK(0x06, 0x08, 300)) { LOG_WARN("Unable to set GPS update rate.\n"); } @@ -416,7 +417,7 @@ bool GPS::setupGPS() // Send the message to the module _serial_gps->write(_message_GGL, sizeof(_message_GGL)); - if (!getACK(0x06, 0x01)) { + if (!getACK(0x06, 0x01, 300)) { LOG_WARN("Unable to disable NMEA GGL.\n"); } @@ -434,7 +435,7 @@ bool GPS::setupGPS() }; UBXChecksum(_message_GSA, sizeof(_message_GSA)); _serial_gps->write(_message_GSA, sizeof(_message_GSA)); - if (!getACK(0x06, 0x01)) { + if (!getACK(0x06, 0x01, 300)) { LOG_WARN("Unable to Enable NMEA GSA.\n"); } @@ -451,7 +452,7 @@ bool GPS::setupGPS() }; UBXChecksum(_message_GSV, sizeof(_message_GSV)); _serial_gps->write(_message_GSV, sizeof(_message_GSV)); - if (!getACK(0x06, 0x01)) { + if (!getACK(0x06, 0x01, 300)) { LOG_WARN("Unable to disable NMEA GSV.\n"); } @@ -469,7 +470,7 @@ bool GPS::setupGPS() }; UBXChecksum(_message_VTG, sizeof(_message_VTG)); _serial_gps->write(_message_VTG, sizeof(_message_VTG)); - if (!getACK(0x06, 0x01)) { + if (!getACK(0x06, 0x01, 300)) { LOG_WARN("Unable to disable NMEA VTG.\n"); } @@ -486,7 +487,7 @@ bool GPS::setupGPS() }; UBXChecksum(_message_RMC, sizeof(_message_RMC)); _serial_gps->write(_message_RMC, sizeof(_message_RMC)); - if (!getACK(0x06, 0x01)) { + if (!getACK(0x06, 0x01, 300)) { LOG_WARN("Unable to enable NMEA RMC.\n"); } @@ -503,7 +504,7 @@ bool GPS::setupGPS() }; UBXChecksum(_message_GGA, sizeof(_message_GGA)); _serial_gps->write(_message_GGA, sizeof(_message_GGA)); - if (!getACK(0x06, 0x01)) { + if (!getACK(0x06, 0x01, 300)) { LOG_WARN("Unable to enable NMEA GGA.\n"); } @@ -536,7 +537,7 @@ bool GPS::setupGPS() // Send the message to the module _serial_gps->write(UBX_CFG_PMS, sizeof(UBX_CFG_PMS)); - if (!getACK(0x06, 0x86)) { + if (!getACK(0x06, 0x86, 300)) { LOG_WARN("Unable to enable powersaving for GPS.\n"); } @@ -558,7 +559,7 @@ bool GPS::setupGPS() // Send the message to the module _serial_gps->write(_message_SAVE, sizeof(_message_SAVE)); - if (!getACK(0x06, 0x09)) { + if (!getACK(0x06, 0x09, 300)) { LOG_WARN("Unable to save GNSS module configuration.\n"); } else { LOG_INFO("GNSS module configuration saved!\n"); @@ -864,7 +865,7 @@ GnssModel_t GPS::probe(int serialSpeed) _serial_gps->updateBaudRate(serialSpeed); #endif memset(&info, 0, sizeof(struct uBloxGnssModelInfo)); - uint8_t buffer[384] = {0}; + uint8_t buffer[768] = {0}; // Close all NMEA sentences , Only valid for MTK platform _serial_gps->write("$PCAS03,0,0,0,0,0,0,0,0,0,0,,,0,0*02\r\n"); @@ -888,9 +889,10 @@ GnssModel_t GPS::probe(int serialSpeed) 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 - if (!getAck(buffer, 384, 0x06, 0x08)) { + if (!getAck(buffer, sizeof(buffer), 0x06, 0x08, 750)) { LOG_WARN("Failed to find UBlox & MTK GNSS Module using baudrate %d\n", serialSpeed); return GNSS_MODEL_UNKNOWN; } @@ -904,9 +906,10 @@ GnssModel_t GPS::probe(int serialSpeed) }; // 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, 384, 0x0A, 0x04); + uint16_t len = getAck(buffer, sizeof(buffer), 0x0A, 0x04, 1200); if (len) { // LOG_DEBUG("monver reply size = %d\n", len); uint16_t position = 0; diff --git a/src/gps/GPS.h b/src/gps/GPS.h index 9ad4a9c3e..1eb96f8cc 100644 --- a/src/gps/GPS.h +++ b/src/gps/GPS.h @@ -161,7 +161,7 @@ class GPS : private concurrency::OSThread */ uint32_t getSleepTime() const; - bool getACK(uint8_t c, uint8_t i); + bool getACK(uint8_t c, uint8_t i, int waitMillis); /** * Tell users we have new GPS readings @@ -174,7 +174,7 @@ class GPS : private concurrency::OSThread String getNMEA(); GnssModel_t probe(int serialSpeed); - int getAck(uint8_t *buffer, uint16_t size, uint8_t requestedClass, uint8_t requestedID); + int getAck(uint8_t *buffer, uint16_t size, uint8_t requestedClass, uint8_t requestedID, int waitMillis); // delay counter to allow more sats before fixed position stops GPS thread uint8_t fixeddelayCtr = 0; From 79cfc4b72546809688e750a02eb2273568d99c95 Mon Sep 17 00:00:00 2001 From: Jonathan Bennett Date: Thu, 31 Aug 2023 20:40:01 -0500 Subject: [PATCH 04/12] Avoid Serial output mangling with RTOS. --- src/RedirectablePrint.cpp | 15 ++++++++++++++- src/RedirectablePrint.h | 10 ++++++++-- src/SerialConsole.cpp | 1 + 3 files changed, 23 insertions(+), 3 deletions(-) diff --git a/src/RedirectablePrint.cpp b/src/RedirectablePrint.cpp index 0e8e1c798..2d73c7c9b 100644 --- a/src/RedirectablePrint.cpp +++ b/src/RedirectablePrint.cpp @@ -18,6 +18,12 @@ NoopPrint noopPrint; #if HAS_WIFI || HAS_ETHERNET extern Syslog syslog; #endif +void RedirectablePrint::rpInit() +{ +#ifdef HAS_FREE_RTOS + inDebugPrint = xSemaphoreCreateMutexStatic(&this->_MutexStorageSpace); +#endif +} void RedirectablePrint::setDestination(Print *_dest) { @@ -66,9 +72,12 @@ size_t RedirectablePrint::log(const char *logLevel, const char *format, ...) return 0; } size_t r = 0; - +#ifdef HAS_FREE_RTOS + if (inDebugPrint != nullptr && xSemaphoreTake(inDebugPrint, portMAX_DELAY) == pdTRUE) { +#else if (!inDebugPrint) { inDebugPrint = true; +#endif va_list arg; va_start(arg, format); @@ -141,7 +150,11 @@ size_t RedirectablePrint::log(const char *logLevel, const char *format, ...) va_end(arg); isContinuationMessage = !hasNewline; +#ifdef HAS_FREE_RTOS + xSemaphoreGive(inDebugPrint); +#else inDebugPrint = false; +#endif } return r; diff --git a/src/RedirectablePrint.h b/src/RedirectablePrint.h index 560021972..31cc1b6ef 100644 --- a/src/RedirectablePrint.h +++ b/src/RedirectablePrint.h @@ -1,5 +1,6 @@ #pragma once +#include "../freertosinc.h" #include #include #include @@ -16,14 +17,19 @@ class RedirectablePrint : public Print /// Used to allow multiple logDebug messages to appear on a single log line bool isContinuationMessage = false; +#ifdef HAS_FREE_RTOS + SemaphoreHandle_t inDebugPrint = nullptr; + StaticSemaphore_t _MutexStorageSpace; +#else volatile bool inDebugPrint = false; - +#endif public: explicit RedirectablePrint(Print *_dest) : dest(_dest) {} /** * Set a new destination */ + void rpInit(); void setDestination(Print *dest); virtual size_t write(uint8_t c); @@ -54,4 +60,4 @@ class NoopPrint : public Print /** * A printer that doesn't go anywhere */ -extern NoopPrint noopPrint; +extern NoopPrint noopPrint; \ No newline at end of file diff --git a/src/SerialConsole.cpp b/src/SerialConsole.cpp index e827dcf3b..ed217c3ed 100644 --- a/src/SerialConsole.cpp +++ b/src/SerialConsole.cpp @@ -12,6 +12,7 @@ SerialConsole *console; void consoleInit() { new SerialConsole(); // Must be dynamically allocated because we are now inheriting from thread + DEBUG_PORT.rpInit(); // Simply sets up semaphore } void consolePrintf(const char *format, ...) From a61f969773de1606d5ad38b2cbedc2aba3f6c527 Mon Sep 17 00:00:00 2001 From: Jonathan Bennett Date: Thu, 31 Aug 2023 22:19:50 -0500 Subject: [PATCH 05/12] Increase GPS detection timeout slightly --- src/gps/GPS.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/gps/GPS.cpp b/src/gps/GPS.cpp index 8d80f8d42..b14d31de4 100644 --- a/src/gps/GPS.cpp +++ b/src/gps/GPS.cpp @@ -892,7 +892,7 @@ GnssModel_t GPS::probe(int serialSpeed) clearBuffer(); _serial_gps->write(cfg_rate, sizeof(cfg_rate)); // Check that the returned response class and message ID are correct - if (!getAck(buffer, sizeof(buffer), 0x06, 0x08, 750)) { + if (!getAck(buffer, sizeof(buffer), 0x06, 0x08, 1000)) { LOG_WARN("Failed to find UBlox & MTK GNSS Module using baudrate %d\n", serialSpeed); return GNSS_MODEL_UNKNOWN; } From 6803fd7949505867a7aa0028c58b3dfeb460b079 Mon Sep 17 00:00:00 2001 From: Jonathan Bennett Date: Fri, 1 Sep 2023 11:55:46 -0500 Subject: [PATCH 06/12] More fixes for GPS chips with unexpected baud --- src/gps/GPS.cpp | 77 +++++++++++++++++++++++++++++++------------------ src/gps/GPS.h | 5 ++-- 2 files changed, 51 insertions(+), 31 deletions(-) diff --git a/src/gps/GPS.cpp b/src/gps/GPS.cpp index b14d31de4..34352032f 100644 --- a/src/gps/GPS.cpp +++ b/src/gps/GPS.cpp @@ -47,6 +47,30 @@ void GPS::UBXChecksum(byte *message, size_t length) message[length - 1] = CK_B; } +bool GPS::getACK(const char *message, int waitMillis) +{ + uint8_t buffer[768] = {0}; + uint8_t b; + int bytesRead; + uint32_t startTimeout = millis() + waitMillis; + while (millis() < startTimeout) { + bytesRead = 0; + while ((_serial_gps->available()) && (millis() < startTimeout)) { + b = _serial_gps->read(); + buffer[bytesRead] = b; + bytesRead++; + if ((bytesRead == 768) || (b == '\r')) + break; + // Get module info , If the correct header is returned, + // it can be determined that it is the MTK chip + } + if (strnstr((char *)buffer, message, bytesRead) != nullptr) { + return true; + } + } + return false; +} + bool GPS::getACK(uint8_t class_id, uint8_t msg_id, int waitMillis) { uint8_t b; @@ -68,7 +92,7 @@ bool GPS::getACK(uint8_t class_id, uint8_t msg_id, int waitMillis) while (millis() - startTime < waitMillis) { if (ack > 9) { - // LOG_INFO("Got ACK for class %02X message %02X in %d millis.\n", class_id, msg_id, millis() - startTime); + LOG_INFO("Got ACK for class %02X message %02X in %d millis.\n", class_id, msg_id, millis() - startTime); return true; // ACK received } if (_serial_gps->available()) { @@ -84,7 +108,7 @@ bool GPS::getACK(uint8_t class_id, uint8_t msg_id, int waitMillis) } } } - // LOG_WARN("No response for class %02X message %02X\n", class_id, msg_id); + LOG_WARN("No response for class %02X message %02X\n", class_id, msg_id); return false; // No response received within timeout } @@ -358,20 +382,6 @@ bool GPS::setupGPS() LOG_WARN("Unable to configure extra settings.\n"); } - /* - tips: NMEA Only should not be set here, otherwise initializing Ublox gnss module again after - setting will not output command messages in UART1, resulting in unrecognized module information - - // Set the UART port to output NMEA only - byte _message_nmea[] = {0xB5, 0x62, 0x06, 0x00, 0x14, 0x00, 0x01, 0x00, 0x00, 0x00, 0xC0, 0x08, 0x00, 0x00, - 0x80, 0x25, 0x00, 0x00, 0x07, 0x00, 0x02, 0x00, 0x00, 0x00, 0x00, 0x00, 0x91, 0xAF}; - _serial_gps->write(_message_nmea, sizeof(_message_nmea)); - if (!getACK(0x06, 0x00, 300)) { - LOG_WARN("Unable to enable NMEA Mode.\n"); - return true; - } - */ - // ublox-M10S can be compatible with UBLOX traditional protocol, so the following sentence settings are also valid // Set GPS update rate to 1Hz @@ -874,17 +884,9 @@ GnssModel_t GPS::probe(int serialSpeed) // Get version information clearBuffer(); _serial_gps->write("$PCAS06,0*1B\r\n"); - uint32_t startTimeout = millis() + 500; - while (millis() < startTimeout) { - if (_serial_gps->available()) { - // Get module info , If the correct header is returned, - // it can be determined that it is the MTK chip - int bytesRead = _serial_gps->readBytesUntil('\r', buffer, sizeof(buffer)); - if (strnstr((char *)buffer, "$GPTXT,01,01,02,SW=", bytesRead) != nullptr) { - LOG_INFO("L76K GNSS init succeeded, using L76K GNSS Module\n"); - return GNSS_MODEL_MTK; - } - } + if (getACK("$GPTXT,01,01,02,SW=", 500)) { + LOG_INFO("L76K GNSS init succeeded, using L76K GNSS Module\n"); + return GNSS_MODEL_MTK; } uint8_t cfg_rate[] = {0xB5, 0x62, 0x06, 0x08, 0x00, 0x00, 0x00, 0x00}; @@ -892,11 +894,30 @@ GnssModel_t GPS::probe(int serialSpeed) clearBuffer(); _serial_gps->write(cfg_rate, sizeof(cfg_rate)); // Check that the returned response class and message ID are correct - if (!getAck(buffer, sizeof(buffer), 0x06, 0x08, 1000)) { + if (!getAck(buffer, sizeof(buffer), 0x06, 0x08, 1500)) { LOG_WARN("Failed to find UBlox & MTK GNSS Module using baudrate %d\n", serialSpeed); return GNSS_MODEL_UNKNOWN; } LOG_INFO("Found a UBlox Module using baudrate %d\n", serialSpeed); + + // tips: NMEA Only should not be set here, otherwise initializing Ublox gnss module again after + // setting will not output command messages in UART1, resulting in unrecognized module information + if (serialSpeed != 9600) { + // Set the UART port to 9600 + byte _message_prt[] = {0xB5, 0x62, 0x06, 0x00, 0x14, 0x00, 0x01, 0x00, 0x00, 0x00, 0xD0, 0x08, 0x00, 0x00, + 0x80, 0x25, 0x00, 0x00, 0x07, 0x00, 0x03, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00}; + UBXChecksum(_message_prt, sizeof(_message_prt)); + _serial_gps->write(_message_prt, sizeof(_message_prt)); + delay(500); + serialSpeed = 9600; +#if defined(ARCH_NRF52) || defined(ARCH_PORTDUINO) || defined(ARCH_RP2040) + _serial_gps->end(); + _serial_gps->begin(serialSpeed); +#else + _serial_gps->updateBaudRate(serialSpeed); +#endif + } + memset(buffer, 0, sizeof(buffer)); byte _message_MONVER[8] = { 0xB5, 0x62, // Sync message for UBX protocol diff --git a/src/gps/GPS.h b/src/gps/GPS.h index 1eb96f8cc..0330151f3 100644 --- a/src/gps/GPS.h +++ b/src/gps/GPS.h @@ -161,8 +161,6 @@ class GPS : private concurrency::OSThread */ uint32_t getSleepTime() const; - bool getACK(uint8_t c, uint8_t i, int waitMillis); - /** * Tell users we have new GPS readings */ @@ -175,7 +173,8 @@ class GPS : private concurrency::OSThread GnssModel_t probe(int serialSpeed); int getAck(uint8_t *buffer, uint16_t size, uint8_t requestedClass, uint8_t requestedID, int waitMillis); - + bool getACK(uint8_t c, uint8_t i, int waitMillis); + bool getACK(const char *message, int waitMillis); // delay counter to allow more sats before fixed position stops GPS thread uint8_t fixeddelayCtr = 0; From 5d6f0ea6c4cec4e24bdfca1803f2bb23b9b6014e Mon Sep 17 00:00:00 2001 From: Jonathan Bennett Date: Fri, 1 Sep 2023 16:23:52 -0500 Subject: [PATCH 07/12] Fix possible edge case in GPS detection --- src/gps/GPS.cpp | 29 ++++++++++++++--------------- src/gps/GPS.h | 2 +- 2 files changed, 15 insertions(+), 16 deletions(-) diff --git a/src/gps/GPS.cpp b/src/gps/GPS.cpp index 34352032f..ec35930eb 100644 --- a/src/gps/GPS.cpp +++ b/src/gps/GPS.cpp @@ -51,21 +51,20 @@ bool GPS::getACK(const char *message, int waitMillis) { uint8_t buffer[768] = {0}; uint8_t b; - int bytesRead; + int bytesRead = 0; uint32_t startTimeout = millis() + waitMillis; while (millis() < startTimeout) { - bytesRead = 0; - while ((_serial_gps->available()) && (millis() < startTimeout)) { + if (_serial_gps->available()) { b = _serial_gps->read(); buffer[bytesRead] = b; bytesRead++; - if ((bytesRead == 768) || (b == '\r')) - break; - // Get module info , If the correct header is returned, - // it can be determined that it is the MTK chip - } - if (strnstr((char *)buffer, message, bytesRead) != nullptr) { - return true; + if ((bytesRead == 768) || (b == '\r')) { + if (strnstr((char *)buffer, message, bytesRead) != nullptr) { + return true; + } else { + bytesRead = 0; + } + } } } return false; @@ -92,7 +91,7 @@ bool GPS::getACK(uint8_t class_id, uint8_t msg_id, int waitMillis) while (millis() - startTime < waitMillis) { if (ack > 9) { - LOG_INFO("Got ACK for class %02X message %02X in %d millis.\n", class_id, msg_id, millis() - startTime); + // LOG_INFO("Got ACK for class %02X message %02X in %d millis.\n", class_id, msg_id, millis() - startTime); return true; // ACK received } if (_serial_gps->available()) { @@ -108,7 +107,7 @@ bool GPS::getACK(uint8_t class_id, uint8_t msg_id, int waitMillis) } } } - LOG_WARN("No response for class %02X message %02X\n", class_id, msg_id); + // LOG_WARN("No response for class %02X message %02X\n", class_id, msg_id); return false; // No response received within timeout } @@ -121,7 +120,7 @@ bool GPS::getACK(uint8_t class_id, uint8_t msg_id, int waitMillis) * @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, int waitMillis) +int GPS::getACK(uint8_t *buffer, uint16_t size, uint8_t requestedClass, uint8_t requestedID, int waitMillis) { uint16_t ubxFrameCounter = 0; uint32_t startTime = millis(); @@ -894,7 +893,7 @@ GnssModel_t GPS::probe(int serialSpeed) clearBuffer(); _serial_gps->write(cfg_rate, sizeof(cfg_rate)); // Check that the returned response class and message ID are correct - if (!getAck(buffer, sizeof(buffer), 0x06, 0x08, 1500)) { + if (!getACK(0x06, 0x08, 500)) { LOG_WARN("Failed to find UBlox & MTK GNSS Module using baudrate %d\n", serialSpeed); return GNSS_MODEL_UNKNOWN; } @@ -930,7 +929,7 @@ GnssModel_t GPS::probe(int serialSpeed) clearBuffer(); _serial_gps->write(_message_MONVER, sizeof(_message_MONVER)); - uint16_t len = getAck(buffer, sizeof(buffer), 0x0A, 0x04, 1200); + uint16_t len = getACK(buffer, sizeof(buffer), 0x0A, 0x04, 1200); if (len) { // LOG_DEBUG("monver reply size = %d\n", len); uint16_t position = 0; diff --git a/src/gps/GPS.h b/src/gps/GPS.h index 0330151f3..ff2ccf0e7 100644 --- a/src/gps/GPS.h +++ b/src/gps/GPS.h @@ -172,7 +172,7 @@ class GPS : private concurrency::OSThread String getNMEA(); GnssModel_t probe(int serialSpeed); - int getAck(uint8_t *buffer, uint16_t size, uint8_t requestedClass, uint8_t requestedID, int waitMillis); + int getACK(uint8_t *buffer, uint16_t size, uint8_t requestedClass, uint8_t requestedID, int waitMillis); bool getACK(uint8_t c, uint8_t i, int waitMillis); bool getACK(const char *message, int waitMillis); // delay counter to allow more sats before fixed position stops GPS thread From 1a178c7d33dd85e9b6dfd0ea182687fd68442cbb Mon Sep 17 00:00:00 2001 From: Jonathan Bennett Date: Sat, 2 Sep 2023 04:25:18 -0500 Subject: [PATCH 08/12] Add check for GPS Frame Errors message --- src/gps/GPS.cpp | 62 +++++++++++++++++++++++++++++++------------------ src/gps/GPS.h | 11 +++++++-- 2 files changed, 48 insertions(+), 25 deletions(-) diff --git a/src/gps/GPS.cpp b/src/gps/GPS.cpp index ec35930eb..d3bc23a21 100644 --- a/src/gps/GPS.cpp +++ b/src/gps/GPS.cpp @@ -47,7 +47,7 @@ void GPS::UBXChecksum(byte *message, size_t length) message[length - 1] = CK_B; } -bool GPS::getACK(const char *message, int waitMillis) +GPS_RESPONSE GPS::getACK(const char *message, int waitMillis) { uint8_t buffer[768] = {0}; uint8_t b; @@ -60,23 +60,25 @@ bool GPS::getACK(const char *message, int waitMillis) bytesRead++; if ((bytesRead == 768) || (b == '\r')) { if (strnstr((char *)buffer, message, bytesRead) != nullptr) { - return true; + return GNSS_RESPONSE_OK; } else { bytesRead = 0; } } } } - return false; + return GNSS_RESPONSE_NONE; } -bool GPS::getACK(uint8_t class_id, uint8_t msg_id, int waitMillis) +GPS_RESPONSE GPS::getACK(uint8_t class_id, uint8_t msg_id, int 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}; unsigned long startTime = millis(); + const char frame_errors[] = "More than 100 frame errors"; + int sCounter = 0; for (int j = 2; j < 6; j++) { buf[8] += buf[j]; @@ -92,23 +94,33 @@ bool GPS::getACK(uint8_t class_id, uint8_t msg_id, int waitMillis) while (millis() - startTime < waitMillis) { if (ack > 9) { // LOG_INFO("Got ACK for class %02X message %02X in %d millis.\n", class_id, msg_id, millis() - startTime); - return true; // ACK received + return GNSS_RESPONSE_OK; // ACK received } if (_serial_gps->available()) { b = _serial_gps->read(); + if (b == frame_errors[sCounter]) { + sCounter++; + if (sCounter == 26) { + return GNSS_RESPONSE_FRAME_ERRORS; + } + } else { + sCounter = 0; + } + // LOG_DEBUG("%02X", b); if (b == buf[ack]) { ack++; } else { ack = 0; // Reset the acknowledgement counter if (buf[3] == 0x00) { // UBX-ACK-NAK message LOG_WARN("Got NAK for class %02X message %02X\n", class_id, msg_id); - return false; // NAK received + return GNSS_RESPONSE_NAK; // NAK received } } } } // LOG_WARN("No response for class %02X message %02X\n", class_id, msg_id); - return false; // No response received within timeout + // LOG_DEBUG("\n"); + return GNSS_RESPONSE_NONE; // No response received within timeout } /** @@ -292,7 +304,7 @@ bool GPS::setupGPS() // Send the message to the module _serial_gps->write(_message_GNSS, sizeof(_message_GNSS)); - if (!getACK(0x06, 0x3e, 300)) { + if (getACK(0x06, 0x3e, 300) != GNSS_RESPONSE_OK) { // It's not critical if the module doesn't acknowledge this configuration. // The module should operate adequately with its factory or previously saved settings. // It appears that there is a firmware bug in some GPS modules: When an attempt is made @@ -332,7 +344,7 @@ bool GPS::setupGPS() // Send the message to the module _serial_gps->write(_message_JAM, sizeof(_message_JAM)); - if (!getACK(0x06, 0x39, 300)) { + if (getACK(0x06, 0x39, 300) != GNSS_RESPONSE_OK) { LOG_WARN("Unable to enable interference resistance.\n"); } @@ -377,7 +389,7 @@ bool GPS::setupGPS() // Send the message to the module _serial_gps->write(_message_NAVX5, sizeof(_message_NAVX5)); - if (!getACK(0x06, 0x23, 300)) { + if (getACK(0x06, 0x23, 300) != GNSS_RESPONSE_OK) { LOG_WARN("Unable to configure extra settings.\n"); } @@ -403,7 +415,7 @@ bool GPS::setupGPS() // Send the message to the module _serial_gps->write(_message_1Hz, sizeof(_message_1Hz)); - if (!getACK(0x06, 0x08, 300)) { + if (getACK(0x06, 0x08, 300) != GNSS_RESPONSE_OK) { LOG_WARN("Unable to set GPS update rate.\n"); } @@ -426,7 +438,7 @@ bool GPS::setupGPS() // Send the message to the module _serial_gps->write(_message_GGL, sizeof(_message_GGL)); - if (!getACK(0x06, 0x01, 300)) { + if (getACK(0x06, 0x01, 300) != GNSS_RESPONSE_OK) { LOG_WARN("Unable to disable NMEA GGL.\n"); } @@ -444,7 +456,7 @@ bool GPS::setupGPS() }; UBXChecksum(_message_GSA, sizeof(_message_GSA)); _serial_gps->write(_message_GSA, sizeof(_message_GSA)); - if (!getACK(0x06, 0x01, 300)) { + if (getACK(0x06, 0x01, 300) != GNSS_RESPONSE_OK) { LOG_WARN("Unable to Enable NMEA GSA.\n"); } @@ -461,7 +473,7 @@ bool GPS::setupGPS() }; UBXChecksum(_message_GSV, sizeof(_message_GSV)); _serial_gps->write(_message_GSV, sizeof(_message_GSV)); - if (!getACK(0x06, 0x01, 300)) { + if (getACK(0x06, 0x01, 300) != GNSS_RESPONSE_OK) { LOG_WARN("Unable to disable NMEA GSV.\n"); } @@ -479,7 +491,7 @@ bool GPS::setupGPS() }; UBXChecksum(_message_VTG, sizeof(_message_VTG)); _serial_gps->write(_message_VTG, sizeof(_message_VTG)); - if (!getACK(0x06, 0x01, 300)) { + if (getACK(0x06, 0x01, 300) != GNSS_RESPONSE_OK) { LOG_WARN("Unable to disable NMEA VTG.\n"); } @@ -496,7 +508,7 @@ bool GPS::setupGPS() }; UBXChecksum(_message_RMC, sizeof(_message_RMC)); _serial_gps->write(_message_RMC, sizeof(_message_RMC)); - if (!getACK(0x06, 0x01, 300)) { + if (getACK(0x06, 0x01, 300) != GNSS_RESPONSE_OK) { LOG_WARN("Unable to enable NMEA RMC.\n"); } @@ -513,7 +525,7 @@ bool GPS::setupGPS() }; UBXChecksum(_message_GGA, sizeof(_message_GGA)); _serial_gps->write(_message_GGA, sizeof(_message_GGA)); - if (!getACK(0x06, 0x01, 300)) { + if (getACK(0x06, 0x01, 300) != GNSS_RESPONSE_OK) { LOG_WARN("Unable to enable NMEA GGA.\n"); } @@ -546,7 +558,7 @@ bool GPS::setupGPS() // Send the message to the module _serial_gps->write(UBX_CFG_PMS, sizeof(UBX_CFG_PMS)); - if (!getACK(0x06, 0x86, 300)) { + if (getACK(0x06, 0x86, 300) != GNSS_RESPONSE_OK) { LOG_WARN("Unable to enable powersaving for GPS.\n"); } @@ -568,7 +580,7 @@ bool GPS::setupGPS() // Send the message to the module _serial_gps->write(_message_SAVE, sizeof(_message_SAVE)); - if (!getACK(0x06, 0x09, 300)) { + if (getACK(0x06, 0x09, 300) != GNSS_RESPONSE_OK) { LOG_WARN("Unable to save GNSS module configuration.\n"); } else { LOG_INFO("GNSS module configuration saved!\n"); @@ -883,21 +895,25 @@ GnssModel_t GPS::probe(int serialSpeed) // Get version information clearBuffer(); _serial_gps->write("$PCAS06,0*1B\r\n"); - if (getACK("$GPTXT,01,01,02,SW=", 500)) { + if (getACK("$GPTXT,01,01,02,SW=", 500) == GNSS_RESPONSE_OK) { LOG_INFO("L76K GNSS init succeeded, using L76K GNSS Module\n"); return GNSS_MODEL_MTK; } uint8_t cfg_rate[] = {0xB5, 0x62, 0x06, 0x08, 0x00, 0x00, 0x00, 0x00}; UBXChecksum(cfg_rate, sizeof(cfg_rate)); - clearBuffer(); + // clearBuffer(); _serial_gps->write(cfg_rate, sizeof(cfg_rate)); // Check that the returned response class and message ID are correct - if (!getACK(0x06, 0x08, 500)) { + GPS_RESPONSE response = getACK(0x06, 0x08, 750); + if (response == GNSS_RESPONSE_NONE) { LOG_WARN("Failed to find UBlox & MTK GNSS Module using baudrate %d\n", serialSpeed); return GNSS_MODEL_UNKNOWN; + } else if (response == GNSS_RESPONSE_FRAME_ERRORS) { + LOG_INFO("UBlox Frame Errors using baudrate %d\n", serialSpeed); + } else if (response == GNSS_RESPONSE_OK) { + LOG_INFO("Found a UBlox Module using baudrate %d\n", serialSpeed); } - LOG_INFO("Found a UBlox Module using baudrate %d\n", serialSpeed); // tips: NMEA Only should not be set here, otherwise initializing Ublox gnss module again after // setting will not output command messages in UART1, resulting in unrecognized module information diff --git a/src/gps/GPS.h b/src/gps/GPS.h index ff2ccf0e7..485d53709 100644 --- a/src/gps/GPS.h +++ b/src/gps/GPS.h @@ -18,6 +18,13 @@ typedef enum { GNSS_MODEL_UNKNOWN, } GnssModel_t; +typedef enum { + GNSS_RESPONSE_NONE, + GNSS_RESPONSE_NAK, + GNSS_RESPONSE_FRAME_ERRORS, + GNSS_RESPONSE_OK, +} GPS_RESPONSE; + // Generate a string representation of DOP const char *getDOPString(uint32_t dop); @@ -173,8 +180,8 @@ class GPS : private concurrency::OSThread GnssModel_t probe(int serialSpeed); int getACK(uint8_t *buffer, uint16_t size, uint8_t requestedClass, uint8_t requestedID, int waitMillis); - bool getACK(uint8_t c, uint8_t i, int waitMillis); - bool getACK(const char *message, int waitMillis); + GPS_RESPONSE getACK(uint8_t c, uint8_t i, int waitMillis); + GPS_RESPONSE getACK(const char *message, int waitMillis); // delay counter to allow more sats before fixed position stops GPS thread uint8_t fixeddelayCtr = 0; From b21368ecfa66a5d18b527da8518bc85dfa96b63b Mon Sep 17 00:00:00 2001 From: Jonathan Bennett Date: Sat, 2 Sep 2023 20:35:38 -0500 Subject: [PATCH 09/12] Add delay and debug code for GPS probe --- src/gps/GPS.cpp | 9 ++++++++- 1 file changed, 8 insertions(+), 1 deletion(-) diff --git a/src/gps/GPS.cpp b/src/gps/GPS.cpp index d3bc23a21..35ac6e8e2 100644 --- a/src/gps/GPS.cpp +++ b/src/gps/GPS.cpp @@ -58,15 +58,21 @@ GPS_RESPONSE GPS::getACK(const char *message, int waitMillis) b = _serial_gps->read(); buffer[bytesRead] = b; bytesRead++; - if ((bytesRead == 768) || (b == '\r')) { + if ((bytesRead == 767) || (b == '\r')) { if (strnstr((char *)buffer, message, bytesRead) != nullptr) { return GNSS_RESPONSE_OK; } else { + buffer[bytesRead] = '\0'; + bytesRead++; + LOG_INFO("Bytes read:%s\n", (char*) buffer); bytesRead = 0; } } } } + buffer[bytesRead] = '\0'; + bytesRead++; + LOG_INFO("Bytes read:%s\n", (char*) buffer); return GNSS_RESPONSE_NONE; } @@ -887,6 +893,7 @@ GnssModel_t GPS::probe(int serialSpeed) #endif memset(&info, 0, sizeof(struct uBloxGnssModelInfo)); uint8_t buffer[768] = {0}; + delay(100); // Close all NMEA sentences , Only valid for MTK platform _serial_gps->write("$PCAS03,0,0,0,0,0,0,0,0,0,0,,,0,0*02\r\n"); From 7c98445ca34e23f0e859ccde444f31efce65f296 Mon Sep 17 00:00:00 2001 From: Jonathan Bennett Date: Sun, 3 Sep 2023 01:50:11 -0500 Subject: [PATCH 10/12] Cleanup in prep for commiting Fix some compilation warnings gate debug code behind GPS_DEBUG minor fix in u-blox protocol detection Begin to gate GPS messages based on protover --- src/gps/GPS.cpp | 64 ++++++++++++++++++++++++++++--------------------- src/gps/GPS.h | 6 ++--- 2 files changed, 40 insertions(+), 30 deletions(-) diff --git a/src/gps/GPS.cpp b/src/gps/GPS.cpp index 35ac6e8e2..650cefd57 100644 --- a/src/gps/GPS.cpp +++ b/src/gps/GPS.cpp @@ -47,7 +47,7 @@ void GPS::UBXChecksum(byte *message, size_t length) message[length - 1] = CK_B; } -GPS_RESPONSE GPS::getACK(const char *message, int waitMillis) +GPS_RESPONSE GPS::getACK(const char *message, uint32_t waitMillis) { uint8_t buffer[768] = {0}; uint8_t b; @@ -62,27 +62,31 @@ GPS_RESPONSE GPS::getACK(const char *message, int waitMillis) if (strnstr((char *)buffer, message, bytesRead) != nullptr) { return GNSS_RESPONSE_OK; } else { +#ifdef GPS_DEBUG buffer[bytesRead] = '\0'; bytesRead++; - LOG_INFO("Bytes read:%s\n", (char*) buffer); + LOG_INFO("Bytes read:%s\n", (char *)buffer); +#endif bytesRead = 0; } } } } +#ifdef GPS_DEBUG buffer[bytesRead] = '\0'; bytesRead++; - LOG_INFO("Bytes read:%s\n", (char*) buffer); + LOG_INFO("Bytes read:%s\n", (char *)buffer); +#endif return GNSS_RESPONSE_NONE; } -GPS_RESPONSE GPS::getACK(uint8_t class_id, uint8_t msg_id, int waitMillis) +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}; - unsigned long startTime = millis(); + uint32_t startTime = millis(); const char frame_errors[] = "More than 100 frame errors"; int sCounter = 0; @@ -99,7 +103,9 @@ GPS_RESPONSE GPS::getACK(uint8_t class_id, uint8_t msg_id, int waitMillis) while (millis() - startTime < waitMillis) { if (ack > 9) { - // LOG_INFO("Got ACK for class %02X message %02X in %d millis.\n", class_id, msg_id, millis() - startTime); +#ifdef GPS_DEBUG + LOG_INFO("Got ACK for class %02X message %02X in %d millis.\n", class_id, msg_id, millis() - startTime); +#endif return GNSS_RESPONSE_OK; // ACK received } if (_serial_gps->available()) { @@ -138,7 +144,7 @@ GPS_RESPONSE GPS::getACK(uint8_t class_id, uint8_t msg_id, int waitMillis) * @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, int waitMillis) +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(); @@ -198,8 +204,10 @@ int GPS::getACK(uint8_t *buffer, uint16_t size, uint8_t requestedClass, uint8_t ubxFrameCounter = 0; } else { // return payload length - // LOG_INFO("Got ACK for class %02X message %02X in %d millis.\n", requestedClass, requestedID, millis() - - // startTime); +#ifdef GPS_DEBUG + LOG_INFO("Got ACK for class %02X message %02X in %d millis.\n", requestedClass, requestedID, + millis() - startTime); +#endif return needRead; } break; @@ -548,26 +556,27 @@ bool GPS::setupGPS() // set to Interval; otherwise, it must be set to '0'. The 'onTime' field specifies the duration of the ON phase and // must be smaller than the period. It is only valid when the powerSetupValue is set to Interval; otherwise, it must // be set to '0'. - byte UBX_CFG_PMS[14] = { - 0xB5, 0x62, // UBX sync characters - 0x06, 0x86, // Message class and ID (UBX-CFG-PMS) - 0x06, 0x00, // Length of payload (6 bytes) - 0x00, // Version (0) - 0x03, // Power setup value - 0x00, 0x00, // period: not applicable, set to 0 - 0x00, 0x00, // onTime: not applicable, set to 0 - 0x00, 0x00 // Placeholder for checksum, will be calculated next - }; + if (uBloxProtocolVersion >= 18) { + byte UBX_CFG_PMS[14] = { + 0xB5, 0x62, // UBX sync characters + 0x06, 0x86, // Message class and ID (UBX-CFG-PMS) + 0x06, 0x00, // Length of payload (6 bytes) + 0x00, // Version (0) + 0x03, // Power setup value + 0x00, 0x00, // period: not applicable, set to 0 + 0x00, 0x00, // onTime: not applicable, set to 0 + 0x00, 0x00 // Placeholder for checksum, will be calculated next + }; - // Calculate the checksum and update the message - UBXChecksum(UBX_CFG_PMS, sizeof(UBX_CFG_PMS)); + // Calculate the checksum and update the message + UBXChecksum(UBX_CFG_PMS, sizeof(UBX_CFG_PMS)); - // Send the message to the module - _serial_gps->write(UBX_CFG_PMS, sizeof(UBX_CFG_PMS)); - if (getACK(0x06, 0x86, 300) != GNSS_RESPONSE_OK) { - LOG_WARN("Unable to enable powersaving for GPS.\n"); + // Send the message to the module + _serial_gps->write(UBX_CFG_PMS, sizeof(UBX_CFG_PMS)); + if (getACK(0x06, 0x86, 300) != GNSS_RESPONSE_OK) { + LOG_WARN("Unable to enable powersaving for GPS.\n"); + } } - // We need save configuration to flash to make our config changes persistent byte _message_SAVE[21] = { 0xB5, 0x62, // UBX protocol header @@ -938,6 +947,7 @@ GnssModel_t GPS::probe(int serialSpeed) #else _serial_gps->updateBaudRate(serialSpeed); #endif + delay(200); } memset(buffer, 0, sizeof(buffer)); @@ -995,7 +1005,7 @@ GnssModel_t GPS::probe(int serialSpeed) } else { LOG_INFO("UBlox GNSS init succeeded, using UBlox GNSS Module\n"); } - } else if (!strncmp(info.extension[i], "PROTVER=", 8)) { + } else if (!strncmp(info.extension[i], "PROTVER", 7)) { char *ptr = nullptr; memset(buffer, 0, sizeof(buffer)); strncpy((char *)buffer, &(info.extension[i][8]), sizeof(buffer)); diff --git a/src/gps/GPS.h b/src/gps/GPS.h index 485d53709..89ae7a916 100644 --- a/src/gps/GPS.h +++ b/src/gps/GPS.h @@ -179,9 +179,9 @@ class GPS : private concurrency::OSThread String getNMEA(); GnssModel_t probe(int serialSpeed); - int getACK(uint8_t *buffer, uint16_t size, uint8_t requestedClass, uint8_t requestedID, int waitMillis); - GPS_RESPONSE getACK(uint8_t c, uint8_t i, int waitMillis); - GPS_RESPONSE getACK(const char *message, int waitMillis); + int getACK(uint8_t *buffer, uint16_t size, uint8_t requestedClass, uint8_t requestedID, uint32_t waitMillis); + GPS_RESPONSE getACK(uint8_t c, uint8_t i, uint32_t waitMillis); + GPS_RESPONSE getACK(const char *message, uint32_t waitMillis); // delay counter to allow more sats before fixed position stops GPS thread uint8_t fixeddelayCtr = 0; From ecdb75aae08ea3b059856e085f4b76f7796e9635 Mon Sep 17 00:00:00 2001 From: Jonathan Bennett Date: Sun, 3 Sep 2023 02:16:29 -0500 Subject: [PATCH 11/12] Correct UBX-CFG-PMS message --- src/gps/GPS.cpp | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/src/gps/GPS.cpp b/src/gps/GPS.cpp index 650cefd57..4f1641617 100644 --- a/src/gps/GPS.cpp +++ b/src/gps/GPS.cpp @@ -557,14 +557,15 @@ bool GPS::setupGPS() // must be smaller than the period. It is only valid when the powerSetupValue is set to Interval; otherwise, it must // be set to '0'. if (uBloxProtocolVersion >= 18) { - byte UBX_CFG_PMS[14] = { + byte UBX_CFG_PMS[16] = { 0xB5, 0x62, // UBX sync characters 0x06, 0x86, // Message class and ID (UBX-CFG-PMS) - 0x06, 0x00, // Length of payload (6 bytes) + 0x08, 0x00, // Length of payload (6 bytes) 0x00, // Version (0) 0x03, // Power setup value 0x00, 0x00, // period: not applicable, set to 0 0x00, 0x00, // onTime: not applicable, set to 0 + 0x97, 0x6F, // reserved, generated by u-center 0x00, 0x00 // Placeholder for checksum, will be calculated next }; From 7ad94da1c6b2e77af7e8b931060e070615e41a1a Mon Sep 17 00:00:00 2001 From: Jonathan Bennett Date: Sun, 3 Sep 2023 16:20:01 -0500 Subject: [PATCH 12/12] Add more to GPS_DEBUG and fix ubx7 CFG-GNSS --- src/gps/GPS.cpp | 89 ++++++++++++++++++++++++++++++++++--------------- 1 file changed, 62 insertions(+), 27 deletions(-) diff --git a/src/gps/GPS.cpp b/src/gps/GPS.cpp index 4f1641617..71e689b22 100644 --- a/src/gps/GPS.cpp +++ b/src/gps/GPS.cpp @@ -104,6 +104,7 @@ GPS_RESPONSE GPS::getACK(uint8_t class_id, uint8_t msg_id, uint32_t waitMillis) while (millis() - startTime < waitMillis) { if (ack > 9) { #ifdef GPS_DEBUG + LOG_DEBUG("\n"); LOG_INFO("Got ACK for class %02X message %02X in %d millis.\n", class_id, msg_id, millis() - startTime); #endif return GNSS_RESPONSE_OK; // ACK received @@ -118,20 +119,27 @@ GPS_RESPONSE GPS::getACK(uint8_t class_id, uint8_t msg_id, uint32_t waitMillis) } else { sCounter = 0; } - // LOG_DEBUG("%02X", b); +#ifdef GPS_DEBUG + LOG_DEBUG("%02X", b); +#endif if (b == buf[ack]) { ack++; } else { - ack = 0; // Reset the acknowledgement counter - if (buf[3] == 0x00) { // UBX-ACK-NAK message + if (ack == 3 && b == 0x00) { // UBX-ACK-NAK message +#ifdef GPS_DEBUG + LOG_DEBUG("\n"); +#endif LOG_WARN("Got NAK for class %02X message %02X\n", class_id, msg_id); return GNSS_RESPONSE_NAK; // NAK received } + ack = 0; // Reset the acknowledgement counter } } } - // LOG_WARN("No response for class %02X message %02X\n", class_id, msg_id); - // LOG_DEBUG("\n"); +#ifdef GPS_DEBUG + LOG_DEBUG("\n"); + LOG_WARN("No response for class %02X message %02X\n", class_id, msg_id); +#endif return GNSS_RESPONSE_NONE; // No response received within timeout } @@ -267,6 +275,10 @@ bool GPS::setupGPS() if (gnssModel != GNSS_MODEL_UNKNOWN) break; } + if (gnssModel == GNSS_MODEL_UNKNOWN) { + LOG_DEBUG("No GPS found, retrying at 9600 baud.\n"); + gnssModel = probe(9600); + } #endif if (gnssModel == GNSS_MODEL_MTK) { @@ -297,28 +309,47 @@ bool GPS::setupGPS() // We need set it because by default it is GPS only, and we want to use GLONASS too // Also we need SBAS for better accuracy and extra features // ToDo: Dynamic configure GNSS systems depending of LoRa region - byte _message_GNSS[36] = { - 0xb5, 0x62, // Sync message for UBX protocol - 0x06, 0x3e, // Message class and ID (UBX-CFG-GNSS) - 0x1c, 0x00, // Length of payload (28 bytes) - 0x00, // msgVer (0 for this version) - 0x00, // numTrkChHw (max number of hardware channels, read only, so it's always 0) - 0xff, // numTrkChUse (max number of channels to use, 0xff = max available) - 0x03, // numConfigBlocks (number of GNSS systems), most modules support maximum 3 GNSS systems - // GNSS config format: gnssId, resTrkCh, maxTrkCh, reserved1, flags - 0x00, 0x08, 0x10, 0x00, 0x01, 0x00, 0x01, 0x01, // GPS - 0x01, 0x01, 0x03, 0x00, 0x01, 0x00, 0x01, 0x01, // SBAS - 0x06, 0x08, 0x0e, 0x00, 0x01, 0x00, 0x01, 0x01, // GLONASS - 0x00, 0x00 // Checksum (to be calculated below) - }; - // Calculate the checksum and update the message. - UBXChecksum(_message_GNSS, sizeof(_message_GNSS)); + if (strncmp(info.hwVersion, "00070000", 8) == 0) { // Max7 seems to only support GPS *or* GLONASS + byte _message_GNSS[28] = { + 0xb5, 0x62, // Sync message for UBX protocol + 0x06, 0x3e, // Message class and ID (UBX-CFG-GNSS) + 0x14, 0x00, // Length of payload (28 bytes) + 0x00, // msgVer (0 for this version) + 0x00, // numTrkChHw (max number of hardware channels, read only, so it's always 0) + 0xff, // numTrkChUse (max number of channels to use, 0xff = max available) + 0x02, // numConfigBlocks (number of GNSS systems), most modules support maximum 3 GNSS systems + // GNSS config format: gnssId, resTrkCh, maxTrkCh, reserved1, flags + 0x00, 0x08, 0x10, 0x00, 0x01, 0x00, 0x00, 0x01, // GPS + 0x01, 0x01, 0x03, 0x00, 0x01, 0x00, 0x00, 0x01, // SBAS + 0x00, 0x00 // Checksum (to be calculated below) + }; + // Calculate the checksum and update the message. + UBXChecksum(_message_GNSS, sizeof(_message_GNSS)); + // Send the message to the module + _serial_gps->write(_message_GNSS, sizeof(_message_GNSS)); + } else { + byte _message_GNSS[36] = { + 0xb5, 0x62, // Sync message for UBX protocol + 0x06, 0x3e, // Message class and ID (UBX-CFG-GNSS) + 0x1c, 0x00, // Length of payload (28 bytes) + 0x00, // msgVer (0 for this version) + 0x00, // numTrkChHw (max number of hardware channels, read only, so it's always 0) + 0xff, // numTrkChUse (max number of channels to use, 0xff = max available) + 0x03, // numConfigBlocks (number of GNSS systems), most modules support maximum 3 GNSS systems + // GNSS config format: gnssId, resTrkCh, maxTrkCh, reserved1, flags + 0x00, 0x08, 0x10, 0x00, 0x01, 0x00, 0x01, 0x01, // GPS + 0x01, 0x01, 0x03, 0x00, 0x01, 0x00, 0x01, 0x01, // SBAS + 0x06, 0x08, 0x0e, 0x00, 0x01, 0x00, 0x01, 0x01, // GLONASS + 0x00, 0x00 // Checksum (to be calculated below) + }; + // Calculate the checksum and update the message. + UBXChecksum(_message_GNSS, sizeof(_message_GNSS)); + // Send the message to the module + _serial_gps->write(_message_GNSS, sizeof(_message_GNSS)); + } - // Send the message to the module - _serial_gps->write(_message_GNSS, sizeof(_message_GNSS)); - - if (getACK(0x06, 0x3e, 300) != GNSS_RESPONSE_OK) { + if (getACK(0x06, 0x3e, 800) == GNSS_RESPONSE_NAK) { // It's not critical if the module doesn't acknowledge this configuration. // The module should operate adequately with its factory or previously saved settings. // It appears that there is a firmware bug in some GPS modules: When an attempt is made @@ -327,7 +358,11 @@ bool GPS::setupGPS() // There is also a possibility that the module may be GPS-only. LOG_INFO("Unable to reconfigure GNSS - defaults maintained. Is this module GPS-only?\n"); } else { - LOG_INFO("GNSS configured for GPS+SBAS+GLONASS. Pause for 0.75s before sending next command.\n"); + if (strncmp(info.hwVersion, "00070000", 8) == 0) { + LOG_INFO("GNSS configured for GPS+SBAS. Pause for 0.75s before sending next command.\n"); + } else { + LOG_INFO("GNSS configured for GPS+SBAS+GLONASS. Pause for 0.75s before sending next command.\n"); + } // Documentation say, we need wait atleast 0.5s after reconfiguration of GNSS module, before sending next commands delay(750); } @@ -919,7 +954,7 @@ GnssModel_t GPS::probe(int serialSpeed) uint8_t cfg_rate[] = {0xB5, 0x62, 0x06, 0x08, 0x00, 0x00, 0x00, 0x00}; UBXChecksum(cfg_rate, sizeof(cfg_rate)); - // clearBuffer(); + 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);