Add support for MSP2_SENSOR_GPS message from RemoteID module

betaflight · Sep 8, 2023 · 2549eda · 2549eda
1 parent 5cd2ab5
commit 2549eda
Show file tree

Hide file tree

Showing 3 changed files with 63 additions and 11 deletions.
diff --git a/src/main/io/gps.c b/src/main/io/gps.c
@@ -1355,10 +1355,29 @@ void gpsUpdate(timeUs_t currentTimeUs)
             isFast = false;
             rescheduleTask(TASK_SELF, TASK_PERIOD_HZ(TASK_GPS_RATE));
         }
-    } else if (GPS_update & GPS_MSP_UPDATE) { // GPS data received via MSP
-        gpsSetState(GPS_STATE_RECEIVING_DATA);
-        onGpsNewData();
-        GPS_update &= ~GPS_MSP_UPDATE;
+    } else if (gpsConfig()->provider == GPS_MSP) {
+        if (GPS_update & GPS_MSP_UPDATE) { // GPS data received via MSP
+            if (gpsData.state == GPS_STATE_INITIALIZED) {
+                gpsSetState(GPS_STATE_RECEIVING_DATA);
+            }
+
+            // Data is available
+            DEBUG_SET(DEBUG_GPS_CONNECTION, 3, gpsData.now - gpsData.lastNavMessage); // interval since last Nav data was received
+            gpsData.lastNavMessage = gpsData.now;
+            sensorsSet(SENSOR_GPS);
+
+            GPS_update ^= GPS_DIRECT_TICK;
+
+            onGpsNewData();
+
+            GPS_update &= ~GPS_MSP_UPDATE;
+        } else {
+            DEBUG_SET(DEBUG_GPS_CONNECTION, 2, gpsData.now - gpsData.lastNavMessage); // time since last Nav data, updated each GPS task interval
+            // check for no data/gps timeout/cable disconnection etc
+            if (cmp32(gpsData.now, gpsData.lastNavMessage) > GPS_TIMEOUT_MS) {
+                gpsSetState(GPS_STATE_LOST_COMMUNICATION);
+            }
+        }
     }
 
     switch (gpsData.state) {
@@ -1382,11 +1401,13 @@ void gpsUpdate(timeUs_t currentTimeUs)
 
         case GPS_STATE_RECEIVING_DATA:
 #ifdef USE_GPS_UBLOX
-            if (gpsConfig()->autoConfig == GPS_AUTOCONFIG_ON) {
-                // when we are connected up, and get a 3D fix, enable the 'flight' fix model
-                if (!gpsData.ubloxUsingFlightModel && STATE(GPS_FIX)) {
-                    gpsData.ubloxUsingFlightModel = true;
-                    ubloxSendNAV5Message(gpsConfig()->gps_ublox_flight_model);
+            if (gpsConfig()->provider != GPS_MSP) {
+                if (gpsConfig()->autoConfig == GPS_AUTOCONFIG_ON) {
+                    // when we are connected up, and get a 3D fix, enable the 'flight' fix model
+                    if (!gpsData.ubloxUsingFlightModel && STATE(GPS_FIX)) {
+                        gpsData.ubloxUsingFlightModel = true;
+                        ubloxSendNAV5Message(gpsConfig()->gps_ublox_flight_model);
+                    }
                 }
             }
 #endif
@@ -1434,14 +1455,14 @@ void gpsUpdate(timeUs_t currentTimeUs)
     schedulerSetNextStateTime(gpsStateDurationFractionUs[gpsCurrentState] >> GPS_TASK_DECAY_SHIFT);
 
     DEBUG_SET(DEBUG_GPS_CONNECTION, 5, executeTimeUs);
-//    keeping temporarily, to be used when debugging the sheduler stuff
+//    keeping temporarily, to be used when debugging the scheduler stuff
 //    DEBUG_SET(DEBUG_GPS_CONNECTION, 6, (gpsStateDurationFractionUs[gpsCurrentState] >> GPS_TASK_DECAY_SHIFT));
 }
 
 static void gpsNewData(uint16_t c)
 {
     DEBUG_SET(DEBUG_GPS_CONNECTION, 1, gpsSol.navIntervalMs);
-   if (!gpsNewFrame(c)) {
+    if (!gpsNewFrame(c)) {
         // no new nav solution data
         return;
     }

diff --git a/src/main/msp/msp.c b/src/main/msp/msp.c
@@ -3604,6 +3604,34 @@ static mspResult_e mspProcessInCommand(mspDescriptor_t srcDesc, int16_t cmdMSP,
 #endif
 
 #ifdef USE_GPS
+    case MSP2_SENSOR_GPS:
+        (void)sbufReadU8(src);              // instance
+        (void)sbufReadU16(src);             // gps_week
+        gpsSol.time = sbufReadU32(src);     // ms_tow
+        gpsSetFixState(sbufReadU8(src) != 0); // fix_type
+        gpsSol.numSat = sbufReadU8(src);    // satellites_in_view
+        gpsSol.acc.hAcc = sbufReadU16(src) * 10; // horizontal_pos_accuracy mm to cm
+        gpsSol.acc.vAcc = sbufReadU16(src) * 10; // vertical_pos_accuracy mm to cm
+        gpsSol.acc.sAcc = sbufReadU16(src) * 10; // horizontal_vel_accuracy mm to cm
+        gpsSol.dop.hdop = sbufReadU16(src); // hdop
+        gpsSol.llh.lon = sbufReadU32(src);
+        gpsSol.llh.lat = sbufReadU32(src);
+        gpsSol.llh.altCm = sbufReadU32(src); // alt
+        int32_t ned_vel_north = (int32_t)sbufReadU32(src);  // ned_vel_north
+        int32_t ned_vel_east = (int32_t)sbufReadU32(src);   // ned_vel_east
+        gpsSol.groundSpeed = (uint16_t)sqrtf((ned_vel_north * ned_vel_north) + (ned_vel_east * ned_vel_east));
+        (void)sbufReadU32(src);             // ned_vel_down
+        gpsSol.groundCourse = ((uint16_t)sbufReadU16(src) % 360);   // ground_course
+        (void)sbufReadU16(src);             // true_yaw
+        (void)sbufReadU16(src);             // year
+        (void)sbufReadU8(src);              // month
+        (void)sbufReadU8(src);              // day
+        (void)sbufReadU8(src);              // hour
+        (void)sbufReadU8(src);              // min
+        (void)sbufReadU8(src);              // sec
+        GPS_update |= GPS_MSP_UPDATE;        // MSP data signalisation to GPS functions
+        break;
+
     case MSP_SET_RAW_GPS:
         gpsSetFixState(sbufReadU8(src));
         gpsSol.numSat = sbufReadU8(src);

diff --git a/src/main/msp/msp_protocol_v2_common.h b/src/main/msp/msp_protocol_v2_common.h
@@ -20,3 +20,6 @@
 
 #define MSP2_COMMON_SERIAL_CONFIG       0x1009
 #define MSP2_COMMON_SET_SERIAL_CONFIG   0x100A
+
+// Sensors
+#define MSP2_SENSOR_GPS                 0x1F03