M10 ValSet support, unit connection and reconnect stability (#12799)

* WIP

* start of implement m10 code

* Fetch MON-VER from unit to check for unit version

* test nav5 m10 command

* missing empty lines

* offload detect to config file

* copy from hasli and organization

* fix platform.h include

* fix cli_unittest gps include

* fix cli_unittest for gps calls

* guard ublox version in gpsData

* print human readable hw version

* add utc_standard param and transfer with nav5 set
add nav5x message for autonomous mode for m10

* fix typo

* revert order structure, remove functions and reduce flash size

* revert order structure, remove functions and reduce flash size

* fix gps init and navx5 message

* generalized nav5 message

* remove unguarded debug

* change ubx version detection, baud rate negotiation fix and save found baud

* revert indentation

* revert indentation and refactorings

* the new code works with faster baud rate changes

* remove unguarded debug statement

* fix cli commands, major space reduce finished, removed extensions for now

* ubx version checks, add valset for M10

* beta of valset, change suggestions from ledvinap and macgivergim

* valset helper function and combine set nav rate valsets

* more valset refactoring

* remove big array and replace with macro

* remove assert, as it can stop bf completely

* refactoring to offsetof

* making reconnect more resilient, reorganize rate setup, so it doesnt get missed on init

* improved lost communcation detection, dont rely on ACK/NACK anymore

* paket rate debug

* adding debug mode, fixing major flight mode bug

* revert fake flight "isConfiguratorConnected"

* fixed proto detection, fixed reconfigure on too low updaterate

* valset doesnt always send ACK, so we dont wait for it

* size optimization, debug mode rename, minor fixes

* implemented some requested changes

* changed wait delay millisecond based

* fixes from ctzsnooze and zzXyz

* timer fixes

* CamelCase new settings names

* indent

* Fix failure to enter flight model on GPS Fix

* remove old commented out debugs

* simplify timeouts

* Clarify skip_acc and remove development valset code

* accept PL's advice to remove  >> (8 * 0

* Simplify package counter, remove reconfiguration based on packet count

* fix error in package count introduced in previous commit

* Fix delay detecting Configurator, ANA disable (for another PR)

* address payload comments and fix logical error

* indentation edits

* delete old enum

* log gps and firmware nav interval times

* fix payload size, inc Rx buffer to 256, ifDef for sw_proto

* remove token parsing (Petr suggestions)

* fixes from reviews

* Basic NMEA improvements

* Address comments from karate

* only check platform version - thanks zzyzx

* Fix for too many sats problem - thanks zzyzx

* tidy up comments, ifdef some ublox definitions

* Use Nav packet intervals, NMEA and UBX, for time delta

* Resolve comments and flatten conditionals

* editorial change

* single function for gpsSol.navIntervalMs

* adam-ah suggestion for payload optimisation

* ACK/NAK & polled message timer fixes

* Revert timer fixes - unexpected side effects

* Revert adam-ah suggestion for payload optimisation"

This reverts commit 42fc8c04fdbc436c9ef196b88f0764ffcbb9239f.
Broke the display of sat info when more than 32 sats in view

* implement a number of comments

* Fast task rate on new data, don't spam at the start

thanks adam-ah

* include PDOP for M10 via NAV-PVT

* Address some of PL's recent comments

* don't recalculate millis so many times

* tidy up baudrate connect code

* Split receiving of GPS bytes from processing by adding GPS_STATE_PROCESS_DATA

* Split receiving of GPS bytes from processing by adding GPS_STATE_PROCESS_DATA

* Preserve state whilst processing packets

* Set gpsData.state directly as gpsSetState() clobbers gpsData.state_position

* Restore original read time check

* Schedule gpsUpdate() to run immediately again when a packet is received for processing

* add debugs to display scheduler valuesl

* simpler scheduler solution

* minor debug change

* FIxes: M10 connection, pDop, NMEA disable; thanks zzyxz

NB: Breaks unit's neat reconnection methods
M8 need a lot of settling time before using the serial port

* ubx parse length sanity + cleanup + dashboard conditional compiles

* Address recent comments from PL

---------

Co-authored-by: ctzsnooze <chris.thompson@sydney.edu.au>
Co-authored-by: ZzyzxTek <zzyzx@zzyzxtek.com>
Co-authored-by: Steve Evans <Steve@SCEvans.com>

src/main/build/debug.c

@@ -104,6 +104,7 @@ const char * const debugModeNames[DEBUG_COUNT] = {
     "GPS_RESCUE_VELOCITY",
     "GPS_RESCUE_HEADING",
     "GPS_RESCUE_TRACKING",
+    "GPS_CONNECTION",
     "ATTITUDE",
     "VTX_MSP",
     "GPS_DOP",

src/main/build/debug.h

@@ -104,6 +104,7 @@ typedef enum {
     DEBUG_GPS_RESCUE_VELOCITY,
     DEBUG_GPS_RESCUE_HEADING,
     DEBUG_GPS_RESCUE_TRACKING,
+    DEBUG_GPS_CONNECTION,
     DEBUG_ATTITUDE,
     DEBUG_VTX_MSP,
     DEBUG_GPS_DOP,

src/main/cli/cli.c

@@ -4787,7 +4787,7 @@ if (buildKey) {
 #ifdef USE_GPS
     cliPrint("GPS: ");
     if (featureIsEnabled(FEATURE_GPS)) {
-        if (gpsIsHealthy()) {
+        if (gpsData.state >= GPS_STATE_CONFIGURE) {
             cliPrint("connected, ");
         } else {
             cliPrint("NOT CONNECTED, ");
@@ -4808,7 +4808,7 @@ if (buildKey) {
                 cliPrint("), ");
             }
         }
-        if (!gpsIsHealthy()) {
+        if (gpsData.state <= GPS_STATE_CONFIGURE) {
             cliPrint("NOT CONFIGURED");
         } else {
             if (gpsConfig()->autoConfig == GPS_AUTOCONFIG_OFF) {
@@ -4817,6 +4817,12 @@ if (buildKey) {
                 cliPrint("configured");
             }
         }
+        if (gpsData.platformVersion != UBX_VERSION_UNDEF) {
+            cliPrint(", version = ");
+            cliPrintf("%s", ubloxVersionMap[gpsData.platformVersion].str);
+        } else {
+            cliPrint("unknown");
+        }
     } else {
         cliPrint("NOT ENABLED");
     }
@@ -4864,15 +4870,15 @@ static void cliTasks(const char *cmdName, char *cmdline)
             if (systemConfig()->task_statistics) {
 #if defined(USE_LATE_TASK_STATISTICS)
                 cliPrintLinef("%6d %7d %7d %4d.%1d%% %4d.%1d%% %9d %6d %6d %7d",
-                        taskFrequency, taskInfo.maxExecutionTimeUs, taskInfo.averageExecutionTime10thUs / 10,
+                    taskFrequency, taskInfo.maxExecutionTimeUs, taskInfo.averageExecutionTime10thUs / 10,
-                        maxLoad/10, maxLoad%10, averageLoad/10, averageLoad%10,
+                    maxLoad/10, maxLoad%10, averageLoad/10, averageLoad%10,
-                        taskInfo.totalExecutionTimeUs / 1000,
+                    taskInfo.totalExecutionTimeUs / 1000,
-                        taskInfo.lateCount, taskInfo.runCount, taskInfo.execTime);
+                    taskInfo.lateCount, taskInfo.runCount, taskInfo.execTime);
 #else
                 cliPrintLinef("%6d %7d %7d %4d.%1d%% %4d.%1d%% %9d",
-                        taskFrequency, taskInfo.maxExecutionTimeUs, taskInfo.averageExecutionTime10thUs / 10,
+                    taskFrequency, taskInfo.maxExecutionTimeUs, taskInfo.averageExecutionTime10thUs / 10,
-                        maxLoad/10, maxLoad%10, averageLoad/10, averageLoad%10,
+                    maxLoad/10, maxLoad%10, averageLoad/10, averageLoad%10,
-                        taskInfo.totalExecutionTimeUs / 1000);
+                    taskInfo.totalExecutionTimeUs / 1000);
 #endif
             } else {
                 cliPrintLinef("%6d", taskFrequency);

src/main/cli/settings.c

@@ -198,17 +198,21 @@ static const char * const lookupTableGyro[] = {
 #endif
 
 #ifdef USE_GPS
-static const char * const lookupTableGPSProvider[] = {
+static const char * const lookupTableGpsProvider[] = {
     "NMEA", "UBLOX", "MSP"
 };
 
-static const char * const lookupTableGPSSBASMode[] = {
+static const char * const lookupTableGpsSbasMode[] = {
     "AUTO", "EGNOS", "WAAS", "MSAS", "GAGAN", "NONE"
 };
 
-static const char * const lookupTableGPSUBLOXModels[] = {
+static const char * const lookupTableGpsUbloxModels[] = {
     "PORTABLE", "STATIONARY", "PEDESTRIAN", "AUTOMOTIVE", "AT_SEA", "AIRBORNE_1G", "AIRBORNE_2G", "AIRBORNE_4G"
 };
+
+static const char * const lookupTableGpsUbloxUtcStandard[] = {
+    "AUTO", "USNO", "EU", "SU", "NTSC"
+};
 #endif
 
 #ifdef USE_SERVOS
@@ -533,9 +537,10 @@ const lookupTableEntry_t lookupTables[] = {
     LOOKUP_TABLE_ENTRY(lookupTableUnit),
     LOOKUP_TABLE_ENTRY(lookupTableAlignment),
 #ifdef USE_GPS
-    LOOKUP_TABLE_ENTRY(lookupTableGPSProvider),
+    LOOKUP_TABLE_ENTRY(lookupTableGpsProvider),
-    LOOKUP_TABLE_ENTRY(lookupTableGPSSBASMode),
+    LOOKUP_TABLE_ENTRY(lookupTableGpsSbasMode),
-    LOOKUP_TABLE_ENTRY(lookupTableGPSUBLOXModels),
+    LOOKUP_TABLE_ENTRY(lookupTableGpsUbloxModels),
+    LOOKUP_TABLE_ENTRY(lookupTableGpsUbloxUtcStandard),
 #ifdef USE_GPS_RESCUE
     LOOKUP_TABLE_ENTRY(lookupTableRescueSanityType),
     LOOKUP_TABLE_ENTRY(lookupTableRescueAltitudeMode),
@@ -1017,9 +1022,9 @@ const clivalue_t valueTable[] = {
     { PARAM_NAME_GPS_AUTO_BAUD,              VAR_UINT8  | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON },         PG_GPS_CONFIG, offsetof(gpsConfig_t, autoBaud) },
     { PARAM_NAME_GPS_UBLOX_ACQUIRE_MODEL,    VAR_UINT8  | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_GPS_UBLOX_MODELS }, PG_GPS_CONFIG, offsetof(gpsConfig_t, gps_ublox_acquire_model) },
     { PARAM_NAME_GPS_UBLOX_FLIGHT_MODEL,     VAR_UINT8  | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_GPS_UBLOX_MODELS }, PG_GPS_CONFIG, offsetof(gpsConfig_t, gps_ublox_flight_model) },
-    { PARAM_NAME_GPS_UPDATE_RATE_HZ,         VAR_UINT8  | MASTER_VALUE,               .config.minmaxUnsigned = {1, 19},          PG_GPS_CONFIG, offsetof(gpsConfig_t, gps_update_rate_hz) },
+    { PARAM_NAME_GPS_UPDATE_RATE_HZ,         VAR_UINT8  | MASTER_VALUE,               .config.minmaxUnsigned = {1, 20},          PG_GPS_CONFIG, offsetof(gpsConfig_t, gps_update_rate_hz) },
+    { PARAM_NAME_GPS_UBLOX_UTC_STANDARD,     VAR_UINT8  | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_GPS_UBLOX_UTC_STANDARD }, PG_GPS_CONFIG, offsetof(gpsConfig_t, gps_ublox_utc_standard) },
     { PARAM_NAME_GPS_UBLOX_USE_GALILEO,      VAR_UINT8  | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON },         PG_GPS_CONFIG, offsetof(gpsConfig_t, gps_ublox_use_galileo) },
-    { PARAM_NAME_GPS_UBLOX_FULL_POWER,       VAR_UINT8  | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON },         PG_GPS_CONFIG, offsetof(gpsConfig_t, gps_ublox_full_power) },
     { PARAM_NAME_GPS_SET_HOME_POINT_ONCE,    VAR_UINT8  | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON },         PG_GPS_CONFIG, offsetof(gpsConfig_t, gps_set_home_point_once) },
     { PARAM_NAME_GPS_USE_3D_SPEED,           VAR_UINT8  | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON },         PG_GPS_CONFIG, offsetof(gpsConfig_t, gps_use_3d_speed) },
     { PARAM_NAME_GPS_SBAS_INTEGRITY,         VAR_UINT8  | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON },         PG_GPS_CONFIG, offsetof(gpsConfig_t, sbas_integrity) },

src/main/cli/settings.h

@@ -33,6 +33,7 @@ typedef enum {
     TABLE_GPS_PROVIDER,
     TABLE_GPS_SBAS_MODE,
     TABLE_GPS_UBLOX_MODELS,
+    TABLE_GPS_UBLOX_UTC_STANDARD,
 #endif
 #ifdef USE_GPS_RESCUE
     TABLE_GPS_RESCUE_SANITY_CHECK,

src/main/drivers/serial_uart_impl.h

@@ -26,7 +26,7 @@
 #define UARTDEV_COUNT_MAX 6
 #define UARTHARDWARE_MAX_PINS 4
 #ifndef UART_RX_BUFFER_SIZE
-#define UART_RX_BUFFER_SIZE     128
+#define UART_RX_BUFFER_SIZE     256
 #endif
 #ifndef UART_TX_BUFFER_SIZE
 #ifdef USE_MSP_DISPLAYPORT
@@ -39,7 +39,7 @@
 #define UARTDEV_COUNT_MAX 8
 #define UARTHARDWARE_MAX_PINS 4
 #ifndef UART_RX_BUFFER_SIZE
-#define UART_RX_BUFFER_SIZE     128
+#define UART_RX_BUFFER_SIZE     256
 #endif
 #ifndef UART_TX_BUFFER_SIZE
 #ifdef USE_MSP_DISPLAYPORT
@@ -52,7 +52,7 @@
 #define UARTDEV_COUNT_MAX 11 // UARTs 1 to 10 + LPUART1
 #define UARTHARDWARE_MAX_PINS 5
 #ifndef UART_RX_BUFFER_SIZE
-#define UART_RX_BUFFER_SIZE     128
+#define UART_RX_BUFFER_SIZE     256
 #endif
 #ifndef UART_TX_BUFFER_SIZE
 #ifdef USE_MSP_DISPLAYPORT
@@ -65,7 +65,7 @@
 #define UARTDEV_COUNT_MAX 11  // UARTs 1 to 5 + LPUART1 (index 10)
 #define UARTHARDWARE_MAX_PINS 3
 #ifndef UART_RX_BUFFER_SIZE
-#define UART_RX_BUFFER_SIZE     128
+#define UART_RX_BUFFER_SIZE     256
 #endif
 #ifndef UART_TX_BUFFER_SIZE
 #ifdef USE_MSP_DISPLAYPORT
@@ -78,7 +78,7 @@
 #define UARTDEV_COUNT_MAX 8  // UARTs 1 to 5 + LPUART1 (index 9)
 #define UARTHARDWARE_MAX_PINS 5
 #ifndef UART_RX_BUFFER_SIZE
-#define UART_RX_BUFFER_SIZE     128
+#define UART_RX_BUFFER_SIZE     256
 #endif
 #ifndef UART_TX_BUFFER_SIZE
 #ifdef USE_MSP_DISPLAYPORT

src/main/fc/parameter_names.h

@@ -143,9 +143,9 @@
 #define PARAM_NAME_GPS_AUTO_CONFIG "gps_auto_config"
 #define PARAM_NAME_GPS_AUTO_BAUD "gps_auto_baud"
 #define PARAM_NAME_GPS_UBLOX_USE_GALILEO "gps_ublox_use_galileo"
-#define PARAM_NAME_GPS_UBLOX_FULL_POWER "gps_ublox_full_power"
 #define PARAM_NAME_GPS_UBLOX_ACQUIRE_MODEL "gps_ublox_acquire_model"
 #define PARAM_NAME_GPS_UBLOX_FLIGHT_MODEL "gps_ublox_flight_model"
+#define PARAM_NAME_GPS_UBLOX_UTC_STANDARD "gps_ublox_utc_standard"
 #define PARAM_NAME_GPS_SET_HOME_POINT_ONCE "gps_set_home_point_once"
 #define PARAM_NAME_GPS_USE_3D_SPEED "gps_use_3d_speed"
 #define PARAM_NAME_GPS_NMEA_CUSTOM_COMMANDS "gps_nmea_custom_commands"

src/main/fc/tasks.c

@@ -251,8 +251,6 @@ static void taskGpsRescue(timeUs_t currentTimeUs)
 
     if (gpsRescueIsConfigured()) {
         gpsRescueUpdate();
-    } else {
-        schedulerIgnoreTaskStateTime();
     }
 }
 #endif

src/main/io/dashboard.c

@@ -379,8 +379,8 @@ static void showGpsPage(void)
 
     static uint8_t gpsTicker = 0;
     static uint32_t lastGPSSvInfoReceivedCount = 0;
-    if (GPS_svInfoReceivedCount != lastGPSSvInfoReceivedCount) {
+    if (dashboardGpsNavSvInfoRcvCount != lastGPSSvInfoReceivedCount) {
-        lastGPSSvInfoReceivedCount = GPS_svInfoReceivedCount;
+        lastGPSSvInfoReceivedCount = dashboardGpsNavSvInfoRcvCount;
         gpsTicker++;
         gpsTicker = gpsTicker % TICKER_CHARACTER_COUNT;
     }
@@ -419,7 +419,7 @@ static void showGpsPage(void)
     i2c_OLED_set_xy(dev, HALF_SCREEN_CHARACTER_COLUMN_COUNT, rowIndex++);
     i2c_OLED_send_string(dev, lineBuffer);
 
-    tfp_sprintf(lineBuffer, "RX: %d", GPS_packetCount);
+    tfp_sprintf(lineBuffer, "RX: %d", dashboardGpsPacketCount);
     padHalfLineBuffer();
     i2c_OLED_set_line(dev, rowIndex);
     i2c_OLED_send_string(dev, lineBuffer);
@@ -429,7 +429,7 @@ static void showGpsPage(void)
     i2c_OLED_set_xy(dev, HALF_SCREEN_CHARACTER_COLUMN_COUNT, rowIndex++);
     i2c_OLED_send_string(dev, lineBuffer);
 
-    tfp_sprintf(lineBuffer, "Dt: %d", gpsData.lastMessage - gpsData.lastLastMessage);
+    tfp_sprintf(lineBuffer, "Dt: %d", gpsSol.navIntervalMs);
     padHalfLineBuffer();
     i2c_OLED_set_line(dev, rowIndex);
     i2c_OLED_send_string(dev, lineBuffer);
@@ -439,7 +439,7 @@ static void showGpsPage(void)
     i2c_OLED_set_xy(dev, HALF_SCREEN_CHARACTER_COLUMN_COUNT, rowIndex++);
     i2c_OLED_send_string(dev, lineBuffer);
 
-    strncpy(lineBuffer, gpsPacketLog, GPS_PACKET_LOG_ENTRY_COUNT);
+    strncpy(lineBuffer, dashboardGpsPacketLog, GPS_PACKET_LOG_ENTRY_COUNT);
     padHalfLineBuffer();
     i2c_OLED_set_line(dev, rowIndex++);
     i2c_OLED_send_string(dev, lineBuffer);

src/main/io/gps.h

@@ -35,6 +35,138 @@
 #define GPS_Y 0
 #define GPS_MIN_SAT_COUNT 4      // number of sats to trigger low sat count sanity check
 
+#ifdef USE_GPS_UBLOX
+typedef enum {
+    UBX_VERSION_UNDEF = 0,
+    UBX_VERSION_M5,
+    UBX_VERSION_M6,
+    UBX_VERSION_M7,
+    UBX_VERSION_M8,
+    UBX_VERSION_M9,
+    UBX_VERSION_M10,
+    UBX_VERSION_COUNT
+} ubloxVersion_e;
+
+typedef enum {
+    CFG_RATE_MEAS = 0x30210001, // U2
+    CFG_RATE_NAV = 0x30210002, // U2
+    CFG_RATE_TIMEREF = 0x20210003, // E1
+    CFG_NAVSPG_FIXMODE = 0x20110011, // E1
+    CFG_NAVSPG_DYNMODEL = 0x20110021, // E1
+    CFG_NAVSPG_UTCSTANDARD = 0x2011001c, // E1
+    CFG_NAVSPG_CONSTR_ALT = 0x401100c1, // I4
+    CFG_NAVSPG_CONSTR_ALTVAR = 0x401100c2, // U4
+    CFG_NAVSPG_CONSTR_DGNSSTO = 0x201100c4, // U1
+    CFG_NAVSPG_INFIL_MINELEV = 0x201100a4, // I1
+    CFG_NAVSPG_INFIL_CNOTHRS = 0x201100ab, // U1
+    CFG_NAVSPG_INFIL_NCNOTHRS = 0x201100aa, // U1
+    CFG_NAVSPG_OUTFIL_PDOP = 0x301100b1, // U2
+    CFG_NAVSPG_OUTFIL_TDOP = 0x301100b2, // U2
+    CFG_NAVSPG_OUTFIL_PACC = 0x301100b3, // U2
+    CFG_NAVSPG_OUTFIL_TACC = 0x301100b4, // U2
+    CFG_MOT_GNSSSPEED_THRS = 0x20250038, // U1
+    CFG_MOT_GNSSDIST_THRS = 0x3025003b, // U2
+    CFG_ANA_USE_ANA = 0x10230001, // L
+    CFG_MSGOUT_NMEA_ID_VTG_I2C = 0x209100b0, // U1
+    CFG_MSGOUT_NMEA_ID_VTG_SPI = 0x209100b4, // U1
+    CFG_MSGOUT_NMEA_ID_VTG_UART1 = 0x209100b1, // U1
+    CFG_MSGOUT_NMEA_ID_GSV_I2C = 0x209100c4, // U1
+    CFG_MSGOUT_NMEA_ID_GSV_SPI = 0x209100c8, // U1
+    CFG_MSGOUT_NMEA_ID_GSV_UART1 = 0x209100c5, // U1
+    CFG_MSGOUT_NMEA_ID_GLL_I2C = 0x209100c9, // U1
+    CFG_MSGOUT_NMEA_ID_GLL_SPI = 0x209100cd, // U1
+    CFG_MSGOUT_NMEA_ID_GLL_UART1 = 0x209100ca, // U1
+    CFG_MSGOUT_NMEA_ID_GGA_I2C = 0x209100ba, // U1
+    CFG_MSGOUT_NMEA_ID_GGA_SPI = 0x209100be, // U1
+    CFG_MSGOUT_NMEA_ID_GGA_UART1 = 0x209100bb, // U1
+    CFG_MSGOUT_NMEA_ID_GSA_I2C = 0x209100bf, // U1
+    CFG_MSGOUT_NMEA_ID_GSA_SPI = 0x209100c3, // U1
+    CFG_MSGOUT_NMEA_ID_GSA_UART1 = 0x209100c0, // U1
+    CFG_MSGOUT_NMEA_ID_RMC_I2C = 0x209100ab, // U1
+    CFG_MSGOUT_NMEA_ID_RMC_SPI = 0x209100af, // U1
+    CFG_MSGOUT_NMEA_ID_RMC_UART1 = 0x209100ac, // U1
+    CFG_MSGOUT_UBX_NAV_PVT_I2C = 0x20910006, // U1
+    CFG_MSGOUT_UBX_NAV_PVT_SPI = 0x2091000a, // U1
+    CFG_MSGOUT_UBX_NAV_PVT_UART1 = 0x20910007, // U1
+    CFG_MSGOUT_UBX_NAV_SAT_I2C = 0x20910015, // U1
+    CFG_MSGOUT_UBX_NAV_SAT_SPI = 0x20910019, // U1
+    CFG_MSGOUT_UBX_NAV_SAT_UART1 = 0x20910016, // U1
+    CFG_MSGOUT_UBX_NAV_DOP_I2C = 0x20910038, // U1
+    CFG_MSGOUT_UBX_NAV_DOP_SPI = 0x2091003c, // U1
+    CFG_MSGOUT_UBX_NAV_DOP_UART1 = 0x20910039, // U1
+    CFG_SBAS_USE_TESTMODE = 0x10360002, // L
+    CFG_SBAS_USE_RANGING = 0x10360003, // L
+    CFG_SBAS_USE_DIFFCORR = 0x10360004, // L
+    CFG_SBAS_USE_INTEGRITY = 0x10360005, // L
+    CFG_SBAS_PRNSCANMASK = 0x50360006, // X8
+    CFG_SIGNAL_GPS_ENA = 0x1031001f, // L
+    CFG_SIGNAL_SBAS_ENA = 0x10310020, // L
+    CFG_SIGNAL_GAL_ENA = 0x10310021, // L
+    CFG_SIGNAL_BDS_ENA = 0x10310022, // L
+    CFG_SIGNAL_QZSS_ENA = 0x10310024, // L
+    CFG_SIGNAL_GLO_ENA = 0x10310025, // L
+    CFG_PM_OPERATEMODE = 0x20d00001, // E1
+} ubxValGetSetBytes_e;
+
+/*
+ * replaced by following macro, to save space; keeping for reference
+typedef enum {
+    SBAS_SEARCH_ALL = 0,
+    SBAS_SEARCH_PRN120,
+    SBAS_SEARCH_PRN121,
+    // repeat to 158
+    SBAS_SEARCH_PRN158,
+} ubxSbasPrnScan_e;
+*/
+
+#define SBAS_SEARCH_ALL    0x0
+#define SBAS_SEARCH_PRN(i) (1 << (i - 120))
+
+typedef enum {
+    UBX_VAL_LAYER_RAM = 0x01,
+    UBX_VAL_LAYER_BBR = 0x02,
+    UBX_VAL_LAYER_FLASH = 0x04,
+} ubloxValLayer_e;
+
+typedef enum {
+    UBLOX_MODEL_PORTABLE = 0,
+    UBLOX_MODEL_STATIONARY,
+    UBLOX_MODEL_PEDESTRIAN,
+    UBLOX_MODEL_AUTOMOTIVE,
+    UBLOX_MODEL_AT_SEA,
+    UBLOX_MODEL_AIRBORNE_1G,
+    UBLOX_MODEL_AIRBORNE_2G,
+    UBLOX_MODEL_AIRBORNE_4G,
+} ubloxModel_e;
+
+typedef enum {
+    UBLOX_UTC_STANDARD_AUTO = 0,
+    UBLOX_UTC_STANDARD_USNO = 3,
+    UBLOX_UTC_STANDARD_EU = 5,
+    UBLOX_UTC_STANDARD_SU = 6,
+    UBLOX_UTC_STANDARD_NTSC = 7,
+} ubloxUtcStandard_e;
+
+struct ubloxVersion_s {
+    uint32_t hw;
+    const char* str;
+};
+extern struct ubloxVersion_s ubloxVersionMap[];
+
+#endif // USE_GPS_UBLOX
+
+typedef enum {
+    GPS_STATE_UNKNOWN = 0,
+    GPS_STATE_DETECT_BAUD,
+    GPS_STATE_INITIALIZED,
+    GPS_STATE_CHANGE_BAUD,
+    GPS_STATE_CONFIGURE,
+    GPS_STATE_RECEIVING_DATA,
+    GPS_STATE_PROCESS_DATA,
+    GPS_STATE_LOST_COMMUNICATION,
+    GPS_STATE_COUNT
+} gpsState_e;
+
 typedef enum {
     GPS_LATITUDE,
     GPS_LONGITUDE
@@ -57,17 +189,6 @@ typedef enum {
 
 #define SBAS_MODE_MAX SBAS_GAGAN
 
-typedef enum {
-    UBLOX_MODEL_PORTABLE = 0,
-    UBLOX_MODEL_STATIONARY,
-    UBLOX_MODEL_PEDESTRIAN,
-    UBLOX_MODEL_AUTOMOTIVE,
-    UBLOX_MODEL_AT_SEA,
-    UBLOX_MODEL_AIRBORNE_1G,
-    UBLOX_MODEL_AIRBORNE_2G,
-    UBLOX_MODEL_AIRBORNE_4G,
-} ubloxModel_e;
-
 typedef enum {
     GPS_BAUDRATE_115200 = 0,
     GPS_BAUDRATE_57600,
@@ -130,37 +251,61 @@ typedef struct gpsSolutionData_s {
     uint16_t groundCourse;          // degrees * 10
     uint8_t numSat;
     uint32_t time;                  // GPS msToW
+    uint32_t navIntervalMs;         // interval between nav solutions in ms
 } gpsSolutionData_t;
 
+/*
+* keeping this table for reference
+typedef enum {
+    UBX_CAP_SAT_NONE = 0x0,
+    UBX_CAP_SAT_GPS = 0x0001,
+    UBX_CAP_SAT_GLO = 0x0002,
+    UBX_CAP_SAT_GAL = 0x0004,
+    UBX_CAP_SAT_BDS = 0x0008,
+    UBX_CAP_SAT_SBAS = 0x0010,
+    UBX_CAP_SAT_QZSS = 0x0020,
+} ubxCapabilities_e;
+*/
+
+typedef struct ubxMonVer_s {
+        char swVersion[30];
+        char hwVersion[10];
+        char extension[300]; // variable size but not more than 300 values
+} ubxMonVer_t;
+
 typedef struct gpsData_s {
     uint32_t errors;                // gps error counter - crc error/lost of data/sync etc..
     uint32_t timeouts;
-    uint32_t lastMessage;           // last time valid GPS data was received (millis)
+    uint32_t lastNavMessage;        // time of last valid GPS speed and position data
-    uint32_t lastLastMessage;       // last-last valid GPS message. Used to calculate delta.
+    uint32_t now;
-
+    uint32_t lastMessageSent;       // time last message was sent
     uint32_t state_position;        // incremental variable for loops
     uint32_t state_ts;              // timestamp for last state_position increment
     uint8_t state;                  // GPS thread state. Used for detecting cable disconnects and configuring attached devices
-    uint8_t baudrateIndex;          // index into auto-detecting or current baudrate
+    uint8_t userBaudRateIndex;          // index into auto-detecting or current baudrate
+    uint8_t tempBaudRateIndex;          // index into auto-detecting or current baudrate
 
     uint8_t ackWaitingMsgId;        // Message id when waiting for ACK
-    uint8_t ackTimeoutCounter;      // Ack timeout counter
+    ubloxAckState_e ackState;       // Ack State
-    ubloxAckState_e ackState;
+    uint8_t updateRateHz;
-    bool ubloxUsePVT;
+    bool ubloxM7orAbove;
-    bool ubloxUseSAT;
+    bool ubloxM8orAbove;
+    bool ubloxM9orAbove;
+    bool ubloxUsingFlightModel;     // false = Acquire model, true = Flight model
+    bool satMessagesDisabled;
+#ifdef USE_GPS_UBLOX
+    uint32_t lastNavSolTs;          // time stamp of last UBCX message.  Used to calculate message delta
+    ubloxVersion_e platformVersion; // module platform version, mapped from reported hardware version
+#endif
 } gpsData_t;
 
-#define GPS_PACKET_LOG_ENTRY_COUNT 21 // To make this useful we should log as many packets as we can fit characters a single line of a OLED display.
-extern char gpsPacketLog[GPS_PACKET_LOG_ENTRY_COUNT];
-
 extern int32_t GPS_home[2];
-extern uint16_t GPS_distanceToHome;        // distance to home point in meters
+extern uint16_t GPS_distanceToHome;             // distance to home point in meters
-extern uint32_t GPS_distanceToHomeCm;      // distance to home point in cm
+extern uint32_t GPS_distanceToHomeCm;           // distance to home point in cm
-extern int16_t GPS_directionToHome;        // direction to home or hol point in degrees
+extern int16_t GPS_directionToHome;             // direction to home or hol point in degrees
-extern uint32_t GPS_distanceFlownInCm;     // distance flown since armed in centimeters
+extern uint32_t GPS_distanceFlownInCm;          // distance flown since armed in centimeters
-extern int16_t GPS_angle[ANGLE_INDEX_COUNT];                // it's the angles that must be applied for GPS correction
+extern int16_t GPS_angle[ANGLE_INDEX_COUNT];    // it's the angles that must be applied for GPS correction
-extern float GPS_scaleLonDown;  // this is used to offset the shrinking longitude as we go towards the poles
+extern float GPS_scaleLonDown;                  // this is used to offset the shrinking longitude as we go towards the poles
-extern int16_t nav_takeoff_bearing;
 
 typedef enum {
     GPS_DIRECT_TICK = 1 << 0,
@@ -174,9 +319,7 @@ extern gpsSolutionData_t gpsSol;
 #define GPS_SV_MAXSATS_M8N      32U
 #define GPS_SV_MAXSATS_M9N      42U
 
-extern uint8_t GPS_update;       // toogle to distinct a GPS position update (directly or via MSP)
+extern uint8_t GPS_update;                              // toggles on GPS nav position update (directly or via MSP)
-extern uint32_t GPS_packetCount;
-extern uint32_t GPS_svInfoReceivedCount;
 extern uint8_t GPS_numCh;                               // Number of channels
 extern uint8_t GPS_svinfo_chn[GPS_SV_MAXSATS_M8N];      // When NumCh is 16 or less: Channel number
                                                         // When NumCh is more than 16: GNSS Id
@@ -204,16 +347,46 @@ extern uint8_t GPS_svinfo_quality[GPS_SV_MAXSATS_M8N];  // When NumCh is 16 or l
                                                         //     1 = carrier smoothed pseudorange used
 extern uint8_t GPS_svinfo_cno[GPS_SV_MAXSATS_M8N];      // Carrier to Noise Ratio (Signal Strength)
 
+#define TASK_GPS_RATE       100     // default update rate of GPS task
+#define TASK_GPS_RATE_FAST  500    // update rate of GPS task while Rx buffer is not empty
+
+
+#ifdef USE_DASHBOARD
+// Data used *only* by the dashboard device (OLED display).
+// Note this data should probably be in the dashboard module, not here. On the refactor list...
+
+// OLED display shows a scrolling string of chars for each packet type/event received.
+#define DASHBOARD_LOG_ERROR        '?'
+#define DASHBOARD_LOG_IGNORED      '!'
+#define DASHBOARD_LOG_SKIPPED      '>'
+#define DASHBOARD_LOG_NMEA_GGA     'g'
+#define DASHBOARD_LOG_NMEA_GSA     's'
+#define DASHBOARD_LOG_NMEA_RMC     'r'
+#define DASHBOARD_LOG_UBLOX_DOP    'D'
+#define DASHBOARD_LOG_UBLOX_SOL    'O'
+#define DASHBOARD_LOG_UBLOX_STATUS 'S'
+#define DASHBOARD_LOG_UBLOX_SVINFO 'I'
+#define DASHBOARD_LOG_UBLOX_POSLLH 'P'
+#define DASHBOARD_LOG_UBLOX_VELNED 'V'
+#define DASHBOARD_LOG_UBLOX_MONVER 'M'
+
+#define GPS_PACKET_LOG_ENTRY_COUNT 21 // To make this useful we should log as many packets as we can fit characters a single line of a OLED display.
+extern char dashboardGpsPacketLog[GPS_PACKET_LOG_ENTRY_COUNT];  // OLED display of a char for each packet type/event received.
+extern uint32_t dashboardGpsPacketCount;                        // Packet received count.
+extern uint32_t dashboardGpsNavSvInfoRcvCount;                  // Count of times sat info received & updated.
+
 #define GPS_DBHZ_MIN 0
 #define GPS_DBHZ_MAX 55
+#endif  // USE_DASHBOARD
 
-#define TASK_GPS_RATE       100
-#define TASK_GPS_RATE_FAST  1000
 
+#ifdef USE_GPS_UBLOX
+ubloxVersion_e ubloxParseVersion(const uint32_t version);
+#endif
 void gpsInit(void);
 void gpsUpdate(timeUs_t currentTimeUs);
 bool gpsNewFrame(uint8_t c);
-bool gpsIsHealthy(void); // Check for healthy communications
+bool gpsIsHealthy(void); // Returns true when the gps state is RECEIVING_DATA
 struct serialPort_s;
 void gpsEnablePassthrough(struct serialPort_s *gpsPassthroughPort);
 void onGpsNewData(void);
@@ -221,5 +394,5 @@ void GPS_reset_home_position(void);
 void GPS_calc_longitude_scaling(int32_t lat);
 void GPS_distance_cm_bearing(int32_t *currentLat1, int32_t *currentLon1, int32_t *destinationLat2, int32_t *destinationLon2, uint32_t *dist, int32_t *bearing);
 void gpsSetFixState(bool state);
-float getGpsDataIntervalSeconds(void);
+float getGpsDataIntervalSeconds(void);      // sends GPS Nav Data interval to GPS Rescue
 baudRate_e getGpsPortActualBaudRateIndex(void);

src/main/osd/osd_elements.c

@@ -1114,7 +1114,7 @@ static void osdElementGpsSats(osdElementParms_t *element)
         int pos = tfp_sprintf(element->buff, "%c%c%2d", SYM_SAT_L, SYM_SAT_R, gpsSol.numSat);
         if (osdConfig()->gps_sats_show_hdop) { // add on the GPS module HDOP estimate
             element->buff[pos++] = ' ';
-            osdPrintFloat(element->buff + pos, SYM_NONE, gpsSol.dop.hdop / 100.0f, "", 1, true, SYM_NONE);
+            osdPrintFloat(element->buff + pos, SYM_NONE, gpsSol.dop.pdop / 100.0f, "", 1, true, SYM_NONE);
         }
     }
 }

src/main/pg/gps.c

@@ -29,7 +29,7 @@
 
 #include "gps.h"
 
-PG_REGISTER_WITH_RESET_TEMPLATE(gpsConfig_t, gpsConfig, PG_GPS_CONFIG, 2);
+PG_REGISTER_WITH_RESET_TEMPLATE(gpsConfig_t, gpsConfig, PG_GPS_CONFIG, 3);
 
 PG_RESET_TEMPLATE(gpsConfig_t, gpsConfig,
     .provider = GPS_UBLOX,
@@ -40,10 +40,10 @@ PG_RESET_TEMPLATE(gpsConfig_t, gpsConfig,
     .gps_ublox_flight_model = UBLOX_MODEL_AIRBORNE_4G,
     .gps_update_rate_hz = 10,
     .gps_ublox_use_galileo = false,
-    .gps_ublox_full_power = true,
     .gps_set_home_point_once = false,
     .gps_use_3d_speed = false,
     .sbas_integrity = false,
+    .gps_ublox_utc_standard = UBLOX_UTC_STANDARD_AUTO,
 );
 
 #endif // USE_GPS

src/main/pg/gps.h

@@ -36,10 +36,10 @@ typedef struct gpsConfig_s {
     uint8_t gps_ublox_flight_model;
     uint8_t gps_update_rate_hz;
     bool gps_ublox_use_galileo;
-    bool gps_ublox_full_power;
     bool gps_set_home_point_once;
     bool gps_use_3d_speed;
     bool sbas_integrity;
+    uint8_t gps_ublox_utc_standard;
     char nmeaCustomCommands[NMEA_CUSTOM_COMMANDS_MAX_LENGTH + 1];
 } gpsConfig_t;
 

src/test/Makefile

@@ -109,8 +109,13 @@ cli_unittest_SRC := \
 		$(USER_DIR)/common/crc.c \
 		$(USER_DIR)/common/printf.c \
 		$(USER_DIR)/common/streambuf.c \
+		$(USER_DIR)/common/maths.c \
+		$(USER_DIR)/common/gps_conversion.c \
 		$(USER_DIR)/config/feature.c \
+		$(USER_DIR)/io/gps.c \
 		$(USER_DIR)/pg/pg.c \
+		$(USER_DIR)/fc/runtime_config.c \
+		$(USER_DIR)/drivers/serial.c \
 		$(USER_DIR)/common/typeconversion.c
 
 cli_unittest_DEFINES := \

src/test/unit/cli_unittest.cc

@@ -26,9 +26,12 @@ extern "C" {
     #include "platform.h"
     #include "target.h"
     #include "build/version.h"
+    #include "io/gps.h"
     #include "cli/cli.h"
     #include "cli/settings.h"
     #include "common/printf.h"
+    #include "common/maths.h"
+    #include "common/gps_conversion.h"
     #include "config/feature.h"
     #include "drivers/buf_writer.h"
     #include "drivers/vtx_common.h"
@@ -39,7 +42,6 @@ extern "C" {
     #include "flight/pid.h"
     #include "flight/servos.h"
     #include "io/beeper.h"
-    #include "io/gps.h"
     #include "io/ledstrip.h"
     #include "io/serial.h"
     #include "io/vtx.h"
@@ -47,6 +49,7 @@ extern "C" {
     #include "msp/msp_box.h"
     #include "osd/osd.h"
     #include "pg/pg.h"
+    #include "pg/gps_rescue.h"
     #include "pg/pg_ids.h"
     #include "pg/beeper.h"
     #include "pg/gps.h"
@@ -92,6 +95,7 @@ extern "C" {
     PG_REGISTER(pidConfig_t, pidConfig, PG_PID_CONFIG, 0);
     PG_REGISTER(gyroConfig_t, gyroConfig, PG_GYRO_CONFIG, 0);
     PG_REGISTER(gpsConfig_t, gpsConfig, PG_GPS_CONFIG, 0);
+    PG_REGISTER(gpsRescueConfig_t, gpsRescueConfig, PG_GPS_RESCUE, 0);
 
     PG_REGISTER_WITH_RESET_FN(int8_t, unitTestData, PG_RESERVED_FOR_TESTING_1, 0);
 }
@@ -201,6 +205,7 @@ TEST(CLIUnittest, TestCliSetStringWriteOnce)
 // STUBS
 extern "C" {
 
+int16_t debug[8];
 float motor_disarmed[MAX_SUPPORTED_MOTORS];
 
 uint16_t batteryWarningVoltage;
@@ -280,9 +285,13 @@ bool parseColor(int, const char *) {return false; }
 bool resetEEPROM(void) { return true; }
 void bufWriterFlush(bufWriter_t *) {}
 void mixerResetDisarmedMotors(void) {}
-void gpsEnablePassthrough(struct serialPort_s *) {}
+
-bool gpsIsHealthy(void) { return true; }
+typedef enum {
-baudRate_e getGpsPortActualBaudRateIndex(void) { return BAUD_AUTO; }
+    DUMMY
+} pageId_e;
+
+void dashboardShowFixedPage(pageId_e){}
+void dashboardUpdate(timeUs_t) {}
 
 bool parseLedStripConfig(int, const char *){return false; }
 const char rcChannelLetters[] = "AERT12345678abcdefgh";
@@ -302,8 +311,6 @@ baudRate_e lookupBaudRateIndex(uint32_t){return BAUD_9600; }
 serialPortUsage_t *findSerialPortUsageByIdentifier(serialPortIdentifier_e){ return NULL; }
 serialPort_t *openSerialPort(serialPortIdentifier_e, serialPortFunction_e, serialReceiveCallbackPtr, void *, uint32_t, portMode_e, portOptions_e) { return NULL; }
 const serialPortConfig_t *findSerialPortConfig(serialPortFunction_e) { return NULL; }
-void serialSetBaudRate(serialPort_t *, uint32_t) {}
-void serialSetMode(serialPort_t *, portMode_e) {}
 void serialPassthrough(serialPort_t *, serialPort_t *, serialConsumer *, serialConsumer *) {}
 uint32_t millis(void) { return 0; }
 uint8_t getBatteryCellCount(void) { return 1; }
@@ -320,11 +327,10 @@ uint16_t averageSystemLoadPercent = 0;
 
 timeDelta_t getTaskDeltaTimeUs(taskId_e){ return 0; }
 uint16_t currentRxIntervalUs = 9000;
-armingDisableFlags_e getArmingDisableFlags(void) { return ARMING_DISABLED_NO_GYRO; }
 
-const char *armingDisableFlagNames[]= {
+/*const char *armingDisableFlagNames[]= {
 "DUMMYDISABLEFLAGNAME"
-};
+};*/
 
 void getTaskInfo(taskId_e, taskInfo_t *) {}
 void getCheckFuncInfo(cfCheckFuncInfo_t *) {}
@@ -336,13 +342,13 @@ const char* const buildDate = "Jan 01 2017";
 const char * const buildTime = "00:00:00";
 const char * const shortGitRevision = "MASTER";
 
-uint32_t serialRxBytesWaiting(const serialPort_t *) {return 0;}
+//uint32_t serialRxBytesWaiting(const serialPort_t *) {return 0;}
-uint8_t serialRead(serialPort_t *){return 0;}
+//uint8_t serialRead(serialPort_t *){return 0;}
 
 void bufWriterAppend(bufWriter_t *, uint8_t ch){ printf("%c", ch); }
-void serialWriteBufShim(void *, const uint8_t *, int) {}
+//void serialWriteBufShim(void *, const uint8_t *, int) {}
 void bufWriterInit(bufWriter_t *, uint8_t *, int, bufWrite_t, void *) { }
-void setArmingDisabled(armingDisableFlags_e) {}
+//void setArmingDisabled(armingDisableFlags_e) {}
 
 void waitForSerialPortToFinishTransmitting(serialPort_t *) {}
 void systemResetToBootloader(void) {}
@@ -353,15 +359,16 @@ void writeUnmodifiedConfigToEEPROM(void) {}
 void changePidProfile(uint8_t) {}
 bool serialIsPortAvailable(serialPortIdentifier_e) { return false; }
 void generateLedConfig(ledConfig_t *, char *, size_t) {}
-bool isSerialTransmitBufferEmpty(const serialPort_t *) {return true; }
+//bool isSerialTransmitBufferEmpty(const serialPort_t *) {return true; }
-void serialWrite(serialPort_t *, uint8_t ch) { printf("%c", ch);}
+//void serialWrite(serialPort_t *, uint8_t ch) { printf("%c", ch);}
 
-void serialSetCtrlLineStateCb(serialPort_t *, void (*)(void *, uint16_t ), void *) {}
+//void serialSetCtrlLineStateCb(serialPort_t *, void (*)(void *, uint16_t ), void *) {}
 void serialSetCtrlLineStateDtrPin(serialPort_t *, ioTag_t ) {}
 void serialSetCtrlLineState(serialPort_t *, uint16_t ) {}
 
-void serialSetBaudRateCb(serialPort_t *, void (*)(serialPort_t *context, uint32_t baud), serialPort_t *) {}
+//void serialSetBaudRateCb(serialPort_t *, void (*)(serialPort_t *context, uint32_t baud), serialPort_t *) {}
-
+void rescheduleTask(taskId_e, timeDelta_t){}
+void schedulerSetNextStateTime(timeDelta_t ){}
 char *getBoardName(void) { return NULL; }
 char *getManufacturerId(void) { return NULL; }
 bool boardInformationIsSet(void) { return true; }

src/test/unit/target.h

@@ -32,6 +32,8 @@
 #define USE_MAG
 #define USE_BARO
 #define USE_GPS
+#define USE_GPS_NMEA
+#define USE_GPS_UBLOX
 #define USE_DASHBOARD
 #define USE_SERIALRX
 #define USE_RX_MSP