diff --git a/mk/source.mk b/mk/source.mk
index f1536360c83..ebf5f1e69ae 100644
--- a/mk/source.mk
+++ b/mk/source.mk
@@ -12,6 +12,7 @@ PG_SRC = \
pg/displayport_profiles.c \
pg/dyn_notch.c \
pg/flash.c \
+ pg/gimbal.c \
pg/gps.c \
pg/gps_lap_timer.c \
pg/gps_rescue.c \
@@ -248,6 +249,7 @@ COMMON_SRC = \
io/displayport_crsf.c \
io/displayport_hott.c \
io/frsky_osd.c \
+ io/gimbal_control.c \
io/rcdevice_cam.c \
io/rcdevice.c \
io/gps.c \
diff --git a/src/main/build/debug.c b/src/main/build/debug.c
index 3a34d97c1bf..40bcfbcf864 100644
--- a/src/main/build/debug.c
+++ b/src/main/build/debug.c
@@ -120,4 +120,5 @@ const char * const debugModeNames[DEBUG_COUNT] = {
[DEBUG_S_TERM] = "S_TERM",
[DEBUG_SPA] = "SPA",
[DEBUG_TASK] = "TASK",
+ [DEBUG_GIMBAL] = "GIMBAL",
};
diff --git a/src/main/build/debug.h b/src/main/build/debug.h
index 08906437547..dcc6f8be991 100644
--- a/src/main/build/debug.h
+++ b/src/main/build/debug.h
@@ -122,6 +122,7 @@ typedef enum {
DEBUG_S_TERM,
DEBUG_SPA,
DEBUG_TASK,
+ DEBUG_GIMBAL,
DEBUG_COUNT
} debugType_e;
diff --git a/src/main/cli/settings.c b/src/main/cli/settings.c
index 67cd4386614..bc2fa391aaf 100644
--- a/src/main/cli/settings.c
+++ b/src/main/cli/settings.c
@@ -87,6 +87,7 @@
#include "pg/displayport_profiles.h"
#include "pg/dyn_notch.h"
#include "pg/flash.h"
+#include "pg/gimbal.h"
#include "pg/gyrodev.h"
#include "pg/max7456.h"
#include "pg/mco.h"
@@ -1854,6 +1855,26 @@ const clivalue_t valueTable[] = {
{ "box_user_3_name", VAR_UINT8 | HARDWARE_VALUE | MODE_STRING, .config.string = { 1, MAX_BOX_USER_NAME_LENGTH, STRING_FLAGS_NONE }, PG_MODE_ACTIVATION_CONFIG, offsetof(modeActivationConfig_t, box_user_names[2]) },
{ "box_user_4_name", VAR_UINT8 | HARDWARE_VALUE | MODE_STRING, .config.string = { 1, MAX_BOX_USER_NAME_LENGTH, STRING_FLAGS_NONE }, PG_MODE_ACTIVATION_CONFIG, offsetof(modeActivationConfig_t, box_user_names[3]) },
#endif
+
+#if defined(USE_GIMBAL)
+ { "gimbal_roll_rc_gain", VAR_INT8 | MASTER_VALUE, .config.minmaxUnsigned = { -100, 100 }, PG_GIMBAL_TRACK_CONFIG, offsetof(gimbalTrackConfig_t, gimbal_roll_rc_gain) },
+ { "gimbal_pitch_rc_thr_gain", VAR_INT8 | MASTER_VALUE, .config.minmaxUnsigned = { -100, 100 }, PG_GIMBAL_TRACK_CONFIG, offsetof(gimbalTrackConfig_t, gimbal_pitch_rc_thr_gain) },
+ { "gimbal_pitch_rc_low_gain", VAR_INT8 | MASTER_VALUE, .config.minmaxUnsigned = { -100, 100 }, PG_GIMBAL_TRACK_CONFIG, offsetof(gimbalTrackConfig_t, gimbal_pitch_rc_low_gain) },
+ { "gimbal_pitch_rc_high_gain", VAR_INT8 | MASTER_VALUE, .config.minmaxUnsigned = { -100, 100 }, PG_GIMBAL_TRACK_CONFIG, offsetof(gimbalTrackConfig_t, gimbal_pitch_rc_high_gain) },
+ { "gimbal_yaw_rc_gain", VAR_INT8 | MASTER_VALUE, .config.minmaxUnsigned = { -100, 100 }, PG_GIMBAL_TRACK_CONFIG, offsetof(gimbalTrackConfig_t, gimbal_yaw_rc_gain) },
+ { "gimbal_roll_gain", VAR_INT8 | MASTER_VALUE, .config.minmaxUnsigned = { -100, 100 }, PG_GIMBAL_TRACK_CONFIG, offsetof(gimbalTrackConfig_t, gimbal_roll_gain) },
+ { "gimbal_roll_offset", VAR_INT8 | MASTER_VALUE, .config.minmaxUnsigned = { -100, 100 }, PG_GIMBAL_TRACK_CONFIG, offsetof(gimbalTrackConfig_t, gimbal_roll_offset) },
+ { "gimbal_roll_limit", VAR_INT8 | MASTER_VALUE, .config.minmaxUnsigned = { -100, 100 }, PG_GIMBAL_TRACK_CONFIG, offsetof(gimbalTrackConfig_t, gimbal_roll_limit) },
+ { "gimbal_pitch_gain", VAR_INT8 | MASTER_VALUE, .config.minmaxUnsigned = { -100, 100 }, PG_GIMBAL_TRACK_CONFIG, offsetof(gimbalTrackConfig_t, gimbal_pitch_gain) },
+ { "gimbal_pitch_offset", VAR_INT8 | MASTER_VALUE, .config.minmaxUnsigned = { -100, 100 }, PG_GIMBAL_TRACK_CONFIG, offsetof(gimbalTrackConfig_t, gimbal_pitch_offset) },
+ { "gimbal_pitch_low_limit", VAR_INT8 | MASTER_VALUE, .config.minmaxUnsigned = { -100, 100 }, PG_GIMBAL_TRACK_CONFIG, offsetof(gimbalTrackConfig_t, gimbal_pitch_low_limit) },
+ { "gimbal_pitch_high_limit", VAR_INT8 | MASTER_VALUE, .config.minmaxUnsigned = { -100, 100 }, PG_GIMBAL_TRACK_CONFIG, offsetof(gimbalTrackConfig_t, gimbal_pitch_high_limit) },
+ { "gimbal_yaw_gain", VAR_INT8 | MASTER_VALUE, .config.minmaxUnsigned = { -100, 100 }, PG_GIMBAL_TRACK_CONFIG, offsetof(gimbalTrackConfig_t, gimbal_yaw_gain) },
+ { "gimbal_yaw_offset", VAR_INT8 | MASTER_VALUE, .config.minmaxUnsigned = { -100, 100 }, PG_GIMBAL_TRACK_CONFIG, offsetof(gimbalTrackConfig_t, gimbal_yaw_offset) },
+ { "gimbal_yaw_limit", VAR_INT8 | MASTER_VALUE, .config.minmaxUnsigned = { -100, 100 }, PG_GIMBAL_TRACK_CONFIG, offsetof(gimbalTrackConfig_t, gimbal_yaw_limit) },
+ { "gimbal_stabilisation", VAR_INT8 | MASTER_VALUE, .config.minmaxUnsigned = { 0, 7 }, PG_GIMBAL_TRACK_CONFIG, offsetof(gimbalTrackConfig_t, gimbal_stabilisation) },
+ { "gimbal_sensitivity", VAR_INT8 | MASTER_VALUE, .config.minmaxUnsigned = { -16, 15 }, PG_GIMBAL_TRACK_CONFIG, offsetof(gimbalTrackConfig_t, gimbal_sensitivity) },
+#endif
};
const uint16_t valueTableEntryCount = ARRAYLEN(valueTable);
diff --git a/src/main/fc/init.c b/src/main/fc/init.c
index b2ba5330ab7..e5406da28af 100644
--- a/src/main/fc/init.c
+++ b/src/main/fc/init.c
@@ -113,6 +113,7 @@
#include "io/displayport_msp.h"
#include "io/flashfs.h"
#include "io/gimbal.h"
+#include "io/gimbal_control.h"
#include "io/gps.h"
#include "io/ledstrip.h"
#include "io/pidaudio.h"
@@ -865,6 +866,10 @@ void init(void)
#endif // VTX_CONTROL
+#ifdef USE_GIMBAL
+ gimbalInit();
+#endif
+
batteryInit(); // always needs doing, regardless of features.
#ifdef USE_RCDEVICE
diff --git a/src/main/fc/rc_controls.c b/src/main/fc/rc_controls.c
index 00246035bed..f5620c5d4b5 100644
--- a/src/main/fc/rc_controls.c
+++ b/src/main/fc/rc_controls.c
@@ -48,6 +48,7 @@
#include "io/beeper.h"
#include "io/usb_cdc_hid.h"
#include "io/dashboard.h"
+#include "io/gimbal_control.h"
#include "io/gps.h"
#include "io/vtx_control.h"
diff --git a/src/main/fc/tasks.c b/src/main/fc/tasks.c
index 61ea391278f..33a08755967 100644
--- a/src/main/fc/tasks.c
+++ b/src/main/fc/tasks.c
@@ -64,6 +64,7 @@
#include "io/beeper.h"
#include "io/dashboard.h"
#include "io/flashfs.h"
+#include "io/gimbal_control.h"
#include "io/gps.h"
#include "io/ledstrip.h"
#include "io/piniobox.h"
@@ -452,6 +453,9 @@ task_attribute_t task_attributes[TASK_COUNT] = {
[TASK_RC_STATS] = DEFINE_TASK("RC_STATS", NULL, NULL, rcStatsUpdate, TASK_PERIOD_HZ(100), TASK_PRIORITY_LOW),
#endif
+#ifdef USE_GIMBAL
+ [TASK_GIMBAL] = DEFINE_TASK("GIMBAL", NULL, NULL, gimbalUpdate, TASK_PERIOD_HZ(100), TASK_PRIORITY_MEDIUM),
+#endif
};
task_t *getTask(unsigned taskId)
@@ -631,4 +635,8 @@ void tasksInit(void)
#ifdef USE_RC_STATS
setTaskEnabled(TASK_RC_STATS, true);
#endif
+
+#ifdef USE_GIMBAL
+ setTaskEnabled(TASK_GIMBAL, true);
+#endif
}
diff --git a/src/main/io/gimbal_control.c b/src/main/io/gimbal_control.c
new file mode 100644
index 00000000000..1c618c7f6b0
--- /dev/null
+++ b/src/main/io/gimbal_control.c
@@ -0,0 +1,329 @@
+/*
+ * This file is part of Betaflight.
+ *
+ * Betaflight is free software. You can redistribute this software
+ * and/or modify this software 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.
+ *
+ * Betaflight 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 this software.
+ *
+ * If not, see .
+ */
+
+#include
+#include
+#include
+#include
+#include
+
+#include "platform.h"
+
+#if defined(USE_GIMBAL)
+
+#include "build/debug.h"
+
+#include "common/crc.h"
+#include "common/maths.h"
+
+#include "drivers/time.h"
+
+#include "fc/rc_controls.h"
+
+#include "io/serial.h"
+
+#include "pg/pg.h"
+#include "pg/pg_ids.h"
+#include "pg/gimbal.h"
+
+#include "rx/rx.h"
+
+#include "scheduler/scheduler.h"
+
+/* The setting below accommodate both -ve and +ve setting so the gimbal can be mounted either way up.
+ *
+ * |---------------------------|-----|------------|----------------------------------------------------------------------------------------|
+ * | Setting | Def | Range | Purpose |
+ * |---------------------------|-----|------------|----------------------------------------------------------------------------------------|
+ * | gimbal_roll_rc_gain | 40 | -100 - 100 | Adjusts amount of roll in % from RC roll channel |
+ * | gimbal_pitch_rc_thr_gain | 10 | -100 - 100 | Adjusts amount of pitch increase for input in % from RC throttle channel |
+ * | gimbal_pitch_rc_low_gain | 10 | -100 - 100 | Adjusts amount of pitch increase for low pitch input in % from RC pitch channel |
+ * | gimbal_pitch_rc_high_gain | -20 | -100 - 100 | Adjusts amount of pitch decrease for high pitch input in % from RC pitch channel |
+ * | gimbal_yaw_rc_gain | 20 | -100 - 100 | Adjusts amount of yaw in % from RC yaw channel |
+ * | gimbal_roll_gain | 100 | -100 - 100 | Adjusts amount of roll in % |
+ * | gimbal_roll_offset | 0 | -100 - 100 | Adjust roll position for neutral roll input |
+ * | gimbal_roll_limit | 100 | 0 - 100 | Adjusts roll range as a % of maximum |
+ * | gimbal_pitch_gain | 50 | -100 - 100 | Adjusts amount of pitch in % |
+ * | gimbal_pitch_offset | -10 | -100 - 100 | Adjust pitch position for neutral pitch input |
+ * | gimbal_pitch_low_limit | 100 | 0 - 100 | Adjusts low pitch range as a % of maximum |
+ * | gimbal_pitch_high_limit | 100 | 0 - 100 | Adjusts high pitch range as a % of maximum |
+ * | gimbal_yaw_gain | 50 | -100 - 100 | Adjusts amount of yaw in % |
+ * | gimbal_yaw_offset | 0 | -100 - 100 | Adjust yaw position for neutral yaw input |
+ * | gimbal_yaw_limit | 100 | 0 - 100 | Adjusts yaw range as a % of maximum |
+ * | gimbal_stabilisation | 0 | 0 - 7 | 0 no stabilisation, 1 pitch stabilisation, 2 roll/pitch stabilisation, 3 - 7 reserved |
+ * | gimbal_sensitivity | 15 | -16 - 15 | With higher values camera more rigidly tracks quad motion |
+ * |---------------------------|-----|------------|----------------------------------------------------------------------------------------|
+ *
+ * To enable the gimbal control on a port set bit 18 thus:
+ *
+ * serial 262144 115200 57600 0 115200
+ */
+
+#define GIMBAL_CMD_NONE 0x00 //empty order
+#define GIMBAL_CMD_ACCE_CALIB 0x01 //Acceleration Calibration
+#define GIMBAL_CMD_GYRO_CALIB 0x02 //Gyro Calibration
+#define GIMBAL_CMD_MAGN_CALIB 0x03 //Calibration of magnetometers
+#define GIMBAL_CMD_AHRS_GZERO 0x04 //Zeroing the attitude angle
+#define GIMBAL_CMD_QUIT_CALIB 0x05 //Exit current calibration
+
+// Command Code Return Macro Definition
+#define GIMBAL_STATUS_ERR 0x80
+
+// This packet is sent to control the gimbal mode, sensivity and position
+#define GIMBAL_CMD_S 0x5AA5
+typedef struct {
+ uint16_t opcode;
+ unsigned mode:3; // Mode [0~7] Only 0 1 2 modes are supported
+ signed sens:5; // Sensitivity [-16~15]
+ unsigned padding:4;
+ signed roll:12; // Roll angle [-2048~2047] - ±180deg
+ signed pitch:12; // Pich angle [-2048~2047] - ±180deg
+ signed yaw:12; // Yaw angle [-2048~2047] - ±180deg
+ uint16_t crc;
+} __attribute__ ((__packed__)) gimbalCmd_t;
+
+/* This packet is sent by the user to the head chase to realize the head
+ * chase calibration and reset function
+ */
+#define GIMBAL_OPCODE_L 0x6EC5
+typedef struct {
+ uint16_t opcode;
+ uint8_t cmd; // Command
+ unsigned pwm:5; // Pulse width data [0~31] => [0%~100%]
+ unsigned iom:3; // GPIO mode [0~7]
+ unsigned mode:3; // Mode [0~7] Only 0 1 2 modes are supported
+ signed sens:5; // Sensitivity [-16~15]
+ struct {
+ unsigned chan:3; // Channel [0~7] [CH56,CH57,CH58,CH67,CH68,CH78,CH78,CH78]
+ unsigned revs:2; // Reverse [0~3] [Normal, Horizontal Reverse, Vertical Reverse, All Reverse]
+ unsigned rngx:2; // Range [0~3] [90 degrees, 120 degrees, 180 degrees, 360 degrees]
+ unsigned rngy:2; // Range [0~3] [60 degrees, 90 degrees, 120 degrees, 180 degrees]
+ signed zerx:6; // Zero [-32~31] [Resolution:5us]
+ signed zery:6; // Zero [-32~31] [Resolution:5us]
+ unsigned padding:11;
+ } ppm;
+ uint8_t padding2[5];
+ uint16_t crc;
+} __attribute__ ((__packed__)) gimbalCal_t;
+
+// Status reponse packet
+#define GIMBAL_OPCODE_STAT 0x913A
+typedef struct {
+ uint16_t opcode;
+ uint8_t cmd; // Command response status
+ uint8_t ctyp; // Calibration type [0:Idle 1:Acceleration calibration 2:Gyroscope calibration 3:Magnetometer calibration]
+ uint8_t cprg; // Calibration progress [0%~100%]
+ uint8_t cerr; // Calibration error [0%~100%]
+ uint8_t padding[8];
+ uint16_t crc;
+} __attribute__ ((__packed__)) gimbalCalStatus_t;
+
+// Expected input range from RC channels
+#define GIMBAL_RC_SET_MIN -500
+#define GIMBAL_RC_SET_MAX 500
+
+// Expect input range from head-tracker
+#define GIMBAL_SET_MIN -2047
+#define GIMBAL_SET_MAX 2047
+#define GIMBAL_SET_ROLL_MIN -900
+#define GIMBAL_SET_ROLL_MAX 900
+
+// Output range for full scale deflection to gimbal
+#define GIMBAL_ROLL_MIN -500
+#define GIMBAL_ROLL_MAX 500
+#define GIMBAL_PITCH_MIN -1150
+#define GIMBAL_PITCH_MAX 1750
+#define GIMBAL_YAW_MIN -2047
+#define GIMBAL_YAW_MAX 2047
+
+// Timeout after which headtracker input is ignored
+#define GIMBAL_HT_TIMEOUT_US 250000
+
+static struct {
+ union {
+ gimbalCmd_t gimbalCmd;
+ uint8_t bytes[sizeof(gimbalCmd_t)];
+ } u;
+} gimbalCmdIn;
+static uint32_t gimbalInCount = 0;
+
+static gimbalCmd_t gimbalCmdOut = {0};
+static serialPort_t *gimbalSerialPort = NULL;
+
+static uint16_t gimbalCrc(uint8_t *buf, uint32_t size)
+{
+ return __builtin_bswap16(crc16_ccitt_update(0x0000, buf, size));
+}
+
+// Set the gimbal position on each axis
+static bool gimbalSet(int16_t headtracker_roll, int16_t headtracker_pitch, int16_t headtracker_yaw)
+{
+ DEBUG_SET(DEBUG_GIMBAL, 0, headtracker_roll);
+ DEBUG_SET(DEBUG_GIMBAL, 1, headtracker_pitch);
+ DEBUG_SET(DEBUG_GIMBAL, 2, headtracker_yaw);
+
+ if (!gimbalSerialPort) {
+ return false;
+ }
+
+ // Scale the expected incoming range to the max values accepted by the gimbal
+ int16_t roll = scaleRange(headtracker_roll, GIMBAL_SET_ROLL_MIN, GIMBAL_SET_ROLL_MAX,
+ GIMBAL_ROLL_MIN * gimbalTrackConfig()->gimbal_roll_gain / 100,
+ GIMBAL_ROLL_MAX * gimbalTrackConfig()->gimbal_roll_gain / 100);
+ int16_t pitch = scaleRange(headtracker_pitch, GIMBAL_SET_MIN, GIMBAL_SET_MAX,
+ GIMBAL_PITCH_MIN * gimbalTrackConfig()->gimbal_pitch_gain / 100,
+ GIMBAL_PITCH_MAX * gimbalTrackConfig()->gimbal_pitch_gain / 100);
+ int16_t yaw = scaleRange(headtracker_yaw, GIMBAL_SET_MIN, GIMBAL_SET_MAX,
+ GIMBAL_YAW_MIN * gimbalTrackConfig()->gimbal_yaw_gain / 100,
+ GIMBAL_YAW_MAX * gimbalTrackConfig()->gimbal_yaw_gain / 100);
+
+
+ // Scale the RC stick inputs and add
+ roll += scaleRange(rcData[ROLL] - rxConfig()->midrc, GIMBAL_RC_SET_MIN, GIMBAL_RC_SET_MAX,
+ GIMBAL_ROLL_MIN * gimbalTrackConfig()->gimbal_roll_rc_gain / 100,
+ GIMBAL_ROLL_MAX * gimbalTrackConfig()->gimbal_roll_rc_gain / 100);
+ if (rcData[PITCH] < rxConfig()->midrc) {
+ pitch += scaleRange(rcData[PITCH] - rxConfig()->midrc, GIMBAL_RC_SET_MIN, 0,
+ GIMBAL_PITCH_MAX * gimbalTrackConfig()->gimbal_pitch_rc_low_gain / 100,
+ 0);
+ } else {
+ pitch += scaleRange(rcData[PITCH] - rxConfig()->midrc, 0, GIMBAL_RC_SET_MAX,
+ 0,
+ GIMBAL_PITCH_MIN * gimbalTrackConfig()->gimbal_pitch_rc_high_gain / 100);
+ }
+ yaw += scaleRange(rcData[YAW] - rxConfig()->midrc, GIMBAL_RC_SET_MIN, GIMBAL_RC_SET_MAX,
+ GIMBAL_YAW_MIN * gimbalTrackConfig()->gimbal_yaw_rc_gain / 100,
+ GIMBAL_YAW_MAX * gimbalTrackConfig()->gimbal_yaw_rc_gain / 100);
+
+ pitch += scaleRange(rcData[THROTTLE] - rxConfig()->midrc, GIMBAL_RC_SET_MIN, GIMBAL_RC_SET_MAX,
+ GIMBAL_PITCH_MIN * gimbalTrackConfig()->gimbal_pitch_rc_thr_gain / 100,
+ GIMBAL_PITCH_MAX * gimbalTrackConfig()->gimbal_pitch_rc_thr_gain / 100);
+
+ // Apply offsets
+ roll += GIMBAL_ROLL_MAX * gimbalTrackConfig()->gimbal_roll_offset / 100;
+ pitch += GIMBAL_PITCH_MAX * gimbalTrackConfig()->gimbal_pitch_offset / 100;
+ yaw += GIMBAL_YAW_MAX * gimbalTrackConfig()->gimbal_yaw_offset / 100;
+
+ DEBUG_SET(DEBUG_GIMBAL, 3, roll);
+ DEBUG_SET(DEBUG_GIMBAL, 4, pitch);
+ DEBUG_SET(DEBUG_GIMBAL, 5, yaw);
+
+ // Constrain to set limits
+ gimbalCmdOut.roll = constrain(roll, GIMBAL_ROLL_MIN * gimbalTrackConfig()->gimbal_roll_limit / 100,
+ GIMBAL_ROLL_MAX * gimbalTrackConfig()->gimbal_roll_limit / 100);
+ gimbalCmdOut.pitch = constrain(pitch, GIMBAL_PITCH_MIN * gimbalTrackConfig()->gimbal_pitch_low_limit / 100,
+ GIMBAL_PITCH_MAX * gimbalTrackConfig()->gimbal_pitch_high_limit / 100);
+ gimbalCmdOut.yaw = constrain(yaw, GIMBAL_YAW_MIN * gimbalTrackConfig()->gimbal_yaw_limit / 100,
+ GIMBAL_YAW_MAX * gimbalTrackConfig()->gimbal_yaw_limit / 100);
+
+ gimbalCmdOut.mode = gimbalTrackConfig()->gimbal_stabilisation;
+ gimbalCmdOut.sens = gimbalTrackConfig()->gimbal_sensitivity;
+
+ uint16_t crc = gimbalCrc((uint8_t *)&gimbalCmdOut, sizeof(gimbalCmdOut) - 2);
+
+ gimbalCmdOut.crc = crc;
+
+ return true;
+}
+
+// Gimbal updates should be sent at 100Hz or the gimbal will self center after approx. 2 seconds
+void gimbalUpdate(timeUs_t currentTimeUs)
+{
+ static enum {GIMBAL_OP1, GIMBAL_OP2, GIMBAL_CMD} gimbalParseState = GIMBAL_OP1;
+ static timeUs_t lastRxTimeUs = 0;
+
+ if (!gimbalSerialPort) {
+ setTaskEnabled(TASK_GIMBAL, false);
+ return;
+ }
+
+ // Read bytes from the VTX gimbal serial data stream
+
+ uint32_t bytes = serialRxBytesWaiting(gimbalSerialPort);
+
+ if (bytes > 0) {
+ lastRxTimeUs = currentTimeUs;
+ while (bytes--) {
+ uint8_t inData = serialRead(gimbalSerialPort);
+
+ // If the packet is a gimbalCmd_t structure then parse, otherwise pass through
+ switch (gimbalParseState) {
+ default:
+ case GIMBAL_OP1:
+ if (inData == (GIMBAL_CMD_S & 0xff)) {
+ gimbalParseState = GIMBAL_OP2;
+ } else {
+ serialWrite(gimbalSerialPort, inData);
+ }
+ break;
+ case GIMBAL_OP2:
+ if (inData == ((GIMBAL_CMD_S >> 8) & 0xff)) {
+ gimbalParseState = GIMBAL_CMD;
+ gimbalInCount = sizeof(gimbalCmdIn.u.gimbalCmd.opcode);
+ } else {
+ serialWrite(gimbalSerialPort, GIMBAL_CMD_S && 0xff);
+ serialWrite(gimbalSerialPort, inData);
+ gimbalParseState = GIMBAL_OP1;
+ }
+ break;
+ case GIMBAL_CMD:
+ gimbalCmdIn.u.bytes[gimbalInCount++] = inData;
+ if (gimbalInCount == sizeof(gimbalCmdIn.u.gimbalCmd)) {
+ uint16_t crc = gimbalCrc((uint8_t *)&gimbalCmdIn, sizeof(gimbalCmdIn) - 2);
+ // Only use the data if the CRC is correct
+ if (gimbalCmdIn.u.crc == crc) {
+ gimbalCmdOut = gimbalCmdIn.u.gimbalCmd;
+ gimbalSet(gimbalCmdIn.u.gimbalCmd.roll, gimbalCmdIn.u.gimbalCmd.pitch, gimbalCmdIn.u.gimbalCmd.yaw);
+ serialWriteBuf(gimbalSerialPort, (uint8_t *)&gimbalCmdOut, sizeof(gimbalCmdOut));
+ }
+ gimbalParseState = GIMBAL_OP1;
+ }
+ break;
+ }
+ }
+ } else if (cmpTimeUs(currentTimeUs, lastRxTimeUs) > GIMBAL_HT_TIMEOUT_US) {
+ gimbalSet(0, 0, 0);
+ serialWriteBuf(gimbalSerialPort, (uint8_t *)&gimbalCmdOut, sizeof(gimbalCmdOut));
+ }
+}
+
+bool gimbalInit(void)
+{
+ const serialPortConfig_t *portConfig = findSerialPortConfig(FUNCTION_GIMBAL);
+ if (!portConfig) {
+ return false;
+ }
+
+ // Serial communications is 115200 8N1
+ gimbalSerialPort = openSerialPort(portConfig->identifier, FUNCTION_GIMBAL,
+ NULL, NULL,
+ 115200, MODE_RXTX, SERIAL_STOPBITS_1 | SERIAL_PARITY_NO);
+
+ gimbalCmdIn.u.gimbalCmd.opcode = GIMBAL_CMD_S;
+ gimbalCmdOut.opcode = GIMBAL_CMD_S;
+
+ // Set gimbal initial position
+ gimbalSet(0, 0, 0);
+
+ return true;
+}
+
+#endif
diff --git a/src/main/io/gimbal_control.h b/src/main/io/gimbal_control.h
new file mode 100644
index 00000000000..d0d05fbac6e
--- /dev/null
+++ b/src/main/io/gimbal_control.h
@@ -0,0 +1,28 @@
+/*
+ * This file is part of Betaflight.
+ *
+ * Betaflight is free software. You can redistribute this software
+ * and/or modify this software 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.
+ *
+ * Betaflight 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 this software.
+ *
+ * If not, see .
+ */
+
+#pragma once
+
+#include
+
+bool gimbalInit(void);
+void gimbalUpdate(timeUs_t currentTimeUs);
+
diff --git a/src/main/io/serial.h b/src/main/io/serial.h
index 86cbb9afa35..9d2a3ce667c 100644
--- a/src/main/io/serial.h
+++ b/src/main/io/serial.h
@@ -51,6 +51,7 @@ typedef enum {
FUNCTION_LIDAR_TF = (1 << 15), // 32768
FUNCTION_FRSKY_OSD = (1 << 16), // 65536
FUNCTION_VTX_MSP = (1 << 17), // 131072
+ FUNCTION_GIMBAL = (1 << 18), // 262144
} serialPortFunction_e;
#define TELEMETRY_SHAREABLE_PORT_FUNCTIONS_MASK (FUNCTION_TELEMETRY_FRSKY_HUB | FUNCTION_TELEMETRY_LTM | FUNCTION_TELEMETRY_MAVLINK)
diff --git a/src/main/pg/gimbal.c b/src/main/pg/gimbal.c
new file mode 100644
index 00000000000..c3e5931b650
--- /dev/null
+++ b/src/main/pg/gimbal.c
@@ -0,0 +1,53 @@
+/*
+ * This file is part of Betaflight.
+ *
+ * Betaflight is free software. You can redistribute this software
+ * and/or modify this software 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.
+ *
+ * Betaflight 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 this software.
+ *
+ * If not, see .
+ */
+
+#include "platform.h"
+
+#ifdef USE_GIMBAL
+
+#include "pg/pg.h"
+#include "pg/pg_ids.h"
+
+#include "gimbal.h"
+
+PG_REGISTER_WITH_RESET_TEMPLATE(gimbalTrackConfig_t, gimbalTrackConfig, PG_GIMBAL_TRACK_CONFIG, 0);
+
+PG_RESET_TEMPLATE(gimbalTrackConfig_t, gimbalTrackConfig,
+ // Default to full gain and no offset
+ .gimbal_roll_rc_gain = 40,
+ .gimbal_pitch_rc_thr_gain = 10,
+ .gimbal_pitch_rc_low_gain = 10,
+ .gimbal_pitch_rc_high_gain = -20,
+ .gimbal_yaw_rc_gain = 20,
+ .gimbal_roll_gain = 100,
+ .gimbal_roll_offset = 0,
+ .gimbal_roll_limit = 100,
+ .gimbal_pitch_gain = 50,
+ .gimbal_pitch_offset = -10,
+ .gimbal_pitch_low_limit = 100,
+ .gimbal_pitch_high_limit = 100,
+ .gimbal_yaw_gain = 50,
+ .gimbal_yaw_offset = 0,
+ .gimbal_yaw_limit = 100,
+ .gimbal_stabilisation = 0,
+ .gimbal_sensitivity = 15
+);
+#endif
diff --git a/src/main/pg/gimbal.h b/src/main/pg/gimbal.h
new file mode 100644
index 00000000000..f1a8a038689
--- /dev/null
+++ b/src/main/pg/gimbal.h
@@ -0,0 +1,47 @@
+/*
+ * This file is part of Betaflight.
+ *
+ * Betaflight is free software. You can redistribute this software
+ * and/or modify this software 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.
+ *
+ * Betaflight 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 this software.
+ *
+ * If not, see .
+ */
+
+#pragma once
+
+#include "pg/pg.h"
+#include "drivers/io_types.h"
+
+typedef struct gimbalTrackConfig_s {
+ int8_t gimbal_roll_rc_gain;
+ int8_t gimbal_pitch_rc_thr_gain;
+ int8_t gimbal_pitch_rc_low_gain;
+ int8_t gimbal_pitch_rc_high_gain;
+ int8_t gimbal_yaw_rc_gain;
+ int8_t gimbal_roll_gain;
+ int8_t gimbal_roll_offset;
+ int8_t gimbal_roll_limit;
+ int8_t gimbal_pitch_gain;
+ int8_t gimbal_pitch_offset;
+ int8_t gimbal_pitch_low_limit;
+ int8_t gimbal_pitch_high_limit;
+ int8_t gimbal_yaw_gain;
+ int8_t gimbal_yaw_offset;
+ int8_t gimbal_yaw_limit;
+ int8_t gimbal_stabilisation;
+ int8_t gimbal_sensitivity;
+} gimbalTrackConfig_t;
+
+PG_DECLARE(gimbalTrackConfig_t, gimbalTrackConfig);
diff --git a/src/main/pg/pg_ids.h b/src/main/pg/pg_ids.h
index 6342fc8cc4e..9e69a2aa16c 100644
--- a/src/main/pg/pg_ids.h
+++ b/src/main/pg/pg_ids.h
@@ -158,7 +158,8 @@
#define PG_SCHEDULER_CONFIG 556
#define PG_MSP_CONFIG 557
//#define PG_SOFTSERIAL_PIN_CONFIG 558 // removed, merged into SERIAL_PIN_CONFIG
-#define PG_BETAFLIGHT_END 557
+#define PG_GIMBAL_TRACK_CONFIG 559
+#define PG_BETAFLIGHT_END 559
// OSD configuration (subject to change)
diff --git a/src/main/scheduler/scheduler.h b/src/main/scheduler/scheduler.h
index 394d29fe39c..f79bc7faabc 100644
--- a/src/main/scheduler/scheduler.h
+++ b/src/main/scheduler/scheduler.h
@@ -182,6 +182,9 @@ typedef enum {
#ifdef USE_RC_STATS
TASK_RC_STATS,
#endif
+#ifdef USE_GIMBAL
+ TASK_GIMBAL,
+#endif
/* Count of real tasks */
TASK_COUNT,