From 3ef2c56c2e2b23c14a6593d1f6ec045e19ac9de5 Mon Sep 17 00:00:00 2001 From: Marcelo Bezerra <23555060+mmosca@users.noreply.github.com> Date: Sat, 7 Sep 2024 00:00:58 +0200 Subject: [PATCH 1/3] Small fixes for serial gimbal --- src/main/fc/fc_msp.c | 2 +- src/main/io/gimbal_serial.c | 6 ++++-- src/main/io/headtracker_msp.c | 17 +++++++++++++++-- src/main/io/headtracker_msp.h | 1 + 4 files changed, 21 insertions(+), 5 deletions(-) diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c index 2b544ee449a..e7a7a4aa3a6 100644 --- 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 index 4eec2903947..085590f0880 100644 --- a/src/main/io/gimbal_serial.c +++ b/src/main/io/gimbal_serial.c @@ -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 index 24c409469ed..ad2a8842f7c 100644 --- 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, }; @@ -47,6 +48,9 @@ static headTrackerDevice_t headTrackerMspDevice = { void mspHeadTrackerInit(void) { + for(int i = 0; i < 8;++i) { + DEBUG_SET(DEBUG_HEADTRACKING, i, 0); + } if(headTrackerConfig()->devType == HEADTRACKER_MSP) { headTrackerCommonSetDevice(&headTrackerMspDevice); } @@ -54,7 +58,7 @@ 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++); @@ -62,6 +66,8 @@ void mspHeadTrackerReceiverNewData(uint8_t *data, unsigned int dataSize) DEBUG_SET(DEBUG_HEADTRACKING, 6, dataSize); return; } + isReady = true; + DEBUG_SET(DEBUG_HEADTRACKING, 6, dataSize); headtrackerMspMessage_t *status = (headtrackerMspMessage_t *)data; @@ -81,4 +87,11 @@ headTrackerDevType_e heatTrackerMspGetDeviceType(const headTrackerDevice_t *head return HEADTRACKER_MSP; } + +bool heatTrackerMspIsReady(const headTrackerDevice_t *headTrackerDevice) +{ + UNUSED(headTrackerDevice); + return headTrackerConfig()->devType == HEADTRACKER_MSP && isReady; +} + #endif \ No newline at end of file diff --git a/src/main/io/headtracker_msp.h b/src/main/io/headtracker_msp.h index f767b1c6286..e082d0db8ae 100644 --- 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 From 2ac360336c8d4bd01c5dbe1a46810ac78dfba8b7 Mon Sep 17 00:00:00 2001 From: Marcelo Bezerra <23555060+mmosca@users.noreply.github.com> Date: Sat, 7 Sep 2024 00:03:06 +0200 Subject: [PATCH 2/3] Update headtracker_msp.c --- src/main/io/headtracker_msp.c | 5 +---- 1 file changed, 1 insertion(+), 4 deletions(-) diff --git a/src/main/io/headtracker_msp.c b/src/main/io/headtracker_msp.c index ad2a8842f7c..e53fd3ab207 100644 --- a/src/main/io/headtracker_msp.c +++ b/src/main/io/headtracker_msp.c @@ -48,9 +48,6 @@ static headTrackerDevice_t headTrackerMspDevice = { void mspHeadTrackerInit(void) { - for(int i = 0; i < 8;++i) { - DEBUG_SET(DEBUG_HEADTRACKING, i, 0); - } if(headTrackerConfig()->devType == HEADTRACKER_MSP) { headTrackerCommonSetDevice(&headTrackerMspDevice); } @@ -94,4 +91,4 @@ bool heatTrackerMspIsReady(const headTrackerDevice_t *headTrackerDevice) return headTrackerConfig()->devType == HEADTRACKER_MSP && isReady; } -#endif \ No newline at end of file +#endif From 2511cc31cb0edfa8bb229e11c911e2180c35af3e Mon Sep 17 00:00:00 2001 From: Marcelo Bezerra <23555060+mmosca@users.noreply.github.com> Date: Sat, 7 Sep 2024 00:15:58 +0200 Subject: [PATCH 3/3] Unit test fixes --- src/main/drivers/headtracker_common.c | 4 ---- src/main/drivers/headtracker_common.h | 3 ++- src/main/drivers/time.h | 7 +++++++ src/main/io/gimbal_serial.c | 2 +- src/test/unit/CMakeLists.txt | 4 ++-- src/test/unit/gimbal_serial_unittest.cc | 6 ++++++ 6 files changed, 18 insertions(+), 8 deletions(-) diff --git a/src/main/drivers/headtracker_common.c b/src/main/drivers/headtracker_common.c index feec581da8a..8ea71cd2f66 100644 --- 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 index 8c177e3e0f0..92dac7e61e6 100644 --- 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 @@ -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 index 6901d57c19b..ea30592515e 100644 --- a/src/main/drivers/time.h +++ b/src/main/drivers/time.h @@ -19,6 +19,9 @@ #include +#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 \ No newline at end of file diff --git a/src/main/io/gimbal_serial.c b/src/main/io/gimbal_serial.c index 085590f0880..fd6294c9959 100644 --- 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 = { diff --git a/src/test/unit/CMakeLists.txt b/src/test/unit/CMakeLists.txt index 300721b8f53..2cb53b64ecb 100644 --- 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 index 1f0c47231c6..4ca55e17ee2 100644 --- 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);