Merge pull request #10342 from iNavFlight/mmosca-msp-gimbal-update

MSP headtracker parsing change
iNavFlight · Sep 5, 2024 · 9a1d8d6 · 9a1d8d6
2 parents d9596a1 + 9dcdb54
commit 9a1d8d6
Show file tree

Hide file tree

Showing 3 changed files with 8 additions and 5 deletions.
diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c
@@ -4127,7 +4127,7 @@ static mspResult_e mspProcessSensorCommand(uint16_t cmdMSP, sbuf_t *src)
 
 #if (defined(USE_HEADTRACKER) && defined(USE_HEADTRACKER_MSP))
         case MSP2_SENSOR_HEADTRACKER:
-            mspHeadTrackerReceiverNewData(sbufPtr(src), dataSize);
+            mspHeadTrackerReceiverNewData(sbufPtr(src), sbufBytesRemaining(src));
             break;
 #endif
     }

diff --git a/src/main/io/headtracker_msp.c b/src/main/io/headtracker_msp.c
@@ -52,11 +52,14 @@ void mspHeadTrackerInit(void)
     }
 }
 
-void mspHeadTrackerReceiverNewData(uint8_t *data, int dataSize)
+void mspHeadTrackerReceiverNewData(uint8_t *data, unsigned int dataSize)
 {
-    if(dataSize != sizeof(headtrackerMspMessage_t)) {
+    if(dataSize >= sizeof(headtrackerMspMessage_t)) {
         SD(fprintf(stderr, "[headTracker]: invalid data size %d\n", dataSize));
-        DEBUG_SET(DEBUG_HEADTRACKING, 7, 1);
+        static int errorCount = 0;
+        DEBUG_SET(DEBUG_HEADTRACKING, 7, errorCount++);
+        DEBUG_SET(DEBUG_HEADTRACKING, 5, (sizeof(headtrackerMspMessage_t)));
+        DEBUG_SET(DEBUG_HEADTRACKING, 6, dataSize);
         return;
     }
 

diff --git a/src/main/io/headtracker_msp.h b/src/main/io/headtracker_msp.h
@@ -37,7 +37,7 @@ typedef struct headtrackerMspMessage_s {
 
 void mspHeadTrackerInit(void);
 
-void mspHeadTrackerReceiverNewData(uint8_t *data, int dataSize);
+void mspHeadTrackerReceiverNewData(uint8_t *data, unsigned int dataSize);
 
 headTrackerDevType_e heatTrackerMspGetDeviceType(const headTrackerDevice_t *headTrackerDevice);