Skip to content

Commit ca6921b

Browse files
mmoscammosca
authored
Merge pull request iNavFlight#10109 from iNavFlight/mmosca-gimbal-test
Serial gimbal and headtracker support
2 parents c9d9882 + 677734d commit ca6921b

29 files changed

+1573
-10
lines changed

‎.gitignore

+4
Original file line numberDiff line numberDiff line change
@@ -1,10 +1,12 @@
11
*.o
22
.DS_Store
33
*~
4+
*.swp
45
*.uvopt
56
*.dep
67
*.bak
78
*.uvgui.*
9+
*.ubx
810
.project
911
.settings
1012
.cproject
@@ -16,6 +18,8 @@ startup_stm32f10x_md_gcc.s
1618
cov-int*
1719
/build/
1820
/build_SITL/
21+
/[hs]itl/
22+
/ninja/
1923
/obj/
2024
/patches/
2125
/tools/

‎docs/Settings.md

+90
Original file line numberDiff line numberDiff line change
@@ -1462,6 +1462,56 @@ Yaw Iterm is frozen when bank angle is above this threshold [degrees]. This solv
14621462

14631463
---
14641464

1465+
### gimbal_pan_channel
1466+
1467+
Gimbal pan rc channel index. 0 is no channel.
1468+
1469+
| Default | Min | Max |
1470+
| --- | --- | --- |
1471+
| 0 | 0 | 32 |
1472+
1473+
---
1474+
1475+
### gimbal_roll_channel
1476+
1477+
Gimbal roll rc channel index. 0 is no channel.
1478+
1479+
| Default | Min | Max |
1480+
| --- | --- | --- |
1481+
| 0 | 0 | 32 |
1482+
1483+
---
1484+
1485+
### gimbal_sensitivity
1486+
1487+
Gimbal sensitivity is similar to gain and will affect how quickly the gimbal will react.
1488+
1489+
| Default | Min | Max |
1490+
| --- | --- | --- |
1491+
| 0 | -16 | 15 |
1492+
1493+
---
1494+
1495+
### gimbal_serial_single_uart
1496+
1497+
Gimbal serial and headtracker device share same UART. FC RX goes to headtracker device, FC TX goes to gimbal.
1498+
1499+
| Default | Min | Max |
1500+
| --- | --- | --- |
1501+
| OFF | OFF | ON |
1502+
1503+
---
1504+
1505+
### gimbal_tilt_channel
1506+
1507+
Gimbal tilt rc channel index. 0 is no channel.
1508+
1509+
| Default | Min | Max |
1510+
| --- | --- | --- |
1511+
| 0 | 0 | 32 |
1512+
1513+
---
1514+
14651515
### gps_auto_baud
14661516

14671517
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
17721822

17731823
---
17741824

1825+
### headtracker_pan_ratio
1826+
1827+
Head pan movement vs camera movement ratio
1828+
1829+
| Default | Min | Max |
1830+
| --- | --- | --- |
1831+
| 1 | 0 | 5 |
1832+
1833+
---
1834+
1835+
### headtracker_roll_ratio
1836+
1837+
Head roll movement vs camera movement ratio
1838+
1839+
| Default | Min | Max |
1840+
| --- | --- | --- |
1841+
| 1 | 0 | 5 |
1842+
1843+
---
1844+
1845+
### headtracker_tilt_ratio
1846+
1847+
Head tilt movement vs camera movement ratio
1848+
1849+
| Default | Min | Max |
1850+
| --- | --- | --- |
1851+
| 1 | 0 | 5 |
1852+
1853+
---
1854+
1855+
### headtracker_type
1856+
1857+
Type of headtrackr dervice
1858+
1859+
| Default | Min | Max |
1860+
| --- | --- | --- |
1861+
| NONE | | |
1862+
1863+
---
1864+
17751865
### hott_alarm_sound_interval
17761866

17771867
Battery alarm delay in seconds for Hott telemetry

‎src/main/CMakeLists.txt

+8
Original file line numberDiff line numberDiff line change
@@ -177,6 +177,10 @@ main_sources(COMMON_SRC
177177
drivers/flash_m25p16.h
178178
drivers/flash_w25n01g.c
179179
drivers/flash_w25n01g.h
180+
drivers/gimbal_common.h
181+
drivers/gimbal_common.c
182+
drivers/headtracker_common.h
183+
drivers/headtracker_common.c
180184
drivers/io.c
181185
drivers/io.h
182186
drivers/io_pcf8574.c
@@ -356,6 +360,10 @@ main_sources(COMMON_SRC
356360
io/servo_sbus.h
357361
io/frsky_osd.c
358362
io/frsky_osd.h
363+
io/gimbal_serial.c
364+
io/gimbal_serial.h
365+
io/headtracker_msp.c
366+
io/headtracker_msp.h
359367
io/osd_dji_hd.c
360368
io/osd_dji_hd.h
361369
io/lights.c

‎src/main/build/debug.h

+9
Original file line numberDiff line numberDiff line change
@@ -21,6 +21,8 @@
2121
#include <stdint.h>
2222
#include <stdbool.h>
2323

24+
#include "platform.h"
25+
2426
#define DEBUG32_VALUE_COUNT 8
2527
extern int32_t debug[DEBUG32_VALUE_COUNT];
2628
extern uint8_t debugMode;
@@ -73,5 +75,12 @@ typedef enum {
7375
DEBUG_LANDING,
7476
DEBUG_POS_EST,
7577
DEBUG_ADAPTIVE_FILTER,
78+
DEBUG_HEADTRACKING,
7679
DEBUG_COUNT
7780
} debugType_e;
81+
82+
#ifdef SITL_BUILD
83+
#define SD(X) (X)
84+
#else
85+
#define SD(X)
86+
#endif

‎src/main/config/parameter_group_ids.h

+4-1
Original file line numberDiff line numberDiff line change
@@ -126,7 +126,10 @@
126126
#define PG_FW_AUTOLAND_CONFIG 1036
127127
#define PG_FW_AUTOLAND_APPROACH_CONFIG 1037
128128
#define PG_OSD_CUSTOM_ELEMENTS_CONFIG 1038
129-
#define PG_INAV_END PG_OSD_CUSTOM_ELEMENTS_CONFIG
129+
#define PG_GIMBAL_CONFIG 1039
130+
#define PG_GIMBAL_SERIAL_CONFIG 1040
131+
#define PG_HEADTRACKER_CONFIG 1041
132+
#define PG_INAV_END PG_HEADTRACKER_CONFIG
130133

131134
// OSD configuration (subject to change)
132135
//#define PG_OSD_FONT_CONFIG 2047

‎src/main/drivers/gimbal_common.c

+117
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,117 @@
1+
/*
2+
* This file is part of INAV.
3+
*
4+
* Cleanflight is free software: you can redistribute it and/or modify
5+
* it under the terms of the GNU General Public License as published by
6+
* the Free Software Foundation, either version 3 of the License, or
7+
* (at your option) any later version.
8+
*
9+
* Cleanflight is distributed in the hope that it will be useful,
10+
* but WITHOUT ANY WARRANTY; without even the implied warranty of
11+
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
12+
* GNU General Public License for more details.
13+
*
14+
* You should have received a copy of the GNU General Public License
15+
* along with INAV. If not, see <http://www.gnu.org/licenses/>.
16+
*/
17+
18+
#include "platform.h"
19+
20+
#ifdef USE_SERIAL_GIMBAL
21+
22+
#include <stdio.h>
23+
#include <stdint.h>
24+
#include <stdlib.h>
25+
26+
#include <build/debug.h>
27+
#include <config/parameter_group_ids.h>
28+
29+
#include "common/time.h"
30+
31+
#include "fc/cli.h"
32+
33+
#include "drivers/gimbal_common.h"
34+
35+
36+
PG_REGISTER(gimbalConfig_t, gimbalConfig, PG_GIMBAL_CONFIG, 0);
37+
38+
39+
static gimbalDevice_t *commonGimbalDevice = NULL;
40+
41+
void gimbalCommonInit(void)
42+
{
43+
}
44+
45+
void gimbalCommonSetDevice(gimbalDevice_t *gimbalDevice)
46+
{
47+
SD(fprintf(stderr, "[GIMBAL]: device added %p\n", gimbalDevice));
48+
commonGimbalDevice = gimbalDevice;
49+
}
50+
51+
gimbalDevice_t *gimbalCommonDevice(void)
52+
{
53+
return commonGimbalDevice;
54+
}
55+
56+
void gimbalCommonProcess(gimbalDevice_t *gimbalDevice, timeUs_t currentTimeUs)
57+
{
58+
if (gimbalDevice && gimbalDevice->vTable->process && gimbalCommonIsReady(gimbalDevice)) {
59+
gimbalDevice->vTable->process(gimbalDevice, currentTimeUs);
60+
}
61+
}
62+
63+
gimbalDevType_e gimbalCommonGetDeviceType(gimbalDevice_t *gimbalDevice)
64+
{
65+
if (!gimbalDevice || !gimbalDevice->vTable->getDeviceType) {
66+
return GIMBAL_DEV_UNKNOWN;
67+
}
68+
69+
return gimbalDevice->vTable->getDeviceType(gimbalDevice);
70+
}
71+
72+
bool gimbalCommonIsReady(gimbalDevice_t *gimbalDevice)
73+
{
74+
if (gimbalDevice && gimbalDevice->vTable->isReady) {
75+
return gimbalDevice->vTable->isReady(gimbalDevice);
76+
}
77+
return false;
78+
}
79+
80+
#ifdef GIMBAL_UNIT_TEST
81+
void taskUpdateGimbal(timeUs_t currentTimeUs)
82+
{
83+
}
84+
#else
85+
void taskUpdateGimbal(timeUs_t currentTimeUs)
86+
{
87+
if (cliMode) {
88+
return;
89+
}
90+
91+
gimbalDevice_t *gimbalDevice = gimbalCommonDevice();
92+
93+
if(gimbalDevice) {
94+
gimbalCommonProcess(gimbalDevice, currentTimeUs);
95+
}
96+
}
97+
#endif
98+
99+
// TODO: check if any gimbal types are enabled
100+
bool gimbalCommonIsEnabled(void)
101+
{
102+
return true;
103+
}
104+
105+
106+
bool gimbalCommonHtrkIsEnabled(void)
107+
{
108+
const gimbalDevice_t *dev = gimbalCommonDevice();
109+
if(dev && dev->vTable->hasHeadTracker) {
110+
bool ret = dev->vTable->hasHeadTracker(dev);
111+
return ret;
112+
}
113+
114+
return false;
115+
}
116+
117+
#endif

‎src/main/drivers/gimbal_common.h

+97
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,97 @@
1+
/*
2+
* This file is part of INAV.
3+
*
4+
* Cleanflight is free software: you can redistribute it and/or modify
5+
* it under the terms of the GNU General Public License as published by
6+
* the Free Software Foundation, either version 3 of the License, or
7+
* (at your option) any later version.
8+
*
9+
* Cleanflight is distributed in the hope that it will be useful,
10+
* but WITHOUT ANY WARRANTY; without even the implied warranty of
11+
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
12+
* GNU General Public License for more details.
13+
*
14+
* You should have received a copy of the GNU General Public License
15+
* along with INAV. If not, see <http://www.gnu.org/licenses/>.
16+
*/
17+
18+
#pragma once
19+
20+
#include "platform.h"
21+
22+
#ifdef USE_SERIAL_GIMBAL
23+
24+
#include <stdint.h>
25+
26+
#include "config/feature.h"
27+
#include "common/time.h"
28+
29+
#ifdef __cplusplus
30+
extern "C" {
31+
#endif
32+
33+
typedef enum {
34+
GIMBAL_DEV_UNSUPPORTED = 0,
35+
GIMBAL_DEV_SERIAL,
36+
GIMBAL_DEV_UNKNOWN=0xFF
37+
} gimbalDevType_e;
38+
39+
40+
struct gimbalVTable_s;
41+
42+
typedef struct gimbalDevice_s {
43+
const struct gimbalVTable_s *vTable;
44+
} gimbalDevice_t;
45+
46+
// {set,get}BandAndChannel: band and channel are 1 origin
47+
// {set,get}PowerByIndex: 0 = Power OFF, 1 = device dependent
48+
// {set,get}PitMode: 0 = OFF, 1 = ON
49+
50+
typedef struct gimbalVTable_s {
51+
void (*process)(gimbalDevice_t *gimbalDevice, timeUs_t currentTimeUs);
52+
gimbalDevType_e (*getDeviceType)(const gimbalDevice_t *gimbalDevice);
53+
bool (*isReady)(const gimbalDevice_t *gimbalDevice);
54+
bool (*hasHeadTracker)(const gimbalDevice_t *gimbalDevice);
55+
} gimbalVTable_t;
56+
57+
58+
typedef struct gimbalConfig_s {
59+
uint8_t panChannel;
60+
uint8_t tiltChannel;
61+
uint8_t rollChannel;
62+
uint8_t sensitivity;
63+
} gimbalConfig_t;
64+
65+
PG_DECLARE(gimbalConfig_t, gimbalConfig);
66+
67+
typedef enum {
68+
GIMBAL_MODE_FOLLOW = 0,
69+
GIMBAL_MODE_TILT_LOCK = (1<<0),
70+
GIMBAL_MODE_ROLL_LOCK = (1<<1),
71+
GIMBAL_MODE_PAN_LOCK = (1<<2),
72+
} gimbal_htk_mode_e;
73+
74+
#define GIMBAL_MODE_DEFAULT GIMBAL_MODE_FOLLOW
75+
#define GIMBAL_MODE_TILT_ROLL_LOCK (GIMBAL_MODE_TILT_LOCK | GIMBAL_MODE_ROLL_LOCK)
76+
#define GIMBAL_MODE_PAN_TILT_ROLL_LOCK (GIMBAL_MODE_TILT_LOCK | GIMBAL_MODE_ROLL_LOCK | GIMBAL_MODE_PAN_LOCK)
77+
78+
void gimbalCommonInit(void);
79+
void gimbalCommonSetDevice(gimbalDevice_t *gimbalDevice);
80+
gimbalDevice_t *gimbalCommonDevice(void);
81+
82+
// VTable functions
83+
void gimbalCommonProcess(gimbalDevice_t *gimbalDevice, timeUs_t currentTimeUs);
84+
gimbalDevType_e gimbalCommonGetDeviceType(gimbalDevice_t *gimbalDevice);
85+
bool gimbalCommonIsReady(gimbalDevice_t *gimbalDevice);
86+
87+
88+
void taskUpdateGimbal(timeUs_t currentTimeUs);
89+
90+
bool gimbalCommonIsEnabled(void);
91+
bool gimbalCommonHtrkIsEnabled(void);
92+
93+
#ifdef __cplusplus
94+
}
95+
#endif
96+
97+
#endif

0 commit comments

Comments
 (0)