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..580859fec0c 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: "); @@ -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 --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_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 52b6329d9f2..fbe4a7f74ff 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; @@ -427,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]); @@ -438,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); @@ -601,6 +605,12 @@ static bool gpsParseFrameUBLOX(void) } break; case MSG_PVT: + { + static int pvtCount = 0; + 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; @@ -684,6 +694,46 @@ static bool gpsParseFrameUBLOX(void) } } break; + case MSG_NAV_SAT: + if (_class == CLASS_NAV) { + static int satInfoCount = 0; + 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; + } + + 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) { + satelites->gnssId = 0xFF; + satelites->svId = 0xFF; + } + } + break; + 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; + + 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)); + } + for(int i = _buffer.navsig.numSigs; i < UBLOX_MAX_SIGNALS; ++i) + { + satelites[i].svId = 0xFF; // no used + satelites[i].gnssId = 0xFF; + } + } + } + break; case MSG_ACK_ACK: if ((_ack_state == UBX_ACK_WAITING) && (_buffer.ack.msg == _ack_waiting_msg)) { _ack_state = UBX_ACK_GOT_ACK; @@ -698,6 +748,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) { @@ -831,134 +885,118 @@ 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); // 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) { + 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 + }; + + 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); - configureMSG(MSG_CLASS_UBX, MSG_VELNED, 0); - ptWait(_ack_state == UBX_ACK_GOT_ACK); - - configureMSG(MSG_CLASS_UBX, MSG_TIMEUTC, 0); + configureMSG(MSG_CLASS_UBX, MSG_SOL, 0); ptWait(_ack_state == UBX_ACK_GOT_ACK); - configureMSG(MSG_CLASS_UBX, MSG_PVT, 1); + configureMSG(MSG_CLASS_UBX, MSG_VELNED, 0); ptWait(_ack_state == UBX_ACK_GOT_ACK); - configureMSG(MSG_CLASS_UBX, MSG_NAV_SAT, 0); + configureMSG(MSG_CLASS_UBX, MSG_TIMEUTC, 0); ptWait(_ack_state == UBX_ACK_GOT_ACK); - configureMSG(MSG_CLASS_UBX, MSG_NAV_SIG, 0); + configureMSG(MSG_CLASS_UBX, MSG_SVINFO, 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_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 { - // 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); - - 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_SVINFO, 0); - ptWait(_ack_state == UBX_ACK_GOT_ACK); + 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); - 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); + configureMSG(MSG_CLASS_UBX, MSG_STATUS, 1); + ptWait(_ack_state == UBX_ACK_GOT_ACK); - 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_SOL, 1); + 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_VELNED, 1); + 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_TIMEUTC, 10); + 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_SVINFO, 0); + ptWait(_ack_state == UBX_ACK_GOT_ACK); + }// end message config - configureMSG(MSG_CLASS_UBX, MSG_TIMEUTC, 10); - ptWait(_ack_state == UBX_ACK_GOT_ACK); + ptWaitTimeout((_ack_state == UBX_ACK_GOT_ACK || _ack_state == UBX_ACK_GOT_NAK), GPS_SHORT_TIMEOUT); + if ((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); - // 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); + 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); + } - 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); - } - } + 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, @@ -971,7 +1009,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; } @@ -981,7 +1019,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; @@ -990,6 +1028,12 @@ STATIC_PROTOTHREAD(gpsConfigure) } } + for(int i = 0; i < UBLOX_MAX_SIGNALS; ++i) + { + satelites[i].svId = 0xFF; // no used + satelites[i].gnssId = 0xFF; + } + ptEnd(0); } @@ -1115,6 +1159,13 @@ 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; + satelites[i].gnssId = 0xFF; + } + ptSemaphoreInit(semNewDataReady); ptRestart(ptGetHandle(gpsProtocolReceiverThread)); ptRestart(ptGetHandle(gpsProtocolStateThread)); @@ -1134,11 +1185,46 @@ 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 && satelites[index].svId != 0xFF && satelites[index].gnssId != 0xFF) { + return &satelites[index]; + } + + return NULL; +} + +bool ubloxVersionLT(uint8_t mj, uint8_t mn) +{ + return gpsState.swVersionMajor < mj || (gpsState.swVersionMajor == mj && gpsState.swVersionMinor < mn); +} + +bool ubloxVersionGT(uint8_t mj, uint8_t mn) +{ + return gpsState.swVersionMajor > mj || (gpsState.swVersionMajor == mj && gpsState.swVersionMinor > mn); +} + +bool ubloxVersionGTE(uint8_t mj, uint8_t mn) +{ + return ubloxVersionE(mj, mn) || ubloxVersionGT(mj, mn); +} + +bool ubloxVersionLTE(uint8_t mj, uint8_t mn) +{ + return ubloxVersionE(mj, mn) || ubloxVersionLT(mj, mn); +} + +bool ubloxVersionE(uint8_t mj, uint8_t mn) +{ + 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 87cffa9c50b..287124581d4 100644 --- a/src/main/io/gps_ublox.h +++ b/src/main/io/gps_ublox.h @@ -21,6 +21,9 @@ #include #include "common/time.h" +#include "common/maths.h" +#include "common/utils.h" +#include "build/debug.h" #ifdef __cplusplus extern "C" { @@ -28,11 +31,16 @@ extern "C" { #define GPS_CFG_CMD_TIMEOUT_MS 500 #define GPS_VERSION_RETRY_TIMES 3 -#define MAX_UBLOX_PAYLOAD_SIZE 256 +#ifndef UBLOX_MAX_SIGNALS +#define UBLOX_MAX_SIGNALS 64 +#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 @@ -55,6 +63,18 @@ 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_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 @@ -169,6 +189,63 @@ 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 + 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) + 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 UBLOX_SIG_HEALTH_MASK + // 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]; +} __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 + 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) @@ -270,22 +347,23 @@ 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; +STATIC_ASSERT(sizeof(ubx_nav_svinfo_channel) == 12, wrong_ubx_nav_svinfo_channel_size); + 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-1 + 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 { @@ -350,6 +428,8 @@ typedef struct { uint8_t reserverd3; } ubx_mon_gnss; + + typedef struct { uint8_t msg_class; uint8_t msg; @@ -387,14 +467,14 @@ 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, MSG_CFG_NAV_SETTINGS = 0x24, MSG_CFG_SBAS = 0x16, MSG_CFG_GNSS = 0x3e, - MSG_MON_GNSS = 0x28 + MSG_MON_GNSS = 0x28, + MSG_NAV_SIG = 0x43 } ubx_protocol_bytes_t; typedef enum { @@ -428,6 +508,13 @@ bool gpsUbloxSendCommand(uint8_t *rawCommand, uint16_t commandLen, uint16_t time bool isGpsUblox(void); +const ubx_nav_sig_info *gpsGetUbloxSatelite(uint8_t index); + +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 } diff --git a/src/main/io/gps_ublox_utils.c b/src/main/io/gps_ublox_utils.c index 97c5bf5cd9a..a2adf1df18b 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(2)); + 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 bc20b340251..ea5164b32a0 100644 --- a/src/test/unit/gps_ublox_unittest.cc +++ b/src/test/unit/gps_ublox_unittest.cc @@ -87,4 +87,14 @@ 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))); + + 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