Add MSG_MON_VER

betaflight · May 3, 2023 · 77b3750 · 77b3750
1 parent bb59a7d
commit 77b3750
Showing 1 changed file with 11 additions and 8 deletions.
diff --git a/src/main/io/gps.c b/src/main/io/gps.c
@@ -136,6 +136,7 @@ typedef enum {
     CLASS_NAV = 0x01,
     CLASS_ACK = 0x05,
     CLASS_CFG = 0x06,
+    CLASS_MON = 0x0A,
     MSG_ACK_NACK = 0x00,
     MSG_ACK_ACK = 0x01,
     MSG_POSLLH = 0x02,
@@ -277,7 +278,8 @@ typedef enum {
     UBLOX_SET_NAV_RATE, // 13. set GPS sample rate
     UBLOX_SET_SBAS,     // 14. Sets SBAS
     UBLOX_SET_PMS,      // 15. Sets Power Mode
-    UBLOX_MSG_CFG_GNSS  // 16. For not SBAS or GALILEO otherwise GPS_STATE_REVEIVING_DATA
+    UBLOX_MSG_CFG_GNSS, // 16. For not SBAS or GALILEO otherwise GPS_STATE_REVEIVING_DATA
+    UBLOX_MON_VER,      // 17. MON_VER: get firmware version
 } ubloxStatePosition_e;
 
 #endif // USE_GPS_UBLOX
@@ -480,12 +482,12 @@ static void ubloxSendConfigMessage(ubxMessage_t *message, uint8_t msg_id, uint8_
     ubloxSendMessage((const uint8_t *) message, length + 6);
 }
 
-static void ubloxSendPollMessage(uint8_t msg_id)
+static void ubloxSendPollMessage(uint8_t messageClass, uint8_t msg_id)
 {
     ubxMessage_t tx_buffer;
     tx_buffer.header.preamble1 = PREAMBLE1;
     tx_buffer.header.preamble2 = PREAMBLE2;
-    tx_buffer.header.msg_class = CLASS_CFG;
+    tx_buffer.header.msg_class = messageClass;
     tx_buffer.header.msg_id = msg_id;
     tx_buffer.header.length = 0;
     ubloxSendMessage((const uint8_t *) &tx_buffer, 6);
@@ -721,12 +723,13 @@ void gpsInitUblox(void)
                         ubloxSendPowerMode();
                         break;
                     case UBLOX_MSG_CFG_GNSS:
-                        if ((gpsConfig()->sbasMode == SBAS_NONE) || (gpsConfig()->gps_ublox_use_galileo)) {
-                            ubloxSendPollMessage(MSG_CFG_GNSS);
-                        } else {
-                            gpsSetState(GPS_STATE_RECEIVING_DATA);
+                        if (gpsConfig()->sbasMode == SBAS_NONE || gpsConfig()->gps_ublox_use_galileo) {
+                            ubloxSendPollMessage(CLASS_NAV, MSG_CFG_GNSS);
                         }
                         break;
+                    case UBLOX_MON_VER:
+                        ubloxSendPollMessage(CLASS_MON, MSG_MON_VER);
+                        break;
                     default:
                         break;
                 }
@@ -741,7 +744,7 @@ void gpsInitUblox(void)
                     }
                     break;
                 case UBLOX_ACK_GOT_ACK:
-                    if (gpsData.state_position == UBLOX_MSG_CFG_GNSS) {
+                    if (gpsData.state_position == UBLOX_MSG_MON_VER) {
                         // ublox should be initialised, try receiving
                         gpsSetState(GPS_STATE_RECEIVING_DATA);
                     } else {