From 78a800bdd53629dc833765a5293f7aea0f9ea302 Mon Sep 17 00:00:00 2001 From: Marcelo Bezerra <23555060+mmosca@users.noreply.github.com> Date: Wed, 24 Jul 2024 00:37:04 +0200 Subject: [PATCH] Add files to CMakeLists and build fixes. --- src/main/CMakeLists.txt | 2 + src/main/drivers/gimbal_common.c | 6 +- src/main/drivers/gimbal_common.h | 8 +- src/main/fc/fc_init.c | 5 +- src/main/io/gimbal_mavlink.c | 202 +++---------------------------- src/main/io/gimbal_mavlink.h | 6 +- 6 files changed, 33 insertions(+), 196 deletions(-) diff --git a/src/main/CMakeLists.txt b/src/main/CMakeLists.txt index b6ff0e10985..9ca2877ae1e 100755 --- a/src/main/CMakeLists.txt +++ b/src/main/CMakeLists.txt @@ -362,6 +362,8 @@ main_sources(COMMON_SRC io/servo_sbus.h io/frsky_osd.c io/frsky_osd.h + io/gimbal_mavlink.c + io/gimbal_mavlink.h io/gimbal_serial.c io/gimbal_serial.h io/headtracker_msp.c diff --git a/src/main/drivers/gimbal_common.c b/src/main/drivers/gimbal_common.c index 2fa75f2331f..2bbfd4f2b19 100644 --- a/src/main/drivers/gimbal_common.c +++ b/src/main/drivers/gimbal_common.c @@ -66,14 +66,14 @@ gimbalDevice_t *gimbalCommonDevice(void) return commonGimbalDevice; } -void gimbalCommonProcess(gimbalDevice_t *gimbalDevice, timeUs_t currentTimeUs) +void gimbalCommonProcess(const gimbalDevice_t *gimbalDevice, timeUs_t currentTimeUs) { if (gimbalDevice && gimbalDevice->vTable->process && gimbalCommonIsReady(gimbalDevice)) { gimbalDevice->vTable->process(gimbalDevice, currentTimeUs); } } -gimbalDevType_e gimbalCommonGetDeviceType(gimbalDevice_t *gimbalDevice) +gimbalDevType_e gimbalCommonGetDeviceType(const gimbalDevice_t *gimbalDevice) { if (!gimbalDevice || !gimbalDevice->vTable->getDeviceType) { return GIMBAL_DEV_UNKNOWN; @@ -82,7 +82,7 @@ gimbalDevType_e gimbalCommonGetDeviceType(gimbalDevice_t *gimbalDevice) return gimbalDevice->vTable->getDeviceType(gimbalDevice); } -bool gimbalCommonIsReady(gimbalDevice_t *gimbalDevice) +bool gimbalCommonIsReady(const gimbalDevice_t *gimbalDevice) { if (gimbalDevice && gimbalDevice->vTable->isReady) { return gimbalDevice->vTable->isReady(gimbalDevice); diff --git a/src/main/drivers/gimbal_common.h b/src/main/drivers/gimbal_common.h index 116876ace28..281ca637e86 100644 --- a/src/main/drivers/gimbal_common.h +++ b/src/main/drivers/gimbal_common.h @@ -51,7 +51,7 @@ typedef struct gimbalDevice_s { // {set,get}PitMode: 0 = OFF, 1 = ON typedef struct gimbalVTable_s { - void (*process)(gimbalDevice_t *gimbalDevice, timeUs_t currentTimeUs); + void (*process)(const gimbalDevice_t *gimbalDevice, timeUs_t currentTimeUs); gimbalDevType_e (*getDeviceType)(const gimbalDevice_t *gimbalDevice); bool (*isReady)(const gimbalDevice_t *gimbalDevice); bool (*hasHeadTracker)(const gimbalDevice_t *gimbalDevice); @@ -89,9 +89,9 @@ void gimbalCommonSetDevice(gimbalDevice_t *gimbalDevice); gimbalDevice_t *gimbalCommonDevice(void); // VTable functions -void gimbalCommonProcess(gimbalDevice_t *gimbalDevice, timeUs_t currentTimeUs); -gimbalDevType_e gimbalCommonGetDeviceType(gimbalDevice_t *gimbalDevice); -bool gimbalCommonIsReady(gimbalDevice_t *gimbalDevice); +void gimbalCommonProcess(const gimbalDevice_t *gimbalDevice, timeUs_t currentTimeUs); +gimbalDevType_e gimbalCommonGetDeviceType(const gimbalDevice_t *gimbalDevice); +bool gimbalCommonIsReady(const gimbalDevice_t *gimbalDevice); void taskUpdateGimbal(timeUs_t currentTimeUs); diff --git a/src/main/fc/fc_init.c b/src/main/fc/fc_init.c index 38ee3eb58d2..f70964cb410 100644 --- a/src/main/fc/fc_init.c +++ b/src/main/fc/fc_init.c @@ -109,6 +109,7 @@ #include "io/displayport_msp_osd.h" #include "io/displayport_srxl.h" #include "io/flashfs.h" +#include "io/gimbal_mavlink.h" #include "io/gimbal_serial.h" #include "io/headtracker_msp.h" #include "io/gps.h" @@ -693,11 +694,13 @@ void init(void) #ifdef USE_GIMBAL gimbalCommonInit(); - #ifdef USE_GIMBAL_SERIAL gimbalSerialInit(); #endif +#ifdef USE_GIMBAL_MAVLINK + gimbalMavlinkInit(); #endif +#endif // USE_GIMBAL #ifdef USE_HEADTRACKER headTrackerCommonInit(); diff --git a/src/main/io/gimbal_mavlink.c b/src/main/io/gimbal_mavlink.c index 0876595b7dd..5b20c8bf60b 100644 --- a/src/main/io/gimbal_mavlink.c +++ b/src/main/io/gimbal_mavlink.c @@ -27,7 +27,6 @@ #include #include #include -#include #include #include @@ -35,7 +34,7 @@ #include #include -#include +#include #include #include @@ -43,8 +42,19 @@ #include +#include + #include "settings_generated.h" +#pragma GCC diagnostic push +#pragma GCC diagnostic ignored "-Wunused-function" +#ifndef MAVLINK_COMM_NUM_BUFFERS +#define MAVLINK_COMM_NUM_BUFFERS 1 +#endif +#include "common/mavlink.h" +#pragma GCC diagnostic pop + + gimbalVTable_t gimbalMavlinkVTable = { .process = gimbalMavlinkProcess, .getDeviceType = gimbalMavlinkGetDeviceType, @@ -65,11 +75,12 @@ gimbalDevType_e gimbalMavlinkGetDeviceType(const gimbalDevice_t *gimbalDevice) bool gimbalMavlinkIsReady(const gimbalDevice_t *gimbalDevice) { - return gimbalPort != NULL && gimbalDevice->vTable != NULL && isMAVLinkTelemetryEnabled(); + return (gimbalCommonGetDeviceType(gimbalDevice) == GIMBAL_DEV_MAVLINK) && isMAVLinkTelemetryEnabled(); } bool gimbalMavlinkHasHeadTracker(const gimbalDevice_t *gimbalDevice) { + UNUSED(gimbalDevice); return false; } @@ -77,7 +88,7 @@ bool gimbalMavlinkInit(void) { if (gimbalMavlinkDetect()) { SD(fprintf(stderr, "Setting gimbal device\n")); - gimbalCommonSetDevice(&serialGimbalDevice); + gimbalCommonSetDevice(&mavlinkGimbalDevice); return true; } @@ -94,12 +105,12 @@ bool gimbalMavlinkDetect(void) return portConfig && gimbalConfig()->gimbalType == GIMBAL_DEV_MAVLINK; } -void gimbalMavlinklProcess(gimbalDevice_t *gimbalDevice, timeUs_t currentTime) +void gimbalMavlinkProcess(const gimbalDevice_t *gimbalDevice, timeUs_t currentTime) { UNUSED(currentTime); UNUSED(gimbalDevice); - if (!gimbalMavlinklIsReady(gimbalDevice)) { + if (!gimbalCommonIsReady(gimbalDevice)) { SD(fprintf(stderr, "[GIMBAL] gimbal not ready...\n")); return; } @@ -140,25 +151,7 @@ void gimbalMavlinklProcess(gimbalDevice_t *gimbalDevice, timeUs_t currentTime) } } -#ifdef USE_HEADTRACKER - if(IS_RC_MODE_ACTIVE(BOXGIMBALHTRK)) { - headTrackerDevice_t *dev = headTrackerCommonDevice(); - if (gimbalCommonHtrkIsEnabled() && dev && headTrackerCommonIsValid(dev)) { - //attitude.pan = headTrackerCommonGetPan(dev); - //attitude.tilt = headTrackerCommonGetTilt(dev); - //attitude.roll = headTrackerCommonGetRoll(dev); - - DEBUG_SET(DEBUG_HEADTRACKING, 4, 1); - } else { - //attitude.pan = constrain(gimbal_scale12(PWM_RANGE_MIN, PWM_RANGE_MAX, PWM_RANGE_MIDDLE + cfg->panTrim), HEADTRACKER_RANGE_MIN, HEADTRACKER_RANGE_MAX); - //attitude.tilt = constrain(gimbal_scale12(PWM_RANGE_MIN, PWM_RANGE_MAX, PWM_RANGE_MIDDLE + cfg->tiltTrim), HEADTRACKER_RANGE_MIN, HEADTRACKER_RANGE_MAX); - //attitude.roll = constrain(gimbal_scale12(PWM_RANGE_MIN, PWM_RANGE_MAX, PWM_RANGE_MIDDLE + cfg->rollTrim), HEADTRACKER_RANGE_MIN, HEADTRACKER_RANGE_MAX); - DEBUG_SET(DEBUG_HEADTRACKING, 4, -1); - } - } else { -#else { -#endif DEBUG_SET(DEBUG_HEADTRACKING, 4, 0); // Radio endpoints may need to be adjusted, as it seems ot go a bit // bananas at the extremes @@ -180,164 +173,3 @@ void gimbalMavlinklProcess(gimbalDevice_t *gimbalDevice, timeUs_t currentTime) //serialEndWrite(gimbalPort); } #endif - -int16_t gimbal2pwm(int16_t value) -{ - int16_t ret = 0; - ret = scaleRange(value, HEADTRACKER_RANGE_MIN, HEADTRACKER_RANGE_MAX, PWM_RANGE_MIN, PWM_RANGE_MAX); - return ret; -} - - -int16_t gimbal_scale12(int16_t inputMin, int16_t inputMax, int16_t value) -{ - int16_t ret = 0; - ret = scaleRange(value, inputMin, inputMax, HEADTRACKER_RANGE_MIN, HEADTRACKER_RANGE_MAX); - return ret; -} - -#ifndef GIMBAL_UNIT_TEST - -#if (defined(USE_HEADTRACKER) && defined(USE_HEADTRACKER_SERIAL)) -static void resetState(gimbalSerialHtrkState_t *state) -{ - state->state = WAITING_HDR1; - state->payloadSize = 0; -} - -static bool checkCrc(gimbalHtkAttitudePkt_t *attitude) -{ - uint8_t *attitudePkt = (uint8_t *)attitude; - uint16_t crc = 0; - - for(uint8_t i = 0; i < sizeof(gimbalHtkAttitudePkt_t) - 2; ++i) { - crc = crc16_ccitt(crc, attitudePkt[i]); - } - - return (attitude->crch == ((crc >> 8) & 0xFF)) && - (attitude->crcl == (crc & 0xFF)); -} - -void gimbalSerialHeadTrackerReceive(uint16_t c, void *data) -{ - static int charCount = 0; - static int pktCount = 0; - static int errorCount = 0; - gimbalSerialHtrkState_t *state = (gimbalSerialHtrkState_t *)data; - uint8_t *payload = (uint8_t *)&(state->attitude); - payload += 2; - - DEBUG_SET(DEBUG_HEADTRACKING, 0, charCount++); - DEBUG_SET(DEBUG_HEADTRACKING, 1, state->state); - - switch(state->state) { - case WAITING_HDR1: - if(c == HTKATTITUDE_SYNC0) { - state->attitude.sync[0] = c; - state->state = WAITING_HDR2; - } - break; - case WAITING_HDR2: - if(c == HTKATTITUDE_SYNC1) { - state->attitude.sync[1] = c; - state->state = WAITING_PAYLOAD; - } else { - resetState(state); - } - break; - case WAITING_PAYLOAD: - payload[state->payloadSize++] = c; - if(state->payloadSize == HEADTRACKER_PAYLOAD_SIZE) - { - state->state = WAITING_CRCH; - } - break; - case WAITING_CRCH: - state->attitude.crch = c; - state->state = WAITING_CRCL; - break; - case WAITING_CRCL: - state->attitude.crcl = c; - if(checkCrc(&(state->attitude))) { - headTrackerDevice.expires = micros() + MAX_HEADTRACKER_DATA_AGE_US; - headTrackerDevice.pan = constrain(state->attitude.pan, HEADTRACKER_RANGE_MIN, HEADTRACKER_RANGE_MAX); - headTrackerDevice.tilt = constrain(state->attitude.tilt, HEADTRACKER_RANGE_MIN, HEADTRACKER_RANGE_MAX); - headTrackerDevice.roll = constrain(state->attitude.roll, HEADTRACKER_RANGE_MIN, HEADTRACKER_RANGE_MAX); - DEBUG_SET(DEBUG_HEADTRACKING, 2, pktCount++); - } else { - DEBUG_SET(DEBUG_HEADTRACKING, 3, errorCount++); - } - resetState(state); - break; - } -} - - -bool gimbalSerialHeadTrackerDetect(void) -{ - bool singleUart = gimbalSerialConfig()->singleUart; - - SD(fprintf(stderr, "[GIMBAL_HTRK]: headtracker Detect...\n")); - serialPortConfig_t *portConfig = singleUart ? NULL : findSerialPortConfig(FUNCTION_GIMBAL_HEADTRACKER); - - if (portConfig) { - SD(fprintf(stderr, "[GIMBAL_HTRK]: found port...\n")); - headTrackerPort = openSerialPort(portConfig->identifier, FUNCTION_GIMBAL_HEADTRACKER, gimbalSerialHeadTrackerReceive, &headTrackerState, - baudRates[portConfig->peripheral_baudrateIndex], MODE_RXTX, SERIAL_NOT_INVERTED); - - if (headTrackerPort) { - SD(fprintf(stderr, "[GIMBAL_HTRK]: port open!\n")); - headTrackerPort->txBuffer = txBuffer; - headTrackerPort->txBufferSize = GIMBAL_SERIAL_BUFFER_SIZE; - headTrackerPort->txBufferTail = 0; - headTrackerPort->txBufferHead = 0; - } else { - SD(fprintf(stderr, "[GIMBAL_HTRK]: port NOT open!\n")); - return false; - } - } - - SD(fprintf(stderr, "[GIMBAL]: gimbalPort: %p headTrackerPort: %p\n", gimbalPort, headTrackerPort)); - return (singleUart && gimbalPort) || headTrackerPort; -} - -bool gimbalSerialHeadTrackerInit(void) -{ - if(headTrackerConfig()->devType == HEADTRACKER_SERIAL) { - if (gimbalSerialHeadTrackerDetect()) { - SD(fprintf(stderr, "Setting gimbal device\n")); - headTrackerCommonSetDevice(&headTrackerDevice); - - return true; - } - } - - return false; -} - -void headtrackerSerialProcess(headTrackerDevice_t *headTrackerDevice, timeUs_t currentTimeUs) -{ - UNUSED(headTrackerDevice); - UNUSED(currentTimeUs); - return; -} - -headTrackerDevType_e headtrackerSerialGetDeviceType(const headTrackerDevice_t *headTrackerDevice) -{ - UNUSED(headTrackerDevice); - return HEADTRACKER_SERIAL; -} - -#else - -void gimbalSerialHeadTrackerReceive(uint16_t c, void *data) -{ - UNUSED(c); - UNUSED(data); -} - -#endif - -#endif - -#endif \ No newline at end of file diff --git a/src/main/io/gimbal_mavlink.h b/src/main/io/gimbal_mavlink.h index ed56979bc30..08236debbaf 100644 --- a/src/main/io/gimbal_mavlink.h +++ b/src/main/io/gimbal_mavlink.h @@ -23,7 +23,6 @@ #include "common/time.h" #include "drivers/gimbal_common.h" -#include "drivers/headtracker_common.h" #ifdef __cplusplus extern "C" { @@ -31,10 +30,11 @@ extern "C" { #ifdef USE_GIMBAL_MAVLINK -bool gimbalMavlinklInit(void); +bool gimbalMavlinkInit(void); bool gimbalMavlinkDetect(void); -void gimbalMavlinkProcess(gimbalDevice_t *gimbalDevice, timeUs_t currentTime); +void gimbalMavlinkProcess(const gimbalDevice_t *gimbalDevice, timeUs_t currentTime); bool gimbalMavlinkIsReady(const gimbalDevice_t *gimbalDevice); +bool gimbalMavlinkHasHeadTracker(const gimbalDevice_t *gimbalDevice); gimbalDevType_e gimbalMavlinkGetDeviceType(const gimbalDevice_t *gimbalDevice); #endif