Mavlink gimbal support

docs/Settings.md

@@ -1622,6 +1622,16 @@ Trim gimbal tilt center position.
 
 ---
 
+### gimbal_type
+
+Type of gimbval dervice
+
+| Default | Min | Max |
+| --- | --- | --- |
+| SERIAL |  |  |
+
+---
+
 ### gps_auto_baud
 
 Automatic configuration of GPS baudrate(The specified baudrate in configured in ports will be used) when used with UBLOX GPS

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

src/main/drivers/gimbal_common.c

@@ -17,7 +17,7 @@
 
 #include "platform.h"
 
-#ifdef USE_SERIAL_GIMBAL
+#ifdef USE_GIMBAL
 
 #include <stdio.h>
 #include <stdint.h>
@@ -45,7 +45,8 @@ PG_RESET_TEMPLATE(gimbalConfig_t, gimbalConfig,
     .sensitivity = SETTING_GIMBAL_SENSITIVITY_DEFAULT,
     .panTrim = SETTING_GIMBAL_PAN_TRIM_DEFAULT,
     .tiltTrim = SETTING_GIMBAL_TILT_TRIM_DEFAULT,
-    .rollTrim = SETTING_GIMBAL_ROLL_TRIM_DEFAULT
+    .rollTrim = SETTING_GIMBAL_ROLL_TRIM_DEFAULT,
+    .gimbalType = SETTING_GIMBAL_TYPE_DEFAULT,
 );
 
 static gimbalDevice_t *commonGimbalDevice = NULL;
@@ -65,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;
@@ -81,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);
@@ -111,7 +112,7 @@ void taskUpdateGimbal(timeUs_t currentTimeUs)
 // TODO: check if any gimbal types are enabled
 bool gimbalCommonIsEnabled(void)
 {
-    return true;
+    return gimbalCommonDevice() != NULL;;
 }
 
 

src/main/drivers/gimbal_common.h

@@ -19,7 +19,7 @@
 
 #include "platform.h"
 
-#ifdef USE_SERIAL_GIMBAL
+#ifdef USE_GIMBAL
 
 #include <stdint.h>
 
@@ -33,6 +33,7 @@ extern "C" {
 typedef enum {
     GIMBAL_DEV_UNSUPPORTED = 0,
     GIMBAL_DEV_SERIAL,
+    GIMBAL_DEV_MAVLINK,
     GIMBAL_DEV_UNKNOWN=0xFF
 } gimbalDevType_e;
 
@@ -41,6 +42,7 @@ struct gimbalVTable_s;
 
 typedef struct gimbalDevice_s {
     const struct gimbalVTable_s *vTable;
+    // for OSD display
     int16_t currentPanPWM;
 } gimbalDevice_t;
 
@@ -49,10 +51,11 @@ 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);
+    // Used by OSD pan compensation
     int16_t (*getGimbalPanPWM)(const gimbalDevice_t *gimbalDevice);
 } gimbalVTable_t;
 
@@ -65,6 +68,7 @@ typedef struct gimbalConfig_s {
     uint16_t panTrim;
     uint16_t tiltTrim;
     uint16_t rollTrim;
+    gimbalDevType_e gimbalType;
 } gimbalConfig_t;
 
 PG_DECLARE(gimbalConfig_t, gimbalConfig);
@@ -85,16 +89,17 @@ 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);
 
 bool gimbalCommonIsEnabled(void);
 bool gimbalCommonHtrkIsEnabled(void);
 
+// For OSD pan compensation
 int16_t gimbalCommonGetPanPwm(const gimbalDevice_t *gimbalDevice);
 
 #ifdef __cplusplus

src/main/drivers/headtracker_common.h

@@ -33,9 +33,6 @@
 #include "config/feature.h"
 
 
-#ifdef __cplusplus
-extern "C" {
-#endif
 
 typedef enum {
     HEADTRACKER_NONE   = 0,
@@ -84,6 +81,10 @@ typedef struct headTrackerConfig_s {
 
 PG_DECLARE(headTrackerConfig_t, headTrackerConfig);
 
+#ifdef __cplusplus
+extern "C" {
+#endif
+
 void headTrackerCommonInit(void);
 void headTrackerCommonSetDevice(headTrackerDevice_t *headTrackerDevice);
 headTrackerDevice_t *headTrackerCommonDevice(void);
@@ -113,4 +114,4 @@ bool headtrackerCommonIsEnabled(void);
 }
 #endif
 
-#endif
+#endif

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"
@@ -695,11 +696,15 @@ void init(void)
     initDShotCommands();
 #endif
 
-#ifdef USE_SERIAL_GIMBAL
+#ifdef USE_GIMBAL
     gimbalCommonInit();
-    // Needs to be called before gimbalSerialHeadTrackerInit
+#ifdef USE_GIMBAL_SERIAL
     gimbalSerialInit();
 #endif
+#ifdef USE_GIMBAL_MAVLINK
+    gimbalMavlinkInit();
+#endif
+#endif // USE_GIMBAL
 
 #ifdef USE_HEADTRACKER
     headTrackerCommonInit();

src/main/fc/fc_msp_box.c

@@ -366,7 +366,7 @@ void initActiveBoxIds(void)
     ADD_ACTIVE_BOX(BOXMIXERTRANSITION);
 #endif
 
-#ifdef USE_SERIAL_GIMBAL
+#ifdef USE_GIMBAL
     if (gimbalCommonIsEnabled()) {
         ADD_ACTIVE_BOX(BOXGIMBALTLOCK);
         ADD_ACTIVE_BOX(BOXGIMBALRLOCK);
@@ -452,7 +452,7 @@ void packBoxModeFlags(boxBitmask_t * mspBoxModeFlags)
 #endif
     CHECK_ACTIVE_BOX(IS_ENABLED(IS_RC_MODE_ACTIVE(BOXANGLEHOLD)),       BOXANGLEHOLD);
 
-#ifdef USE_SERIAL_GIMBAL
+#ifdef USE_GIMBAL
     if(IS_RC_MODE_ACTIVE(BOXGIMBALCENTER)) {
         CHECK_ACTIVE_BOX(IS_ENABLED(IS_RC_MODE_ACTIVE(BOXGIMBALCENTER)), BOXGIMBALCENTER);
 #ifdef USE_HEADTRACKER

src/main/fc/fc_tasks.c

@@ -436,7 +436,7 @@ void fcTasksInit(void)
     setTaskEnabled(TASK_SMARTPORT_MASTER, true);
 #endif
 
-#ifdef USE_SERIAL_GIMBAL
+#ifdef USE_GIMBAL
     setTaskEnabled(TASK_GIMBAL, true);
 #endif
 
@@ -724,7 +724,7 @@ cfTask_t cfTasks[TASK_COUNT] = {
     },
 #endif
 
-#ifdef USE_SERIAL_GIMBAL
+#ifdef USE_GIMBAL
     [TASK_GIMBAL] = {
         .taskName = "GIMBAL",
         .taskFunc = taskUpdateGimbal,

src/main/fc/settings.yaml

@@ -198,6 +198,9 @@ tables:
   - name: headtracker_dev_type
     values: ["NONE", "SERIAL", "MSP"]
     enum: headTrackerDevType_e
+  - name: gimbal_dev_type
+    values: ["NONE", "SERIAL", "MAVLINK"]
+    enum: gimbalDevType_e
   - name: mavlink_radio_type
     values: ["GENERIC", "ELRS", "SIK"]
     enum: mavlinkRadio_e
@@ -3229,7 +3232,6 @@ groups:
         table: mavlink_radio_type
         default_value: "GENERIC"
         type: uint8_t
-
   - name: PG_OSD_CONFIG
     type: osdConfig_t
     headers: ["io/osd.h", "drivers/osd.h"]
@@ -4281,8 +4283,14 @@ groups:
   - name: PG_GIMBAL_CONFIG
     type: gimbalConfig_t
     headers: ["drivers/gimbal_common.h"]
-    condition: USE_SERIAL_GIMBAL
+    condition: USE_GIMBAL
     members:
+      - name: gimbal_type
+        field: gimbalType
+        description: "Type of gimbval dervice"
+        default_value: "SERIAL"
+        table: gimbal_dev_type
+        type: uint8_t
       - name: gimbal_pan_channel
         description: "Gimbal pan rc channel index. 0 is no channel."
         default_value: 0
@@ -4328,7 +4336,7 @@ groups:
   - name: PG_GIMBAL_SERIAL_CONFIG
     type: gimbalSerialConfig_t
     headers: ["io/gimbal_serial.h"]
-    condition: USE_SERIAL_GIMBAL
+    condition: USE_GIMBAL_SERIAL
     members:
     - name: gimbal_serial_single_uart
       description: "Gimbal serial and headtracker device share same UART. FC RX goes to headtracker device, FC TX goes to gimbal."