Walksnail Serial gimbal and headtracker support

.gitignore

@@ -1,10 +1,12 @@
 *.o
 .DS_Store
 *~
+*.swp
 *.uvopt
 *.dep
 *.bak
 *.uvgui.*
+*.ubx
 .project
 .settings
 .cproject
@@ -16,6 +18,8 @@ startup_stm32f10x_md_gcc.s
 cov-int*
 /build/
 /build_SITL/
+/[hs]itl/
+/ninja/
 /obj/
 /patches/
 /tools/

docs/Settings.md

@@ -1462,6 +1462,56 @@ Yaw Iterm is frozen when bank angle is above this threshold [degrees]. This solv
 
 ---
 
+### gimbal_pan_channel
+
+Gimbal pan rc channel index. 0 is no channel.
+
+| Default | Min | Max |
+| --- | --- | --- |
+| 0 | 0 | 32 |
+
+---
+
+### gimbal_roll_channel
+
+Gimbal roll rc channel index. 0 is no channel.
+
+| Default | Min | Max |
+| --- | --- | --- |
+| 0 | 0 | 32 |
+
+---
+
+### gimbal_sensitivity
+
+Gimbal sensitivity is similar to gain and will affect how quickly the gimbal will react.
+
+| Default | Min | Max |
+| --- | --- | --- |
+| 0 | -16 | 15 |
+
+---
+
+### gimbal_serial_single_uart
+
+Gimbal serial and headtracker device share same UART. FC RX goes to headtracker device, FC TX goes to gimbal.
+
+| Default | Min | Max |
+| --- | --- | --- |
+| OFF | OFF | ON |
+
+---
+
+### gimbal_tilt_channel
+
+Gimbal tilt rc channel index. 0 is no channel.
+
+| Default | Min | Max |
+| --- | --- | --- |
+| 0 | 0 | 32 |
+
+---
+
 ### gps_auto_baud
 
 Automatic configuration of GPS baudrate(The specified baudrate in configured in ports will be used) when used with UBLOX GPS
@@ -1772,6 +1822,46 @@ This setting limits yaw rotation rate that HEADING_HOLD controller can request f
 
 ---
 
+### headtracker_pan_ratio
+
+Head pan movement vs camera movement ratio
+
+| Default | Min | Max |
+| --- | --- | --- |
+| 1 | 0 | 5 |
+
+---
+
+### headtracker_roll_ratio
+
+Head roll movement vs camera movement ratio
+
+| Default | Min | Max |
+| --- | --- | --- |
+| 1 | 0 | 5 |
+
+---
+
+### headtracker_tilt_ratio
+
+Head tilt movement vs camera movement ratio
+
+| Default | Min | Max |
+| --- | --- | --- |
+| 1 | 0 | 5 |
+
+---
+
+### headtracker_type
+
+Type of headtrackr dervice
+
+| Default | Min | Max |
+| --- | --- | --- |
+| NONE |  |  |
+
+---
+
 ### hott_alarm_sound_interval
 
 Battery alarm delay in seconds for Hott telemetry

src/main/CMakeLists.txt

@@ -177,6 +177,10 @@ main_sources(COMMON_SRC
     drivers/flash_m25p16.h
     drivers/flash_w25n01g.c
     drivers/flash_w25n01g.h
+    drivers/gimbal_common.h
+    drivers/gimbal_common.c
+    drivers/headtracker_common.h
+    drivers/headtracker_common.c
     drivers/io.c
     drivers/io.h
     drivers/io_pcf8574.c
@@ -356,6 +360,10 @@ main_sources(COMMON_SRC
     io/servo_sbus.h
     io/frsky_osd.c
     io/frsky_osd.h
+    io/gimbal_serial.c
+    io/gimbal_serial.h
+    io/headtracker_msp.c
+    io/headtracker_msp.h
     io/osd_dji_hd.c
     io/osd_dji_hd.h
     io/lights.c

src/main/build/debug.h

@@ -21,6 +21,8 @@
 #include <stdint.h>
 #include <stdbool.h>
 
+#include "platform.h"
+
 #define DEBUG32_VALUE_COUNT 8
 extern int32_t debug[DEBUG32_VALUE_COUNT];
 extern uint8_t debugMode;
@@ -73,5 +75,12 @@ typedef enum {
     DEBUG_LANDING,
     DEBUG_POS_EST,
     DEBUG_ADAPTIVE_FILTER,
+    DEBUG_HEADTRACKING,
     DEBUG_COUNT
 } debugType_e;
+
+#ifdef SITL_BUILD
+#define SD(X) (X)
+#else
+#define SD(X)
+#endif

src/main/config/parameter_group_ids.h

@@ -126,7 +126,10 @@
 #define PG_FW_AUTOLAND_CONFIG 1036
 #define PG_FW_AUTOLAND_APPROACH_CONFIG 1037
 #define PG_OSD_CUSTOM_ELEMENTS_CONFIG 1038
-#define PG_INAV_END PG_OSD_CUSTOM_ELEMENTS_CONFIG
+#define PG_GIMBAL_CONFIG 1039
+#define PG_GIMBAL_SERIAL_CONFIG 1040
+#define PG_HEADTRACKER_CONFIG 1041
+#define PG_INAV_END PG_HEADTRACKER_CONFIG
 
 // OSD configuration (subject to change)
 //#define PG_OSD_FONT_CONFIG 2047

src/main/drivers/gimbal_common.c

@@ -0,0 +1,117 @@
+/*
+ * This file is part of INAV.
+ *
+ * Cleanflight is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * Cleanflight is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with INAV.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#include "platform.h"
+
+#ifdef USE_SERIAL_GIMBAL
+
+#include <stdio.h>
+#include <stdint.h>
+#include <stdlib.h>
+
+#include <build/debug.h>
+#include <config/parameter_group_ids.h>
+
+#include "common/time.h"
+
+#include "fc/cli.h"
+
+#include "drivers/gimbal_common.h"
+
+
+PG_REGISTER(gimbalConfig_t, gimbalConfig, PG_GIMBAL_CONFIG, 0);
+
+
+static gimbalDevice_t *commonGimbalDevice = NULL;
+
+void gimbalCommonInit(void)
+{
+}
+
+void gimbalCommonSetDevice(gimbalDevice_t *gimbalDevice)
+{
+    SD(fprintf(stderr, "[GIMBAL]: device added %p\n", gimbalDevice));
+    commonGimbalDevice = gimbalDevice;
+}
+
+gimbalDevice_t *gimbalCommonDevice(void)
+{
+    return commonGimbalDevice;
+}
+
+void gimbalCommonProcess(gimbalDevice_t *gimbalDevice, timeUs_t currentTimeUs)
+{
+    if (gimbalDevice && gimbalDevice->vTable->process && gimbalCommonIsReady(gimbalDevice)) {
+        gimbalDevice->vTable->process(gimbalDevice, currentTimeUs);
+    }
+}
+
+gimbalDevType_e gimbalCommonGetDeviceType(gimbalDevice_t *gimbalDevice)
+{
+    if (!gimbalDevice || !gimbalDevice->vTable->getDeviceType) {
+        return GIMBAL_DEV_UNKNOWN;
+    }
+
+    return gimbalDevice->vTable->getDeviceType(gimbalDevice);
+}
+
+bool gimbalCommonIsReady(gimbalDevice_t *gimbalDevice)
+{
+    if (gimbalDevice && gimbalDevice->vTable->isReady) {
+        return gimbalDevice->vTable->isReady(gimbalDevice);
+    }
+    return false;
+}
+
+#ifdef GIMBAL_UNIT_TEST
+void taskUpdateGimbal(timeUs_t currentTimeUs)
+{
+}
+#else
+void taskUpdateGimbal(timeUs_t currentTimeUs)
+{
+    if (cliMode) {
+        return;
+    }
+
+    gimbalDevice_t *gimbalDevice = gimbalCommonDevice();
+
+    if(gimbalDevice) {
+        gimbalCommonProcess(gimbalDevice, currentTimeUs);
+    }
+}
+#endif
+
+// TODO: check if any gimbal types are enabled
+bool gimbalCommonIsEnabled(void)
+{
+    return true;
+}
+
+
+bool gimbalCommonHtrkIsEnabled(void)
+{
+    const gimbalDevice_t *dev = gimbalCommonDevice();
+    if(dev && dev->vTable->hasHeadTracker) {
+        bool ret = dev->vTable->hasHeadTracker(dev);
+        return ret;
+    }
+
+    return false;
+}
+
+#endif

src/main/drivers/gimbal_common.h

@@ -0,0 +1,97 @@
+/*
+ * This file is part of INAV.
+ *
+ * Cleanflight is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * Cleanflight is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with INAV.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#pragma once
+
+#include "platform.h"
+
+#ifdef USE_SERIAL_GIMBAL
+
+#include <stdint.h>
+
+#include "config/feature.h"
+#include "common/time.h"
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+typedef enum {
+    GIMBAL_DEV_UNSUPPORTED = 0,
+    GIMBAL_DEV_SERIAL,
+    GIMBAL_DEV_UNKNOWN=0xFF
+} gimbalDevType_e;
+
+
+struct gimbalVTable_s;
+
+typedef struct gimbalDevice_s {
+    const struct gimbalVTable_s *vTable;
+} gimbalDevice_t;
+
+// {set,get}BandAndChannel: band and channel are 1 origin
+// {set,get}PowerByIndex: 0 = Power OFF, 1 = device dependent
+// {set,get}PitMode: 0 = OFF, 1 = ON
+
+typedef struct gimbalVTable_s {
+    void (*process)(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);
+} gimbalVTable_t;
+
+
+typedef struct gimbalConfig_s {
+    uint8_t panChannel;
+    uint8_t tiltChannel;
+    uint8_t rollChannel;
+    uint8_t sensitivity;
+} gimbalConfig_t;
+
+PG_DECLARE(gimbalConfig_t, gimbalConfig);
+
+typedef enum {
+    GIMBAL_MODE_FOLLOW = 0,
+    GIMBAL_MODE_TILT_LOCK = (1<<0),
+    GIMBAL_MODE_ROLL_LOCK = (1<<1),
+    GIMBAL_MODE_PAN_LOCK  = (1<<2),
+} gimbal_htk_mode_e;
+
+#define GIMBAL_MODE_DEFAULT             GIMBAL_MODE_FOLLOW
+#define GIMBAL_MODE_TILT_ROLL_LOCK      (GIMBAL_MODE_TILT_LOCK | GIMBAL_MODE_ROLL_LOCK)
+#define GIMBAL_MODE_PAN_TILT_ROLL_LOCK  (GIMBAL_MODE_TILT_LOCK | GIMBAL_MODE_ROLL_LOCK | GIMBAL_MODE_PAN_LOCK)
+
+void gimbalCommonInit(void);
+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 taskUpdateGimbal(timeUs_t currentTimeUs);
+
+bool gimbalCommonIsEnabled(void);
+bool gimbalCommonHtrkIsEnabled(void);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif