Add MSP request for UBLOX SatInfo and disable SatInfo on ARMING. (bet…

…aflight#13240)

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
 }
 

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);

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;

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

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,