diff --git a/.gitignore b/.gitignore index ab82cc0fcd4..fd9c771bea2 100644 --- a/.gitignore +++ b/.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/ diff --git a/docs/Settings.md b/docs/Settings.md index 296d86931ee..a23ff66a4d2 100644 --- a/docs/Settings.md +++ b/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 diff --git a/src/main/CMakeLists.txt b/src/main/CMakeLists.txt index 5e5efd62ec8..2e2b6cb2b20 100755 --- a/src/main/CMakeLists.txt +++ b/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 diff --git a/src/main/build/debug.h b/src/main/build/debug.h index 60b22df27bc..fea424b9005 100644 --- a/src/main/build/debug.h +++ b/src/main/build/debug.h @@ -21,6 +21,8 @@ #include #include +#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 diff --git a/src/main/config/parameter_group_ids.h b/src/main/config/parameter_group_ids.h index 822e85f137a..b4062201c72 100644 --- a/src/main/config/parameter_group_ids.h +++ b/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 diff --git a/src/main/drivers/gimbal_common.c b/src/main/drivers/gimbal_common.c new file mode 100644 index 00000000000..bf2db50a261 --- /dev/null +++ b/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 . + */ + +#include "platform.h" + +#ifdef USE_SERIAL_GIMBAL + +#include +#include +#include + +#include +#include + +#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 \ 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 00000000000..2d914bbf77a --- /dev/null +++ b/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 . + */ + +#pragma once + +#include "platform.h" + +#ifdef USE_SERIAL_GIMBAL + +#include + +#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 \ No newline at end of file diff --git a/src/main/drivers/headtracker_common.c b/src/main/drivers/headtracker_common.c new file mode 100644 index 00000000000..feec581da8a --- /dev/null +++ b/src/main/drivers/headtracker_common.c @@ -0,0 +1,193 @@ +/* + * 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 "platform.h" + +#ifdef USE_HEADTRACKER + +#include +#include +#include +#include + +#include +#include + +#include "settings_generated.h" + +#include "common/time.h" +#include "common/maths.h" + +#include "drivers/time.h" + +#include "fc/cli.h" + +#include "rx/rx.h" + +#include "drivers/headtracker_common.h" + +PG_REGISTER_WITH_RESET_TEMPLATE(headTrackerConfig_t, headTrackerConfig, PG_HEADTRACKER_CONFIG, 1); + +PG_RESET_TEMPLATE(headTrackerConfig_t, headTrackerConfig, + .devType = SETTING_HEADTRACKER_TYPE_DEFAULT, + .pan_ratio = SETTING_HEADTRACKER_PAN_RATIO_DEFAULT, + .tilt_ratio = SETTING_HEADTRACKER_TILT_RATIO_DEFAULT, + .roll_ratio = SETTING_HEADTRACKER_ROLL_RATIO_DEFAULT, +); + +static headTrackerDevice_t *commonHeadTrackerDevice = NULL; + +void headTrackerCommonInit(void) +{ +} + +void headTrackerCommonSetDevice(headTrackerDevice_t *headTrackerDevice) +{ + SD(fprintf(stderr, "[headTracker]: device added %p\n", headTrackerDevice)); + commonHeadTrackerDevice = headTrackerDevice; +} + +headTrackerDevice_t *headTrackerCommonDevice(void) +{ + return commonHeadTrackerDevice; +} + +void headTrackerCommonProcess(headTrackerDevice_t *headTrackerDevice, timeUs_t currentTimeUs) +{ + if (headTrackerDevice && headTrackerDevice->vTable->process && headTrackerCommonIsReady(headTrackerDevice)) { + headTrackerDevice->vTable->process(headTrackerDevice, currentTimeUs); + } +} + +headTrackerDevType_e headTrackerCommonGetDeviceType(const headTrackerDevice_t *headTrackerDevice) +{ + if (!headTrackerDevice || !headTrackerDevice->vTable->getDeviceType) { + return HEADTRACKER_UNKNOWN; + } + + return headTrackerDevice->vTable->getDeviceType(headTrackerDevice); +} + +bool headTrackerCommonIsReady(const headTrackerDevice_t *headTrackerDevice) +{ + if (headTrackerDevice && headTrackerDevice->vTable->isReady) { + return headTrackerDevice->vTable->isReady(headTrackerDevice); + } + return false; +} + +int headTrackerCommonGetPan(const headTrackerDevice_t *headTrackerDevice) +{ + if(headTrackerDevice && headTrackerDevice->vTable && headTrackerDevice->vTable->getPan) { + return headTrackerDevice->vTable->getPan(headTrackerDevice); + } + + return constrain((headTrackerDevice->pan * headTrackerConfig()->pan_ratio) + 0.5f, HEADTRACKER_RANGE_MIN, HEADTRACKER_RANGE_MAX); +} + +int headTrackerCommonGetTilt(const headTrackerDevice_t *headTrackerDevice) +{ + if(headTrackerDevice && headTrackerDevice->vTable && headTrackerDevice->vTable->getTilt) { + return headTrackerDevice->vTable->getTilt(headTrackerDevice); + } + + return constrain((headTrackerDevice->tilt * headTrackerConfig()->tilt_ratio) + 0.5f, HEADTRACKER_RANGE_MIN, HEADTRACKER_RANGE_MAX); +} + +int headTrackerCommonGetRoll(const headTrackerDevice_t *headTrackerDevice) +{ + if(headTrackerDevice && headTrackerDevice->vTable && headTrackerDevice->vTable->getRoll) { + return headTrackerDevice->vTable->getRollPWM(headTrackerDevice); + } + + return constrain((headTrackerDevice->roll * headTrackerConfig()->roll_ratio) + 0.5f, HEADTRACKER_RANGE_MIN, HEADTRACKER_RANGE_MAX); +} + +int headTracker2PWM(int value) +{ + return constrain(scaleRange(value, HEADTRACKER_RANGE_MIN, HEADTRACKER_RANGE_MAX, PWM_RANGE_MIN, PWM_RANGE_MAX), PWM_RANGE_MIN, PWM_RANGE_MAX); +} + +int headTrackerCommonGetPanPWM(const headTrackerDevice_t *headTrackerDevice) +{ + if(headTrackerDevice && headTrackerDevice->vTable && headTrackerDevice->vTable->getPanPWM) { + return headTrackerDevice->vTable->getPanPWM(headTrackerDevice); + } + + return headTracker2PWM(headTrackerCommonGetPan(headTrackerDevice)); +} + +int headTrackerCommonGetTiltPWM(const headTrackerDevice_t *headTrackerDevice) +{ + if(headTrackerDevice && headTrackerDevice->vTable && headTrackerDevice->vTable->getTiltPWM) { + return headTrackerDevice->vTable->getTiltPWM(headTrackerDevice); + } + + return headTracker2PWM(headTrackerCommonGetTilt(headTrackerDevice)); +} + +int headTrackerCommonGetRollPWM(const headTrackerDevice_t *headTrackerDevice) +{ + if(headTrackerDevice && headTrackerDevice->vTable && headTrackerDevice->vTable->getRollPWM) { + return headTrackerDevice->vTable->getRollPWM(headTrackerDevice); + } + + return headTracker2PWM(headTrackerCommonGetRoll(headTrackerDevice)); +} + + +#ifdef headTracker_UNIT_TEST +void taskUpdateHeadTracker(timeUs_t currentTimeUs) +{ +} +#else +void taskUpdateHeadTracker(timeUs_t currentTimeUs) +{ + if (cliMode) { + return; + } + + headTrackerDevice_t *headTrackerDevice = headTrackerCommonDevice(); + + if(headTrackerDevice) { + headTrackerCommonProcess(headTrackerDevice, currentTimeUs); + } +} + +// TODO: check if any headTracker types are enabled +bool headTrackerCommonIsEnabled(void) +{ + if (commonHeadTrackerDevice && headTrackerCommonIsReady(commonHeadTrackerDevice)) { + return true; + } + + return false; +} + +bool headTrackerCommonIsValid(const headTrackerDevice_t *dev) { + if(dev && dev->vTable && dev->vTable->isValid) { + return dev->vTable->isValid(dev); + } + + return micros() < dev->expires; +} + + +#endif + + +#endif \ No newline at end of file diff --git a/src/main/drivers/headtracker_common.h b/src/main/drivers/headtracker_common.h new file mode 100644 index 00000000000..7b46b5390b8 --- /dev/null +++ b/src/main/drivers/headtracker_common.h @@ -0,0 +1,114 @@ +/* + * 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 "platform.h" + +#ifdef USE_HEADTRACKER + +#include + +#include "common/time.h" + +#include "drivers/time.h" + +#include "config/feature.h" + +#define MAX_HEADTRACKER_DATA_AGE_US HZ2US(25) +#define HEADTRACKER_RANGE_MIN -2048 +#define HEADTRACKER_RANGE_MAX 2047 + +#ifdef __cplusplus +extern "C" { +#endif + +typedef enum { + HEADTRACKER_NONE = 0, + HEADTRACKER_SERIAL = 1, + HEADTRACKER_MSP = 2, + HEADTRACKER_UNKNOWN = 0xFF +} headTrackerDevType_e; + + +struct headTrackerVTable_s; + +typedef struct headTrackerDevice_s { + const struct headTrackerVTable_s *vTable; + int pan; + int tilt; + int roll; + timeUs_t expires; +} headTrackerDevice_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 headTrackerVTable_s { + void (*process)(headTrackerDevice_t *headTrackerDevice, timeUs_t currentTimeUs); + headTrackerDevType_e (*getDeviceType)(const headTrackerDevice_t *headTrackerDevice); + bool (*isReady)(const headTrackerDevice_t *headTrackerDevice); + bool (*isValid)(const headTrackerDevice_t *headTrackerDevice); + int (*getPanPWM)(const headTrackerDevice_t *headTrackerDevice); + int (*getTiltPWM)(const headTrackerDevice_t *headTrackerDevice); + int (*getRollPWM)(const headTrackerDevice_t *headTrackerDevice); + int (*getPan)(const headTrackerDevice_t *headTrackerDevice); + int (*getTilt)(const headTrackerDevice_t *headTrackerDevice); + int (*getRoll)(const headTrackerDevice_t *headTrackerDevice); +} headTrackerVTable_t; + + +typedef struct headTrackerConfig_s { + headTrackerDevType_e devType; + float pan_ratio; + float tilt_ratio; + float roll_ratio; +} headTrackerConfig_t; + +PG_DECLARE(headTrackerConfig_t, headTrackerConfig); + +void headTrackerCommonInit(void); +void headTrackerCommonSetDevice(headTrackerDevice_t *headTrackerDevice); +headTrackerDevice_t *headTrackerCommonDevice(void); + +// VTable functions +void headTrackerCommonProcess(headTrackerDevice_t *headTrackerDevice, timeUs_t currentTimeUs); +headTrackerDevType_e headTrackerCommonGetDeviceType(const headTrackerDevice_t *headTrackerDevice); +bool headTrackerCommonIsReady(const headTrackerDevice_t *headtrackerDevice); +bool headTrackerCommonIsValid(const headTrackerDevice_t *headtrackerDevice); + +// Scaled value, constrained to PWM_RANGE_MIN~PWM_RANGE_MAX +int headTrackerCommonGetPanPWM(const headTrackerDevice_t *headTrackerDevice); +int headTrackerCommonGetTiltPWM(const headTrackerDevice_t *headTrackerDevice); +int headTrackerCommonGetRollPWM(const headTrackerDevice_t *headTrackerDevice); + +// Scaled value, constrained to -2048~2047 +int headTrackerCommonGetPan(const headTrackerDevice_t *headTrackerDevice); +int headTrackerCommonGetTilt(const headTrackerDevice_t *headTrackerDevice); +int headTrackerCommonGetRoll(const headTrackerDevice_t *headTrackerDevice); + +void taskUpdateHeadTracker(timeUs_t currentTimeUs); + +bool headtrackerCommonIsEnabled(void); + + +#ifdef __cplusplus +} +#endif + +#endif \ No newline at end of file diff --git a/src/main/fc/fc_init.c b/src/main/fc/fc_init.c index f2b368b8fcb..e9ed6efe4c1 100644 --- a/src/main/fc/fc_init.c +++ b/src/main/fc/fc_init.c @@ -53,6 +53,8 @@ #include "drivers/exti.h" #include "drivers/io.h" #include "drivers/flash.h" +#include "drivers/gimbal_common.h" +#include "drivers/headtracker_common.h" #include "drivers/light_led.h" #include "drivers/nvic.h" #include "drivers/osd.h" @@ -107,6 +109,8 @@ #include "io/displayport_msp_osd.h" #include "io/displayport_srxl.h" #include "io/flashfs.h" +#include "io/gimbal_serial.h" +#include "io/headtracker_msp.h" #include "io/gps.h" #include "io/ledstrip.h" #include "io/osd.h" @@ -687,6 +691,23 @@ void init(void) initDShotCommands(); #endif +#ifdef USE_SERIAL_GIMBAL + gimbalCommonInit(); + // Needs to be called before gimbalSerialHeadTrackerInit + gimbalSerialInit(); +#endif + +#ifdef USE_HEADTRACKER + headTrackerCommonInit(); +#ifdef USE_HEADTRACKER_SERIAL + // Needs to be called after gimbalSerialInit + gimbalSerialHeadTrackerInit(); +#endif +#ifdef USE_HEADTRACKER_MSP + mspHeadTrackerInit(); +#endif +#endif + // Latch active features AGAIN since some may be modified by init(). latchActiveFeatures(); motorControlEnable = true; diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c index 2109d3e08a9..6362901e45b 100644 --- a/src/main/fc/fc_msp.c +++ b/src/main/fc/fc_msp.c @@ -96,6 +96,7 @@ #include "io/vtx.h" #include "io/vtx_string.h" #include "io/gps_private.h" //for MSP_SIMULATOR +#include "io/headtracker_msp.h" #include "io/osd/custom_elements.h" @@ -4048,7 +4049,8 @@ bool mspFCProcessInOutCommand(uint16_t cmdMSP, sbuf_t *dst, sbuf_t *src, mspResu static mspResult_e mspProcessSensorCommand(uint16_t cmdMSP, sbuf_t *src) { - UNUSED(src); + int dataSize = sbufBytesRemaining(src); + UNUSED(dataSize); switch (cmdMSP) { #if defined(USE_RANGEFINDER_MSP) @@ -4086,6 +4088,12 @@ static mspResult_e mspProcessSensorCommand(uint16_t cmdMSP, sbuf_t *src) mspPitotmeterReceiveNewData(sbufPtr(src)); break; #endif + +#if (defined(USE_HEADTRACKER) && defined(USE_HEADTRACKER_MSP)) + case MSP2_SENSOR_HEADTRACKER: + mspHeadTrackerReceiverNewData(sbufPtr(src), dataSize); + break; +#endif } return MSP_RESULT_NO_REPLY; diff --git a/src/main/fc/fc_msp_box.c b/src/main/fc/fc_msp_box.c index 7ffbbefd95e..a20a23e0b24 100644 --- a/src/main/fc/fc_msp_box.c +++ b/src/main/fc/fc_msp_box.c @@ -43,6 +43,9 @@ #include "telemetry/telemetry.h" +#include "drivers/gimbal_common.h" +#include "drivers/headtracker_common.h" + #define BOX_SUFFIX ';' #define BOX_SUFFIX_LEN 1 @@ -102,6 +105,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 = BOXGIMBALTLOCK, .boxName = "GIMBAL LEVEL TILT", .permanentId = 65 }, + { .boxId = BOXGIMBALRLOCK, .boxName = "GIMBAL LEVEL ROLL", .permanentId = 66 }, + { .boxId = BOXGIMBALCENTER, .boxName = "GIMBAL CENTER", .permanentId = 67 }, + { .boxId = BOXGIMBALHTRK, .boxName = "GIMBAL HEADTRACKER", .permanentId = 68 }, { .boxId = CHECKBOX_ITEM_COUNT, .boxName = NULL, .permanentId = 0xFF } }; @@ -358,6 +365,19 @@ void initActiveBoxIds(void) ADD_ACTIVE_BOX(BOXMIXERPROFILE); ADD_ACTIVE_BOX(BOXMIXERTRANSITION); #endif + +#ifdef USE_SERIAL_GIMBAL + if (gimbalCommonIsEnabled()) { + ADD_ACTIVE_BOX(BOXGIMBALTLOCK); + ADD_ACTIVE_BOX(BOXGIMBALRLOCK); + ADD_ACTIVE_BOX(BOXGIMBALCENTER); + } +#endif +#ifdef USE_HEADTRACKER + if(headTrackerConfig()->devType != HEADTRACKER_NONE) { + ADD_ACTIVE_BOX(BOXGIMBALHTRK); + } +#endif } #define IS_ENABLED(mask) ((mask) == 0 ? 0 : 1) @@ -431,6 +451,21 @@ 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); + +#ifdef USE_SERIAL_GIMBAL + if(IS_RC_MODE_ACTIVE(BOXGIMBALCENTER)) { + CHECK_ACTIVE_BOX(IS_ENABLED(IS_RC_MODE_ACTIVE(BOXGIMBALCENTER)), BOXGIMBALCENTER); +#ifdef USE_HEADTRACKER + } else if (headTrackerCommonIsReady(headTrackerCommonDevice()) && IS_RC_MODE_ACTIVE(BOXGIMBALHTRK)) { + CHECK_ACTIVE_BOX(IS_ENABLED(IS_RC_MODE_ACTIVE(BOXGIMBALHTRK)), BOXGIMBALHTRK); +#endif + } else { + CHECK_ACTIVE_BOX(IS_ENABLED(IS_RC_MODE_ACTIVE(BOXGIMBALTLOCK) && !IS_RC_MODE_ACTIVE(BOXGIMBALCENTER)), BOXGIMBALTLOCK); + CHECK_ACTIVE_BOX(IS_ENABLED(IS_RC_MODE_ACTIVE(BOXGIMBALRLOCK) && !IS_RC_MODE_ACTIVE(BOXGIMBALCENTER)), BOXGIMBALRLOCK); + CHECK_ACTIVE_BOX(IS_ENABLED(IS_RC_MODE_ACTIVE(BOXGIMBALHTRK) && !IS_RC_MODE_ACTIVE(BOXGIMBALCENTER)), BOXGIMBALRLOCK); + } +#endif + memset(mspBoxModeFlags, 0, sizeof(boxBitmask_t)); for (uint32_t i = 0; i < activeBoxIdCount; i++) { if (activeBoxes[activeBoxIds[i]]) { diff --git a/src/main/fc/fc_tasks.c b/src/main/fc/fc_tasks.c index 1230f6f01d7..6918b9a6dab 100755 --- a/src/main/fc/fc_tasks.c +++ b/src/main/fc/fc_tasks.c @@ -34,6 +34,8 @@ #include "drivers/serial.h" #include "drivers/stack_check.h" #include "drivers/pwm_mapping.h" +#include "drivers/gimbal_common.h" +#include "drivers/headtracker_common.h" #include "fc/cli.h" #include "fc/config.h" @@ -427,6 +429,14 @@ void fcTasksInit(void) setTaskEnabled(TASK_SMARTPORT_MASTER, true); #endif +#ifdef USE_SERIAL_GIMBAL + setTaskEnabled(TASK_GIMBAL, true); +#endif + +#ifdef USE_HEADTRACKER + setTaskEnabled(TASK_HEADTRACKER, true); +#endif + #ifdef USE_ADAPTIVE_FILTER setTaskEnabled(TASK_ADAPTIVE_FILTER, ( gyroConfig()->gyroFilterMode == GYRO_FILTER_MODE_ADAPTIVE && @@ -697,4 +707,23 @@ cfTask_t cfTasks[TASK_COUNT] = { .staticPriority = TASK_PRIORITY_LOW, }, #endif + +#ifdef USE_SERIAL_GIMBAL + [TASK_GIMBAL] = { + .taskName = "GIMBAL", + .taskFunc = taskUpdateGimbal, + .desiredPeriod = TASK_PERIOD_HZ(50), + .staticPriority = TASK_PRIORITY_MEDIUM, + }, +#endif + +#ifdef USE_HEADTRACKER + [TASK_HEADTRACKER] = { + .taskName = "HEADTRACKER", + .taskFunc = taskUpdateHeadTracker, + .desiredPeriod = TASK_PERIOD_HZ(50), + .staticPriority = TASK_PRIORITY_MEDIUM, + }, +#endif + }; diff --git a/src/main/fc/rc_modes.h b/src/main/fc/rc_modes.h index 04aea681bc9..c4b21a9005b 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, + BOXGIMBALTLOCK = 56, + BOXGIMBALRLOCK = 57, + BOXGIMBALCENTER = 58, + BOXGIMBALHTRK = 59, CHECKBOX_ITEM_COUNT } boxId_e; diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index 0d358f58ffb..df5be7efca6 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -83,7 +83,7 @@ tables: values: ["NONE", "AGL", "FLOW_RAW", "FLOW", "ALWAYS", "SAG_COMP_VOLTAGE", "VIBE", "CRUISE", "REM_FLIGHT_TIME", "SMARTAUDIO", "ACC", "NAV_YAW", "PCF8574", "DYN_GYRO_LPF", "AUTOLEVEL", "ALTITUDE", - "AUTOTRIM", "AUTOTUNE", "RATE_DYNAMICS", "LANDING", "POS_EST", "ADAPTIVE_FILTER"] + "AUTOTRIM", "AUTOTUNE", "RATE_DYNAMICS", "LANDING", "POS_EST", "ADAPTIVE_FILTER", "HEADTRACKER" ] - name: aux_operator values: ["OR", "AND"] enum: modeActivationOperator_e @@ -194,6 +194,9 @@ tables: - name: gyro_filter_mode values: ["STATIC", "DYNAMIC", "ADAPTIVE"] enum: gyroFilterType_e + - name: headtracker_dev_type + values: ["NONE", "SERIAL", "MSP"] + enum: headTrackerDevType_e constants: RPYL_PID_MIN: 0 @@ -4198,3 +4201,74 @@ groups: field: maxTailwind min: 0 max: 3000 + - name: PG_GIMBAL_CONFIG + type: gimbalConfig_t + headers: ["drivers/gimbal_common.h"] + condition: USE_SERIAL_GIMBAL + members: + - name: gimbal_pan_channel + description: "Gimbal pan rc channel index. 0 is no channel." + default_value: 0 + field: panChannel + min: 0 + max: 32 + - name: gimbal_roll_channel + description: "Gimbal roll rc channel index. 0 is no channel." + default_value: 0 + field: rollChannel + min: 0 + max: 32 + - name: gimbal_tilt_channel + description: "Gimbal tilt rc channel index. 0 is no channel." + default_value: 0 + field: tiltChannel + min: 0 + max: 32 + - name: gimbal_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 + - name: PG_GIMBAL_SERIAL_CONFIG + type: gimbalSerialConfig_t + headers: ["io/gimbal_serial.h"] + condition: USE_SERIAL_GIMBAL + 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." + type: bool + default_value: OFF + field: singleUart + - name: PG_HEADTRACKER_CONFIG + type: headTrackerConfig_t + headers: ["drivers/headtracker_common.h"] + condition: USE_HEADTRACKER + members: + - name: headtracker_type + description: "Type of headtrackr dervice" + default_value: "NONE" + field: devType + type: uint8_t + table: headtracker_dev_type + - name: headtracker_pan_ratio + description: "Head pan movement vs camera movement ratio" + type: float + default_value: 1 + field: pan_ratio + min: 0 + max: 5 + - name: headtracker_tilt_ratio + description: "Head tilt movement vs camera movement ratio" + type: float + default_value: 1 + field: tilt_ratio + min: 0 + max: 5 + - name: headtracker_roll_ratio + description: "Head roll movement vs camera movement ratio" + type: float + default_value: 1 + field: roll_ratio + min: 0 + max: 5 \ No newline at end of file diff --git a/src/main/flight/servos.c b/src/main/flight/servos.c index f38a4ea108c..3830660660a 100755 --- a/src/main/flight/servos.c +++ b/src/main/flight/servos.c @@ -38,6 +38,8 @@ #include "drivers/pwm_output.h" #include "drivers/pwm_mapping.h" #include "drivers/time.h" +#include "drivers/gimbal_common.h" +#include "drivers/headtracker_common.h" #include "fc/config.h" #include "fc/fc_core.h" @@ -347,6 +349,23 @@ void servoMixer(float dT) input[INPUT_RC_CH16] = GET_RX_CHANNEL_INPUT(AUX12); #undef GET_RX_CHANNEL_INPUT +#ifdef USE_HEADTRACKER + headTrackerDevice_t *dev = headTrackerCommonDevice(); + if(dev && headTrackerCommonIsValid(dev) && !IS_RC_MODE_ACTIVE(BOXGIMBALCENTER)) { + input[INPUT_HEADTRACKER_PAN] = headTrackerCommonGetPanPWM(dev) - PWM_RANGE_MIDDLE; + input[INPUT_HEADTRACKER_TILT] = headTrackerCommonGetTiltPWM(dev) - PWM_RANGE_MIDDLE; + input[INPUT_HEADTRACKER_ROLL] = headTrackerCommonGetRollPWM(dev) - PWM_RANGE_MIDDLE; + } else { + input[INPUT_HEADTRACKER_PAN] = 0; + input[INPUT_HEADTRACKER_TILT] = 0; + input[INPUT_HEADTRACKER_ROLL] = 0; + } +#else + input[INPUT_HEADTRACKER_PAN] = 0; + input[INPUT_HEADTRACKER_TILT] = 0; + input[INPUT_HEADTRACKER_ROLL] = 0; +#endif + #ifdef USE_SIMULATOR simulatorData.input[INPUT_STABILIZED_ROLL] = input[INPUT_STABILIZED_ROLL]; simulatorData.input[INPUT_STABILIZED_PITCH] = input[INPUT_STABILIZED_PITCH]; diff --git a/src/main/flight/servos.h b/src/main/flight/servos.h index 1dd65912218..d271142ece0 100644 --- a/src/main/flight/servos.h +++ b/src/main/flight/servos.h @@ -63,6 +63,9 @@ typedef enum { INPUT_GVAR_6 = 36, INPUT_GVAR_7 = 37, INPUT_MIXER_TRANSITION = 38, + INPUT_HEADTRACKER_PAN = 39, + INPUT_HEADTRACKER_TILT = 40, + INPUT_HEADTRACKER_ROLL = 41, INPUT_SOURCE_COUNT } inputSource_e; diff --git a/src/main/io/gimbal_serial.c b/src/main/io/gimbal_serial.c new file mode 100644 index 00000000000..323bc99e564 --- /dev/null +++ b/src/main/io/gimbal_serial.c @@ -0,0 +1,427 @@ +/* + * 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 + +#include "platform.h" + +#ifdef USE_SERIAL_GIMBAL + +#include +#include +#include +#include + +#include +#include +#include +#include + +#include +#include + +#include +#include + +#include + +#include "settings_generated.h" + +PG_REGISTER_WITH_RESET_TEMPLATE(gimbalSerialConfig_t, gimbalSerialConfig, PG_GIMBAL_SERIAL_CONFIG, 0); + +PG_RESET_TEMPLATE(gimbalSerialConfig_t, gimbalSerialConfig, + .singleUart = SETTING_GIMBAL_SERIAL_SINGLE_UART_DEFAULT +); + +STATIC_ASSERT(sizeof(gimbalHtkAttitudePkt_t) == 10, gimbalHtkAttitudePkt_t_size_not_10); + +#define GIMBAL_SERIAL_BUFFER_SIZE 512 + +#ifndef GIMBAL_UNIT_TEST +static volatile uint8_t txBuffer[GIMBAL_SERIAL_BUFFER_SIZE]; + +#if defined(USE_HEADTRACKER) && defined(USE_HEADTRACKER_SERIAL) +static gimbalSerialHtrkState_t headTrackerState = { + .payloadSize = 0, + .state = WAITING_HDR1, +}; +#endif + +#endif + +static serialPort_t *headTrackerPort = NULL; +static serialPort_t *gimbalPort = NULL; + +gimbalVTable_t gimbalSerialVTable = { + .process = gimbalSerialProcess, + .getDeviceType = gimbalSerialGetDeviceType, + .isReady = gimbalSerialIsReady, + .hasHeadTracker = gimbalSerialHasHeadTracker, + +}; + +static gimbalDevice_t serialGimbalDevice = { + .vTable = &gimbalSerialVTable +}; + +#if (defined(USE_HEADTRACKER) && defined(USE_HEADTRACKER_SERIAL)) + +static headTrackerVTable_t headTrackerVTable = { + .process = headtrackerSerialProcess, + .getDeviceType = headtrackerSerialGetDeviceType, +}; + + +headTrackerDevice_t headTrackerDevice = { + .vTable = &headTrackerVTable, + .pan = 0, + .tilt = 0, + .roll = 0, + .expires = 0 +}; + +#endif + + +gimbalDevType_e gimbalSerialGetDeviceType(const gimbalDevice_t *gimbalDevice) +{ + UNUSED(gimbalDevice); + return GIMBAL_DEV_SERIAL; +} + +bool gimbalSerialIsReady(const gimbalDevice_t *gimbalDevice) +{ + return gimbalPort != NULL && gimbalDevice->vTable != NULL; +} + +bool gimbalSerialHasHeadTracker(const gimbalDevice_t *gimbalDevice) +{ + UNUSED(gimbalDevice); + return headTrackerPort || (gimbalSerialConfig()->singleUart && gimbalPort); +} + +bool gimbalSerialInit(void) +{ + if (gimbalSerialDetect()) { + SD(fprintf(stderr, "Setting gimbal device\n")); + gimbalCommonSetDevice(&serialGimbalDevice); + return true; + } + + return false; +} + +#ifdef GIMBAL_UNIT_TEST +bool gimbalSerialDetect(void) +{ + return false; +} +#else +bool gimbalSerialDetect(void) +{ + + SD(fprintf(stderr, "[GIMBAL]: serial Detect...\n")); + serialPortConfig_t *portConfig = findSerialPortConfig(FUNCTION_GIMBAL); + bool singleUart = gimbalSerialConfig()->singleUart; + + + if (portConfig) { + SD(fprintf(stderr, "[GIMBAL]: found port...\n")); +#if defined(USE_HEADTRACKER) && defined(USE_HEADTRACKER_SERIAL) + gimbalPort = openSerialPort(portConfig->identifier, FUNCTION_GIMBAL, singleUart ? gimbalSerialHeadTrackerReceive : NULL, singleUart ? &headTrackerState : NULL, + baudRates[portConfig->peripheral_baudrateIndex], MODE_RXTX, SERIAL_NOT_INVERTED); +#else + UNUSED(singleUart); + gimbalPort = openSerialPort(portConfig->identifier, FUNCTION_GIMBAL, NULL, NULL, + baudRates[portConfig->peripheral_baudrateIndex], MODE_RXTX, SERIAL_NOT_INVERTED); +#endif + + if (gimbalPort) { + SD(fprintf(stderr, "[GIMBAL]: port open!\n")); + gimbalPort->txBuffer = txBuffer; + gimbalPort->txBufferSize = GIMBAL_SERIAL_BUFFER_SIZE; + gimbalPort->txBufferTail = 0; + gimbalPort->txBufferHead = 0; + } else { + SD(fprintf(stderr, "[GIMBAL]: port NOT open!\n")); + return false; + } + } + + SD(fprintf(stderr, "[GIMBAL]: gimbalPort: %p\n", gimbalPort)); + return gimbalPort; +} +#endif + +#ifdef GIMBAL_UNIT_TEST +void gimbalSerialProcess(gimbalDevice_t *gimbalDevice, timeUs_t currentTime) +{ + UNUSED(gimbalDevice); + UNUSED(currentTime); +} +#else +void gimbalSerialProcess(gimbalDevice_t *gimbalDevice, timeUs_t currentTime) +{ + UNUSED(currentTime); + + if (!gimbalSerialIsReady(gimbalDevice)) { + SD(fprintf(stderr, "[GIMBAL] gimbal not ready...\n")); + return; + } + + gimbalHtkAttitudePkt_t attitude = { + .sync = {HTKATTITUDE_SYNC0, HTKATTITUDE_SYNC1}, + .mode = GIMBAL_MODE_DEFAULT + }; + + const gimbalConfig_t *cfg = gimbalConfig(); + + int pan = PWM_RANGE_MIDDLE; + int tilt = PWM_RANGE_MIDDLE; + int roll = PWM_RANGE_MIDDLE; + + if (IS_RC_MODE_ACTIVE(BOXGIMBALTLOCK)) { + attitude.mode |= GIMBAL_MODE_TILT_LOCK; + } + + if (IS_RC_MODE_ACTIVE(BOXGIMBALRLOCK)) { + attitude.mode |= GIMBAL_MODE_ROLL_LOCK; + } + + // Follow center overrides all + if (IS_RC_MODE_ACTIVE(BOXGIMBALCENTER) || IS_RC_MODE_ACTIVE(BOXGIMBALHTRK)) { + attitude.mode = GIMBAL_MODE_FOLLOW; + } + + if (rxAreFlightChannelsValid() && !IS_RC_MODE_ACTIVE(BOXGIMBALCENTER)) { + if (cfg->panChannel > 0) { + pan = rxGetChannelValue(cfg->panChannel - 1); + pan = constrain(pan, PWM_RANGE_MIN, PWM_RANGE_MAX); + } + + if (cfg->tiltChannel > 0) { + tilt = rxGetChannelValue(cfg->tiltChannel - 1); + tilt = constrain(tilt, PWM_RANGE_MIN, PWM_RANGE_MAX); + } + + if (cfg->rollChannel > 0) { + roll = rxGetChannelValue(cfg->rollChannel - 1); + roll = constrain(roll, PWM_RANGE_MIN, PWM_RANGE_MAX); + } + } + +#ifdef USE_HEADTRACKER + if(IS_RC_MODE_ACTIVE(BOXGIMBALHTRK)) { + headTrackerDevice_t *dev = headTrackerCommonDevice(); + if (gimbalCommonHtrkIsEnabled() && dev && headTrackerCommonIsValid(dev)) { + attitude.pan = headTrackerCommonGetPan(dev); + attitude.tilt = headTrackerCommonGetTilt(dev); + attitude.roll = headTrackerCommonGetRoll(dev); + DEBUG_SET(DEBUG_HEADTRACKING, 4, 1); + } else { + attitude.pan = 0; + attitude.tilt = 0; + attitude.roll = 0; + DEBUG_SET(DEBUG_HEADTRACKING, 4, -1); + } + } else { +#else + { +#endif + DEBUG_SET(DEBUG_HEADTRACKING, 4, 0); + // Radio endpoints may need to be adjusted, as it seems ot go a bit + // bananas at the extremes + attitude.pan = gimbal_scale12(PWM_RANGE_MIN, PWM_RANGE_MAX, pan); + attitude.tilt = gimbal_scale12(PWM_RANGE_MIN, PWM_RANGE_MAX, tilt); + attitude.roll = gimbal_scale12(PWM_RANGE_MIN, PWM_RANGE_MAX, roll); + } + + DEBUG_SET(DEBUG_HEADTRACKING, 5, attitude.pan); + DEBUG_SET(DEBUG_HEADTRACKING, 6, attitude.tilt); + DEBUG_SET(DEBUG_HEADTRACKING, 7, attitude.roll); + + attitude.sensibility = cfg->sensitivity; + + uint16_t crc16 = 0; + uint8_t *b = (uint8_t *)&attitude; + for (uint8_t i = 0; i < sizeof(gimbalHtkAttitudePkt_t) - 2; i++) { + crc16 = crc16_ccitt(crc16, *(b + i)); + } + attitude.crch = (crc16 >> 8) & 0xFF; + attitude.crcl = crc16 & 0xFF; + + serialBeginWrite(gimbalPort); + serialWriteBuf(gimbalPort, (uint8_t *)&attitude, sizeof(gimbalHtkAttitudePkt_t)); + serialEndWrite(gimbalPort); +} +#endif + +int16_t gimbal_scale12(int16_t inputMin, int16_t inputMax, int16_t value) +{ + int16_t ret = 0; + ret = scaleRange(value, inputMin, inputMax, -2048, 2047); + return ret; +} + +#ifndef GIMBAL_UNIT_TEST + +#if (defined(USE_HEADTRACKER) && defined(USE_HEADTRACKER_SERIAL)) +static void resetState(gimbalSerialHtrkState_t *state) +{ + state->state = WAITING_HDR1; + state->payloadSize = 0; +} + +static bool checkCrc(gimbalHtkAttitudePkt_t *attitude) +{ + uint8_t *attitudePkt = (uint8_t *)attitude; + uint16_t crc = 0; + + for(uint8_t i = 0; i < sizeof(gimbalHtkAttitudePkt_t) - 2; ++i) { + crc = crc16_ccitt(crc, attitudePkt[i]); + } + + return (attitude->crch == ((crc >> 8) & 0xFF)) && + (attitude->crcl == (crc & 0xFF)); +} + +void gimbalSerialHeadTrackerReceive(uint16_t c, void *data) +{ + static int charCount = 0; + static int pktCount = 0; + static int errorCount = 0; + gimbalSerialHtrkState_t *state = (gimbalSerialHtrkState_t *)data; + uint8_t *payload = (uint8_t *)&(state->attitude); + payload += 2; + + DEBUG_SET(DEBUG_HEADTRACKING, 0, charCount++); + DEBUG_SET(DEBUG_HEADTRACKING, 1, state->state); + + switch(state->state) { + case WAITING_HDR1: + if(c == HTKATTITUDE_SYNC0) { + state->attitude.sync[0] = c; + state->state = WAITING_HDR2; + } + break; + case WAITING_HDR2: + if(c == HTKATTITUDE_SYNC1) { + state->attitude.sync[1] = c; + state->state = WAITING_PAYLOAD; + } else { + resetState(state); + } + break; + case WAITING_PAYLOAD: + payload[state->payloadSize++] = c; + if(state->payloadSize == HEADTRACKER_PAYLOAD_SIZE) + { + state->state = WAITING_CRCH; + } + break; + case WAITING_CRCH: + state->attitude.crch = c; + state->state = WAITING_CRCL; + break; + case WAITING_CRCL: + state->attitude.crcl = c; + if(checkCrc(&(state->attitude))) { + headTrackerDevice.expires = micros() + MAX_HEADTRACKER_DATA_AGE_US; + headTrackerDevice.pan = constrain(state->attitude.pan, HEADTRACKER_RANGE_MIN, HEADTRACKER_RANGE_MAX); + headTrackerDevice.tilt = constrain(state->attitude.tilt, HEADTRACKER_RANGE_MIN, HEADTRACKER_RANGE_MAX); + headTrackerDevice.roll = constrain(state->attitude.roll, HEADTRACKER_RANGE_MIN, HEADTRACKER_RANGE_MAX); + DEBUG_SET(DEBUG_HEADTRACKING, 2, pktCount++); + } else { + DEBUG_SET(DEBUG_HEADTRACKING, 3, errorCount++); + } + resetState(state); + break; + } +} + + +bool gimbalSerialHeadTrackerDetect(void) +{ + bool singleUart = gimbalSerialConfig()->singleUart; + + SD(fprintf(stderr, "[GIMBAL_HTRK]: headtracker Detect...\n")); + serialPortConfig_t *portConfig = singleUart ? NULL : findSerialPortConfig(FUNCTION_GIMBAL_HEADTRACKER); + + if (portConfig) { + SD(fprintf(stderr, "[GIMBAL_HTRK]: found port...\n")); + headTrackerPort = openSerialPort(portConfig->identifier, FUNCTION_GIMBAL_HEADTRACKER, gimbalSerialHeadTrackerReceive, &headTrackerState, + baudRates[portConfig->peripheral_baudrateIndex], MODE_RXTX, SERIAL_NOT_INVERTED); + + if (headTrackerPort) { + SD(fprintf(stderr, "[GIMBAL_HTRK]: port open!\n")); + headTrackerPort->txBuffer = txBuffer; + headTrackerPort->txBufferSize = GIMBAL_SERIAL_BUFFER_SIZE; + headTrackerPort->txBufferTail = 0; + headTrackerPort->txBufferHead = 0; + } else { + SD(fprintf(stderr, "[GIMBAL_HTRK]: port NOT open!\n")); + return false; + } + } + + SD(fprintf(stderr, "[GIMBAL]: gimbalPort: %p headTrackerPort: %p\n", gimbalPort, headTrackerPort)); + return (singleUart && gimbalPort) || headTrackerPort; +} + +bool gimbalSerialHeadTrackerInit(void) +{ + if(headTrackerConfig()->devType == HEADTRACKER_SERIAL) { + if (gimbalSerialHeadTrackerDetect()) { + SD(fprintf(stderr, "Setting gimbal device\n")); + headTrackerCommonSetDevice(&headTrackerDevice); + + return true; + } + } + + return false; +} + +void headtrackerSerialProcess(headTrackerDevice_t *headTrackerDevice, timeUs_t currentTimeUs) +{ + UNUSED(headTrackerDevice); + UNUSED(currentTimeUs); + return; +} + +headTrackerDevType_e headtrackerSerialGetDeviceType(const headTrackerDevice_t *headTrackerDevice) +{ + UNUSED(headTrackerDevice); + return HEADTRACKER_SERIAL; +} + +#else + +void gimbalSerialHeadTrackerReceive(uint16_t c, void *data) +{ + UNUSED(c); + UNUSED(data); +} + +#endif + +#endif + +#endif \ No newline at end of file diff --git a/src/main/io/gimbal_serial.h b/src/main/io/gimbal_serial.h new file mode 100644 index 00000000000..6c6bf427672 --- /dev/null +++ b/src/main/io/gimbal_serial.h @@ -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 . + */ + +#pragma once + +#include + +#include "platform.h" + +#include "common/time.h" +#include "drivers/gimbal_common.h" +#include "drivers/headtracker_common.h" + +#ifdef __cplusplus +extern "C" { +#endif + +#ifdef USE_SERIAL_GIMBAL + + +#define HTKATTITUDE_SYNC0 0xA5 +#define HTKATTITUDE_SYNC1 0x5A +typedef struct gimbalHtkAttitudePkt_s +{ + uint8_t sync[2]; //data synchronization 0xA5, 0x5A + uint8_t mode:3; //Gimbal Mode [0~7] [Only 0 1 2 modes are supported for the time being] + int16_t sensibility:5; // Stabilization sensibility [-16~15] + uint8_t reserved:4; //hold on to one's reserve + int32_t roll:12; //Roll angle [-2048~2047] => [-180~180] + int32_t tilt:12; //Pich angle [-2048~2047] => [-180~180] + int32_t pan:12; //Yaw angle [-2048~2047] => [-180~180] + uint8_t crch; //Data validation H + uint8_t crcl; //Data validation L +} __attribute__((packed)) gimbalHtkAttitudePkt_t; + + +#define HEADTRACKER_PAYLOAD_SIZE (sizeof(gimbalHtkAttitudePkt_t) - 4) + +typedef enum { + WAITING_HDR1, + WAITING_HDR2, + WAITING_PAYLOAD, + WAITING_CRCH, + WAITING_CRCL, +} gimbalHeadtrackerState_e; + +typedef struct gimbalSerialHtrkState_s { + uint8_t payloadSize; + gimbalHeadtrackerState_e state; + gimbalHtkAttitudePkt_t attitude; +} gimbalSerialHtrkState_t; + +typedef struct gimbalSerialConfig_s { + bool singleUart; +} gimbalSerialConfig_t; + +PG_DECLARE(gimbalSerialConfig_t, gimbalSerialConfig); + + +int16_t gimbal_scale12(int16_t inputMin, int16_t inputMax, int16_t value); + +bool gimbalSerialInit(void); +bool gimbalSerialDetect(void); +void gimbalSerialProcess(gimbalDevice_t *gimbalDevice, timeUs_t currentTime); +bool gimbalSerialIsReady(const gimbalDevice_t *gimbalDevice); +gimbalDevType_e gimbalSerialGetDeviceType(const gimbalDevice_t *gimbalDevice); +bool gimbalSerialHasHeadTracker(const gimbalDevice_t *gimbalDevice); +void gimbalSerialHeadTrackerReceive(uint16_t c, void *data); + + +#if (defined(USE_HEADTRACKER) && defined(USE_HEADTRACKER_SERIAL)) +bool gimbalSerialHeadTrackerInit(void); +bool gimbalSerialHeadTrackerDetect(void); +void headtrackerSerialProcess(headTrackerDevice_t *headTrackerDevice, timeUs_t currentTimeUs); +headTrackerDevType_e headtrackerSerialGetDeviceType(const headTrackerDevice_t *headTrackerDevice); +bool headTrackerSerialIsReady(const headTrackerDevice_t *headTrackerDevice); +bool headTrackerSerialIsValid(const headTrackerDevice_t *headTrackerDevice); +int headTrackerSerialGetPanPWM(const headTrackerDevice_t *headTrackerDevice); +int headTrackerSerialGetTiltPWM(const headTrackerDevice_t *headTrackerDevice); +int headTrackerSerialGetRollPWM(const headTrackerDevice_t *headTrackerDevice); +#endif + +#endif + +#ifdef __cplusplus +} +#endif \ No newline at end of file diff --git a/src/main/io/headtracker_msp.c b/src/main/io/headtracker_msp.c new file mode 100644 index 00000000000..6a4b73dbbfb --- /dev/null +++ b/src/main/io/headtracker_msp.c @@ -0,0 +1,92 @@ +/* + * 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 + +#if (defined(USE_HEADTRACKER_MSP) && defined(USE_HEADTRACKER)) + +#include "build/debug.h" + +#include "common/utils.h" +#include "common/time.h" +#include "common/maths.h" + +#include "drivers/headtracker_common.h" + +#include "io/headtracker_msp.h" + +/* +typedef struct headTrackerVTable_s { + void (*process)(headTrackerDevice_t *headTrackerDevice, timeUs_t currentTimeUs); + headTrackerDevType_e (*getDeviceType)(const headTrackerDevice_t *headTrackerDevice); + bool (*isReady)(const headTrackerDevice_t *headTrackerDevice); + bool (*isValid)(const headTrackerDevice_t *headTrackerDevice); + int (*getPanPWM)(const headTrackerDevice_t *headTrackerDevice); + int (*getTiltPWM)(const headTrackerDevice_t *headTrackerDevice); + int (*getRollPWM)(const headTrackerDevice_t *headTrackerDevice); +} headtrackerVTable_t; +*/ + +static headTrackerVTable_t headTrackerMspVTable = { + .process = NULL, + .getDeviceType = heatTrackerMspGetDeviceType, + .isReady = NULL, + .isValid = NULL, +}; + +static headTrackerDevice_t headTrackerMspDevice = { + .vTable = &headTrackerMspVTable, + .pan = 0, + .tilt = 0, + .roll = 0, + .expires = 0, +}; + +void mspHeadTrackerInit(void) +{ + if(headTrackerConfig()->devType == HEADTRACKER_MSP) { + headTrackerCommonSetDevice(&headTrackerMspDevice); + } +} + +void mspHeadTrackerReceiverNewData(uint8_t *data, int dataSize) +{ + if(dataSize != sizeof(headtrackerMspMessage_t)) { + SD(fprintf(stderr, "[headTracker]: invalid data size %d\n", dataSize)); + return; + } + + headtrackerMspMessage_t *status = (headtrackerMspMessage_t *)data; + + headTrackerMspDevice.pan = constrain(status->pan, HEADTRACKER_RANGE_MIN, HEADTRACKER_RANGE_MAX); + headTrackerMspDevice.tilt = constrain(status->tilt, HEADTRACKER_RANGE_MIN, HEADTRACKER_RANGE_MAX); + headTrackerMspDevice.roll = constrain(status->roll, HEADTRACKER_RANGE_MIN, HEADTRACKER_RANGE_MAX); + headTrackerMspDevice.expires = micros() + MAX_HEADTRACKER_DATA_AGE_US; + + SD(fprintf(stderr, "[headTracker]: pan: %d tilt: %d roll: %d\n", status->pan, status->tilt, status->roll)); + SD(fprintf(stderr, "[headTracker]: scaled pan: %d tilt: %d roll: %d\n", headTrackerMspDevice.pan, headTrackerMspDevice.tilt, headTrackerMspDevice.roll)); + + UNUSED(status); +} + +headTrackerDevType_e heatTrackerMspGetDeviceType(const headTrackerDevice_t *headTrackerDevice) { + UNUSED(headTrackerDevice); + return HEADTRACKER_MSP; +} + +#endif \ No newline at end of file diff --git a/src/main/io/headtracker_msp.h b/src/main/io/headtracker_msp.h new file mode 100644 index 00000000000..edcfb465874 --- /dev/null +++ b/src/main/io/headtracker_msp.h @@ -0,0 +1,44 @@ +/* + * 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 + +#if (defined(USE_HEADTRACKER_MSP) && defined(USE_HEADTRACKER)) + +#include +#include + +#include "drivers/headtracker_common.h" + +typedef struct headtrackerMspMessage_s { + uint8_t version; // 0 + int16_t pan; // -2048~2047. Scale is min/max angle for gimbal + int16_t tilt; // -2048~2047. Scale is min/max angle for gimbal + int16_t roll; // -2048~2047. Scale is min/max angle for gimbal + int16_t sensitivity; // -16~15. Scale is min/max angle for gimbal +} __attribute__((packed)) headtrackerMspMessage_t; + +void mspHeadTrackerInit(void); + +void mspHeadTrackerReceiverNewData(uint8_t *data, int dataSize); + +headTrackerDevType_e heatTrackerMspGetDeviceType(const headTrackerDevice_t *headTrackerDevice); + +#endif diff --git a/src/main/io/serial.h b/src/main/io/serial.h index 7766679106b..aa9d709472e 100644 --- a/src/main/io/serial.h +++ b/src/main/io/serial.h @@ -57,6 +57,8 @@ typedef enum { FUNCTION_TELEMETRY_SMARTPORT_MASTER = (1 << 23), // 8388608 FUNCTION_UNUSED_2 = (1 << 24), // 16777216 FUNCTION_MSP_OSD = (1 << 25), // 33554432 + FUNCTION_GIMBAL = (1 << 26), // 67108864 + FUNCTION_GIMBAL_HEADTRACKER = (1 << 27), // 134217728 } serialPortFunction_e; #define FUNCTION_VTX_MSP FUNCTION_MSP_OSD diff --git a/src/main/msp/msp_protocol_v2_sensor.h b/src/main/msp/msp_protocol_v2_sensor.h index d913723be7d..b243448e9f9 100644 --- a/src/main/msp/msp_protocol_v2_sensor.h +++ b/src/main/msp/msp_protocol_v2_sensor.h @@ -22,4 +22,5 @@ #define MSP2_SENSOR_GPS 0x1F03 #define MSP2_SENSOR_COMPASS 0x1F04 #define MSP2_SENSOR_BAROMETER 0x1F05 -#define MSP2_SENSOR_AIRSPEED 0x1F06 \ No newline at end of file +#define MSP2_SENSOR_AIRSPEED 0x1F06 +#define MSP2_SENSOR_HEADTRACKER 0x1F07 \ No newline at end of file diff --git a/src/main/scheduler/scheduler.h b/src/main/scheduler/scheduler.h index c1e0ab6bb53..a43206840fe 100755 --- a/src/main/scheduler/scheduler.h +++ b/src/main/scheduler/scheduler.h @@ -125,6 +125,14 @@ typedef enum { #ifdef USE_ADAPTIVE_FILTER TASK_ADAPTIVE_FILTER, #endif +#ifdef USE_SERIAL_GIMBAL + TASK_GIMBAL, +#endif + +#ifdef USE_HEADTRACKER + TASK_HEADTRACKER, +#endif + /* Count of real tasks */ TASK_COUNT, diff --git a/src/main/target/SITL/target.h b/src/main/target/SITL/target.h index 048626ad8dd..f6d80f74971 100644 --- a/src/main/target/SITL/target.h +++ b/src/main/target/SITL/target.h @@ -74,6 +74,10 @@ #define USE_MSP_OSD #define USE_OSD +#define USE_SERIAL_GIMBAL +#define USE_HEADTRACKER +#define USE_HEADTRACKER_SERIAL +#define USE_HEADTRACKER_MSP #undef USE_DASHBOARD diff --git a/src/main/target/common.h b/src/main/target/common.h index 408609485ab..8bb735fc437 100644 --- a/src/main/target/common.h +++ b/src/main/target/common.h @@ -190,6 +190,10 @@ #define ADSB_LIMIT_CM 6400000 #endif +#define USE_SERIAL_GIMBAL +#define USE_HEADTRACKER +#define USE_HEADTRACKER_SERIAL +#define USE_HEADTRACKER_MSP //Designed to free space of F722 and F411 MCUs #if (MCU_FLASH_SIZE > 512) diff --git a/src/test/unit/CMakeLists.txt b/src/test/unit/CMakeLists.txt index ebfd3b78d4f..300721b8f53 100644 --- a/src/test/unit/CMakeLists.txt +++ b/src/test/unit/CMakeLists.txt @@ -40,6 +40,9 @@ set_property(SOURCE osd_unittest.cc PROPERTY definitions OSD_UNIT_TEST USE_MSP_D set_property(SOURCE gps_ublox_unittest.cc PROPERTY depends "io/gps_ublox_utils.c") set_property(SOURCE gps_ublox_unittest.cc PROPERTY definitions GPS_UBLOX_UNIT_TEST) +set_property(SOURCE gimbal_serial_unittest.cc PROPERTY depends "io/gimbal_serial.c" "drivers/gimbal_common.c" "common/maths.c") +set_property(SOURCE gimbal_serial_unittest.cc PROPERTY definitions USE_SERIAL_GIMBAL GIMBAL_UNIT_TEST) + function(unit_test src) get_filename_component(basename ${src} NAME) string(REPLACE ".cc" "" name ${basename} ) diff --git a/src/test/unit/gimbal_serial_unittest.cc b/src/test/unit/gimbal_serial_unittest.cc new file mode 100644 index 00000000000..77ae4c93df8 --- /dev/null +++ b/src/test/unit/gimbal_serial_unittest.cc @@ -0,0 +1,49 @@ +/* + * 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 + +#include "gtest/gtest.h" +#include "unittest_macros.h" + +#include "io/gimbal_serial.h" + +void dumpMemory(uint8_t *mem, int size) +{ + for(int i =0; i < size; ++i) { + printf("%02x ", mem[i]); + } + printf("\n"); +} + +TEST(GimbalSerialTest, TestGimbalSerialScale) +{ + int16_t res16 = gimbal_scale12(1000, 2000, 2000); + EXPECT_TRUE(res16 == 2047); + res16 = gimbal_scale12(1000, 2000, 1000); + printf("res16: %i\n", res16); + EXPECT_TRUE(res16 == -2048); + res16 = gimbal_scale12(1000, 2000, 1500); + printf("res16: %i\n", res16); + EXPECT_TRUE(res16 == -1); + res16 = gimbal_scale12(1000, 2000, 1501); + printf("res16: %i\n", res16); + EXPECT_TRUE(res16 == 3); +} \ No newline at end of file diff --git a/src/test/unit/maths_unittest.cc b/src/test/unit/maths_unittest.cc index 538f28086af..77779d366e2 100644 --- a/src/test/unit/maths_unittest.cc +++ b/src/test/unit/maths_unittest.cc @@ -128,28 +128,28 @@ void expectVectorsAreEqual(fpVector3_t *a, fpVector3_t *b) TEST(MathsUnittest, TestRotateVectorWithNoAngle) { - fpVector3_t vector = { 1.0f, 0.0f, 0.0f}; + fpVector3_t vector = { .x = 1.0f, .y = 0.0f, .z = 0.0f}; fp_angles_t euler_angles = {.raw={0.0f, 0.0f, 0.0f}}; fpMat3_t rmat; rotationMatrixFromAngles(&rmat, &euler_angles); rotationMatrixRotateVector(&vector, &vector, &rmat); - fpVector3_t expected_result = { 1.0f, 0.0f, 0.0f}; + fpVector3_t expected_result = { .x = 1.0f, .y = 0.0f, .z = 0.0f}; expectVectorsAreEqual(&vector, &expected_result); } TEST(MathsUnittest, TestRotateVectorAroundAxis) { // Rotate a vector <1, 0, 0> around an each axis x y and z. - fpVector3_t vector = { 1.0f, 0.0f, 0.0f}; + fpVector3_t vector = { .x = 1.0f, .y = 0.0f, .z = 0.0f}; fp_angles_t euler_angles = {.raw={90.0f, 0.0f, 0.0f}}; fpMat3_t rmat; rotationMatrixFromAngles(&rmat, &euler_angles); rotationMatrixRotateVector(&vector, &vector, &rmat); - fpVector3_t expected_result = { 1.0f, 0.0f, 0.0f}; + fpVector3_t expected_result = { .x = 1.0f, .y = 0.0f, .z = 0.0f}; expectVectorsAreEqual(&vector, &expected_result); }