Add files to CMakeLists and build fixes.

iNavFlight · Jul 23, 2024 · 78a800b · 78a800b
1 parent 138f5aa
commit 78a800b
Show file tree

Hide file tree

Showing 6 changed files with 33 additions and 196 deletions.
diff --git 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
@@ -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
@@ -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
@@ -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
@@ -27,24 +27,34 @@
 #include <common/crc.h>
 #include <common/utils.h>
 #include <common/maths.h>
-#include <common/mavlink.h>
 #include <build/debug.h>
 
 #include <drivers/gimbal_common.h>
 #include <drivers/headtracker_common.h>
 #include <drivers/serial.h>
 #include <drivers/time.h>
 
-#include <io/gimbal_serial.h>
+#include <io/gimbal_mavlink.h>
 #include <io/serial.h>
 
 #include <rx/rx.h>
 #include <fc/rc_modes.h>
 
 #include <config/parameter_group_ids.h>
 
+#include <telemetry/mavlink.h>
+
 #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,19 +75,20 @@ 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;
 }
 
 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
diff --git a/src/main/io/gimbal_mavlink.h b/src/main/io/gimbal_mavlink.h
@@ -23,18 +23,18 @@
 
 #include "common/time.h"
 #include "drivers/gimbal_common.h"
-#include "drivers/headtracker_common.h"
 
 #ifdef __cplusplus
 extern "C" {
 #endif
 
 #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