Skip to content

Commit

Permalink
Merge pull request iNavFlight#10179 from iNavFlight/mmosca-ublox-satinfo
Browse files Browse the repository at this point in the history 
[GPS] Fix message rate for newer ublox devices and add some extra debugging features
  • Loading branch information
@mmosca
mmosca authored Jul 4, 2024
2 parents a35d44b + 017cb30 commit ca82d1c
Show file tree
Hide file tree
Showing 10 changed files with 508 additions and 119 deletions.
3 changes: 2 additions & 1 deletion src/main/build/debug.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down
1 change: 1 addition & 0 deletions src/main/common/printf.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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)
Expand Down
176 changes: 173 additions & 3 deletions src/main/fc/cli.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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[] = {
Expand Down Expand Up @@ -253,6 +283,7 @@ static void cliPrintLine(const char *str)
cliPrintLinefeed();
}


static void cliPrintError(const char *str)
{
cliPrint("### ERROR: ");
Expand Down Expand Up @@ -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");
Expand Down Expand Up @@ -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()
Expand Down Expand Up @@ -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
Expand Down Expand Up @@ -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
Expand Down
3 changes: 2 additions & 1 deletion src/main/fc/settings.yaml
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down
6 changes: 6 additions & 0 deletions src/main/io/gps_private.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand Down
Loading

0 comments on commit ca82d1c

Please sign in to comment.