From 22ce586550f532dfe5e37c21d33c91db48394cb9 Mon Sep 17 00:00:00 2001 From: Mark Haslinghuis Date: Mon, 25 Dec 2023 00:50:34 +0100 Subject: [PATCH] Add MSP request for UBLOX SatInfo and disable SatInfo on ARMING. (#13240) --- src/main/io/gps.c | 39 ++++++++++------------- src/main/io/gps.h | 4 ++- src/main/msp/msp.c | 6 ++++ src/main/msp/msp_protocol_v2_betaflight.h | 1 + src/main/pg/gps.c | 2 +- 5 files changed, 28 insertions(+), 24 deletions(-) diff --git a/src/main/io/gps.c b/src/main/io/gps.c index bb82b182bd9..01ed5feb5d5 100644 --- a/src/main/io/gps.c +++ b/src/main/io/gps.c @@ -329,10 +329,9 @@ typedef enum { UBLOX_MSG_STATUS, // 15: set STATUS MSG rate UBLOX_MSG_VELNED, // 16. set VELNED MSG rate UBLOX_MSG_DOP, // 17. MSG_NAV_DOP - UBLOX_SAT_INFO, // 18. MSG_NAV_SAT message - UBLOX_SET_NAV_RATE, // 19. set to user requested GPS sample rate - UBLOX_MSG_CFG_GNSS, // 20. For not SBAS or GALILEO - UBLOX_CONFIG_COMPLETE // 21. Config finished, start receiving data + UBLOX_SET_NAV_RATE, // 18. set to user requested GPS sample rate + UBLOX_MSG_CFG_GNSS, // 19. For not SBAS or GALILEO + UBLOX_CONFIG_COMPLETE // 20. Config finished, start receiving data } ubloxStatePosition_e; baudRate_e initBaudRateIndex; @@ -367,10 +366,15 @@ static void logErrorToPacketLog(void) } #endif // USE_DASHBOARD -static bool isConfiguratorConnected(void) +// Enable sat info using MSP request +#ifdef USE_GPS_UBLOX +void gpsRequestSatInfo(void) { - return (getArmingDisableFlags() & ARMING_DISABLED_MSP); + if (!ARMING_FLAG(ARMED)) { + setSatInfoMessageRate(5); + } } +#endif static void gpsNewData(uint16_t c); #ifdef USE_GPS_NMEA @@ -395,7 +399,6 @@ void gpsInit(void) gpsDataIntervalSeconds = 0.1f; gpsData.userBaudRateIndex = 0; gpsData.timeouts = 0; - gpsData.satMessagesDisabled = false; gpsData.state_ts = millis(); #ifdef USE_GPS_UBLOX gpsData.ubloxUsingFlightModel = false; @@ -939,7 +942,6 @@ static void ubloxSetSbas(void) void setSatInfoMessageRate(uint8_t divisor) { // enable satInfoMessage at 1:5 of the nav rate if configurator is connected - divisor = (isConfiguratorConnected()) ? 5 : 0; if (gpsData.ubloxM9orAbove) { ubloxSetMessageRateValSet(CFG_MSGOUT_UBX_NAV_SAT_UART1, divisor); } else if (gpsData.ubloxM8orAbove) { @@ -1106,16 +1108,6 @@ void gpsConfigureUblox(void) break; } - // allow 3s for the Configurator connection to stabilise, to get the correct answer when we test the state of the connection. - // 3s is an arbitrary time at present, maybe should be defined or user adjustable. - // This delays the appearance of GPS data in OSD when not connected to configurator by 3s. - // Note that state_ts is set to millis() on the previous gpsSetState() command - if (!isConfiguratorConnected()) { - if (cmp32(gpsData.now, gpsData.state_ts) < 3000) { - return; - } - } - if (gpsData.ackState == UBLOX_ACK_IDLE) { // short delay before between commands, including the first command @@ -1240,10 +1232,6 @@ void gpsConfigureUblox(void) ubloxSetMessageRate(CLASS_NAV, MSG_NAV_DOP, 1); } break; - case UBLOX_SAT_INFO: - // enable by default, turned off when armed and receiving data to reduce in-flight traffic - setSatInfoMessageRate(5); - break; case UBLOX_SET_NAV_RATE: // set the nav solution rate to the user's configured update rate gpsData.updateRateHz = gpsConfig()->gps_update_rate_hz; @@ -2568,6 +2556,13 @@ void GPS_reset_home_position(void) // PS: to test for gyro cal, check for !ARMED, since we cannot be here while disarmed other than via gyro cal } } + +#ifdef USE_GPS_UBLOX + // disable Sat Info requests on arming + if (ARMING_FLAG(ARMED)) { + setSatInfoMessageRate(0); + } +#endif GPS_calculateDistanceFlown(true); // Initialize } diff --git a/src/main/io/gps.h b/src/main/io/gps.h index 43aa9239cb0..fd5b26c91d4 100644 --- a/src/main/io/gps.h +++ b/src/main/io/gps.h @@ -292,7 +292,6 @@ typedef struct gpsData_s { bool ubloxM8orAbove; bool ubloxM9orAbove; bool ubloxUsingFlightModel; // false = Acquire model, true = Flight model - bool satMessagesDisabled; #ifdef USE_GPS_UBLOX uint32_t lastNavSolTs; // time stamp of last UBCX message. Used to calculate message delta ubloxVersion_e platformVersion; // module platform version, mapped from reported hardware version @@ -382,7 +381,10 @@ extern uint32_t dashboardGpsNavSvInfoRcvCount; // Count of time #ifdef USE_GPS_UBLOX ubloxVersion_e ubloxParseVersion(const uint32_t version); +void gpsRequestSatInfo(void); +void setSatInfoMessageRate(uint8_t divisor); #endif + void gpsInit(void); void gpsUpdate(timeUs_t currentTimeUs); bool gpsNewFrame(uint8_t c); diff --git a/src/main/msp/msp.c b/src/main/msp/msp.c index dab1e0eb04e..ecaf8a346ef 100644 --- a/src/main/msp/msp.c +++ b/src/main/msp/msp.c @@ -4052,6 +4052,12 @@ static mspResult_e mspProcessInCommand(mspDescriptor_t srcDesc, int16_t cmdMSP, break; #endif +#ifdef USE_GPS_UBLOX + case MSP2_UBLOX_REQUEST_SV_INFO: + gpsRequestSatInfo(); + break; +#endif + default: // we do not know how to handle the (valid) message, indicate error MSP $M! return MSP_RESULT_ERROR; diff --git a/src/main/msp/msp_protocol_v2_betaflight.h b/src/main/msp/msp_protocol_v2_betaflight.h index b039defb71e..97f63a798d8 100644 --- a/src/main/msp/msp_protocol_v2_betaflight.h +++ b/src/main/msp/msp_protocol_v2_betaflight.h @@ -29,6 +29,7 @@ #define MSP2_GET_LED_STRIP_CONFIG_VALUES 0x3008 #define MSP2_SET_LED_STRIP_CONFIG_VALUES 0x3009 #define MSP2_SENSOR_CONFIG_ACTIVE 0x300A +#define MSP2_UBLOX_REQUEST_SV_INFO 0x300B // MSP2_SET_TEXT and MSP2_GET_TEXT variable types #define MSP2TEXT_PILOT_NAME 1 diff --git a/src/main/pg/gps.c b/src/main/pg/gps.c index 750e43530e2..e90af7b5aea 100644 --- a/src/main/pg/gps.c +++ b/src/main/pg/gps.c @@ -29,7 +29,7 @@ #include "gps.h" -PG_REGISTER_WITH_RESET_TEMPLATE(gpsConfig_t, gpsConfig, PG_GPS_CONFIG, 3); +PG_REGISTER_WITH_RESET_TEMPLATE(gpsConfig_t, gpsConfig, PG_GPS_CONFIG, 4); PG_RESET_TEMPLATE(gpsConfig_t, gpsConfig, .provider = GPS_UBLOX,