From 48d05dcb50de2a1228b1fe9b299c9f5529db8d08 Mon Sep 17 00:00:00 2001
From: Marcelo Bezerra <23555060+mmosca@users.noreply.github.com>
Date: Sun, 2 Jun 2024 21:19:34 +0200
Subject: [PATCH] Initial draft for gimbal support.
Needs plumbing so we have a gimbal task running to send gumbal updates
---
docs/Settings.md | 40 ++++++++++++++
src/main/CMakeLists.txt | 4 ++
src/main/config/parameter_group_ids.h | 3 +-
src/main/drivers/gimbal_common.c | 23 ++++++++
src/main/drivers/gimbal_common.h | 37 +++++++++++++
src/main/fc/fc_msp_box.c | 10 ++++
src/main/fc/rc_modes.h | 8 ++-
src/main/fc/settings.yaml | 28 ++++++++++
src/main/io/gimbal_htk.c | 101 ++++++++++++++++++++++++++++++++++
src/main/io/gimbal_htk.h | 42 ++++++++++++++
src/main/io/serial.h | 1 +
11 files changed, 294 insertions(+), 3 deletions(-)
create mode 100644 src/main/drivers/gimbal_common.c
create mode 100644 src/main/drivers/gimbal_common.h
create mode 100644 src/main/io/gimbal_htk.c
create mode 100644 src/main/io/gimbal_htk.h
diff --git a/docs/Settings.md b/docs/Settings.md
index a1c339c1d..5af90a7e1 100644
--- a/docs/Settings.md
+++ b/docs/Settings.md
@@ -1462,6 +1462,46 @@ Yaw Iterm is frozen when bank angle is above this threshold [degrees]. This solv
---
+### gimabl_pitch_channel
+
+Gimbal pitch rc channel index. 0 is no channel.
+
+| Default | Min | Max |
+| --- | --- | --- |
+| 0 | 0 | 32 |
+
+---
+
+### gimabl_roll_channel
+
+Gimbal roll rc channel index. 0 is no channel.
+
+| Default | Min | Max |
+| --- | --- | --- |
+| 0 | 0 | 32 |
+
+---
+
+### gimabl_sensitivity
+
+Gimbal sensitivity is similar to gain and will affect how quickly the gimbal will react.
+
+| Default | Min | Max |
+| --- | --- | --- |
+| 0 | -16 | 15 |
+
+---
+
+### gimabl_yaw_channel
+
+Gimbal yaw 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
diff --git a/src/main/CMakeLists.txt b/src/main/CMakeLists.txt
index c87abc178..31d09c716 100755
--- a/src/main/CMakeLists.txt
+++ b/src/main/CMakeLists.txt
@@ -177,6 +177,8 @@ 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/io.c
drivers/io.h
drivers/io_pcf8574.c
@@ -354,6 +356,8 @@ main_sources(COMMON_SRC
io/servo_sbus.h
io/frsky_osd.c
io/frsky_osd.h
+ io/gimbal_htk.c
+ io/gimbal_htk.h
io/osd_dji_hd.c
io/osd_dji_hd.h
io/lights.c
diff --git a/src/main/config/parameter_group_ids.h b/src/main/config/parameter_group_ids.h
index 822e85f13..90085fae6 100644
--- a/src/main/config/parameter_group_ids.h
+++ b/src/main/config/parameter_group_ids.h
@@ -126,7 +126,8 @@
#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_INAV_END PG_GIMBAL_CONFIG
// OSD configuration (subject to change)
//#define PG_OSD_FONT_CONFIG 2047
diff --git a/src/main/drivers/gimbal_common.c b/src/main/drivers/gimbal_common.c
new file mode 100644
index 000000000..a5fbf38be
--- /dev/null
+++ b/src/main/drivers/gimbal_common.c
@@ -0,0 +1,23 @@
+/*
+ * 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 .
+ */
+
+#include
+#include
+
+#include "drivers/gimbal_common.h"
+
+PG_REGISTER(gimbalConfig_t, gimbalConfig, PG_GIMBAL_CONFIG, 0);
\ No newline at end of file
diff --git a/src/main/drivers/gimbal_common.h b/src/main/drivers/gimbal_common.h
new file mode 100644
index 000000000..9ac2ad3d9
--- /dev/null
+++ b/src/main/drivers/gimbal_common.h
@@ -0,0 +1,37 @@
+/*
+ * 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 .
+ */
+
+#pragma once
+
+#include
+
+#include "config/feature.h"
+
+typedef struct gimbalConfig_s {
+ uint8_t yawChannel;
+ uint8_t pitchChannel;
+ uint8_t rollChannel;
+ uint8_t sensitivity;
+} gimbalConfig_t;
+
+PG_DECLARE(gimbalConfig_t, gimbalConfig);
+
+typedef enum {
+ GIMBAL_MODE_PITCH_ROLL_LOCK = 0,
+ GIMBAL_MODE_PITCH_LOCK = 1,
+ GIMBAL_MODE_FOLLOW = 2
+} gimbal_htk_mode_e;
\ No newline at end of file
diff --git a/src/main/fc/fc_msp_box.c b/src/main/fc/fc_msp_box.c
index 7ffbbefd9..77aacd2f4 100644
--- a/src/main/fc/fc_msp_box.c
+++ b/src/main/fc/fc_msp_box.c
@@ -102,6 +102,10 @@ static const box_t boxes[CHECKBOX_ITEM_COUNT + 1] = {
{ .boxId = BOXMIXERPROFILE, .boxName = "MIXER PROFILE 2", .permanentId = 62 },
{ .boxId = BOXMIXERTRANSITION, .boxName = "MIXER TRANSITION", .permanentId = 63 },
{ .boxId = BOXANGLEHOLD, .boxName = "ANGLE HOLD", .permanentId = 64 },
+ { .boxId = BOXGIMBALLOCK, .boxName = "GIMBAL LOCK", .permanentId = 65 },
+ { .boxId = BOXGIMBALPLOCK, .boxName = "GIMBAL LOCK P", .permanentId = 66 },
+ { .boxId = BOXGIMBALPRLOCK, .boxName = "GIMBAL LOCK P+R", .permanentId = 66 },
+ { .boxId = BOXGIMBALCENTER, .boxName = "GIMBAL CENTER", .permanentId = 67 },
{ .boxId = CHECKBOX_ITEM_COUNT, .boxName = NULL, .permanentId = 0xFF }
};
@@ -431,6 +435,12 @@ void packBoxModeFlags(boxBitmask_t * mspBoxModeFlags)
CHECK_ACTIVE_BOX(IS_ENABLED(IS_RC_MODE_ACTIVE(BOXMIXERTRANSITION)), BOXMIXERTRANSITION);
#endif
CHECK_ACTIVE_BOX(IS_ENABLED(IS_RC_MODE_ACTIVE(BOXANGLEHOLD)), BOXANGLEHOLD);
+
+ CHECK_ACTIVE_BOX(IS_ENABLED(IS_RC_MODE_ACTIVE(BOXGIMBALLOCK)), BOXGIMBALLOCK);
+ CHECK_ACTIVE_BOX(IS_ENABLED(IS_RC_MODE_ACTIVE(BOXGIMBALPLOCK)), BOXGIMBALPLOCK);
+ CHECK_ACTIVE_BOX(IS_ENABLED(IS_RC_MODE_ACTIVE(BOXGIMBALPRLOCK)), BOXGIMBALPRLOCK);
+ CHECK_ACTIVE_BOX(IS_ENABLED(IS_RC_MODE_ACTIVE(BOXGIMBALCENTER)), BOXGIMBALCENTER);
+
memset(mspBoxModeFlags, 0, sizeof(boxBitmask_t));
for (uint32_t i = 0; i < activeBoxIdCount; i++) {
if (activeBoxes[activeBoxIds[i]]) {
diff --git a/src/main/fc/rc_modes.h b/src/main/fc/rc_modes.h
index 04aea681b..cac8528b6 100644
--- a/src/main/fc/rc_modes.h
+++ b/src/main/fc/rc_modes.h
@@ -78,9 +78,13 @@ typedef enum {
BOXCHANGEMISSION = 50,
BOXBEEPERMUTE = 51,
BOXMULTIFUNCTION = 52,
- BOXMIXERPROFILE = 53,
- BOXMIXERTRANSITION = 54,
+ BOXMIXERPROFILE = 53,
+ BOXMIXERTRANSITION = 54,
BOXANGLEHOLD = 55,
+ BOXGIMBALLOCK = 56,
+ BOXGIMBALPLOCK = 57,
+ BOXGIMBALPRLOCK = 58,
+ BOXGIMBALCENTER = 59,
CHECKBOX_ITEM_COUNT
} boxId_e;
diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml
index d9aa83c93..a8fdbbd40 100644
--- a/src/main/fc/settings.yaml
+++ b/src/main/fc/settings.yaml
@@ -4146,3 +4146,31 @@ groups:
field: maxTailwind
min: 0
max: 3000
+ - name: PG_GIMBAL_CONFIG
+ type: gimbalConfig_t
+ headers: ["drivers/gimbal_common.h"]
+ members:
+ - name: gimabl_yaw_channel
+ description: "Gimbal yaw rc channel index. 0 is no channel."
+ default_value: 0
+ field: yawChannel
+ min: 0
+ max: 32
+ - name: gimabl_roll_channel
+ description: "Gimbal roll rc channel index. 0 is no channel."
+ default_value: 0
+ field: yawChannel
+ min: 0
+ max: 32
+ - name: gimabl_pitch_channel
+ description: "Gimbal pitch rc channel index. 0 is no channel."
+ default_value: 0
+ field: yawChannel
+ min: 0
+ max: 32
+ - name: gimabl_sensitivity
+ description: "Gimbal sensitivity is similar to gain and will affect how quickly the gimbal will react."
+ default_value: 0
+ field: sensitivity
+ min: -16
+ max: 15
\ No newline at end of file
diff --git a/src/main/io/gimbal_htk.c b/src/main/io/gimbal_htk.c
new file mode 100644
index 000000000..0633423ae
--- /dev/null
+++ b/src/main/io/gimbal_htk.c
@@ -0,0 +1,101 @@
+/*
+ * 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 .
+ */
+
+#include
+
+#include
+#include
+
+#include
+
+
+void gimbal_htk_update(void)
+{
+ const gimbalConfig_t *cfg = gimbalConfig();
+
+ GimbalHtkAttitudePkt_t attittude = {
+ .sync = {HTKATTITUDE_SYNC0, HTKATTITUDE_SYNC1},
+ .mode = GIMBAL_MODE_FOLLOW,
+ };
+
+ int yaw = 1500;
+ int pitch = 1500;
+ int roll = 1500;
+
+ if (rxAreFlightChannelsValid()) {
+ if (cfg->yawChannel > 0) {
+ yaw = rxGetChannelValue(cfg->yawChannel - 1);
+ // const rxChannelRangeConfig_t *channelRanges =
+ // rxChannelRangeConfigs(cfg->pitchChannel - 1);
+ if (yaw < 1000) {
+ yaw = 1000;
+ } else if (yaw > 2000) {
+ yaw = 2000;
+ }
+ }
+
+ if (cfg->pitchChannel > 0) {
+ pitch = rxGetChannelValue(cfg->pitchChannel - 1);
+ // const rxChannelRangeConfig_t *channelRanges =
+ // rxChannelRangeConfigs(cfg->pitchChannel - 1);
+ if (pitch < 1000) {
+ pitch = 1000;
+ } else if (pitch > 2000) {
+ pitch = 2000;
+ }
+ }
+
+ if (cfg->rollChannel > 0) {
+ roll = rxGetChannelValue(cfg->rollChannel - 1);
+ // const rxChannelRangeConfig_t *channelRanges =
+ // rxChannelRangeConfigs(cfg->pitchChannel - 1);
+ if (roll < 1000) {
+ roll = 1000;
+ } else if (roll > 2000) {
+ roll = 2000;
+ }
+ }
+ }
+
+ attittude.sensibility = gimbal_scale8(-16, 15, 0, 31, cfg->sensitivity);
+
+ attittude.yaw = gimbal_scale16(1000, 2000, 0, 4095, yaw);
+ attittude.pitch = gimbal_scale16(1000, 2000, 0, 4095, pitch);
+ attittude.roll = gimbal_scale16(1000, 2000, 0, 4095, roll);
+
+ uint16_t crc16 = 0;
+ uint8_t *b = (uint8_t *)&attittude;
+ for (uint8_t i = 0; i < sizeof(GimbalHtkAttitudePkt_t) - 2; i++) {
+ crc16 = crc16_ccitt(crc16, *(b + i));
+ }
+ attittude.crch = (crc16 >> 8) & 0xFF;
+ attittude.crcl = crc16 & 0xFF;
+
+ // Send new data
+}
+
+uint8_t gimbal_scale8(int8_t inputMin, int8_t inputMax, int8_t outputMin, int8_t outputMax, int8_t value)
+{
+ float m = (1.0f * outputMax - outputMin) / (inputMax - inputMin);
+ return (uint8_t)((outputMin + (m * (value - inputMin))) + 0.5f);
+}
+
+uint16_t gimbal_scale16(int16_t inputMin, int16_t inputMax, int16_t outputMin, int16_t outputMax, int16_t value)
+{
+ float m = (1.0f * outputMax - outputMin) / (inputMax - inputMin);
+ return (uint16_t)((outputMin + (m * (value - inputMin))) + 0.5f);
+}
\ No newline at end of file
diff --git a/src/main/io/gimbal_htk.h b/src/main/io/gimbal_htk.h
new file mode 100644
index 000000000..1802ae337
--- /dev/null
+++ b/src/main/io/gimbal_htk.h
@@ -0,0 +1,42 @@
+/*
+ * 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 .
+ */
+
+#pragma once
+
+#include
+
+#define GIMBAL_HTK_MODE_DEFAULT GIMBAL_HTK_MODE_FOLLOW
+
+#define HTKATTITUDE_SYNC0 0xA5
+#define HTKATTITUDE_SYNC1 0x5A
+typedef struct
+{
+ uint8_t sync[2]; //data synchronization 0xA5, 0x5A
+ uint64_t mode:3; //Gimbal Mode [0~7] [Only 0 1 2 modes are supported for the time being]
+ int64_t sensibility:5; //Cloud sensibility [-16~15]
+ uint64_t reserved:4; //hold on to one's reserve
+ int64_t roll:12; //Roll angle [-2048~2047] => [-180~180]
+ int64_t pitch:12; //Pich angle [-2048~2047] => [-180~180]
+ int64_t yaw:12; //Yaw angle [-2048~2047] => [-180~180]
+ uint64_t crch:8; //Data validation H
+ uint64_t crcl:8; //Data validation L
+} __attribute__((packed)) GimbalHtkAttitudePkt_t;
+
+uint8_t gimbal_scale8(int8_t inputMin, int8_t inputMax, int8_t outputMin, int8_t outputMax, int8_t value);
+uint16_t gimbal_scale16(int16_t inputMin, int16_t inputMax, int16_t outputMin, int16_t outputMax, int16_t value);
+
+void gimbal_htk_update(void);
\ No newline at end of file
diff --git a/src/main/io/serial.h b/src/main/io/serial.h
index 776667910..dad56a9f4 100644
--- a/src/main/io/serial.h
+++ b/src/main/io/serial.h
@@ -57,6 +57,7 @@ typedef enum {
FUNCTION_TELEMETRY_SMARTPORT_MASTER = (1 << 23), // 8388608
FUNCTION_UNUSED_2 = (1 << 24), // 16777216
FUNCTION_MSP_OSD = (1 << 25), // 33554432
+ FUNCTION_HTK_GIMBAL = (1 << 26), // 67108864
} serialPortFunction_e;
#define FUNCTION_VTX_MSP FUNCTION_MSP_OSD
--
2.11.4.GIT