Merge pull request iNavFlight#10179 from iNavFlight/mmosca-ublox-satinfo

[GPS] Fix message rate for newer ublox devices and add some extra debugging features

Author: mmosca

Diff for: src/main/build/debug.h

@@ -76,7 +76,8 @@ typedef enum {
     DEBUG_POS_EST,
     DEBUG_ADAPTIVE_FILTER,
     DEBUG_HEADTRACKING,
-    DEBUG_COUNT
+    DEBUG_GPS,
+    DEBUG_COUNT // also update debugModeNames in cli.c
 } debugType_e;
 
 #ifdef SITL_BUILD

Diff for: src/main/common/printf.c

@@ -162,6 +162,7 @@ int tfp_nformat(void *putp, int size, void (*putf) (void *, char), const char *f
                     written += putchw(putp, end, putf, w, lz, bf);
                     break;
                 }
+            case 'i':
             case 'd':{
 #ifdef REQUIRE_PRINTF_LONG_SUPPORT
                     if (lng)

Diff for: src/main/fc/cli.c

@@ -187,9 +187,39 @@ static const char * const blackboxIncludeFlagNames[] = {
 };
 #endif
 
-/* Sensor names (used in lookup tables for *_hardware settings and in status command output) */
+static const char *debugModeNames[DEBUG_COUNT] = {
+    "NONE",
+    "AGL",
+    "FLOW_RAW",
+    "FLOW",
+    "ALWAYS",
+    "SAG_COMP_VOLTAGE",
+    "VIBE",
+    "CRUISE",
+    "REM_FLIGHT_TIME",
+    "SMARTAUDIO",
+    "ACC",
+    "NAV_YAW",
+    "PCF8574",
+    "DYN_GYRO_LPF",
+    "AUTOLEVEL",
+    "ALTITUDE",
+    "AUTOTRIM",
+    "AUTOTUNE",
+    "RATE_DYNAMICS",
+    "LANDING",
+    "POS_EST",
+    "ADAPTIVE_FILTER",
+    "HEADTRACKER",
+    "GPS"
+};
+
+/* Sensor names (used in lookup tables for *_hardware settings and in status
+   command output) */
 // sync with gyroSensor_e
-static const char * const gyroNames[] = { "NONE", "AUTO", "MPU6000", "MPU6500", "MPU9250", "BMI160", "ICM20689", "BMI088", "ICM42605", "BMI270","LSM6DXX", "FAKE"};
+static const char *const gyroNames[] = {
+    "NONE",     "AUTO",   "MPU6000",  "MPU6500", "MPU9250", "BMI160",
+    "ICM20689", "BMI088", "ICM42605", "BMI270",  "LSM6DXX", "FAKE"};
 
 // sync this with sensors_e
 static const char * const sensorTypeNames[] = {
@@ -253,6 +283,7 @@ static void cliPrintLine(const char *str)
     cliPrintLinefeed();
 }
 
+
 static void cliPrintError(const char *str)
 {
     cliPrint("### ERROR: ");
@@ -3858,10 +3889,17 @@ static void cliStatus(char *cmdline)
     cliPrintLinefeed();
 #endif
 
-    if (featureConfigured(FEATURE_GPS) && (gpsConfig()->provider == GPS_UBLOX || gpsConfig()->provider == GPS_UBLOX7PLUS)) {
+    if (featureConfigured(FEATURE_GPS) && isGpsUblox()) {
         cliPrint("GPS: ");
         cliPrintf("HW Version: %s Proto: %d.%02d Baud: %d", getGpsHwVersion(), getGpsProtoMajorVersion(), getGpsProtoMinorVersion(), getGpsBaudrate());
+        if(ubloxVersionLT(15, 0)) {
+            cliPrintf(" (UBLOX Proto >= 15.0 required)");
+        }
         cliPrintLinefeed();
+        cliPrintLinef("  SATS: %i", gpsSol.numSat);
+        cliPrintLinef("  HDOP: %f", (double)(gpsSol.hdop / (float)HDOP_SCALE));
+        cliPrintLinef("  EPH : %f m", (double)(gpsSol.eph / 100.0f));
+        cliPrintLinef("  EPV : %f m", (double)(gpsSol.epv / 100.0f));
         //cliPrintLinef("  GNSS Capabilities: %d", gpsUbloxCapLastUpdate());
         cliPrintLinef("  GNSS Capabilities:");
         cliPrintLine("    GNSS Provider active/default");
@@ -4259,6 +4297,136 @@ typedef struct {
 }
 #endif
 
+static void cliCmdDebug(char *arg)
+{
+    UNUSED(arg);
+    if (debugMode != DEBUG_NONE) {
+        cliPrintLinef("Debug fields: [%s (%i)]", debugMode < DEBUG_COUNT ? debugModeNames[debugMode] : "unknown", debugMode);
+        for (int i = 0; i < DEBUG32_VALUE_COUNT; i++) {
+            cliPrintLinef("debug[%d] = %d", i, debug[i]);
+        }
+    } else {
+        cliPrintLine("Debug mode is disabled");
+    }
+}
+
+
+#if defined(USE_GPS) && defined(USE_GPS_PROTO_UBLOX)
+
+static const char* _ubloxGetSigId(uint8_t gnssId, uint8_t sigId)
+{
+    if(gnssId == 0) {
+        switch(sigId) {
+            case 0: return "GPS L1C/A";
+            case 3: return "GPS L2 CL";
+            case 4: return "GPS L2 CM";
+            case 6: return "GPS L5 I";
+            case 7: return "GPS L5 Q";
+            default: return "GPS Unknown";
+        }
+    } else if(gnssId == 1) {
+        switch(sigId) {
+            case 0: return "SBAS L1C/A";
+            default: return "SBAS Unknown";
+        }
+    } else if(gnssId == 2) {
+        switch(sigId) {
+            case 0: return "Galileo E1 C";
+            case 1: return "Galileo E1 B";
+            case 3: return "Galileo E5 al";
+            case 4: return "Galileo E5 aQ";
+            case 5: return "Galileo E5 bl";
+            case 6: return "Galileo E5 bQ";
+            default: return "Galileo Unknown";
+        }
+    } else if(gnssId == 3) {
+        switch(sigId) {
+            case 0: return "BeiDou B1I D1";
+            case 1: return "BeiDou B1I D2";
+            case 2: return "BeiDou B2I D1";
+            case 3: return "BeiDou B2I D2";
+            case 5: return "BeiDou B1C";
+            case 7: return "BeiDou B2a";
+            default: return "BeiDou Unknown";
+        }
+    } else if(gnssId == 5) {
+        switch(sigId) {
+            case 0: return "QZSS L1C/A";
+            case 1: return "QZSS L1S";
+            case 4: return "QZSS L2 CM";
+            case 5: return "QZSS L2 CL";
+            case 8: return "QZSS L5 I";
+            case 9: return "QZSS L5 Q";
+            default: return "QZSS Unknown";
+        }
+    } else if(gnssId == 6) {
+        switch(sigId) {
+            case 0: return "GLONASS L1 OF";
+            case 2: return "GLONASS L2 OF";
+            default: return "GLONASS Unknown";
+        }
+    }
+
+    return "Unknown GNSS/SigId";
+}
+
+static const char *_ubloxGetQuality(uint8_t quality)
+{
+    switch(quality) {
+        case UBLOX_SIG_QUALITY_NOSIGNAL: return "No signal";
+        case UBLOX_SIG_QUALITY_SEARCHING: return "Searching signal...";
+        case UBLOX_SIG_QUALITY_ACQUIRED: return "Signal acquired";
+        case UBLOX_SIG_QUALITY_UNUSABLE: return "Signal detected but unusable";
+        case UBLOX_SIG_QUALITY_CODE_LOCK_TIME_SYNC: return "Code locked and time sync";
+        case UBLOX_SIG_QUALITY_CODE_CARRIER_LOCK_TIME_SYNC:
+        case UBLOX_SIG_QUALITY_CODE_CARRIER_LOCK_TIME_SYNC2:
+        case UBLOX_SIG_QUALITY_CODE_CARRIER_LOCK_TIME_SYNC3: 
+            return "Code and carrier locked and time sync";
+        default: return "Unknown";
+    }
+}
+
+static void cliUbloxPrintSatelites(char *arg)
+{
+    UNUSED(arg);
+    if(!isGpsUblox() /*|| !(gpsState.flags.sig || gpsState.flags.sat)*/) {
+        cliPrint("GPS is not UBLOX or does not report satelites.");
+        return;
+    }
+
+    cliPrintLine("UBLOX Satelites");
+
+    for(int i = 0; i < UBLOX_MAX_SIGNALS; ++i)
+    {
+        const ubx_nav_sig_info *sat = gpsGetUbloxSatelite(i);
+        if(sat == NULL) {
+            continue;
+        }
+
+        cliPrintLinef("satelite[%d]: %d:%d", i+1, sat->gnssId, sat->svId);
+        cliPrintLinef("sigId: %d (%s)", sat->sigId, _ubloxGetSigId(sat->gnssId, sat->sigId));
+        cliPrintLinef("signal strength: %i dbHz", sat->cno);
+        cliPrintLinef("quality: %i (%s)", sat->quality, _ubloxGetQuality(sat->quality));
+        //cliPrintLinef("Correlation: %i", sat->corrSource);
+        //cliPrintLinef("Iono model: %i", sat->ionoModel);
+        cliPrintLinef("signal flags: 0x%02X", sat->sigFlags);
+        switch(sat->sigFlags & UBLOX_SIG_HEALTH_MASK) {
+            case UBLOX_SIG_HEALTH_HEALTHY:
+                cliPrintLine("signal: Healthy");
+                break;
+            case UBLOX_SIG_HEALTH_UNHEALTHY:
+                cliPrintLine("signal: Unhealthy");
+                break;
+            case UBLOX_SIG_HEALTH_UNKNOWN:
+            default:
+                cliPrintLinef("signal: Unknown (0x%X)", sat->sigFlags & UBLOX_SIG_HEALTH_MASK);
+                break;
+        }
+        cliPrintLinefeed();
+    }
+}
+#endif
+
 static void cliHelp(char *cmdline);
 
 // should be sorted a..z for bsearch()
@@ -4318,6 +4486,7 @@ const clicmd_t cmdTable[] = {
     CLI_COMMAND_DEF("get", "get variable value", "[name]", cliGet),
 #ifdef USE_GPS
     CLI_COMMAND_DEF("gpspassthrough", "passthrough gps to serial", NULL, cliGpsPassthrough),
+    CLI_COMMAND_DEF("gpssats", "show GPS satellites", NULL, cliUbloxPrintSatelites),
 #endif
     CLI_COMMAND_DEF("help", NULL, NULL, cliHelp),
 #ifdef USE_LED_STRIP
@@ -4370,6 +4539,7 @@ const clicmd_t cmdTable[] = {
 #ifdef USE_SDCARD
     CLI_COMMAND_DEF("sd_info", "sdcard info", NULL, cliSdInfo),
 #endif
+    CLI_COMMAND_DEF("showdebug", "Show debug fields.", NULL, cliCmdDebug),
     CLI_COMMAND_DEF("status", "show status", NULL, cliStatus),
     CLI_COMMAND_DEF("tasks", "show task stats", NULL, cliTasks),
 #ifdef USE_TEMPERATURE_SENSOR

Diff for: src/main/fc/settings.yaml

@@ -83,7 +83,8 @@ tables:
     values: ["NONE", "AGL", "FLOW_RAW", "FLOW", "ALWAYS", "SAG_COMP_VOLTAGE",
       "VIBE", "CRUISE", "REM_FLIGHT_TIME", "SMARTAUDIO", "ACC",
       "NAV_YAW", "PCF8574", "DYN_GYRO_LPF", "AUTOLEVEL", "ALTITUDE",
-      "AUTOTRIM", "AUTOTUNE", "RATE_DYNAMICS", "LANDING", "POS_EST", "ADAPTIVE_FILTER", "HEADTRACKER" ]
+      "AUTOTRIM", "AUTOTUNE", "RATE_DYNAMICS", "LANDING", "POS_EST", 
+      "ADAPTIVE_FILTER", "HEADTRACKER", "GPS" ]
   - name: aux_operator
     values: ["OR", "AND"]
     enum: modeActivationOperator_e

Diff for: src/main/io/gps_private.h

@@ -50,6 +50,12 @@ typedef struct {
     gpsBaudRate_e   baudrateIndex;
     gpsBaudRate_e   autoBaudrateIndex;      // Driver internal use (for autoBaud)
     uint8_t         autoConfigStep;         // Driver internal use (for autoConfig)
+    struct
+    {
+        uint8_t pvt : 1;
+        uint8_t sig : 1;
+        uint8_t sat : 1;
+    } flags;
 
     timeMs_t        lastStateSwitchMs;
     timeMs_t        lastLastMessageMs;