From ce5156ba18cc1a12f1830a91cf0d8073be3929af Mon Sep 17 00:00:00 2001 From: Marcelo Bezerra <23555060+mmosca@users.noreply.github.com> Date: Tue, 25 Jun 2024 23:08:05 +0200 Subject: [PATCH 01/23] Use correct method to set message rate for newer ublox devices. As a bonus: Add gpssats and showdebug cli commmands. --- src/main/build/debug.h | 3 +- src/main/common/printf.c | 1 + src/main/fc/cli.c | 161 +++++++++++++++++++++++++++++++++++++- src/main/fc/settings.yaml | 3 +- src/main/io/gps_ublox.c | 77 +++++++++++++----- src/main/io/gps_ublox.h | 44 ++++++++++- 6 files changed, 263 insertions(+), 26 deletions(-) diff --git a/src/main/build/debug.h b/src/main/build/debug.h index fea424b9005..7ee74471544 100644 --- a/src/main/build/debug.h +++ b/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 --git a/src/main/common/printf.c b/src/main/common/printf.c index 5e72f726b04..39dc01ed4b5 100644 --- a/src/main/common/printf.c +++ b/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 --git a/src/main/fc/cli.c b/src/main/fc/cli.c index 0d54cab4398..83d3ac51e64 100644 --- a/src/main/fc/cli.c +++ b/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: "); @@ -4259,6 +4290,130 @@ 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 0: return "No signal"; + case 1: return "Searching signal..."; + case 2: return "Signal acquired"; + case 3: return "Signal detected but unusable"; + case 4: return "Code locked and time synch"; + case 5: + case 6: + case 7: + return "Code and carrier locked and time synch"; + default: return "Unknown"; + } +} + +static void cliUbloxPrintSatelites(char *arg) +{ + UNUSED(arg); + if(!isGpsUblox()) { + cliPrint("GPS is not UBLOX"); + return; + } + + cliPrintLine("UBLOX Satelites"); + + for(int i = 0; i < UBLOX_MAX_SIGNALS; ++i) + { + const ubx_nav_sig_info *sat = gpsGetUbloxSatelite(i); + if(sat == NULL || sat->svId == 0) { + continue; + } + + cliPrintLinef("satelite: %d:%d", 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); + if(sat->sigFlags & 0x01) { + cliPrintLine("signal: Healthy"); + } else if (sat->sigFlags & 0x02) { + cliPrintLine("signal: Unhealthy"); + } else { + cliPrintLine("signal: Unknown"); + } + } +} +#endif + static void cliHelp(char *cmdline); // should be sorted a..z for bsearch() @@ -4318,6 +4473,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 +4526,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 --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index a27e9599db7..d8dd7d6fbee 100644 --- a/src/main/fc/settings.yaml +++ b/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 --git a/src/main/io/gps_ublox.c b/src/main/io/gps_ublox.c index 52b6329d9f2..b6ea20e854f 100755 --- a/src/main/io/gps_ublox.c +++ b/src/main/io/gps_ublox.c @@ -43,6 +43,7 @@ #include "fc/config.h" #include "fc/runtime_config.h" #include "fc/settings.h" +#include "fc/cli.h" #include "io/serial.h" #include "io/gps.h" @@ -84,6 +85,8 @@ static const char * baudInitDataNMEA[GPS_BAUDRATE_COUNT] = { "$PUBX,41,1,0003,0001,921600,0*15\r\n" // GPS_BAUDRATE_921600 }; +static ubx_nav_sig_info satelites[UBLOX_MAX_SIGNALS] = {}; + // Packet checksum accumulators static uint8_t _ck_a; static uint8_t _ck_b; @@ -145,6 +148,7 @@ static union { ubx_nav_timeutc timeutc; ubx_ack_ack ack; ubx_mon_gnss gnss; + ubx_nav_sig navsig; uint8_t bytes[UBLOX_BUFFER_SIZE]; } _buffer; @@ -551,6 +555,7 @@ static uint32_t gpsDecodeHardwareVersion(const char * szBuf, unsigned nBufSize) static bool gpsParseFrameUBLOX(void) { + DEBUG_SET(DEBUG_GPS, 7, 42); switch (_msg_id) { case MSG_POSLLH: gpsSolDRV.llh.lon = _buffer.posllh.longitude; @@ -601,6 +606,10 @@ static bool gpsParseFrameUBLOX(void) } break; case MSG_PVT: + { + static int pvtCount = 0; + DEBUG_SET(DEBUG_GPS, 1, pvtCount++); + } next_fix_type = gpsMapFixType(_buffer.pvt.fix_status & NAV_STATUS_FIX_VALID, _buffer.pvt.fix_type); gpsSolDRV.fixType = next_fix_type; gpsSolDRV.llh.lon = _buffer.pvt.longitude; @@ -684,6 +693,30 @@ static bool gpsParseFrameUBLOX(void) } } break; + case MSG_NAV_SAT: + if (_class == CLASS_NAV) { + static int satInfoCount = 0; + DEBUG_SET(DEBUG_GPS, 2, satInfoCount++); + gpsSolDRV.numSat = _buffer.svinfo.numCh; + } + break; + case MSG_SIG_INFO: + if (_class == CLASS_NAV && _buffer.navsig.version == 0) { + if(_buffer.navsig.numSigs < UBLOX_MAX_SIGNALS) + { + for(int i=0; i < UBLOX_MAX_SIGNALS && i < _buffer.navsig.numSigs; ++i) + { + memcpy(&satelites[i], &_buffer.navsig.sig[i], sizeof(ubx_nav_sig_info)); + } + for(int i = _buffer.navsig.numSigs; i < UBLOX_MAX_SIGNALS; ++i) + { + satelites[i].svId = 0; // no used + } + } + static int sigInfoCount = 0; + DEBUG_SET(DEBUG_GPS, 0, sigInfoCount++); + } + break; case MSG_ACK_ACK: if ((_ack_state == UBX_ACK_WAITING) && (_buffer.ack.msg == _ack_waiting_msg)) { _ack_state = UBX_ACK_GOT_ACK; @@ -856,27 +889,20 @@ STATIC_PROTOTHREAD(gpsConfigure) gpsSetProtocolTimeout(GPS_SHORT_TIMEOUT); // M9N & M10 does not support some of the UBX 6/7/8 messages, so we have to configure it using special sequence - if (gpsState.hwVersion >= UBX_HW_VERSION_UBLOX9) { - configureMSG(MSG_CLASS_UBX, MSG_POSLLH, 0); - ptWait(_ack_state == UBX_ACK_GOT_ACK); - - configureMSG(MSG_CLASS_UBX, MSG_STATUS, 0); - ptWait(_ack_state == UBX_ACK_GOT_ACK); - - configureMSG(MSG_CLASS_UBX, MSG_VELNED, 0); - ptWait(_ack_state == UBX_ACK_GOT_ACK); - - configureMSG(MSG_CLASS_UBX, MSG_TIMEUTC, 0); - ptWait(_ack_state == UBX_ACK_GOT_ACK); - - configureMSG(MSG_CLASS_UBX, MSG_PVT, 1); - ptWait(_ack_state == UBX_ACK_GOT_ACK); - - configureMSG(MSG_CLASS_UBX, MSG_NAV_SAT, 0); - ptWait(_ack_state == UBX_ACK_GOT_ACK); + if (gpsState.hwVersion >= UBX_HW_VERSION_UBLOX9 || gpsState.swVersionMajor > 23 || (gpsState.swVersionMajor == 23 && gpsState.swVersionMinor >=1)) { // > 23.01, don't use configureMSG + ubx_config_data8_payload_t rateValues[] = { + {UBLOX_CFG_MSGOUT_NAV_POSLLH_UART1, 0}, + {UBLOX_CFG_MSGOUT_NAV_STATUS_UART1, 0}, + {UBLOX_CFG_MSGOUT_NAV_VELNED_UART1, 0}, + {UBLOX_CFG_MSGOUT_NAV_TIMEUTC_UART1, 0}, + {UBLOX_CFG_MSGOUT_NAV_PVT_UART1, 1}, + {UBLOX_CFG_MSGOUT_NAV_SAT_UART1, 0}, + {UBLOX_CFG_MSGOUT_NAV_SIG_UART1, 1}, + }; - configureMSG(MSG_CLASS_UBX, MSG_NAV_SIG, 0); + ubloxSendSetCfgBytes(rateValues, 7); ptWait(_ack_state == UBX_ACK_GOT_ACK); + //ptWaitTimeout((_ack_state == UBX_ACK_GOT_ACK || _ack_state == UBX_ACK_GOT_NAK), GPS_CFG_CMD_TIMEOUT_MS); if ((gpsState.gpsConfig->provider == GPS_UBLOX7PLUS) && (gpsState.hwVersion >= UBX_HW_VERSION_UBLOX7)) { configureRATE(hz2rate(gpsState.gpsConfig->ubloxNavHz)); // default 10Hz @@ -1134,11 +1160,22 @@ void gpsHandleUBLOX(void) bool isGpsUblox(void) { - if(gpsState.gpsConfig->provider == GPS_UBLOX || gpsState.gpsConfig->provider == GPS_UBLOX7PLUS) { + if(gpsState.gpsPort != NULL && (gpsState.gpsConfig->provider == GPS_UBLOX || gpsState.gpsConfig->provider == GPS_UBLOX7PLUS)) { return true; } return false; } + +const ubx_nav_sig_info *gpsGetUbloxSatelite(uint8_t index) +{ + if(index < UBLOX_MAX_SIGNALS) { + return &satelites[index]; + } + + return NULL; +} + + #endif diff --git a/src/main/io/gps_ublox.h b/src/main/io/gps_ublox.h index 87cffa9c50b..041f98653b5 100644 --- a/src/main/io/gps_ublox.h +++ b/src/main/io/gps_ublox.h @@ -28,7 +28,8 @@ extern "C" { #define GPS_CFG_CMD_TIMEOUT_MS 500 #define GPS_VERSION_RETRY_TIMES 3 -#define MAX_UBLOX_PAYLOAD_SIZE 256 +#define UBLOX_MAX_SIGNALS 32 +#define MAX_UBLOX_PAYLOAD_SIZE 640 // enough for anyone? // UBX-NAV-SIG info would be UBLOX_MAX_SIGNALS + 8 for (32 * 16) + 8 = 520 bytes #define UBLOX_BUFFER_SIZE MAX_UBLOX_PAYLOAD_SIZE #define UBLOX_SBAS_MESSAGE_LENGTH 16 #define GPS_CAPA_INTERVAL 5000 @@ -55,6 +56,13 @@ extern "C" { #define UBX_HW_VERSION_UBLOX9 900 #define UBX_HW_VERSION_UBLOX10 1000 +#define UBLOX_CFG_MSGOUT_NAV_POSLLH_UART1 0x2091002a // U1 +#define UBLOX_CFG_MSGOUT_NAV_SAT_UART1 0x20910016 // U1 +#define UBLOX_CFG_MSGOUT_NAV_SIG_UART1 0x20910346 // U1 +#define UBLOX_CFG_MSGOUT_NAV_STATUS_UART1 0x2091001b // U1 +#define UBLOX_CFG_MSGOUT_NAV_VELNED_UART1 0x20910043 // U1 +#define UBLOX_CFG_MSGOUT_NAV_TIMEUTC_UART1 0x2091005c // U1 +#define UBLOX_CFG_MSGOUT_NAV_PVT_UART1 0x20910007 // U1 #define UBLOX_CFG_SIGNAL_SBAS_ENA 0x10310020 // U1 #define UBLOX_CFG_SIGNAL_SBAS_L1CA_ENA 0x10310005 // U1 @@ -169,6 +177,34 @@ typedef struct { uint8_t reserved; } __attribute__((packed)) ubx_config_data_header_v1_t; +typedef struct { + uint8_t gnssId; // gnssid 0 = GPS, 1 = SBAS, 2 = GALILEO, 3 = BEIDOU, 4 = IMES, 5 = QZSS, 6 = GLONASS + uint8_t svId; // space vehicle ID + uint8_t sigId; // new style signal ID (gnssId:sigId) + uint8_t freqId; // 0-13 slot +, 0-13, glonass only + int16_t prRes; // pseudo range residual (0.1m) + uint8_t cno; // carrier to noise density ratio (dbHz) + uint8_t quality; // 0 = no signal, 1 = search, 2 = acq, 3 = detected, 4 = code lock + time, 5,6,7 = code/carrier lock + time + uint8_t corrSource; // Correction source: 0 = no correction, 1 = SBAS, 2 = BeiDou, 3 = RTCM2, 4 = RTCM3 OSR, 5 = RTCM3 SSR, 6 = QZSS SLAS, 7 = SPARTN + uint8_t ionoModel; // 0 = no mode, 1 = Klobuchar GPS, 2 = SBAS, 3 = Klobuchar BeiDou, 8 = Iono derived from dual frequency observations + uint16_t sigFlags; // bit:0-1, 0 = unknown, 1 = healthy, 2 = unhealthy + // bit2: pseudorange smoothed, + // bit3: pseudorange used, + // bit4: carrioer range used; + // bit5: doppler used + // bit6: pseudorange corrections used + // bit7: carrier correction used + // bit8: doper corrections used + uint8_t reserved; +} __attribute__((packed)) ubx_nav_sig_info; + +typedef struct { + uint32_t time; // GPS iToW + uint8_t version; // We support version 0 + uint8_t numSigs; // number of signals + uint16_t reserved; + ubx_nav_sig_info sig[UBLOX_MAX_SIGNALS]; // 32 signals +} __attribute__((packed)) ubx_nav_sig; #define MAX_GNSS 7 #define MAX_GNSS_SIZE_BYTES (sizeof(ubx_gnss_msg_t) + sizeof(ubx_gnss_element_t)*MAX_GNSS) @@ -350,6 +386,8 @@ typedef struct { uint8_t reserverd3; } ubx_mon_gnss; + + typedef struct { uint8_t msg_class; uint8_t msg; @@ -394,7 +432,8 @@ typedef enum { MSG_CFG_NAV_SETTINGS = 0x24, MSG_CFG_SBAS = 0x16, MSG_CFG_GNSS = 0x3e, - MSG_MON_GNSS = 0x28 + MSG_MON_GNSS = 0x28, + MSG_SIG_INFO = 0x43 } ubx_protocol_bytes_t; typedef enum { @@ -428,6 +467,7 @@ bool gpsUbloxSendCommand(uint8_t *rawCommand, uint16_t commandLen, uint16_t time bool isGpsUblox(void); +const ubx_nav_sig_info *gpsGetUbloxSatelite(uint8_t index); #ifdef __cplusplus } From eca4674af44edd2a2237d8d233d77973afde82c7 Mon Sep 17 00:00:00 2001 From: Marcelo Bezerra <23555060+mmosca@users.noreply.github.com> Date: Tue, 25 Jun 2024 23:24:10 +0200 Subject: [PATCH 02/23] Add some extra gps status info to status command --- src/main/fc/cli.c | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/src/main/fc/cli.c b/src/main/fc/cli.c index 83d3ac51e64..be7858fadda 100644 --- a/src/main/fc/cli.c +++ b/src/main/fc/cli.c @@ -3889,10 +3889,14 @@ 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()); 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"); From 5451413cc3d6671a1a87522af2dd40d8ecceff00 Mon Sep 17 00:00:00 2001 From: Marcelo Bezerra <23555060+mmosca@users.noreply.github.com> Date: Tue, 25 Jun 2024 23:25:06 +0200 Subject: [PATCH 03/23] Status formatting --- src/main/fc/cli.c | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/src/main/fc/cli.c b/src/main/fc/cli.c index be7858fadda..8df40e20066 100644 --- a/src/main/fc/cli.c +++ b/src/main/fc/cli.c @@ -3893,10 +3893,10 @@ static void cliStatus(char *cmdline) cliPrint("GPS: "); cliPrintf("HW Version: %s Proto: %d.%02d Baud: %d", getGpsHwVersion(), getGpsProtoMajorVersion(), getGpsProtoMinorVersion(), getGpsBaudrate()); 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(" 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"); From 615402a9f672831b32846d36759d69fba2992a25 Mon Sep 17 00:00:00 2001 From: Marcelo Bezerra <23555060+mmosca@users.noreply.github.com> Date: Wed, 26 Jun 2024 00:45:12 +0200 Subject: [PATCH 04/23] Add line feed to sparete satelites --- src/main/fc/cli.c | 1 + 1 file changed, 1 insertion(+) diff --git a/src/main/fc/cli.c b/src/main/fc/cli.c index 8df40e20066..872d9cbc933 100644 --- a/src/main/fc/cli.c +++ b/src/main/fc/cli.c @@ -4414,6 +4414,7 @@ static void cliUbloxPrintSatelites(char *arg) } else { cliPrintLine("signal: Unknown"); } + cliPrintLinefeed(); } } #endif From 19a4fc3cdc97f2d74b4e653b5d64c0fc3801674d Mon Sep 17 00:00:00 2001 From: Marcelo Bezerra <23555060+mmosca@users.noreply.github.com> Date: Wed, 26 Jun 2024 01:08:31 +0200 Subject: [PATCH 05/23] Cleanup --- src/main/io/gps_ublox.c | 6 ++---- 1 file changed, 2 insertions(+), 4 deletions(-) diff --git a/src/main/io/gps_ublox.c b/src/main/io/gps_ublox.c index b6ea20e854f..a22858ef8fc 100755 --- a/src/main/io/gps_ublox.c +++ b/src/main/io/gps_ublox.c @@ -555,7 +555,6 @@ static uint32_t gpsDecodeHardwareVersion(const char * szBuf, unsigned nBufSize) static bool gpsParseFrameUBLOX(void) { - DEBUG_SET(DEBUG_GPS, 7, 42); switch (_msg_id) { case MSG_POSLLH: gpsSolDRV.llh.lon = _buffer.posllh.longitude; @@ -702,6 +701,8 @@ static bool gpsParseFrameUBLOX(void) break; case MSG_SIG_INFO: if (_class == CLASS_NAV && _buffer.navsig.version == 0) { + static int sigInfoCount = 0; + DEBUG_SET(DEBUG_GPS, 0, sigInfoCount++); if(_buffer.navsig.numSigs < UBLOX_MAX_SIGNALS) { for(int i=0; i < UBLOX_MAX_SIGNALS && i < _buffer.navsig.numSigs; ++i) @@ -713,8 +714,6 @@ static bool gpsParseFrameUBLOX(void) satelites[i].svId = 0; // no used } } - static int sigInfoCount = 0; - DEBUG_SET(DEBUG_GPS, 0, sigInfoCount++); } break; case MSG_ACK_ACK: @@ -902,7 +901,6 @@ STATIC_PROTOTHREAD(gpsConfigure) ubloxSendSetCfgBytes(rateValues, 7); ptWait(_ack_state == UBX_ACK_GOT_ACK); - //ptWaitTimeout((_ack_state == UBX_ACK_GOT_ACK || _ack_state == UBX_ACK_GOT_NAK), GPS_CFG_CMD_TIMEOUT_MS); if ((gpsState.gpsConfig->provider == GPS_UBLOX7PLUS) && (gpsState.hwVersion >= UBX_HW_VERSION_UBLOX7)) { configureRATE(hz2rate(gpsState.gpsConfig->ubloxNavHz)); // default 10Hz From bab841c83dcf9a6445ac385cfa14007d2e934f6f Mon Sep 17 00:00:00 2001 From: Marcelo Bezerra <23555060+mmosca@users.noreply.github.com> Date: Wed, 26 Jun 2024 11:46:36 +0200 Subject: [PATCH 06/23] UBLOX8 did not support new message and used old configuratoin interface. Provide alternative way of setting message rates in case firmware is too old. Try to enable NAV_SIG but fallback to NAV_SAT. TODO: fill nav sat info into satelites Current status of GPS debug counters 0: NAV_SIG count 1: MSG_PVT count 2: MSG_SAT count 3: 4: 5: flags.pvt 6: flags.sat 7: flags.sig --- src/main/fc/cli.c | 4 +- src/main/io/gps_private.h | 6 +++ src/main/io/gps_ublox.c | 84 +++++++++++++++++++++++++++++++++------ 3 files changed, 79 insertions(+), 15 deletions(-) diff --git a/src/main/fc/cli.c b/src/main/fc/cli.c index 872d9cbc933..fa8ec75b615 100644 --- a/src/main/fc/cli.c +++ b/src/main/fc/cli.c @@ -4386,8 +4386,8 @@ static const char *_ubloxGetQuality(uint8_t quality) static void cliUbloxPrintSatelites(char *arg) { UNUSED(arg); - if(!isGpsUblox()) { - cliPrint("GPS is not UBLOX"); + if(!isGpsUblox() /*|| !(gpsState.flags.sig || gpsState.flags.sat)*/) { + cliPrint("GPS is not UBLOX or does not report satelites."); return; } diff --git a/src/main/io/gps_private.h b/src/main/io/gps_private.h index ee2a0b1e215..e5234e70248 100755 --- a/src/main/io/gps_private.h +++ b/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; diff --git a/src/main/io/gps_ublox.c b/src/main/io/gps_ublox.c index a22858ef8fc..35b0de92ae6 100755 --- a/src/main/io/gps_ublox.c +++ b/src/main/io/gps_ublox.c @@ -696,7 +696,10 @@ static bool gpsParseFrameUBLOX(void) if (_class == CLASS_NAV) { static int satInfoCount = 0; DEBUG_SET(DEBUG_GPS, 2, satInfoCount++); - gpsSolDRV.numSat = _buffer.svinfo.numCh; + if (!gpsState.flags.pvt) { // PVT is the prefered source + gpsSolDRV.numSat = _buffer.svinfo.numCh; + } + // TODO: populate satelites[] with sat info } break; case MSG_SIG_INFO: @@ -888,19 +891,59 @@ STATIC_PROTOTHREAD(gpsConfigure) gpsSetProtocolTimeout(GPS_SHORT_TIMEOUT); // M9N & M10 does not support some of the UBX 6/7/8 messages, so we have to configure it using special sequence - if (gpsState.hwVersion >= UBX_HW_VERSION_UBLOX9 || gpsState.swVersionMajor > 23 || (gpsState.swVersionMajor == 23 && gpsState.swVersionMinor >=1)) { // > 23.01, don't use configureMSG - ubx_config_data8_payload_t rateValues[] = { - {UBLOX_CFG_MSGOUT_NAV_POSLLH_UART1, 0}, - {UBLOX_CFG_MSGOUT_NAV_STATUS_UART1, 0}, - {UBLOX_CFG_MSGOUT_NAV_VELNED_UART1, 0}, - {UBLOX_CFG_MSGOUT_NAV_TIMEUTC_UART1, 0}, - {UBLOX_CFG_MSGOUT_NAV_PVT_UART1, 1}, - {UBLOX_CFG_MSGOUT_NAV_SAT_UART1, 0}, - {UBLOX_CFG_MSGOUT_NAV_SIG_UART1, 1}, - }; + if (gpsState.hwVersion >= UBX_HW_VERSION_UBLOX9) { + // > 23.01, don't use configureMSG + if (gpsState.swVersionMajor > 23 || (gpsState.swVersionMajor == 23 && gpsState.swVersionMinor >= 1)) { + ubx_config_data8_payload_t rateValues[] = { + {UBLOX_CFG_MSGOUT_NAV_POSLLH_UART1, 0}, + {UBLOX_CFG_MSGOUT_NAV_STATUS_UART1, 0}, + {UBLOX_CFG_MSGOUT_NAV_VELNED_UART1, 0}, + {UBLOX_CFG_MSGOUT_NAV_TIMEUTC_UART1, 0}, + {UBLOX_CFG_MSGOUT_NAV_PVT_UART1, 1}, + {UBLOX_CFG_MSGOUT_NAV_SIG_UART1, 1}, + {UBLOX_CFG_MSGOUT_NAV_SAT_UART1, 0}, + }; + + ubloxSendSetCfgBytes(rateValues, 5); + ptWait(_ack_state == UBX_ACK_GOT_ACK); + gpsState.flags.pvt = _ack_state == UBX_ACK_GOT_ACK; + + // Try to enable SIG + ubloxSendSetCfgBytes(rateValues+5, 1); + ptWait(_ack_state == UBX_ACK_GOT_ACK || _ack_state == UBX_ACK_GOT_NAK); + gpsState.flags.sig = _ack_state == UBX_ACK_GOT_ACK; + + // Try to enable SAT if SIG fails + rateValues[6].value = gpsState.flags.sig ? 0 : 1; + ubloxSendSetCfgBytes(rateValues+6, 1); + ptWait(_ack_state == UBX_ACK_GOT_ACK || _ack_state == UBX_ACK_GOT_NAK); + gpsState.flags.sat = _ack_state == UBX_ACK_GOT_ACK ? rateValues[6].value : 0; + + } else { + configureMSG(MSG_CLASS_UBX, MSG_POSLLH, 0); + ptWait(_ack_state == UBX_ACK_GOT_ACK); + + configureMSG(MSG_CLASS_UBX, MSG_STATUS, 0); + ptWait(_ack_state == UBX_ACK_GOT_ACK); + + configureMSG(MSG_CLASS_UBX, MSG_VELNED, 0); + ptWait(_ack_state == UBX_ACK_GOT_ACK); + + configureMSG(MSG_CLASS_UBX, MSG_TIMEUTC, 0); + ptWait(_ack_state == UBX_ACK_GOT_ACK); - ubloxSendSetCfgBytes(rateValues, 7); - ptWait(_ack_state == UBX_ACK_GOT_ACK); + configureMSG(MSG_CLASS_UBX, MSG_PVT, 1); + ptWait(_ack_state == UBX_ACK_GOT_ACK); + gpsState.flags.pvt = _ack_state == UBX_ACK_GOT_ACK; + + configureMSG(MSG_CLASS_UBX, MSG_NAV_SAT, 0); + ptWait(_ack_state == UBX_ACK_GOT_ACK); + gpsState.flags.sat = 0; + + configureMSG(MSG_CLASS_UBX, MSG_SIG_INFO, 1); + ptWait(_ack_state == UBX_ACK_GOT_ACK || _ack_state == UBX_ACK_GOT_NAK); + gpsState.flags.sig = _ack_state == UBX_ACK_GOT_ACK; + } if ((gpsState.gpsConfig->provider == GPS_UBLOX7PLUS) && (gpsState.hwVersion >= UBX_HW_VERSION_UBLOX7)) { configureRATE(hz2rate(gpsState.gpsConfig->ubloxNavHz)); // default 10Hz @@ -936,10 +979,21 @@ STATIC_PROTOTHREAD(gpsConfigure) configureMSG(MSG_CLASS_UBX, MSG_PVT, 1); ptWait(_ack_state == UBX_ACK_GOT_ACK); + gpsState.flags.pvt = _ack_state == UBX_ACK_GOT_ACK; configureMSG(MSG_CLASS_UBX, MSG_SVINFO, 0); ptWait(_ack_state == UBX_ACK_GOT_ACK); + // Needed for satelite information on older devices + configureMSG(MSG_CLASS_UBX, MSG_NAV_SAT, 1); + ptWait(_ack_state == UBX_ACK_GOT_ACK || + _ack_state == UBX_ACK_GOT_NAK); + if (_ack_state == UBX_ACK_GOT_ACK) { + gpsState.flags.sat = 1; + } else { + gpsState.flags.sat = 0; + } + if ((gpsState.gpsConfig->provider == GPS_UBLOX7PLUS) && (gpsState.hwVersion >= UBX_HW_VERSION_UBLOX7)) { configureRATE(hz2rate(gpsState.gpsConfig->ubloxNavHz)); // default 10Hz } @@ -984,6 +1038,10 @@ STATIC_PROTOTHREAD(gpsConfigure) } } + DEBUG_SET(DEBUG_GPS, 5, gpsState.flags.pvt); + DEBUG_SET(DEBUG_GPS, 6, gpsState.flags.sat); + DEBUG_SET(DEBUG_GPS, 7, gpsState.flags.sig); + // Configure SBAS // If particular SBAS setting is not supported by the hardware we'll get a NAK, // however GPS would be functional. We are waiting for any response - ACK/NACK From d7e8525ac972513707bb73a72e6d02f4d7d45be8 Mon Sep 17 00:00:00 2001 From: Marcelo Bezerra <23555060+mmosca@users.noreply.github.com> Date: Wed, 26 Jun 2024 13:31:32 +0200 Subject: [PATCH 07/23] use version checks to pick NAV-SAT or NAV-SIG --- src/main/io/gps_ublox.c | 40 ++++++++++++++++++++++++++++++++-------- src/main/io/gps_ublox.h | 6 ++++++ 2 files changed, 38 insertions(+), 8 deletions(-) diff --git a/src/main/io/gps_ublox.c b/src/main/io/gps_ublox.c index 35b0de92ae6..a3bcb9379a4 100755 --- a/src/main/io/gps_ublox.c +++ b/src/main/io/gps_ublox.c @@ -893,7 +893,7 @@ STATIC_PROTOTHREAD(gpsConfigure) // M9N & M10 does not support some of the UBX 6/7/8 messages, so we have to configure it using special sequence if (gpsState.hwVersion >= UBX_HW_VERSION_UBLOX9) { // > 23.01, don't use configureMSG - if (gpsState.swVersionMajor > 23 || (gpsState.swVersionMajor == 23 && gpsState.swVersionMinor >= 1)) { + if (ubloxVersionGTE(23, 1)) { ubx_config_data8_payload_t rateValues[] = { {UBLOX_CFG_MSGOUT_NAV_POSLLH_UART1, 0}, {UBLOX_CFG_MSGOUT_NAV_STATUS_UART1, 0}, @@ -936,13 +936,12 @@ STATIC_PROTOTHREAD(gpsConfigure) ptWait(_ack_state == UBX_ACK_GOT_ACK); gpsState.flags.pvt = _ack_state == UBX_ACK_GOT_ACK; - configureMSG(MSG_CLASS_UBX, MSG_NAV_SAT, 0); + // NAV-SIG is available from 23.1 onwards, NAV-SAT from 15.0 to 23.1 + configureMSG(MSG_CLASS_UBX, MSG_NAV_SAT, ubloxVersionGTE(15,0)); ptWait(_ack_state == UBX_ACK_GOT_ACK); - gpsState.flags.sat = 0; + gpsState.flags.sat = ubloxVersionGTE(15, 0); - configureMSG(MSG_CLASS_UBX, MSG_SIG_INFO, 1); - ptWait(_ack_state == UBX_ACK_GOT_ACK || _ack_state == UBX_ACK_GOT_NAK); - gpsState.flags.sig = _ack_state == UBX_ACK_GOT_ACK; + gpsState.flags.sig = 0; } if ((gpsState.gpsConfig->provider == GPS_UBLOX7PLUS) && (gpsState.hwVersion >= UBX_HW_VERSION_UBLOX7)) { @@ -984,6 +983,7 @@ STATIC_PROTOTHREAD(gpsConfigure) configureMSG(MSG_CLASS_UBX, MSG_SVINFO, 0); ptWait(_ack_state == UBX_ACK_GOT_ACK); + //if(ubloxVersionGTE(15,0) && ubloxVersionLTE(23, 1)) // Needed for satelite information on older devices configureMSG(MSG_CLASS_UBX, MSG_NAV_SAT, 1); ptWait(_ack_state == UBX_ACK_GOT_ACK || @@ -1053,7 +1053,7 @@ STATIC_PROTOTHREAD(gpsConfigure) if (gpsState.hwVersion >= UBX_HW_VERSION_UBLOX8) { gpsSetProtocolTimeout(GPS_SHORT_TIMEOUT); bool use_VALSET = 0; - if ( (gpsState.swVersionMajor > 23) || (gpsState.swVersionMajor == 23 && gpsState.swVersionMinor > 1) ) { + if (ubloxVersionGTE(23,1)) { use_VALSET = 1; } @@ -1063,7 +1063,7 @@ STATIC_PROTOTHREAD(gpsConfigure) configureGNSS(); } - ptWaitTimeout((_ack_state == UBX_ACK_GOT_ACK || _ack_state == UBX_ACK_GOT_NAK), GPS_CFG_CMD_TIMEOUT_MS); + ptWaitTimeout((_ack_state == UBX_ACK_GOT_ACK || _ack_state == UBX_ACK_GOT_NAK), GPS_CFG_CMD_TIMEOUT_MS); if(_ack_state == UBX_ACK_GOT_NAK) { gpsConfigMutable()->ubloxUseGalileo = SETTING_GPS_UBLOX_USE_GALILEO_DEFAULT; @@ -1233,5 +1233,29 @@ const ubx_nav_sig_info *gpsGetUbloxSatelite(uint8_t index) return NULL; } +bool ubloxVersionLT(uint8_t mj2, uint8_t mn2) +{ + return gpsState.swVersionMajor < mj2 || (gpsState.swVersionMajor == mj2 && gpsState.swVersionMinor < mn2); +} + +bool ubloxVersionGT(uint8_t mj2, uint8_t mn2) +{ + return gpsState.swVersionMajor > mj2 || (gpsState.swVersionMajor == mj2 && gpsState.swVersionMinor > mn2); +} + +bool ubloxVersionGTE(uint8_t mj2, uint8_t mn2) +{ + return ubloxVersionGT(mj2, mn2) || ubloxVersionE(mj2, mn2); +} + +bool ubloxVersionLTE(uint8_t mj2, uint8_t mn2) +{ + return ubloxVersionLT(mj2, mn2) || ubloxVersionE(mj2, mn2); +} + +bool ubloxVersionE(uint8_t mj1, uint8_t mn1, uint8_t mj2, uint8_t mn2) +{ + return gpsState.swVersionMajor == mj2 && gpsState.swVersionMinor == mn2; +} #endif diff --git a/src/main/io/gps_ublox.h b/src/main/io/gps_ublox.h index 041f98653b5..959399ac600 100644 --- a/src/main/io/gps_ublox.h +++ b/src/main/io/gps_ublox.h @@ -469,6 +469,12 @@ bool isGpsUblox(void); const ubx_nav_sig_info *gpsGetUbloxSatelite(uint8_t index); +bool ubloxVersionLTE(uint8_t mj2, uint8_t mn2); +bool ubloxVersionLT(uint8_t mj2, uint8_t mn2); +bool ubloxVersionGT(uint8_t mj2, uint8_t mn2); +bool ubloxVersionGTE(uint8_t mj2, uint8_t mn2); +bool ubloxVersionE(uint8_t mj2, uint8_t mn2); + #ifdef __cplusplus } #endif From 8f7fe5bdff3851eae0c48b4aec210013f7551a23 Mon Sep 17 00:00:00 2001 From: Marcelo Bezerra <23555060+mmosca@users.noreply.github.com> Date: Wed, 26 Jun 2024 13:44:00 +0200 Subject: [PATCH 08/23] missed on of the functions when remove parameters --- src/main/io/gps_ublox.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/io/gps_ublox.c b/src/main/io/gps_ublox.c index a3bcb9379a4..7a8a5d198dd 100755 --- a/src/main/io/gps_ublox.c +++ b/src/main/io/gps_ublox.c @@ -1253,7 +1253,7 @@ bool ubloxVersionLTE(uint8_t mj2, uint8_t mn2) return ubloxVersionLT(mj2, mn2) || ubloxVersionE(mj2, mn2); } -bool ubloxVersionE(uint8_t mj1, uint8_t mn1, uint8_t mj2, uint8_t mn2) +bool ubloxVersionE(uint8_t mj2, uint8_t mn2) { return gpsState.swVersionMajor == mj2 && gpsState.swVersionMinor == mn2; } From 6de09006418cbb4cf75014bbb25cd7b4a3945bdd Mon Sep 17 00:00:00 2001 From: Marcelo Bezerra <23555060+mmosca@users.noreply.github.com> Date: Wed, 26 Jun 2024 13:54:02 +0200 Subject: [PATCH 09/23] rename args --- src/main/io/gps_ublox.c | 20 ++++++++++---------- src/main/io/gps_ublox.h | 10 +++++----- 2 files changed, 15 insertions(+), 15 deletions(-) diff --git a/src/main/io/gps_ublox.c b/src/main/io/gps_ublox.c index 7a8a5d198dd..0c582338020 100755 --- a/src/main/io/gps_ublox.c +++ b/src/main/io/gps_ublox.c @@ -1233,29 +1233,29 @@ const ubx_nav_sig_info *gpsGetUbloxSatelite(uint8_t index) return NULL; } -bool ubloxVersionLT(uint8_t mj2, uint8_t mn2) +bool ubloxVersionLT(uint8_t mj, uint8_t mn) { - return gpsState.swVersionMajor < mj2 || (gpsState.swVersionMajor == mj2 && gpsState.swVersionMinor < mn2); + return gpsState.swVersionMajor < mj || (gpsState.swVersionMajor == mj && gpsState.swVersionMinor < mn); } -bool ubloxVersionGT(uint8_t mj2, uint8_t mn2) +bool ubloxVersionGT(uint8_t mj, uint8_t mn) { - return gpsState.swVersionMajor > mj2 || (gpsState.swVersionMajor == mj2 && gpsState.swVersionMinor > mn2); + return gpsState.swVersionMajor > mj || (gpsState.swVersionMajor == mj && gpsState.swVersionMinor > mn); } -bool ubloxVersionGTE(uint8_t mj2, uint8_t mn2) +bool ubloxVersionGTE(uint8_t mj, uint8_t mn) { - return ubloxVersionGT(mj2, mn2) || ubloxVersionE(mj2, mn2); + return ubloxVersionGT(mj, mn) || ubloxVersionE(mj, mn); } -bool ubloxVersionLTE(uint8_t mj2, uint8_t mn2) +bool ubloxVersionLTE(uint8_t mj, uint8_t mn) { - return ubloxVersionLT(mj2, mn2) || ubloxVersionE(mj2, mn2); + return ubloxVersionLT(mj, mn) || ubloxVersionE(mj, mn); } -bool ubloxVersionE(uint8_t mj2, uint8_t mn2) +bool ubloxVersionE(uint8_t mj, uint8_t mn) { - return gpsState.swVersionMajor == mj2 && gpsState.swVersionMinor == mn2; + return gpsState.swVersionMajor == mj && gpsState.swVersionMinor == mn; } #endif diff --git a/src/main/io/gps_ublox.h b/src/main/io/gps_ublox.h index 959399ac600..b6e56845dac 100644 --- a/src/main/io/gps_ublox.h +++ b/src/main/io/gps_ublox.h @@ -469,11 +469,11 @@ bool isGpsUblox(void); const ubx_nav_sig_info *gpsGetUbloxSatelite(uint8_t index); -bool ubloxVersionLTE(uint8_t mj2, uint8_t mn2); -bool ubloxVersionLT(uint8_t mj2, uint8_t mn2); -bool ubloxVersionGT(uint8_t mj2, uint8_t mn2); -bool ubloxVersionGTE(uint8_t mj2, uint8_t mn2); -bool ubloxVersionE(uint8_t mj2, uint8_t mn2); +bool ubloxVersionLTE(uint8_t mj, uint8_t mn); +bool ubloxVersionLT(uint8_t mj, uint8_t mn); +bool ubloxVersionGT(uint8_t mj, uint8_t mn); +bool ubloxVersionGTE(uint8_t mj, uint8_t mn); +bool ubloxVersionE(uint8_t mj, uint8_t mn); #ifdef __cplusplus } From b612e659a2a28e89b38e840d296052523f981518 Mon Sep 17 00:00:00 2001 From: Marcelo Bezerra <23555060+mmosca@users.noreply.github.com> Date: Wed, 26 Jun 2024 15:24:03 +0200 Subject: [PATCH 10/23] First attempt at parsing signal info --- src/main/io/gps_ublox.c | 26 +++++++++++++++++++++----- src/main/io/gps_ublox.h | 23 +++++++++++------------ 2 files changed, 32 insertions(+), 17 deletions(-) diff --git a/src/main/io/gps_ublox.c b/src/main/io/gps_ublox.c index 0c582338020..e28dc6494f3 100755 --- a/src/main/io/gps_ublox.c +++ b/src/main/io/gps_ublox.c @@ -696,19 +696,35 @@ static bool gpsParseFrameUBLOX(void) if (_class == CLASS_NAV) { static int satInfoCount = 0; DEBUG_SET(DEBUG_GPS, 2, satInfoCount++); + DEBUG_SET(DEBUG_GPS, 3, _buffer.svinfo.numSvs); if (!gpsState.flags.pvt) { // PVT is the prefered source - gpsSolDRV.numSat = _buffer.svinfo.numCh; + gpsSolDRV.numSat = _buffer.svinfo.numSvs; } - // TODO: populate satelites[] with sat info + + for(int i =0; i < MIN(_buffer.svinfo.numSvs, UBLOX_MAX_SIGNALS); ++i) { + satelites[i].svId = _buffer.svinfo.channel[i].svId; + satelites[i].gnssId = _buffer.svinfo.channel[i].gnssId; + satelites[i].sigId = 0; + satelites[i].cno = _buffer.svinfo.channel[i].cno; + satelites[i].cno = _buffer.svinfo.channel[i].flags; + satelites[i].quality = _buffer.svinfo.channel[i].flags & 0x3; + satelites[i].sigFlags = (_buffer.svinfo.channel[i].flags >> 4 & 0x3); // Healthy, not healthy + //satelites[i].cno = _buffer.svinfo.channel[i].quality; + } + for(int i =_buffer.svinfo.numSvs; i < UBLOX_MAX_SIGNALS; ++i) { + satelites->svId = 0; + } + } break; case MSG_SIG_INFO: if (_class == CLASS_NAV && _buffer.navsig.version == 0) { static int sigInfoCount = 0; DEBUG_SET(DEBUG_GPS, 0, sigInfoCount++); + DEBUG_SET(DEBUG_GPS, 4, _buffer.navsig.numSigs); if(_buffer.navsig.numSigs < UBLOX_MAX_SIGNALS) { - for(int i=0; i < UBLOX_MAX_SIGNALS && i < _buffer.navsig.numSigs; ++i) + for(int i=0; i < MIN(UBLOX_MAX_SIGNALS, _buffer.navsig.numSigs); ++i) { memcpy(&satelites[i], &_buffer.navsig.sig[i], sizeof(ubx_nav_sig_info)); } @@ -1245,12 +1261,12 @@ bool ubloxVersionGT(uint8_t mj, uint8_t mn) bool ubloxVersionGTE(uint8_t mj, uint8_t mn) { - return ubloxVersionGT(mj, mn) || ubloxVersionE(mj, mn); + return ubloxVersionE(mj, mn) || ubloxVersionGT(mj, mn); } bool ubloxVersionLTE(uint8_t mj, uint8_t mn) { - return ubloxVersionLT(mj, mn) || ubloxVersionE(mj, mn); + return ubloxVersionE(mj, mn) || ubloxVersionLT(mj, mn); } bool ubloxVersionE(uint8_t mj, uint8_t mn) diff --git a/src/main/io/gps_ublox.h b/src/main/io/gps_ublox.h index b6e56845dac..d887fe0d986 100644 --- a/src/main/io/gps_ublox.h +++ b/src/main/io/gps_ublox.h @@ -180,7 +180,7 @@ typedef struct { typedef struct { uint8_t gnssId; // gnssid 0 = GPS, 1 = SBAS, 2 = GALILEO, 3 = BEIDOU, 4 = IMES, 5 = QZSS, 6 = GLONASS uint8_t svId; // space vehicle ID - uint8_t sigId; // new style signal ID (gnssId:sigId) + uint8_t sigId; // signal ID uint8_t freqId; // 0-13 slot +, 0-13, glonass only int16_t prRes; // pseudo range residual (0.1m) uint8_t cno; // carrier to noise density ratio (dbHz) @@ -306,22 +306,21 @@ typedef struct { } ubx_nav_velned; typedef struct { - uint8_t chn; // Channel number, 255 for SVx not assigned to channel - uint8_t svid; // Satellite ID - uint8_t flags; // Bitmask - uint8_t quality; // Bitfield + uint8_t gnssId; // Channel number, 255 for SVx not assigned to channel + uint8_t svId; // Satellite ID uint8_t cno; // Carrier to Noise Ratio (Signal Strength) // dbHz, 0-55. - uint8_t elev; // Elevation in integer degrees - int16_t azim; // Azimuth in integer degrees - int32_t prRes; // Pseudo range residual in centimetres + int8_t elev; // Elevation in integer degrees +/-90 + int16_t azim; // Azimuth in integer degrees 0-360 + int16_t prRes; // Pseudo range residual in .1m + uint32_t flags; // Bitmask } ubx_nav_svinfo_channel; typedef struct { - uint32_t time; // GPS Millisecond time of week - uint8_t numCh; // Number of channels - uint8_t globalFlags; // Bitmask, Chip hardware generation 0:Antaris, 1:u-blox 5, 2:u-blox 6 + uint32_t itow; // GPS Millisecond time of week + uint8_t version; // Version = 0 + uint8_t numSvs; // (Space vehicle) Satelite count uint16_t reserved2; // Reserved - ubx_nav_svinfo_channel channel[16]; // 16 satellites * 12 byte + ubx_nav_svinfo_channel channel[UBLOX_MAX_SIGNALS]; // UBLOX_MAX_SIGNALS satellites * 12 byte } ubx_nav_svinfo; typedef struct { From e59a635117c4f04da971a3851b268c2574bcdc31 Mon Sep 17 00:00:00 2001 From: Marcelo Bezerra <23555060+mmosca@users.noreply.github.com> Date: Wed, 26 Jun 2024 23:29:39 +0200 Subject: [PATCH 11/23] Simplify message rate setup. 2 cases: Firmware newer than 23.01 => has pvt and new setting message Everything else = legacy. (Including M8) --- src/main/io/gps_ublox.c | 175 +++++++++++----------------------------- 1 file changed, 49 insertions(+), 126 deletions(-) diff --git a/src/main/io/gps_ublox.c b/src/main/io/gps_ublox.c index e28dc6494f3..9f312c1ec8f 100755 --- a/src/main/io/gps_ublox.c +++ b/src/main/io/gps_ublox.c @@ -607,8 +607,10 @@ static bool gpsParseFrameUBLOX(void) case MSG_PVT: { static int pvtCount = 0; - DEBUG_SET(DEBUG_GPS, 1, pvtCount++); + DEBUG_SET(DEBUG_GPS, 0, pvtCount++); } + + gpsState.flags.pvt = 1; next_fix_type = gpsMapFixType(_buffer.pvt.fix_status & NAV_STATUS_FIX_VALID, _buffer.pvt.fix_type); gpsSolDRV.fixType = next_fix_type; gpsSolDRV.llh.lon = _buffer.pvt.longitude; @@ -695,7 +697,8 @@ static bool gpsParseFrameUBLOX(void) case MSG_NAV_SAT: if (_class == CLASS_NAV) { static int satInfoCount = 0; - DEBUG_SET(DEBUG_GPS, 2, satInfoCount++); + gpsState.flags.sat = 1; + DEBUG_SET(DEBUG_GPS, 1, satInfoCount++); DEBUG_SET(DEBUG_GPS, 3, _buffer.svinfo.numSvs); if (!gpsState.flags.pvt) { // PVT is the prefered source gpsSolDRV.numSat = _buffer.svinfo.numSvs; @@ -720,8 +723,9 @@ static bool gpsParseFrameUBLOX(void) case MSG_SIG_INFO: if (_class == CLASS_NAV && _buffer.navsig.version == 0) { static int sigInfoCount = 0; - DEBUG_SET(DEBUG_GPS, 0, sigInfoCount++); + DEBUG_SET(DEBUG_GPS, 2, sigInfoCount++); DEBUG_SET(DEBUG_GPS, 4, _buffer.navsig.numSigs); + gpsState.flags.sig = 1; if(_buffer.navsig.numSigs < UBLOX_MAX_SIGNALS) { for(int i=0; i < MIN(UBLOX_MAX_SIGNALS, _buffer.navsig.numSigs); ++i) @@ -749,6 +753,10 @@ static bool gpsParseFrameUBLOX(void) return false; } + DEBUG_SET(DEBUG_GPS, 5, gpsState.flags.pvt); + DEBUG_SET(DEBUG_GPS, 6, gpsState.flags.sat); + DEBUG_SET(DEBUG_GPS, 7, gpsState.flags.sig); + // we only return true when we get new position and speed data // this ensures we don't use stale data if (_new_position && _new_speed) { @@ -907,58 +915,23 @@ STATIC_PROTOTHREAD(gpsConfigure) gpsSetProtocolTimeout(GPS_SHORT_TIMEOUT); // M9N & M10 does not support some of the UBX 6/7/8 messages, so we have to configure it using special sequence - if (gpsState.hwVersion >= UBX_HW_VERSION_UBLOX9) { - // > 23.01, don't use configureMSG - if (ubloxVersionGTE(23, 1)) { + // > 23.01, don't use configureMSG, and have PVT + + if (ubloxVersionGTE(23, 1)) { ubx_config_data8_payload_t rateValues[] = { - {UBLOX_CFG_MSGOUT_NAV_POSLLH_UART1, 0}, - {UBLOX_CFG_MSGOUT_NAV_STATUS_UART1, 0}, - {UBLOX_CFG_MSGOUT_NAV_VELNED_UART1, 0}, - {UBLOX_CFG_MSGOUT_NAV_TIMEUTC_UART1, 0}, - {UBLOX_CFG_MSGOUT_NAV_PVT_UART1, 1}, - {UBLOX_CFG_MSGOUT_NAV_SIG_UART1, 1}, - {UBLOX_CFG_MSGOUT_NAV_SAT_UART1, 0}, + {UBLOX_CFG_MSGOUT_NAV_POSLLH_UART1, 0}, // 0 + {UBLOX_CFG_MSGOUT_NAV_STATUS_UART1, 0}, // 1 + {UBLOX_CFG_MSGOUT_NAV_VELNED_UART1, 0}, // 2 + {UBLOX_CFG_MSGOUT_NAV_TIMEUTC_UART1, 0}, // 3 + {UBLOX_CFG_MSGOUT_NAV_PVT_UART1, 1}, // 4 + {UBLOX_CFG_MSGOUT_NAV_SIG_UART1, 1}, // 5 + {UBLOX_CFG_MSGOUT_NAV_SAT_UART1, 0} // 6 }; - ubloxSendSetCfgBytes(rateValues, 5); - ptWait(_ack_state == UBX_ACK_GOT_ACK); - gpsState.flags.pvt = _ack_state == UBX_ACK_GOT_ACK; - // Try to enable SIG - ubloxSendSetCfgBytes(rateValues+5, 1); - ptWait(_ack_state == UBX_ACK_GOT_ACK || _ack_state == UBX_ACK_GOT_NAK); - gpsState.flags.sig = _ack_state == UBX_ACK_GOT_ACK; + ubloxSendSetCfgBytes(rateValues, 7); + ptWaitTimeout((_ack_state == UBX_ACK_GOT_ACK || _ack_state == UBX_ACK_GOT_NAK), GPS_CFG_CMD_TIMEOUT_MS); - // Try to enable SAT if SIG fails - rateValues[6].value = gpsState.flags.sig ? 0 : 1; - ubloxSendSetCfgBytes(rateValues+6, 1); - ptWait(_ack_state == UBX_ACK_GOT_ACK || _ack_state == UBX_ACK_GOT_NAK); - gpsState.flags.sat = _ack_state == UBX_ACK_GOT_ACK ? rateValues[6].value : 0; - - } else { - configureMSG(MSG_CLASS_UBX, MSG_POSLLH, 0); - ptWait(_ack_state == UBX_ACK_GOT_ACK); - - configureMSG(MSG_CLASS_UBX, MSG_STATUS, 0); - ptWait(_ack_state == UBX_ACK_GOT_ACK); - - configureMSG(MSG_CLASS_UBX, MSG_VELNED, 0); - ptWait(_ack_state == UBX_ACK_GOT_ACK); - - configureMSG(MSG_CLASS_UBX, MSG_TIMEUTC, 0); - ptWait(_ack_state == UBX_ACK_GOT_ACK); - - configureMSG(MSG_CLASS_UBX, MSG_PVT, 1); - ptWait(_ack_state == UBX_ACK_GOT_ACK); - gpsState.flags.pvt = _ack_state == UBX_ACK_GOT_ACK; - - // NAV-SIG is available from 23.1 onwards, NAV-SAT from 15.0 to 23.1 - configureMSG(MSG_CLASS_UBX, MSG_NAV_SAT, ubloxVersionGTE(15,0)); - ptWait(_ack_state == UBX_ACK_GOT_ACK); - gpsState.flags.sat = ubloxVersionGTE(15, 0); - - gpsState.flags.sig = 0; - } if ((gpsState.gpsConfig->provider == GPS_UBLOX7PLUS) && (gpsState.hwVersion >= UBX_HW_VERSION_UBLOX7)) { configureRATE(hz2rate(gpsState.gpsConfig->ubloxNavHz)); // default 10Hz @@ -972,91 +945,41 @@ STATIC_PROTOTHREAD(gpsConfigure) configureRATE(hz2rate(5)); // 5Hz ptWait(_ack_state == UBX_ACK_GOT_ACK); } - } - else { - // u-Blox 5/6/7/8 or unknown - // u-Blox 7-8 support PVT - if (gpsState.hwVersion >= UBX_HW_VERSION_UBLOX7) { - configureMSG(MSG_CLASS_UBX, MSG_POSLLH, 0); - ptWait(_ack_state == UBX_ACK_GOT_ACK); - - configureMSG(MSG_CLASS_UBX, MSG_STATUS, 0); - ptWait(_ack_state == UBX_ACK_GOT_ACK); - - configureMSG(MSG_CLASS_UBX, MSG_SOL, 1); - ptWait(_ack_state == UBX_ACK_GOT_ACK); - - configureMSG(MSG_CLASS_UBX, MSG_VELNED, 0); - ptWait(_ack_state == UBX_ACK_GOT_ACK); + } else { + configureMSG(MSG_CLASS_UBX, MSG_POSLLH, 1); + ptWait(_ack_state == UBX_ACK_GOT_ACK); - configureMSG(MSG_CLASS_UBX, MSG_TIMEUTC, 0); - ptWait(_ack_state == UBX_ACK_GOT_ACK); + configureMSG(MSG_CLASS_UBX, MSG_STATUS, 1); + ptWait(_ack_state == UBX_ACK_GOT_ACK); - configureMSG(MSG_CLASS_UBX, MSG_PVT, 1); - ptWait(_ack_state == UBX_ACK_GOT_ACK); - gpsState.flags.pvt = _ack_state == UBX_ACK_GOT_ACK; + configureMSG(MSG_CLASS_UBX, MSG_SOL, 1); + ptWait(_ack_state == UBX_ACK_GOT_ACK); - configureMSG(MSG_CLASS_UBX, MSG_SVINFO, 0); - ptWait(_ack_state == UBX_ACK_GOT_ACK); + configureMSG(MSG_CLASS_UBX, MSG_VELNED, 1); + ptWait(_ack_state == UBX_ACK_GOT_ACK); - //if(ubloxVersionGTE(15,0) && ubloxVersionLTE(23, 1)) - // Needed for satelite information on older devices - configureMSG(MSG_CLASS_UBX, MSG_NAV_SAT, 1); - ptWait(_ack_state == UBX_ACK_GOT_ACK || - _ack_state == UBX_ACK_GOT_NAK); - if (_ack_state == UBX_ACK_GOT_ACK) { - gpsState.flags.sat = 1; - } else { - gpsState.flags.sat = 0; - } + configureMSG(MSG_CLASS_UBX, MSG_TIMEUTC, 10); + ptWait(_ack_state == UBX_ACK_GOT_ACK); - if ((gpsState.gpsConfig->provider == GPS_UBLOX7PLUS) && (gpsState.hwVersion >= UBX_HW_VERSION_UBLOX7)) { - configureRATE(hz2rate(gpsState.gpsConfig->ubloxNavHz)); // default 10Hz - } - else { - configureRATE(hz2rate(5)); // 5Hz - } - ptWait(_ack_state == UBX_ACK_GOT_ACK || _ack_state == UBX_ACK_GOT_NAK); - - if(_ack_state == UBX_ACK_GOT_NAK) { // Fallback to safe 5Hz in case of error - configureRATE(hz2rate(5)); // 5Hz - ptWait(_ack_state == UBX_ACK_GOT_ACK); - } - } - // u-Blox 5/6 doesn't support PVT, use legacy config - // UNKNOWN also falls here, use as a last resort - else { - configureMSG(MSG_CLASS_UBX, MSG_POSLLH, 1); - ptWait(_ack_state == UBX_ACK_GOT_ACK); - - configureMSG(MSG_CLASS_UBX, MSG_STATUS, 1); - ptWait(_ack_state == UBX_ACK_GOT_ACK); + // Protocol < 23.01 does not have MSG_PVT + //configureMSG(MSG_CLASS_UBX, MSG_PVT, 0); + //ptWait(_ack_state == UBX_ACK_GOT_ACK || _ack_state == UBX_ACK_GOT_NAK); - configureMSG(MSG_CLASS_UBX, MSG_SOL, 1); - ptWait(_ack_state == UBX_ACK_GOT_ACK); - - configureMSG(MSG_CLASS_UBX, MSG_VELNED, 1); - ptWait(_ack_state == UBX_ACK_GOT_ACK); - - configureMSG(MSG_CLASS_UBX, MSG_TIMEUTC, 10); - ptWait(_ack_state == UBX_ACK_GOT_ACK); - - // This may fail on old UBLOX units, advance forward on both ACK and NAK - configureMSG(MSG_CLASS_UBX, MSG_PVT, 0); - ptWait(_ack_state == UBX_ACK_GOT_ACK || _ack_state == UBX_ACK_GOT_NAK); + // This may fail on old UBLOX units, advance forward on both ACK and NAK + configureMSG(MSG_CLASS_UBX, MSG_NAV_SAT, 1); + ptWait(_ack_state == UBX_ACK_GOT_ACK || _ack_state == UBX_ACK_GOT_NAK); - configureMSG(MSG_CLASS_UBX, MSG_SVINFO, 0); - ptWait(_ack_state == UBX_ACK_GOT_ACK); + configureMSG(MSG_CLASS_UBX, MSG_SVINFO, 0); + ptWait(_ack_state == UBX_ACK_GOT_ACK); - // Configure data rate to 5HZ - configureRATE(200); - ptWait(_ack_state == UBX_ACK_GOT_ACK); - } - } + // Configure data rate to 5HZ + configureRATE(hz2rate(5)); + ptWait(_ack_state == UBX_ACK_GOT_ACK); + }// end message config - DEBUG_SET(DEBUG_GPS, 5, gpsState.flags.pvt); - DEBUG_SET(DEBUG_GPS, 6, gpsState.flags.sat); - DEBUG_SET(DEBUG_GPS, 7, gpsState.flags.sig); + gpsState.flags.pvt = 0; + gpsState.flags.sat = 0; + gpsState.flags.sig = 0; // Configure SBAS // If particular SBAS setting is not supported by the hardware we'll get a NAK, From d0781576ad08bbf912e7051c38d24f3a985c03ac Mon Sep 17 00:00:00 2001 From: Marcelo Bezerra <23555060+mmosca@users.noreply.github.com> Date: Wed, 26 Jun 2024 23:43:42 +0200 Subject: [PATCH 12/23] Still simpler, but 3 cases. 1) New api, nav_pvt and nav_sig (m9+) 2) Old api, nav_pvt and nav_sat (m7?/m8) 3) Old api, no nav_pvt or nav_sat (time to upgrade) --- src/main/io/gps_ublox.c | 79 +++++++++++++++++++++++------------------ 1 file changed, 44 insertions(+), 35 deletions(-) diff --git a/src/main/io/gps_ublox.c b/src/main/io/gps_ublox.c index 9f312c1ec8f..f3fd3506c90 100755 --- a/src/main/io/gps_ublox.c +++ b/src/main/io/gps_ublox.c @@ -915,37 +915,44 @@ STATIC_PROTOTHREAD(gpsConfigure) gpsSetProtocolTimeout(GPS_SHORT_TIMEOUT); // M9N & M10 does not support some of the UBX 6/7/8 messages, so we have to configure it using special sequence - // > 23.01, don't use configureMSG, and have PVT + if (ubloxVersionGTE(23, 1)) { // M9+, new setting API, PVT and NAV_SIG + ubx_config_data8_payload_t rateValues[] = { + {UBLOX_CFG_MSGOUT_NAV_POSLLH_UART1, 0}, // 0 + {UBLOX_CFG_MSGOUT_NAV_STATUS_UART1, 0}, // 1 + {UBLOX_CFG_MSGOUT_NAV_VELNED_UART1, 0}, // 2 + {UBLOX_CFG_MSGOUT_NAV_TIMEUTC_UART1, 0}, // 3 + {UBLOX_CFG_MSGOUT_NAV_PVT_UART1, 1}, // 4 + {UBLOX_CFG_MSGOUT_NAV_SIG_UART1, 1}, // 5 + {UBLOX_CFG_MSGOUT_NAV_SAT_UART1, 0} // 6 + }; - if (ubloxVersionGTE(23, 1)) { - ubx_config_data8_payload_t rateValues[] = { - {UBLOX_CFG_MSGOUT_NAV_POSLLH_UART1, 0}, // 0 - {UBLOX_CFG_MSGOUT_NAV_STATUS_UART1, 0}, // 1 - {UBLOX_CFG_MSGOUT_NAV_VELNED_UART1, 0}, // 2 - {UBLOX_CFG_MSGOUT_NAV_TIMEUTC_UART1, 0}, // 3 - {UBLOX_CFG_MSGOUT_NAV_PVT_UART1, 1}, // 4 - {UBLOX_CFG_MSGOUT_NAV_SIG_UART1, 1}, // 5 - {UBLOX_CFG_MSGOUT_NAV_SAT_UART1, 0} // 6 - }; + ubloxSendSetCfgBytes(rateValues, 7); + ptWaitTimeout((_ack_state == UBX_ACK_GOT_ACK || _ack_state == UBX_ACK_GOT_NAK), GPS_CFG_CMD_TIMEOUT_MS); + } else if(ubloxVersionGTE(15,0)) { // M8 and potentially M7, PVT, NAV_SAT, old setting API + configureMSG(MSG_CLASS_UBX, MSG_POSLLH, 0); + ptWait(_ack_state == UBX_ACK_GOT_ACK); + configureMSG(MSG_CLASS_UBX, MSG_STATUS, 0); + ptWait(_ack_state == UBX_ACK_GOT_ACK); - ubloxSendSetCfgBytes(rateValues, 7); - ptWaitTimeout((_ack_state == UBX_ACK_GOT_ACK || _ack_state == UBX_ACK_GOT_NAK), GPS_CFG_CMD_TIMEOUT_MS); + configureMSG(MSG_CLASS_UBX, MSG_SOL, 0); + ptWait(_ack_state == UBX_ACK_GOT_ACK); + configureMSG(MSG_CLASS_UBX, MSG_VELNED, 0); + ptWait(_ack_state == UBX_ACK_GOT_ACK); - if ((gpsState.gpsConfig->provider == GPS_UBLOX7PLUS) && (gpsState.hwVersion >= UBX_HW_VERSION_UBLOX7)) { - configureRATE(hz2rate(gpsState.gpsConfig->ubloxNavHz)); // default 10Hz - } else { - configureRATE(hz2rate(5)); // 5Hz - gpsConfigMutable()->ubloxNavHz = SETTING_GPS_UBLOX_NAV_HZ_DEFAULT; - } + configureMSG(MSG_CLASS_UBX, MSG_TIMEUTC, 0); + ptWait(_ack_state == UBX_ACK_GOT_ACK); + + configureMSG(MSG_CLASS_UBX, MSG_SVINFO, 0); + ptWait(_ack_state == UBX_ACK_GOT_ACK); + + configureMSG(MSG_CLASS_UBX, MSG_PVT, 1); ptWait(_ack_state == UBX_ACK_GOT_ACK || _ack_state == UBX_ACK_GOT_NAK); - if(_ack_state == UBX_ACK_GOT_NAK) { // Fallback to safe 5Hz in case of error - configureRATE(hz2rate(5)); // 5Hz - ptWait(_ack_state == UBX_ACK_GOT_ACK); - } - } else { + configureMSG(MSG_CLASS_UBX, MSG_NAV_SAT, 1); + ptWait(_ack_state == UBX_ACK_GOT_ACK || _ack_state == UBX_ACK_GOT_NAK); + } else { // Really old stuff, consider upgrading :), ols setting API, no PVT or NAV_SAT or NAV_SIG configureMSG(MSG_CLASS_UBX, MSG_POSLLH, 1); ptWait(_ack_state == UBX_ACK_GOT_ACK); @@ -961,21 +968,23 @@ STATIC_PROTOTHREAD(gpsConfigure) configureMSG(MSG_CLASS_UBX, MSG_TIMEUTC, 10); ptWait(_ack_state == UBX_ACK_GOT_ACK); - // Protocol < 23.01 does not have MSG_PVT - //configureMSG(MSG_CLASS_UBX, MSG_PVT, 0); - //ptWait(_ack_state == UBX_ACK_GOT_ACK || _ack_state == UBX_ACK_GOT_NAK); - - // This may fail on old UBLOX units, advance forward on both ACK and NAK - configureMSG(MSG_CLASS_UBX, MSG_NAV_SAT, 1); - ptWait(_ack_state == UBX_ACK_GOT_ACK || _ack_state == UBX_ACK_GOT_NAK); - configureMSG(MSG_CLASS_UBX, MSG_SVINFO, 0); ptWait(_ack_state == UBX_ACK_GOT_ACK); + }// end message config + + if ((gpsState.gpsConfig->provider == GPS_UBLOX7PLUS) && (gpsState.hwVersion >= UBX_HW_VERSION_UBLOX7)) { + configureRATE(hz2rate(gpsState.gpsConfig->ubloxNavHz)); // default 10Hz + } else { + configureRATE(hz2rate(5)); // 5Hz + gpsConfigMutable()->ubloxNavHz = SETTING_GPS_UBLOX_NAV_HZ_DEFAULT; + } + ptWait(_ack_state == UBX_ACK_GOT_ACK || _ack_state == UBX_ACK_GOT_NAK); - // Configure data rate to 5HZ - configureRATE(hz2rate(5)); + if(_ack_state == UBX_ACK_GOT_NAK) { // Fallback to safe 5Hz in case of error + configureRATE(hz2rate(5)); // 5Hz ptWait(_ack_state == UBX_ACK_GOT_ACK); - }// end message config + } + gpsState.flags.pvt = 0; gpsState.flags.sat = 0; From 9ad5252bc9523690c7949f927e33de0abf135ef2 Mon Sep 17 00:00:00 2001 From: Marcelo Bezerra <23555060+mmosca@users.noreply.github.com> Date: Thu, 27 Jun 2024 01:02:10 +0200 Subject: [PATCH 13/23] Disable nmed with new api for swversion >23.1 --- src/main/io/gps_ublox.c | 39 ++++++++++++++++++++++++++------------- src/main/io/gps_ublox.h | 5 +++++ 2 files changed, 31 insertions(+), 13 deletions(-) diff --git a/src/main/io/gps_ublox.c b/src/main/io/gps_ublox.c index f3fd3506c90..a3065f73daa 100755 --- a/src/main/io/gps_ublox.c +++ b/src/main/io/gps_ublox.c @@ -890,26 +890,38 @@ STATIC_PROTOTHREAD(gpsConfigure) } ptWait(_ack_state == UBX_ACK_GOT_ACK); - // Disable NMEA messages gpsSetProtocolTimeout(GPS_SHORT_TIMEOUT); + // Disable NMEA messages + if (ubloxVersionGTE(23, 1)) { + ubx_config_data8_payload_t nmeaValues[] = { + { UBLOX_CFG_MSGOUT_NMEA_ID_GGA_UART1, 0 }, + { UBLOX_CFG_MSGOUT_NMEA_ID_GLL_UART1, 0 }, + { UBLOX_CFG_MSGOUT_NMEA_ID_GSA_UART1, 0 }, + { UBLOX_CFG_MSGOUT_NMEA_ID_RMC_UART1, 0 }, + { UBLOX_CFG_MSGOUT_NMEA_ID_VTG_UART1, 0 }, + }; - configureMSG(MSG_CLASS_NMEA, MSG_NMEA_GGA, 0); - ptWait(_ack_state == UBX_ACK_GOT_ACK); + ubloxSendSetCfgBytes(nmeaValues, 5); + ptWaitTimeout((_ack_state == UBX_ACK_GOT_ACK), GPS_CFG_CMD_TIMEOUT_MS); + } else { + configureMSG(MSG_CLASS_NMEA, MSG_NMEA_GGA, 0); + ptWait(_ack_state == UBX_ACK_GOT_ACK); - configureMSG(MSG_CLASS_NMEA, MSG_NMEA_GLL, 0); - ptWait(_ack_state == UBX_ACK_GOT_ACK); + configureMSG(MSG_CLASS_NMEA, MSG_NMEA_GLL, 0); + ptWait(_ack_state == UBX_ACK_GOT_ACK); - configureMSG(MSG_CLASS_NMEA, MSG_NMEA_GSA, 0); - ptWait(_ack_state == UBX_ACK_GOT_ACK); + configureMSG(MSG_CLASS_NMEA, MSG_NMEA_GSA, 0); + ptWait(_ack_state == UBX_ACK_GOT_ACK); - configureMSG(MSG_CLASS_NMEA, MSG_NMEA_GSV, 0); - ptWait(_ack_state == UBX_ACK_GOT_ACK); + configureMSG(MSG_CLASS_NMEA, MSG_NMEA_GSV, 0); + ptWait(_ack_state == UBX_ACK_GOT_ACK); - configureMSG(MSG_CLASS_NMEA, MSG_NMEA_RMC, 0); - ptWait(_ack_state == UBX_ACK_GOT_ACK); + configureMSG(MSG_CLASS_NMEA, MSG_NMEA_RMC, 0); + ptWait(_ack_state == UBX_ACK_GOT_ACK); - configureMSG(MSG_CLASS_NMEA, MSG_NMEA_VGS, 0); - ptWait(_ack_state == UBX_ACK_GOT_ACK); + configureMSG(MSG_CLASS_NMEA, MSG_NMEA_VGS, 0); + ptWait(_ack_state == UBX_ACK_GOT_ACK); + } // Configure UBX binary messages gpsSetProtocolTimeout(GPS_SHORT_TIMEOUT); @@ -972,6 +984,7 @@ STATIC_PROTOTHREAD(gpsConfigure) ptWait(_ack_state == UBX_ACK_GOT_ACK); }// end message config + ptWaitTimeout((_ack_state == UBX_ACK_GOT_ACK || _ack_state == UBX_ACK_GOT_NAK), GPS_SHORT_TIMEOUT); if ((gpsState.gpsConfig->provider == GPS_UBLOX7PLUS) && (gpsState.hwVersion >= UBX_HW_VERSION_UBLOX7)) { configureRATE(hz2rate(gpsState.gpsConfig->ubloxNavHz)); // default 10Hz } else { diff --git a/src/main/io/gps_ublox.h b/src/main/io/gps_ublox.h index d887fe0d986..c79c4654292 100644 --- a/src/main/io/gps_ublox.h +++ b/src/main/io/gps_ublox.h @@ -63,6 +63,11 @@ extern "C" { #define UBLOX_CFG_MSGOUT_NAV_VELNED_UART1 0x20910043 // U1 #define UBLOX_CFG_MSGOUT_NAV_TIMEUTC_UART1 0x2091005c // U1 #define UBLOX_CFG_MSGOUT_NAV_PVT_UART1 0x20910007 // U1 +#define UBLOX_CFG_MSGOUT_NMEA_ID_GGA_UART1 0x209100bb // U1 +#define UBLOX_CFG_MSGOUT_NMEA_ID_GLL_UART1 0x209100ca // U1 +#define UBLOX_CFG_MSGOUT_NMEA_ID_GSA_UART1 0x209100c0 // U1 +#define UBLOX_CFG_MSGOUT_NMEA_ID_RMC_UART1 0x209100ac // U1 +#define UBLOX_CFG_MSGOUT_NMEA_ID_VTG_UART1 0x209100b1 // U1 #define UBLOX_CFG_SIGNAL_SBAS_ENA 0x10310020 // U1 #define UBLOX_CFG_SIGNAL_SBAS_L1CA_ENA 0x10310005 // U1 From c9fc18f8e9eaf9d77d836c6411515cb9cf3add75 Mon Sep 17 00:00:00 2001 From: Marcelo Bezerra <23555060+mmosca@users.noreply.github.com> Date: Thu, 27 Jun 2024 01:42:59 +0200 Subject: [PATCH 14/23] use 0xff as no sat. --- src/main/fc/cli.c | 4 ++-- src/main/io/gps_ublox.c | 6 +++--- 2 files changed, 5 insertions(+), 5 deletions(-) diff --git a/src/main/fc/cli.c b/src/main/fc/cli.c index fa8ec75b615..fe54e175b27 100644 --- a/src/main/fc/cli.c +++ b/src/main/fc/cli.c @@ -4396,11 +4396,11 @@ static void cliUbloxPrintSatelites(char *arg) for(int i = 0; i < UBLOX_MAX_SIGNALS; ++i) { const ubx_nav_sig_info *sat = gpsGetUbloxSatelite(i); - if(sat == NULL || sat->svId == 0) { + if(sat == NULL) { continue; } - cliPrintLinef("satelite: %d:%d", sat->gnssId, sat->svId); + 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)); diff --git a/src/main/io/gps_ublox.c b/src/main/io/gps_ublox.c index a3065f73daa..93395e7d1d4 100755 --- a/src/main/io/gps_ublox.c +++ b/src/main/io/gps_ublox.c @@ -726,7 +726,7 @@ static bool gpsParseFrameUBLOX(void) DEBUG_SET(DEBUG_GPS, 2, sigInfoCount++); DEBUG_SET(DEBUG_GPS, 4, _buffer.navsig.numSigs); gpsState.flags.sig = 1; - if(_buffer.navsig.numSigs < UBLOX_MAX_SIGNALS) + if(_buffer.navsig.numSigs > 0) { for(int i=0; i < MIN(UBLOX_MAX_SIGNALS, _buffer.navsig.numSigs); ++i) { @@ -734,7 +734,7 @@ static bool gpsParseFrameUBLOX(void) } for(int i = _buffer.navsig.numSigs; i < UBLOX_MAX_SIGNALS; ++i) { - satelites[i].svId = 0; // no used + satelites[i].svId = 0xFF; // no used } } } @@ -1187,7 +1187,7 @@ bool isGpsUblox(void) const ubx_nav_sig_info *gpsGetUbloxSatelite(uint8_t index) { - if(index < UBLOX_MAX_SIGNALS) { + if(index < UBLOX_MAX_SIGNALS && satelites[index].svId != 0xFF) { return &satelites[index]; } From 2bca68bdca1f59fb5ae117c85528b3f3813166fd Mon Sep 17 00:00:00 2001 From: Marcelo Bezerra <23555060+mmosca@users.noreply.github.com> Date: Thu, 27 Jun 2024 21:45:37 +0200 Subject: [PATCH 15/23] make sure to init satelites as not used --- src/main/io/gps_ublox.c | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/src/main/io/gps_ublox.c b/src/main/io/gps_ublox.c index 93395e7d1d4..679bcf91e33 100755 --- a/src/main/io/gps_ublox.c +++ b/src/main/io/gps_ublox.c @@ -1033,6 +1033,11 @@ STATIC_PROTOTHREAD(gpsConfigure) } } + for(int i = 0; i < UBLOX_MAX_SIGNALS; ++i) + { + satelites[i].svId = 0xFF; + } + ptEnd(0); } From 6e3bf5198dcdc3f52887d0a596431fcecb7bf3fa Mon Sep 17 00:00:00 2001 From: Marcelo Bezerra <23555060+mmosca@users.noreply.github.com> Date: Wed, 3 Jul 2024 20:52:16 +0200 Subject: [PATCH 16/23] Fix signal info struct size for NAV_SIG devices --- src/main/fc/cli.c | 35 +++++++++++++++----------- src/main/io/gps_ublox.c | 13 ++++++++-- src/main/io/gps_ublox.h | 39 ++++++++++++++++++++++++----- src/test/unit/gps_ublox_unittest.cc | 6 +++++ 4 files changed, 70 insertions(+), 23 deletions(-) diff --git a/src/main/fc/cli.c b/src/main/fc/cli.c index fe54e175b27..7320ef060d6 100644 --- a/src/main/fc/cli.c +++ b/src/main/fc/cli.c @@ -4370,15 +4370,15 @@ static const char* _ubloxGetSigId(uint8_t gnssId, uint8_t sigId) static const char *_ubloxGetQuality(uint8_t quality) { switch(quality) { - case 0: return "No signal"; - case 1: return "Searching signal..."; - case 2: return "Signal acquired"; - case 3: return "Signal detected but unusable"; - case 4: return "Code locked and time synch"; - case 5: - case 6: - case 7: - return "Code and carrier locked and time synch"; + 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"; } } @@ -4407,12 +4407,17 @@ static void cliUbloxPrintSatelites(char *arg) //cliPrintLinef("Correlation: %i", sat->corrSource); //cliPrintLinef("Iono model: %i", sat->ionoModel); cliPrintLinef("signal flags: 0x%02X", sat->sigFlags); - if(sat->sigFlags & 0x01) { - cliPrintLine("signal: Healthy"); - } else if (sat->sigFlags & 0x02) { - cliPrintLine("signal: Unhealthy"); - } else { - cliPrintLine("signal: Unknown"); + 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(); } diff --git a/src/main/io/gps_ublox.c b/src/main/io/gps_ublox.c index 679bcf91e33..80bee7db441 100755 --- a/src/main/io/gps_ublox.c +++ b/src/main/io/gps_ublox.c @@ -720,23 +720,32 @@ static bool gpsParseFrameUBLOX(void) } break; - case MSG_SIG_INFO: + case MSG_NAV_SIG: if (_class == CLASS_NAV && _buffer.navsig.version == 0) { static int sigInfoCount = 0; DEBUG_SET(DEBUG_GPS, 2, sigInfoCount++); - DEBUG_SET(DEBUG_GPS, 4, _buffer.navsig.numSigs); gpsState.flags.sig = 1; + // + //gpsSolDRV.numSat = _buffer.navSig.numSigs; // if good + int okSats = 0; if(_buffer.navsig.numSigs > 0) { for(int i=0; i < MIN(UBLOX_MAX_SIGNALS, _buffer.navsig.numSigs); ++i) { memcpy(&satelites[i], &_buffer.navsig.sig[i], sizeof(ubx_nav_sig_info)); + if ((_buffer.navsig.sig[i].sigFlags & UBLOX_SIG_HEALTH_MASK) == UBLOX_SIG_HEALTH_HEALTHY && + _buffer.navsig.sig[i].quality >= UBLOX_SIG_QUALITY_CODE_LOCK_TIME_SYNC) + { + okSats++; + } } for(int i = _buffer.navsig.numSigs; i < UBLOX_MAX_SIGNALS; ++i) { satelites[i].svId = 0xFF; // no used } } + //DEBUG_SET(DEBUG_GPS, 4, _buffer.navsig.numSigs); + DEBUG_SET(DEBUG_GPS, 4, okSats); } break; case MSG_ACK_ACK: diff --git a/src/main/io/gps_ublox.h b/src/main/io/gps_ublox.h index c79c4654292..3a1c145b1b0 100644 --- a/src/main/io/gps_ublox.h +++ b/src/main/io/gps_ublox.h @@ -21,6 +21,7 @@ #include #include "common/time.h" +#include "build/debug.h" #ifdef __cplusplus extern "C" { @@ -28,8 +29,8 @@ extern "C" { #define GPS_CFG_CMD_TIMEOUT_MS 500 #define GPS_VERSION_RETRY_TIMES 3 -#define UBLOX_MAX_SIGNALS 32 -#define MAX_UBLOX_PAYLOAD_SIZE 640 // enough for anyone? // UBX-NAV-SIG info would be UBLOX_MAX_SIGNALS + 8 for (32 * 16) + 8 = 520 bytes +#define UBLOX_MAX_SIGNALS 64 +#define MAX_UBLOX_PAYLOAD_SIZE 1048 // enough for anyone? // UBX-NAV-SIG info would be UBLOX_MAX_SIGNALS * 16 + 8 for (64 * 16) + 8 = 1032 bytes #define UBLOX_BUFFER_SIZE MAX_UBLOX_PAYLOAD_SIZE #define UBLOX_SBAS_MESSAGE_LENGTH 16 #define GPS_CAPA_INTERVAL 5000 @@ -182,6 +183,33 @@ typedef struct { uint8_t reserved; } __attribute__((packed)) ubx_config_data_header_v1_t; +#define UBLOX_SIG_HEALTH_MASK (BIT(0) | BIT(1)) +#define UBLOX_SIG_PRSMOOTHED (BIT(2)) +#define UBLOX_SIG_PRUSED (BIT(3)) +#define UBLOX_SIG_CRUSED (BIT(4)) +#define UBLOX_SIG_DOUSED (BIT(5)) +#define UBLOX_SIG_PRCORRUSED (BIT(6)) +#define UBLOX_SIG_CRCORRUSED (BIT(7)) +#define UBLOX_SIG_DOCORRUSED (BIT(8)) +#define UBLOX_SIG_AUTHSTATUS (BIT(9)) + +typedef enum { + UBLOX_SIG_HEALTH_UNKNOWN = 0, + UBLOX_SIG_HEALTH_HEALTHY = 1, + UBLOX_SIG_HEALTH_UNHEALTHY = 2 +} ublox_nav_sig_health_e; + +typedef enum { + UBLOX_SIG_QUALITY_NOSIGNAL = 0, + UBLOX_SIG_QUALITY_SEARCHING = 1, + UBLOX_SIG_QUALITY_ACQUIRED = 2, + UBLOX_SIG_QUALITY_UNUSABLE = 3, + UBLOX_SIG_QUALITY_CODE_LOCK_TIME_SYNC = 4, + UBLOX_SIG_QUALITY_CODE_CARRIER_LOCK_TIME_SYNC = 5, + UBLOX_SIG_QUALITY_CODE_CARRIER_LOCK_TIME_SYNC2 = 6, + UBLOX_SIG_QUALITY_CODE_CARRIER_LOCK_TIME_SYNC3 = 7, +} ublox_nav_sig_quality; + typedef struct { uint8_t gnssId; // gnssid 0 = GPS, 1 = SBAS, 2 = GALILEO, 3 = BEIDOU, 4 = IMES, 5 = QZSS, 6 = GLONASS uint8_t svId; // space vehicle ID @@ -192,7 +220,7 @@ typedef struct { uint8_t quality; // 0 = no signal, 1 = search, 2 = acq, 3 = detected, 4 = code lock + time, 5,6,7 = code/carrier lock + time uint8_t corrSource; // Correction source: 0 = no correction, 1 = SBAS, 2 = BeiDou, 3 = RTCM2, 4 = RTCM3 OSR, 5 = RTCM3 SSR, 6 = QZSS SLAS, 7 = SPARTN uint8_t ionoModel; // 0 = no mode, 1 = Klobuchar GPS, 2 = SBAS, 3 = Klobuchar BeiDou, 8 = Iono derived from dual frequency observations - uint16_t sigFlags; // bit:0-1, 0 = unknown, 1 = healthy, 2 = unhealthy + uint16_t sigFlags; // bit:0-1 UBLOX_SIG_HEALTH_MASK // bit2: pseudorange smoothed, // bit3: pseudorange used, // bit4: carrioer range used; @@ -200,7 +228,7 @@ typedef struct { // bit6: pseudorange corrections used // bit7: carrier correction used // bit8: doper corrections used - uint8_t reserved; + uint8_t reserved[4]; } __attribute__((packed)) ubx_nav_sig_info; typedef struct { @@ -429,7 +457,6 @@ typedef enum { MSG_TIMEUTC = 0x21, MSG_SVINFO = 0x30, MSG_NAV_SAT = 0x35, - MSG_NAV_SIG = 0x35, MSG_CFG_PRT = 0x00, MSG_CFG_RATE = 0x08, MSG_CFG_SET_RATE = 0x01, @@ -437,7 +464,7 @@ typedef enum { MSG_CFG_SBAS = 0x16, MSG_CFG_GNSS = 0x3e, MSG_MON_GNSS = 0x28, - MSG_SIG_INFO = 0x43 + MSG_NAV_SIG = 0x43 } ubx_protocol_bytes_t; typedef enum { diff --git a/src/test/unit/gps_ublox_unittest.cc b/src/test/unit/gps_ublox_unittest.cc index bc20b340251..04de34e283b 100644 --- a/src/test/unit/gps_ublox_unittest.cc +++ b/src/test/unit/gps_ublox_unittest.cc @@ -87,4 +87,10 @@ TEST(GPSUbloxTest, TestUbloxCfgFillBytes) // osdFormatCentiNumber(buf, 12345, 1, 2, 3, 7); // std::cout << "'" << buf << "'" << std::endl; //EXPECT_FALSE(strcmp(buf, " 123.45")); +} + +TEST(GPSUbloxTest, navSigStructureSizes) { + EXPECT_TRUE(sizeof(ubx_nav_sig_info) == 16); + + EXPECT_TRUE(sizeof(ubx_nav_sig) == (8 + (16 * UBLOX_MAX_SIGNALS))); } \ No newline at end of file From cdcbd3f7ff7b27554612f0cc8c4ff94906e8ef2b Mon Sep 17 00:00:00 2001 From: Marcelo Bezerra <23555060+mmosca@users.noreply.github.com> Date: Wed, 3 Jul 2024 20:53:56 +0200 Subject: [PATCH 17/23] Remove some debugging info --- src/main/io/gps_ublox.c | 12 ++---------- 1 file changed, 2 insertions(+), 10 deletions(-) diff --git a/src/main/io/gps_ublox.c b/src/main/io/gps_ublox.c index 80bee7db441..d674ea2d9de 100755 --- a/src/main/io/gps_ublox.c +++ b/src/main/io/gps_ublox.c @@ -724,28 +724,20 @@ static bool gpsParseFrameUBLOX(void) if (_class == CLASS_NAV && _buffer.navsig.version == 0) { static int sigInfoCount = 0; DEBUG_SET(DEBUG_GPS, 2, sigInfoCount++); + DEBUG_SET(DEBUG_GPS, 4, _buffer.navsig.numSigs); gpsState.flags.sig = 1; - // - //gpsSolDRV.numSat = _buffer.navSig.numSigs; // if good - int okSats = 0; + if(_buffer.navsig.numSigs > 0) { for(int i=0; i < MIN(UBLOX_MAX_SIGNALS, _buffer.navsig.numSigs); ++i) { memcpy(&satelites[i], &_buffer.navsig.sig[i], sizeof(ubx_nav_sig_info)); - if ((_buffer.navsig.sig[i].sigFlags & UBLOX_SIG_HEALTH_MASK) == UBLOX_SIG_HEALTH_HEALTHY && - _buffer.navsig.sig[i].quality >= UBLOX_SIG_QUALITY_CODE_LOCK_TIME_SYNC) - { - okSats++; - } } for(int i = _buffer.navsig.numSigs; i < UBLOX_MAX_SIGNALS; ++i) { satelites[i].svId = 0xFF; // no used } } - //DEBUG_SET(DEBUG_GPS, 4, _buffer.navsig.numSigs); - DEBUG_SET(DEBUG_GPS, 4, okSats); } break; case MSG_ACK_ACK: From db459e5a4971bb78d2a99296197d9ffc7d9bed49 Mon Sep 17 00:00:00 2001 From: Marcelo Bezerra <23555060+mmosca@users.noreply.github.com> Date: Wed, 3 Jul 2024 22:18:09 +0200 Subject: [PATCH 18/23] NAV_SAT parsing --- src/main/io/gps_ublox.c | 12 ++---------- src/main/io/gps_ublox.h | 3 ++- src/main/io/gps_ublox_utils.c | 25 +++++++++++++++++++++++++ src/main/io/gps_ublox_utils.h | 2 ++ src/test/unit/gps_ublox_unittest.cc | 4 ++++ 5 files changed, 35 insertions(+), 11 deletions(-) diff --git a/src/main/io/gps_ublox.c b/src/main/io/gps_ublox.c index d674ea2d9de..ae81326dd62 100755 --- a/src/main/io/gps_ublox.c +++ b/src/main/io/gps_ublox.c @@ -705,19 +705,11 @@ static bool gpsParseFrameUBLOX(void) } for(int i =0; i < MIN(_buffer.svinfo.numSvs, UBLOX_MAX_SIGNALS); ++i) { - satelites[i].svId = _buffer.svinfo.channel[i].svId; - satelites[i].gnssId = _buffer.svinfo.channel[i].gnssId; - satelites[i].sigId = 0; - satelites[i].cno = _buffer.svinfo.channel[i].cno; - satelites[i].cno = _buffer.svinfo.channel[i].flags; - satelites[i].quality = _buffer.svinfo.channel[i].flags & 0x3; - satelites[i].sigFlags = (_buffer.svinfo.channel[i].flags >> 4 & 0x3); // Healthy, not healthy - //satelites[i].cno = _buffer.svinfo.channel[i].quality; + ubloxNavSat2NavSig(&_buffer.svinfo.channel[i], &satelites[i]); } for(int i =_buffer.svinfo.numSvs; i < UBLOX_MAX_SIGNALS; ++i) { - satelites->svId = 0; + satelites->svId = 0xFF; } - } break; case MSG_NAV_SIG: diff --git a/src/main/io/gps_ublox.h b/src/main/io/gps_ublox.h index 3a1c145b1b0..d1e9b148a65 100644 --- a/src/main/io/gps_ublox.h +++ b/src/main/io/gps_ublox.h @@ -21,6 +21,7 @@ #include #include "common/time.h" +#include "common/utils.h" #include "build/debug.h" #ifdef __cplusplus @@ -350,7 +351,7 @@ typedef struct { typedef struct { uint32_t itow; // GPS Millisecond time of week - uint8_t version; // Version = 0 + uint8_t version; // Version = 0-1 uint8_t numSvs; // (Space vehicle) Satelite count uint16_t reserved2; // Reserved ubx_nav_svinfo_channel channel[UBLOX_MAX_SIGNALS]; // UBLOX_MAX_SIGNALS satellites * 12 byte diff --git a/src/main/io/gps_ublox_utils.c b/src/main/io/gps_ublox_utils.c index 97c5bf5cd9a..bf0d0f264c0 100644 --- a/src/main/io/gps_ublox_utils.c +++ b/src/main/io/gps_ublox_utils.c @@ -17,6 +17,7 @@ #include +#include #include "gps_ublox_utils.h" @@ -60,3 +61,27 @@ int ubloxCfgFillBytes(ubx_config_data8_t *cfg, ubx_config_data8_payload_t *kvPai return count; } +void ubloxNavSat2NavSig(const ubx_nav_svinfo_channel *navSat, ubx_nav_sig_info *navSig) +{ + memset(navSig, 0, sizeof(ubx_nav_sig_info)); + navSig->svId = navSat->svId; + navSig->gnssId = navSat->gnssId; + navSig->cno = navSat->cno; + navSig->prRes = navSat->prRes; + navSig->quality = navSat->flags & (BIT(0)|BIT(1)|BIT(3)); + navSig->sigFlags = (navSat->flags >> 4) & (BIT(0)|BIT(1)); // Healthy, not healthy + // non-converted items: + //uint8_t sigId; // signal ID + //uint8_t freqId; // 0-13 slot +, 0-13, glonass only + //uint8_t corrSource; // Correction source: 0 = no correction, 1 = SBAS, 2 = BeiDou, 3 = RTCM2, 4 = RTCM3 OSR, 5 = RTCM3 SSR, 6 = QZSS SLAS, 7 = SPARTN + //uint8_t ionoModel; // 0 = no mode, 1 = Klobuchar GPS, 2 = SBAS, 3 = Klobuchar BeiDou, 8 = Iono derived from dual frequency observations + //uint16_t sigFlags; + // bit2: pseudorange smoothed, + // bit3: pseudorange used, + // bit4: carrioer range used; + // bit5: doppler used + // bit6: pseudorange corrections used + // bit7: carrier correction used + // bit8: doper corrections used + //uint8_t reserved[4]; +} \ No newline at end of file diff --git a/src/main/io/gps_ublox_utils.h b/src/main/io/gps_ublox_utils.h index e7ae0caac90..502bbaf31a6 100644 --- a/src/main/io/gps_ublox_utils.h +++ b/src/main/io/gps_ublox_utils.h @@ -29,6 +29,8 @@ int ubloxCfgFillBytes(ubx_config_data8_t *cfg, ubx_config_data8_payload_t *kvPai void ublox_update_checksum(uint8_t *data, uint8_t len, uint8_t *ck_a, uint8_t *ck_b); +void ubloxNavSat2NavSig(const ubx_nav_svinfo_channel *navSat, ubx_nav_sig_info *navSig); + #ifdef __cplusplus } #endif diff --git a/src/test/unit/gps_ublox_unittest.cc b/src/test/unit/gps_ublox_unittest.cc index 04de34e283b..0628f8a1bdd 100644 --- a/src/test/unit/gps_ublox_unittest.cc +++ b/src/test/unit/gps_ublox_unittest.cc @@ -93,4 +93,8 @@ TEST(GPSUbloxTest, navSigStructureSizes) { EXPECT_TRUE(sizeof(ubx_nav_sig_info) == 16); EXPECT_TRUE(sizeof(ubx_nav_sig) == (8 + (16 * UBLOX_MAX_SIGNALS))); + + EXPECT_TRUE(sizeof(ubx_nav_svinfo_channel) == 12); + + EXPECT_TRUE(sizeof(ubx_nav_svinfo) == (8 + (12 * UBLOX_MAX_SIGNALS); } \ No newline at end of file From 78a6a2a858d62e0ff2cda3a3cb8183a9fbe6411b Mon Sep 17 00:00:00 2001 From: Marcelo Bezerra <23555060+mmosca@users.noreply.github.com> Date: Wed, 3 Jul 2024 22:24:39 +0200 Subject: [PATCH 19/23] Fix test --- src/test/unit/gps_ublox_unittest.cc | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/test/unit/gps_ublox_unittest.cc b/src/test/unit/gps_ublox_unittest.cc index 0628f8a1bdd..ea5164b32a0 100644 --- a/src/test/unit/gps_ublox_unittest.cc +++ b/src/test/unit/gps_ublox_unittest.cc @@ -96,5 +96,5 @@ TEST(GPSUbloxTest, navSigStructureSizes) { EXPECT_TRUE(sizeof(ubx_nav_svinfo_channel) == 12); - EXPECT_TRUE(sizeof(ubx_nav_svinfo) == (8 + (12 * UBLOX_MAX_SIGNALS); + EXPECT_TRUE(sizeof(ubx_nav_svinfo) == (8 + (12 * UBLOX_MAX_SIGNALS))); } \ No newline at end of file From 7677abeb0c19bf50c9844f22ccb0a9b438a9bf0d Mon Sep 17 00:00:00 2001 From: Marcelo Bezerra <23555060+mmosca@users.noreply.github.com> Date: Wed, 3 Jul 2024 23:03:17 +0200 Subject: [PATCH 20/23] cleanup and static asserts --- src/main/io/gps_ublox.h | 11 ++++++++++- 1 file changed, 10 insertions(+), 1 deletion(-) diff --git a/src/main/io/gps_ublox.h b/src/main/io/gps_ublox.h index d1e9b148a65..287124581d4 100644 --- a/src/main/io/gps_ublox.h +++ b/src/main/io/gps_ublox.h @@ -21,6 +21,7 @@ #include #include "common/time.h" +#include "common/maths.h" #include "common/utils.h" #include "build/debug.h" @@ -30,12 +31,16 @@ extern "C" { #define GPS_CFG_CMD_TIMEOUT_MS 500 #define GPS_VERSION_RETRY_TIMES 3 +#ifndef UBLOX_MAX_SIGNALS #define UBLOX_MAX_SIGNALS 64 -#define MAX_UBLOX_PAYLOAD_SIZE 1048 // enough for anyone? // UBX-NAV-SIG info would be UBLOX_MAX_SIGNALS * 16 + 8 for (64 * 16) + 8 = 1032 bytes +#endif +#define MAX_UBLOX_PAYLOAD_SIZE ((UBLOX_MAX_SIGNALS * 16) + 8) // UBX-NAV-SIG info would be UBLOX_MAX_SIGNALS * 16 + 8 #define UBLOX_BUFFER_SIZE MAX_UBLOX_PAYLOAD_SIZE #define UBLOX_SBAS_MESSAGE_LENGTH 16 #define GPS_CAPA_INTERVAL 5000 +STATIC_ASSERT(MAX_UBLOX_PAYLOAD_SIZE >= 256, ubx_size_too_small); + #define UBX_DYNMODEL_PEDESTRIAN 3 #define UBX_DYNMODEL_AUTOMOVITE 4 #define UBX_DYNMODEL_AIR_1G 6 @@ -232,6 +237,8 @@ typedef struct { uint8_t reserved[4]; } __attribute__((packed)) ubx_nav_sig_info; +STATIC_ASSERT(sizeof(ubx_nav_sig_info) == 16, wrong_ubx_nav_sig_info_size); + typedef struct { uint32_t time; // GPS iToW uint8_t version; // We support version 0 @@ -349,6 +356,8 @@ typedef struct { uint32_t flags; // Bitmask } ubx_nav_svinfo_channel; +STATIC_ASSERT(sizeof(ubx_nav_svinfo_channel) == 12, wrong_ubx_nav_svinfo_channel_size); + typedef struct { uint32_t itow; // GPS Millisecond time of week uint8_t version; // Version = 0-1 From 341bb4f15f77c9c8001c68a8bd333531aac4ea36 Mon Sep 17 00:00:00 2001 From: Marcelo Bezerra <23555060+mmosca@users.noreply.github.com> Date: Thu, 4 Jul 2024 19:38:05 +0200 Subject: [PATCH 21/23] Add warning about old ublox versions --- src/main/fc/cli.c | 3 +++ 1 file changed, 3 insertions(+) diff --git a/src/main/fc/cli.c b/src/main/fc/cli.c index 7320ef060d6..c99e9111f5a 100644 --- a/src/main/fc/cli.c +++ b/src/main/fc/cli.c @@ -3892,6 +3892,9 @@ static void cliStatus(char *cmdline) 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 SW >= 15.0 required)"); + } cliPrintLinefeed(); cliPrintLinef(" SATS: %i", gpsSol.numSat); cliPrintLinef(" HDOP: %f", (double)(gpsSol.hdop / (float)HDOP_SCALE)); From 29ee096e336de7d20c5c35a516258f2866326fc1 Mon Sep 17 00:00:00 2001 From: Marcelo Bezerra <23555060+mmosca@users.noreply.github.com> Date: Thu, 4 Jul 2024 19:38:19 +0200 Subject: [PATCH 22/23] Make UBLOX functionally equal to UBLOX7 --- src/main/io/gps_ublox.c | 10 ++++++++-- 1 file changed, 8 insertions(+), 2 deletions(-) diff --git a/src/main/io/gps_ublox.c b/src/main/io/gps_ublox.c index ae81326dd62..175d283ddd4 100755 --- a/src/main/io/gps_ublox.c +++ b/src/main/io/gps_ublox.c @@ -704,7 +704,7 @@ static bool gpsParseFrameUBLOX(void) gpsSolDRV.numSat = _buffer.svinfo.numSvs; } - for(int i =0; i < MIN(_buffer.svinfo.numSvs, UBLOX_MAX_SIGNALS); ++i) { + for(int i = 0; i < MIN(_buffer.svinfo.numSvs, UBLOX_MAX_SIGNALS); ++i) { ubloxNavSat2NavSig(&_buffer.svinfo.channel[i], &satelites[i]); } for(int i =_buffer.svinfo.numSvs; i < UBLOX_MAX_SIGNALS; ++i) { @@ -978,7 +978,7 @@ STATIC_PROTOTHREAD(gpsConfigure) }// end message config ptWaitTimeout((_ack_state == UBX_ACK_GOT_ACK || _ack_state == UBX_ACK_GOT_NAK), GPS_SHORT_TIMEOUT); - if ((gpsState.gpsConfig->provider == GPS_UBLOX7PLUS) && (gpsState.hwVersion >= UBX_HW_VERSION_UBLOX7)) { + if ((gpsState.hwVersion >= UBX_HW_VERSION_UBLOX7)) { configureRATE(hz2rate(gpsState.gpsConfig->ubloxNavHz)); // default 10Hz } else { configureRATE(hz2rate(5)); // 5Hz @@ -1156,6 +1156,12 @@ STATIC_PROTOTHREAD(gpsProtocolStateThread) void gpsRestartUBLOX(void) { + for(int i = 0; i < UBLOX_MAX_SIGNALS; ++i) + { + memset(&satelites[i], 0, sizeof(ubx_nav_sig_info)); + satelites[i].svId = 0xFF; + } + ptSemaphoreInit(semNewDataReady); ptRestart(ptGetHandle(gpsProtocolReceiverThread)); ptRestart(ptGetHandle(gpsProtocolStateThread)); From 017cb30bef73de8c8036e7cb9aa47aba08dbe629 Mon Sep 17 00:00:00 2001 From: Marcelo Bezerra <23555060+mmosca@users.noreply.github.com> Date: Thu, 4 Jul 2024 20:15:55 +0200 Subject: [PATCH 23/23] Small fixes. Add warning about ublox protocol version < 15.00 --- src/main/fc/cli.c | 2 +- src/main/io/gps_ublox.c | 20 ++++++++++++-------- src/main/io/gps_ublox_utils.c | 2 +- 3 files changed, 14 insertions(+), 10 deletions(-) diff --git a/src/main/fc/cli.c b/src/main/fc/cli.c index c99e9111f5a..580859fec0c 100644 --- a/src/main/fc/cli.c +++ b/src/main/fc/cli.c @@ -3893,7 +3893,7 @@ static void cliStatus(char *cmdline) cliPrint("GPS: "); cliPrintf("HW Version: %s Proto: %d.%02d Baud: %d", getGpsHwVersion(), getGpsProtoMajorVersion(), getGpsProtoMinorVersion(), getGpsBaudrate()); if(ubloxVersionLT(15, 0)) { - cliPrintf(" (UBLOX SW >= 15.0 required)"); + cliPrintf(" (UBLOX Proto >= 15.0 required)"); } cliPrintLinefeed(); cliPrintLinef(" SATS: %i", gpsSol.numSat); diff --git a/src/main/io/gps_ublox.c b/src/main/io/gps_ublox.c index 175d283ddd4..fbe4a7f74ff 100755 --- a/src/main/io/gps_ublox.c +++ b/src/main/io/gps_ublox.c @@ -431,10 +431,10 @@ static void configureGNSS(void) { int blocksUsed = 0; send_buffer.message.header.msg_class = CLASS_CFG; - send_buffer.message.header.msg_id = MSG_CFG_GNSS; // message deprecated in protocol > 23.01, should use UBX-CFG-VALSET/UBX-CFG-VALGET + send_buffer.message.header.msg_id = MSG_CFG_GNSS; // message deprecated in protocol > 23.01, should use UBX-CFG-VALSET/UBX-CFG-VALGET send_buffer.message.payload.gnss.msgVer = 0; send_buffer.message.payload.gnss.numTrkChHw = 0; // read only, so unset - send_buffer.message.payload.gnss.numTrkChUse = 0xFF; // If set to 0xFF will use hardware max + send_buffer.message.payload.gnss.numTrkChUse = 0xFF; // If set to 0xFF will use hardware max /* SBAS, always generated */ blocksUsed += configureGNSS_SBAS(&send_buffer.message.payload.gnss.config[blocksUsed]); @@ -442,11 +442,11 @@ static void configureGNSS(void) /* Galileo */ blocksUsed += configureGNSS_GALILEO(&send_buffer.message.payload.gnss.config[blocksUsed]); - /* BeiDou */ - blocksUsed += configureGNSS_BEIDOU(&send_buffer.message.payload.gnss.config[blocksUsed]); + /* BeiDou */ + blocksUsed += configureGNSS_BEIDOU(&send_buffer.message.payload.gnss.config[blocksUsed]); - /* GLONASS */ - blocksUsed += configureGNSS_GLONASS(&send_buffer.message.payload.gnss.config[blocksUsed]); + /* GLONASS */ + blocksUsed += configureGNSS_GLONASS(&send_buffer.message.payload.gnss.config[blocksUsed]); send_buffer.message.payload.gnss.numConfigBlocks = blocksUsed; send_buffer.message.header.length = (sizeof(ubx_gnss_msg_t) + sizeof(ubx_gnss_element_t)* blocksUsed); @@ -708,6 +708,7 @@ static bool gpsParseFrameUBLOX(void) ubloxNavSat2NavSig(&_buffer.svinfo.channel[i], &satelites[i]); } for(int i =_buffer.svinfo.numSvs; i < UBLOX_MAX_SIGNALS; ++i) { + satelites->gnssId = 0xFF; satelites->svId = 0xFF; } } @@ -728,6 +729,7 @@ static bool gpsParseFrameUBLOX(void) for(int i = _buffer.navsig.numSigs; i < UBLOX_MAX_SIGNALS; ++i) { satelites[i].svId = 0xFF; // no used + satelites[i].gnssId = 0xFF; } } } @@ -1028,7 +1030,8 @@ STATIC_PROTOTHREAD(gpsConfigure) for(int i = 0; i < UBLOX_MAX_SIGNALS; ++i) { - satelites[i].svId = 0xFF; + satelites[i].svId = 0xFF; // no used + satelites[i].gnssId = 0xFF; } ptEnd(0); @@ -1160,6 +1163,7 @@ void gpsRestartUBLOX(void) { memset(&satelites[i], 0, sizeof(ubx_nav_sig_info)); satelites[i].svId = 0xFF; + satelites[i].gnssId = 0xFF; } ptSemaphoreInit(semNewDataReady); @@ -1191,7 +1195,7 @@ bool isGpsUblox(void) const ubx_nav_sig_info *gpsGetUbloxSatelite(uint8_t index) { - if(index < UBLOX_MAX_SIGNALS && satelites[index].svId != 0xFF) { + if(index < UBLOX_MAX_SIGNALS && satelites[index].svId != 0xFF && satelites[index].gnssId != 0xFF) { return &satelites[index]; } diff --git a/src/main/io/gps_ublox_utils.c b/src/main/io/gps_ublox_utils.c index bf0d0f264c0..a2adf1df18b 100644 --- a/src/main/io/gps_ublox_utils.c +++ b/src/main/io/gps_ublox_utils.c @@ -68,7 +68,7 @@ void ubloxNavSat2NavSig(const ubx_nav_svinfo_channel *navSat, ubx_nav_sig_info * navSig->gnssId = navSat->gnssId; navSig->cno = navSat->cno; navSig->prRes = navSat->prRes; - navSig->quality = navSat->flags & (BIT(0)|BIT(1)|BIT(3)); + navSig->quality = navSat->flags & (BIT(0)|BIT(1)|BIT(2)); navSig->sigFlags = (navSat->flags >> 4) & (BIT(0)|BIT(1)); // Healthy, not healthy // non-converted items: //uint8_t sigId; // signal ID