Merge pull request #10345 from iNavFlight/mmosca-msp-gimbal-fixes

Small fixes for serial gimbal
iNavFlight · Sep 6, 2024 · 4edba00 · 4edba00
2 parents 044c183 + 2511cc3
commit 4edba00
Show file tree

Hide file tree

Showing 9 changed files with 37 additions and 14 deletions.
diff --git a/src/main/drivers/headtracker_common.c b/src/main/drivers/headtracker_common.c
@@ -157,10 +157,6 @@ void taskUpdateHeadTracker(timeUs_t currentTimeUs)
 #else
 void taskUpdateHeadTracker(timeUs_t currentTimeUs)
 {
-    if (cliMode) {
-        return;
-    }
-
     headTrackerDevice_t *headTrackerDevice = headTrackerCommonDevice();
 
     if(headTrackerDevice) {

diff --git a/src/main/drivers/headtracker_common.h b/src/main/drivers/headtracker_common.h
@@ -23,7 +23,6 @@
 #define HEADTRACKER_RANGE_MIN   -2048
 #define HEADTRACKER_RANGE_MAX   2047
 
-#ifdef USE_HEADTRACKER
 
 #include <stdint.h>
 
@@ -81,6 +80,8 @@ typedef struct headTrackerConfig_s {
     float roll_ratio;
 } headTrackerConfig_t;
 
+#ifdef USE_HEADTRACKER
+
 PG_DECLARE(headTrackerConfig_t, headTrackerConfig);
 
 void headTrackerCommonInit(void);

diff --git a/src/main/drivers/time.h b/src/main/drivers/time.h
@@ -19,6 +19,9 @@
 
 #include <stdint.h>
 
+#ifdef __cplusplus
+extern "C" {
+#endif
 #include "common/time.h"
 
 extern uint32_t usTicks;
@@ -32,3 +35,7 @@ timeUs_t microsISR(void);
 timeMs_t millis(void);
 
 uint32_t ticks(void);
+
+#ifdef __cplusplus
+}
+#endif
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), sbufBytesRemaining(src));
+            mspHeadTrackerReceiverNewData(sbufPtr(src), dataSize);
             break;
 #endif
     }

diff --git a/src/main/io/gimbal_serial.c b/src/main/io/gimbal_serial.c
@@ -63,11 +63,11 @@ static gimbalSerialHtrkState_t headTrackerState = {
     .attitude = {},
     .state = WAITING_HDR1,
 };
+static serialPort_t *headTrackerPort = NULL;
 #endif
 
 #endif
 
-static serialPort_t *headTrackerPort = NULL;
 static serialPort_t *gimbalPort = NULL;
 
 gimbalVTable_t gimbalSerialVTable = {
@@ -116,7 +116,9 @@ bool gimbalSerialIsReady(const gimbalDevice_t *gimbalDevice)
 bool gimbalSerialHasHeadTracker(const gimbalDevice_t *gimbalDevice)
 {
     UNUSED(gimbalDevice);
-    return headTrackerPort || (gimbalSerialConfig()->singleUart && gimbalPort);
+
+    headTrackerDevice_t *htrk = headTrackerCommonDevice();
+    return htrk != NULL && headTrackerCommonIsReady(htrk);
 }
 
 bool gimbalSerialInit(void)
@@ -251,7 +253,7 @@ void gimbalSerialProcess(gimbalDevice_t *gimbalDevice, timeUs_t currentTime)
 #else
     {
 #endif
-        DEBUG_SET(DEBUG_HEADTRACKING, 4, 0);
+        DEBUG_SET(DEBUG_HEADTRACKING, 4, 2);
         // Radio endpoints may need to be adjusted, as it seems ot go a bit
         // bananas at the extremes
         attitude.pan = gimbal_scale12(PWM_RANGE_MIN, PWM_RANGE_MAX, panPWM);

diff --git a/src/main/io/headtracker_msp.c b/src/main/io/headtracker_msp.c
@@ -30,10 +30,11 @@
 
 #include "io/headtracker_msp.h"
 
+static bool isReady = false;
 static headTrackerVTable_t headTrackerMspVTable = {
     .process = NULL,
     .getDeviceType = heatTrackerMspGetDeviceType,
-    .isReady = NULL,
+    .isReady = heatTrackerMspIsReady,
     .isValid = NULL,
 };
 
@@ -54,14 +55,16 @@ void mspHeadTrackerInit(void)
 
 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));
         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;
     }
+    isReady = true;
+    DEBUG_SET(DEBUG_HEADTRACKING, 6, dataSize);
 
     headtrackerMspMessage_t *status = (headtrackerMspMessage_t *)data;
 
@@ -81,4 +84,11 @@ headTrackerDevType_e heatTrackerMspGetDeviceType(const headTrackerDevice_t *head
     return HEADTRACKER_MSP;
 }
 
-#endif
+
+bool heatTrackerMspIsReady(const headTrackerDevice_t *headTrackerDevice)
+{
+    UNUSED(headTrackerDevice);
+    return headTrackerConfig()->devType == HEADTRACKER_MSP && isReady; 
+}
+
+#endif
diff --git a/src/main/io/headtracker_msp.h b/src/main/io/headtracker_msp.h
@@ -40,5 +40,6 @@ void mspHeadTrackerInit(void);
 void mspHeadTrackerReceiverNewData(uint8_t *data, unsigned int dataSize);
 
 headTrackerDevType_e heatTrackerMspGetDeviceType(const headTrackerDevice_t *headTrackerDevice);
+bool heatTrackerMspIsReady(const headTrackerDevice_t *headTrackerDevice);
 
 #endif
diff --git a/src/test/unit/CMakeLists.txt b/src/test/unit/CMakeLists.txt
@@ -40,8 +40,8 @@ set_property(SOURCE osd_unittest.cc PROPERTY definitions OSD_UNIT_TEST USE_MSP_D
 set_property(SOURCE gps_ublox_unittest.cc PROPERTY depends "io/gps_ublox_utils.c")
 set_property(SOURCE gps_ublox_unittest.cc PROPERTY definitions GPS_UBLOX_UNIT_TEST)
 
-set_property(SOURCE gimbal_serial_unittest.cc PROPERTY depends "io/gimbal_serial.c" "drivers/gimbal_common.c" "common/maths.c")
-set_property(SOURCE gimbal_serial_unittest.cc PROPERTY definitions USE_SERIAL_GIMBAL GIMBAL_UNIT_TEST)
+set_property(SOURCE gimbal_serial_unittest.cc PROPERTY depends "io/gimbal_serial.c" "drivers/gimbal_common.c" "common/maths.c" "drivers/headtracker_common.c")
+set_property(SOURCE gimbal_serial_unittest.cc PROPERTY definitions USE_SERIAL_GIMBAL GIMBAL_UNIT_TEST USE_HEADTRACKER)
 
 function(unit_test src)
     get_filename_component(basename ${src} NAME)

diff --git a/src/test/unit/gimbal_serial_unittest.cc b/src/test/unit/gimbal_serial_unittest.cc
@@ -34,6 +34,12 @@ void dumpMemory(uint8_t *mem, int size)
     printf("\n");
 }
 
+extern "C" {
+timeUs_t micros(void) {
+    return 10;
+}
+}
+
 TEST(GimbalSerialTest, TestGimbalSerialScale)
 {
     int16_t res16 = gimbal_scale12(1000, 2000, 2000);