diff --git a/docs/Settings.md b/docs/Settings.md index 305870c066d..2d800a78c54 100644 --- a/docs/Settings.md +++ b/docs/Settings.md @@ -1622,6 +1622,16 @@ Trim gimbal tilt center position. --- +### gimbal_type + +Type of gimbval dervice + +| Default | Min | Max | +| --- | --- | --- | +| SERIAL | | | + +--- + ### 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 7f188974cd9..43b09c8f837 100755 --- a/src/main/CMakeLists.txt +++ b/src/main/CMakeLists.txt @@ -362,6 +362,8 @@ main_sources(COMMON_SRC io/servo_sbus.h io/frsky_osd.c io/frsky_osd.h + io/gimbal_mavlink.c + io/gimbal_mavlink.h io/gimbal_serial.c io/gimbal_serial.h io/headtracker_msp.c diff --git a/src/main/drivers/gimbal_common.c b/src/main/drivers/gimbal_common.c index e03e7fd9ddd..c575886dda3 100644 --- a/src/main/drivers/gimbal_common.c +++ b/src/main/drivers/gimbal_common.c @@ -17,7 +17,7 @@ #include "platform.h" -#ifdef USE_SERIAL_GIMBAL +#ifdef USE_GIMBAL #include #include @@ -45,7 +45,8 @@ PG_RESET_TEMPLATE(gimbalConfig_t, gimbalConfig, .sensitivity = SETTING_GIMBAL_SENSITIVITY_DEFAULT, .panTrim = SETTING_GIMBAL_PAN_TRIM_DEFAULT, .tiltTrim = SETTING_GIMBAL_TILT_TRIM_DEFAULT, - .rollTrim = SETTING_GIMBAL_ROLL_TRIM_DEFAULT + .rollTrim = SETTING_GIMBAL_ROLL_TRIM_DEFAULT, + .gimbalType = SETTING_GIMBAL_TYPE_DEFAULT, ); static gimbalDevice_t *commonGimbalDevice = NULL; @@ -65,14 +66,14 @@ gimbalDevice_t *gimbalCommonDevice(void) return commonGimbalDevice; } -void gimbalCommonProcess(gimbalDevice_t *gimbalDevice, timeUs_t currentTimeUs) +void gimbalCommonProcess(const gimbalDevice_t *gimbalDevice, timeUs_t currentTimeUs) { if (gimbalDevice && gimbalDevice->vTable->process && gimbalCommonIsReady(gimbalDevice)) { gimbalDevice->vTable->process(gimbalDevice, currentTimeUs); } } -gimbalDevType_e gimbalCommonGetDeviceType(gimbalDevice_t *gimbalDevice) +gimbalDevType_e gimbalCommonGetDeviceType(const gimbalDevice_t *gimbalDevice) { if (!gimbalDevice || !gimbalDevice->vTable->getDeviceType) { return GIMBAL_DEV_UNKNOWN; @@ -81,7 +82,7 @@ gimbalDevType_e gimbalCommonGetDeviceType(gimbalDevice_t *gimbalDevice) return gimbalDevice->vTable->getDeviceType(gimbalDevice); } -bool gimbalCommonIsReady(gimbalDevice_t *gimbalDevice) +bool gimbalCommonIsReady(const gimbalDevice_t *gimbalDevice) { if (gimbalDevice && gimbalDevice->vTable->isReady) { return gimbalDevice->vTable->isReady(gimbalDevice); @@ -111,7 +112,7 @@ void taskUpdateGimbal(timeUs_t currentTimeUs) // TODO: check if any gimbal types are enabled bool gimbalCommonIsEnabled(void) { - return true; + return gimbalCommonDevice() != NULL;; } diff --git a/src/main/drivers/gimbal_common.h b/src/main/drivers/gimbal_common.h index 879e81116a0..281ca637e86 100644 --- a/src/main/drivers/gimbal_common.h +++ b/src/main/drivers/gimbal_common.h @@ -19,7 +19,7 @@ #include "platform.h" -#ifdef USE_SERIAL_GIMBAL +#ifdef USE_GIMBAL #include @@ -33,6 +33,7 @@ extern "C" { typedef enum { GIMBAL_DEV_UNSUPPORTED = 0, GIMBAL_DEV_SERIAL, + GIMBAL_DEV_MAVLINK, GIMBAL_DEV_UNKNOWN=0xFF } gimbalDevType_e; @@ -41,6 +42,7 @@ struct gimbalVTable_s; typedef struct gimbalDevice_s { const struct gimbalVTable_s *vTable; + // for OSD display int16_t currentPanPWM; } gimbalDevice_t; @@ -49,10 +51,11 @@ typedef struct gimbalDevice_s { // {set,get}PitMode: 0 = OFF, 1 = ON typedef struct gimbalVTable_s { - void (*process)(gimbalDevice_t *gimbalDevice, timeUs_t currentTimeUs); + void (*process)(const 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); + // Used by OSD pan compensation int16_t (*getGimbalPanPWM)(const gimbalDevice_t *gimbalDevice); } gimbalVTable_t; @@ -65,6 +68,7 @@ typedef struct gimbalConfig_s { uint16_t panTrim; uint16_t tiltTrim; uint16_t rollTrim; + gimbalDevType_e gimbalType; } gimbalConfig_t; PG_DECLARE(gimbalConfig_t, gimbalConfig); @@ -85,9 +89,9 @@ 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 gimbalCommonProcess(const gimbalDevice_t *gimbalDevice, timeUs_t currentTimeUs); +gimbalDevType_e gimbalCommonGetDeviceType(const gimbalDevice_t *gimbalDevice); +bool gimbalCommonIsReady(const gimbalDevice_t *gimbalDevice); void taskUpdateGimbal(timeUs_t currentTimeUs); @@ -95,6 +99,7 @@ void taskUpdateGimbal(timeUs_t currentTimeUs); bool gimbalCommonIsEnabled(void); bool gimbalCommonHtrkIsEnabled(void); +// For OSD pan compensation int16_t gimbalCommonGetPanPwm(const gimbalDevice_t *gimbalDevice); #ifdef __cplusplus diff --git a/src/main/drivers/headtracker_common.h b/src/main/drivers/headtracker_common.h index 92dac7e61e6..1bbc4977c27 100644 --- a/src/main/drivers/headtracker_common.h +++ b/src/main/drivers/headtracker_common.h @@ -33,9 +33,6 @@ #include "config/feature.h" -#ifdef __cplusplus -extern "C" { -#endif typedef enum { HEADTRACKER_NONE = 0, @@ -84,6 +81,10 @@ typedef struct headTrackerConfig_s { PG_DECLARE(headTrackerConfig_t, headTrackerConfig); +#ifdef __cplusplus +extern "C" { +#endif + void headTrackerCommonInit(void); void headTrackerCommonSetDevice(headTrackerDevice_t *headTrackerDevice); headTrackerDevice_t *headTrackerCommonDevice(void); @@ -113,4 +114,4 @@ bool headtrackerCommonIsEnabled(void); } #endif -#endif \ No newline at end of file +#endif diff --git a/src/main/fc/fc_init.c b/src/main/fc/fc_init.c index bc84d380a37..5445311df8d 100644 --- a/src/main/fc/fc_init.c +++ b/src/main/fc/fc_init.c @@ -109,6 +109,7 @@ #include "io/displayport_msp_osd.h" #include "io/displayport_srxl.h" #include "io/flashfs.h" +#include "io/gimbal_mavlink.h" #include "io/gimbal_serial.h" #include "io/headtracker_msp.h" #include "io/gps.h" @@ -695,11 +696,15 @@ void init(void) initDShotCommands(); #endif -#ifdef USE_SERIAL_GIMBAL +#ifdef USE_GIMBAL gimbalCommonInit(); - // Needs to be called before gimbalSerialHeadTrackerInit +#ifdef USE_GIMBAL_SERIAL gimbalSerialInit(); #endif +#ifdef USE_GIMBAL_MAVLINK + gimbalMavlinkInit(); +#endif +#endif // USE_GIMBAL #ifdef USE_HEADTRACKER headTrackerCommonInit(); diff --git a/src/main/fc/fc_msp_box.c b/src/main/fc/fc_msp_box.c index a20a23e0b24..643f2f5250e 100644 --- a/src/main/fc/fc_msp_box.c +++ b/src/main/fc/fc_msp_box.c @@ -366,7 +366,7 @@ void initActiveBoxIds(void) ADD_ACTIVE_BOX(BOXMIXERTRANSITION); #endif -#ifdef USE_SERIAL_GIMBAL +#ifdef USE_GIMBAL if (gimbalCommonIsEnabled()) { ADD_ACTIVE_BOX(BOXGIMBALTLOCK); ADD_ACTIVE_BOX(BOXGIMBALRLOCK); @@ -452,7 +452,7 @@ void packBoxModeFlags(boxBitmask_t * mspBoxModeFlags) #endif CHECK_ACTIVE_BOX(IS_ENABLED(IS_RC_MODE_ACTIVE(BOXANGLEHOLD)), BOXANGLEHOLD); -#ifdef USE_SERIAL_GIMBAL +#ifdef USE_GIMBAL if(IS_RC_MODE_ACTIVE(BOXGIMBALCENTER)) { CHECK_ACTIVE_BOX(IS_ENABLED(IS_RC_MODE_ACTIVE(BOXGIMBALCENTER)), BOXGIMBALCENTER); #ifdef USE_HEADTRACKER diff --git a/src/main/fc/fc_tasks.c b/src/main/fc/fc_tasks.c index 4ca125d5c6a..f7d6e4749ed 100755 --- a/src/main/fc/fc_tasks.c +++ b/src/main/fc/fc_tasks.c @@ -436,7 +436,7 @@ void fcTasksInit(void) setTaskEnabled(TASK_SMARTPORT_MASTER, true); #endif -#ifdef USE_SERIAL_GIMBAL +#ifdef USE_GIMBAL setTaskEnabled(TASK_GIMBAL, true); #endif @@ -724,7 +724,7 @@ cfTask_t cfTasks[TASK_COUNT] = { }, #endif -#ifdef USE_SERIAL_GIMBAL +#ifdef USE_GIMBAL [TASK_GIMBAL] = { .taskName = "GIMBAL", .taskFunc = taskUpdateGimbal, diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index 67331ae6a5b..ef07f483749 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -198,6 +198,9 @@ tables: - name: headtracker_dev_type values: ["NONE", "SERIAL", "MSP"] enum: headTrackerDevType_e + - name: gimbal_dev_type + values: ["NONE", "SERIAL", "MAVLINK"] + enum: gimbalDevType_e - name: mavlink_radio_type values: ["GENERIC", "ELRS", "SIK"] enum: mavlinkRadio_e @@ -3229,7 +3232,6 @@ groups: table: mavlink_radio_type default_value: "GENERIC" type: uint8_t - - name: PG_OSD_CONFIG type: osdConfig_t headers: ["io/osd.h", "drivers/osd.h"] @@ -4281,8 +4283,14 @@ groups: - name: PG_GIMBAL_CONFIG type: gimbalConfig_t headers: ["drivers/gimbal_common.h"] - condition: USE_SERIAL_GIMBAL + condition: USE_GIMBAL members: + - name: gimbal_type + field: gimbalType + description: "Type of gimbval dervice" + default_value: "SERIAL" + table: gimbal_dev_type + type: uint8_t - name: gimbal_pan_channel description: "Gimbal pan rc channel index. 0 is no channel." default_value: 0 @@ -4328,7 +4336,7 @@ groups: - name: PG_GIMBAL_SERIAL_CONFIG type: gimbalSerialConfig_t headers: ["io/gimbal_serial.h"] - condition: USE_SERIAL_GIMBAL + condition: USE_GIMBAL_SERIAL 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." diff --git a/src/main/io/gimbal_mavlink.c b/src/main/io/gimbal_mavlink.c new file mode 100644 index 00000000000..5b20c8bf60b --- /dev/null +++ b/src/main/io/gimbal_mavlink.c @@ -0,0 +1,175 @@ +/* + * 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_GIMBAL_MAVLINK + +#include +#include +#include +#include + +#include +#include +#include +#include + +#include +#include + +#include +#include + +#include + +#include + +#include "settings_generated.h" + +#pragma GCC diagnostic push +#pragma GCC diagnostic ignored "-Wunused-function" +#ifndef MAVLINK_COMM_NUM_BUFFERS +#define MAVLINK_COMM_NUM_BUFFERS 1 +#endif +#include "common/mavlink.h" +#pragma GCC diagnostic pop + + +gimbalVTable_t gimbalMavlinkVTable = { + .process = gimbalMavlinkProcess, + .getDeviceType = gimbalMavlinkGetDeviceType, + .isReady = gimbalMavlinkIsReady, + .hasHeadTracker = gimbalMavlinkHasHeadTracker, +}; + +static gimbalDevice_t mavlinkGimbalDevice = { + .vTable = &gimbalMavlinkVTable, + .currentPanPWM = PWM_RANGE_MIDDLE +}; + +gimbalDevType_e gimbalMavlinkGetDeviceType(const gimbalDevice_t *gimbalDevice) +{ + UNUSED(gimbalDevice); + return GIMBAL_DEV_MAVLINK; +} + +bool gimbalMavlinkIsReady(const gimbalDevice_t *gimbalDevice) +{ + return (gimbalCommonGetDeviceType(gimbalDevice) == GIMBAL_DEV_MAVLINK) && isMAVLinkTelemetryEnabled(); +} + +bool gimbalMavlinkHasHeadTracker(const gimbalDevice_t *gimbalDevice) +{ + UNUSED(gimbalDevice); + return false; +} + +bool gimbalMavlinkInit(void) +{ + if (gimbalMavlinkDetect()) { + SD(fprintf(stderr, "Setting gimbal device\n")); + gimbalCommonSetDevice(&mavlinkGimbalDevice); + return true; + } + + return false; +} + +bool gimbalMavlinkDetect(void) +{ + + SD(fprintf(stderr, "[GIMBAL]: serial Detect...\n")); + serialPortConfig_t *portConfig = findSerialPortConfig(FUNCTION_TELEMETRY_MAVLINK); + SD(fprintf(stderr, "[GIMBAL]: portConfig: %p\n", portConfig)); + SD(fprintf(stderr, "[GIMBAL]: MAVLINK: %s\n", gimbalConfig()->gimbalType == GIMBAL_DEV_MAVLINK ? "yes" : "no")); + return portConfig && gimbalConfig()->gimbalType == GIMBAL_DEV_MAVLINK; +} + +void gimbalMavlinkProcess(const gimbalDevice_t *gimbalDevice, timeUs_t currentTime) +{ + UNUSED(currentTime); + UNUSED(gimbalDevice); + + if (!gimbalCommonIsReady(gimbalDevice)) { + SD(fprintf(stderr, "[GIMBAL] gimbal not ready...\n")); + return; + } + + const gimbalConfig_t *cfg = gimbalConfig(); + + int panPWM = PWM_RANGE_MIDDLE + cfg->panTrim; + int tiltPWM = PWM_RANGE_MIDDLE + cfg->tiltTrim; + int rollPWM = PWM_RANGE_MIDDLE + cfg->rollTrim; + + 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) { + panPWM = rxGetChannelValue(cfg->panChannel - 1) + cfg->panTrim; + panPWM = constrain(panPWM, PWM_RANGE_MIN, PWM_RANGE_MAX); + } + + if (cfg->tiltChannel > 0) { + tiltPWM = rxGetChannelValue(cfg->tiltChannel - 1) + cfg->tiltTrim; + tiltPWM = constrain(tiltPWM, PWM_RANGE_MIN, PWM_RANGE_MAX); + } + + if (cfg->rollChannel > 0) { + rollPWM = rxGetChannelValue(cfg->rollChannel - 1) + cfg->rollTrim; + rollPWM = constrain(rollPWM, PWM_RANGE_MIN, PWM_RANGE_MAX); + } + } + + { + 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, panPWM); + //attitude.tilt = gimbal_scale12(PWM_RANGE_MIN, PWM_RANGE_MAX, tiltPWM); + //attitude.roll = gimbal_scale12(PWM_RANGE_MIN, PWM_RANGE_MAX, rollPWM); + } + + //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; + + //serialGimbalDevice.currentPanPWM = gimbal2pwm(attitude.pan); + + //serialBeginWrite(gimbalPort); + //serialWriteBuf(gimbalPort, (uint8_t *)&attitude, sizeof(gimbalHtkAttitudePkt_t)); + //serialEndWrite(gimbalPort); +} +#endif diff --git a/src/main/io/gimbal_mavlink.h b/src/main/io/gimbal_mavlink.h new file mode 100644 index 00000000000..08236debbaf --- /dev/null +++ b/src/main/io/gimbal_mavlink.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 + +#include "platform.h" + +#include "common/time.h" +#include "drivers/gimbal_common.h" + +#ifdef __cplusplus +extern "C" { +#endif + +#ifdef USE_GIMBAL_MAVLINK + +bool gimbalMavlinkInit(void); +bool gimbalMavlinkDetect(void); +void gimbalMavlinkProcess(const gimbalDevice_t *gimbalDevice, timeUs_t currentTime); +bool gimbalMavlinkIsReady(const gimbalDevice_t *gimbalDevice); +bool gimbalMavlinkHasHeadTracker(const gimbalDevice_t *gimbalDevice); +gimbalDevType_e gimbalMavlinkGetDeviceType(const gimbalDevice_t *gimbalDevice); + +#endif + +#ifdef __cplusplus +} +#endif \ No newline at end of file diff --git a/src/main/io/gimbal_serial.c b/src/main/io/gimbal_serial.c index fd6294c9959..5c7d97ae0be 100644 --- a/src/main/io/gimbal_serial.c +++ b/src/main/io/gimbal_serial.c @@ -22,7 +22,7 @@ #include "platform.h" -#ifdef USE_SERIAL_GIMBAL +#ifdef USE_GIMBAL_SERIAL #include #include @@ -175,13 +175,13 @@ bool gimbalSerialDetect(void) #endif #ifdef GIMBAL_UNIT_TEST -void gimbalSerialProcess(gimbalDevice_t *gimbalDevice, timeUs_t currentTime) +void gimbalSerialProcess(const gimbalDevice_t *gimbalDevice, timeUs_t currentTime) { UNUSED(gimbalDevice); UNUSED(currentTime); } #else -void gimbalSerialProcess(gimbalDevice_t *gimbalDevice, timeUs_t currentTime) +void gimbalSerialProcess(const gimbalDevice_t *gimbalDevice, timeUs_t currentTime) { UNUSED(currentTime); diff --git a/src/main/io/gimbal_serial.h b/src/main/io/gimbal_serial.h index 6c9a3251f89..f11a4238f6e 100644 --- a/src/main/io/gimbal_serial.h +++ b/src/main/io/gimbal_serial.h @@ -29,7 +29,7 @@ extern "C" { #endif -#ifdef USE_SERIAL_GIMBAL +#ifdef USE_GIMBAL_SERIAL #define HTKATTITUDE_SYNC0 0xA5 @@ -76,7 +76,7 @@ int16_t gimbal2pwm(int16_t value); bool gimbalSerialInit(void); bool gimbalSerialDetect(void); -void gimbalSerialProcess(gimbalDevice_t *gimbalDevice, timeUs_t currentTime); +void gimbalSerialProcess(const 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); diff --git a/src/main/scheduler/scheduler.h b/src/main/scheduler/scheduler.h index 5d38a6dba59..6d7a9530cd5 100755 --- a/src/main/scheduler/scheduler.h +++ b/src/main/scheduler/scheduler.h @@ -125,7 +125,7 @@ typedef enum { #ifdef USE_ADAPTIVE_FILTER TASK_ADAPTIVE_FILTER, #endif -#ifdef USE_SERIAL_GIMBAL +#ifdef USE_GIMBAL TASK_GIMBAL, #endif diff --git a/src/main/target/SITL/target.h b/src/main/target/SITL/target.h index f7963dfdd95..72d4f4889dc 100644 --- a/src/main/target/SITL/target.h +++ b/src/main/target/SITL/target.h @@ -74,7 +74,9 @@ #define USE_MSP_OSD #define USE_OSD -#define USE_SERIAL_GIMBAL +#define USE_GIMBAL +#define USE_GIMBAL_SERIAL +#define USE_GIMBAL_MAVLINK #define USE_HEADTRACKER #define USE_HEADTRACKER_SERIAL #define USE_HEADTRACKER_MSP diff --git a/src/main/target/common.h b/src/main/target/common.h index af0de4bf30f..af483671742 100644 --- a/src/main/target/common.h +++ b/src/main/target/common.h @@ -190,7 +190,9 @@ #define ADSB_LIMIT_CM 6400000 #endif -#define USE_SERIAL_GIMBAL +#define USE_GIMBAL +#define USE_GIMBAL_SERIAL +#define USE_GIMBAL_MAVLINK #define USE_HEADTRACKER #define USE_HEADTRACKER_SERIAL #define USE_HEADTRACKER_MSP diff --git a/src/main/telemetry/mavlink.c b/src/main/telemetry/mavlink.c index 92ddeb7a990..8bf38e4c690 100644 --- a/src/main/telemetry/mavlink.c +++ b/src/main/telemetry/mavlink.c @@ -44,6 +44,8 @@ #include "drivers/time.h" #include "drivers/display.h" #include "drivers/osd_symbols.h" +#include "drivers/gimbal_common.h" +#include "drivers/headtracker_common.h" #include "fc/config.h" #include "fc/fc_core.h" @@ -283,6 +285,12 @@ void initMAVLinkTelemetry(void) mavlinkPortSharing = determinePortSharing(portConfig, FUNCTION_TELEMETRY_MAVLINK); } + +bool isMAVLinkTelemetryEnabled(void) +{ + return portConfig && mavlinkPort; +} + void configureMAVLinkTelemetryPort(void) { if (!portConfig) { @@ -491,9 +499,31 @@ void mavlinkSendSystemStatus(void) mavlinkSendMessage(); } +static inline int16_t getChannelValue(uint8_t x) +{ + int16_t channel_value = (rxRuntimeConfig.channelCount > x) ? rxGetChannelValue(x) : 0; +#if defined(USE_GIMBAL) && defined(USE_GIMBAL_MAVLINK) + // override channel values if gimbal is enabled + if(gimbalCommonIsEnabled() && gimbalConfig()->gimbalType == GIMBAL_DEV_MAVLINK && IS_RC_MODE_ACTIVE(BOXGIMBALHTRK)) { + headTrackerDevice_t *dev = headTrackerCommonDevice(); + if (dev != NULL) { + x = x + 1; // channels are 1-indexed + if (x == gimbalConfig()->panChannel) { + channel_value = headTrackerCommonGetPanPWM(dev); + } else if (x == gimbalConfig()->tiltChannel) { + channel_value = headTrackerCommonGetTiltPWM(dev); + } else if (x == gimbalConfig()->rollChannel) { + channel_value = headTrackerCommonGetRollPWM(dev); + } + } + } +#endif + + return channel_value; +} + void mavlinkSendRCChannelsAndRSSI(void) { -#define GET_CHANNEL_VALUE(x) ((rxRuntimeConfig.channelCount >= (x + 1)) ? rxGetChannelValue(x) : 0) if (telemetryConfig()->mavlink.version == 1) { mavlink_msg_rc_channels_raw_pack(mavSystemId, mavComponentId, &mavSendMsg, // time_boot_ms Timestamp (milliseconds since system boot) @@ -501,21 +531,21 @@ void mavlinkSendRCChannelsAndRSSI(void) // port Servo output port (set of 8 outputs = 1 port). Most MAVs will just use one, but this allows to encode more than 8 servos. 0, // chan1_raw RC channel 1 value, in microseconds - GET_CHANNEL_VALUE(0), + getChannelValue(0), // chan2_raw RC channel 2 value, in microseconds - GET_CHANNEL_VALUE(1), + getChannelValue(1), // chan3_raw RC channel 3 value, in microseconds - GET_CHANNEL_VALUE(2), + getChannelValue(2), // chan4_raw RC channel 4 value, in microseconds - GET_CHANNEL_VALUE(3), + getChannelValue(3), // chan5_raw RC channel 5 value, in microseconds - GET_CHANNEL_VALUE(4), + getChannelValue(4), // chan6_raw RC channel 6 value, in microseconds - GET_CHANNEL_VALUE(5), + getChannelValue(5), // chan7_raw RC channel 7 value, in microseconds - GET_CHANNEL_VALUE(6), + getChannelValue(6), // chan8_raw RC channel 8 value, in microseconds - GET_CHANNEL_VALUE(7), + getChannelValue(7), // rssi Receive signal strength indicator, 0: 0%, 254: 100% //https://github.com/mavlink/mavlink/issues/1027 scaleRange(getRSSI(), 0, 1023, 0, 254)); @@ -527,41 +557,41 @@ void mavlinkSendRCChannelsAndRSSI(void) // Total number of RC channels being received. rxRuntimeConfig.channelCount, // chan1_raw RC channel 1 value, in microseconds - GET_CHANNEL_VALUE(0), + getChannelValue(0), // chan2_raw RC channel 2 value, in microseconds - GET_CHANNEL_VALUE(1), + getChannelValue(1), // chan3_raw RC channel 3 value, in microseconds - GET_CHANNEL_VALUE(2), + getChannelValue(2), // chan4_raw RC channel 4 value, in microseconds - GET_CHANNEL_VALUE(3), + getChannelValue(3), // chan5_raw RC channel 5 value, in microseconds - GET_CHANNEL_VALUE(4), + getChannelValue(4), // chan6_raw RC channel 6 value, in microseconds - GET_CHANNEL_VALUE(5), + getChannelValue(5), // chan7_raw RC channel 7 value, in microseconds - GET_CHANNEL_VALUE(6), + getChannelValue(6), // chan8_raw RC channel 8 value, in microseconds - GET_CHANNEL_VALUE(7), + getChannelValue(7), // chan9_raw RC channel 9 value, in microseconds - GET_CHANNEL_VALUE(8), + getChannelValue(8), // chan10_raw RC channel 10 value, in microseconds - GET_CHANNEL_VALUE(9), + getChannelValue(9), // chan11_raw RC channel 11 value, in microseconds - GET_CHANNEL_VALUE(10), + getChannelValue(10), // chan12_raw RC channel 12 value, in microseconds - GET_CHANNEL_VALUE(11), + getChannelValue(11), // chan13_raw RC channel 13 value, in microseconds - GET_CHANNEL_VALUE(12), + getChannelValue(12), // chan14_raw RC channel 14 value, in microseconds - GET_CHANNEL_VALUE(13), + getChannelValue(13), // chan15_raw RC channel 15 value, in microseconds - GET_CHANNEL_VALUE(14), + getChannelValue(14), // chan16_raw RC channel 16 value, in microseconds - GET_CHANNEL_VALUE(15), + getChannelValue(15), // chan17_raw RC channel 17 value, in microseconds - GET_CHANNEL_VALUE(16), + getChannelValue(16), // chan18_raw RC channel 18 value, in microseconds - GET_CHANNEL_VALUE(17), + getChannelValue(17), // rssi Receive signal strength indicator, 0: 0%, 254: 100% //https://github.com/mavlink/mavlink/issues/1027 scaleRange(getRSSI(), 0, 1023, 0, 254)); @@ -905,6 +935,27 @@ void mavlinkSendBatteryTemperatureStatusText(void) } +#if defined(USE_GIMBAL) && defined(USE_GIMBAL_MAVLINK) +void mavlinkSendGimbalAttitude(void) +{ + // https://mavlink.io/en/messages/common.html#AUTOPILOT_STATE_FOR_GIMBAL_DEVICE + uint8_t targetSystem = MAV_TYPE_GIMBAL; + uint8_t targetComponent = MAV_COMP_ID_GIMBAL; + uint8_t time_boot_us = micros(); + float q[4] = {NAN, NAN, NAN, NAN}; + uint16_t estimator_status = (ESTIMATOR_ATTITUDE | ESTIMATOR_VELOCITY_HORIZ | ESTIMATOR_VELOCITY_VERT | ESTIMATOR_POS_HORIZ_REL | ESTIMATOR_POS_HORIZ_ABS | ESTIMATOR_POS_VERT_ABS | ESTIMATOR_POS_VERT_AGL | ESTIMATOR_CONST_POS_MODE | ESTIMATOR_PRED_POS_HORIZ_REL | ESTIMATOR_POS_HORIZ_ABS); + + mavlink_euler_to_quaternion(attitude.values.roll, attitude.values.pitch, attitude.values.yaw, q); + + // https://mavlink.io/en/messages/common.html#AUTOPILOT_STATE_FOR_GIMBAL_DEVICE + mavlink_msg_autopilot_state_for_gimbal_device_pack(mavSystemId, mavComponentId, &mavSendMsg, + targetSystem, targetComponent, time_boot_us, q, 0, getEstimatedActualVelocity(X), getEstimatedActualVelocity(Y), getEstimatedActualVelocity(Z), 0, + NAN, estimator_status, MAV_LANDED_STATE_UNDEFINED); + + mavlinkSendMessage(); +} +#endif + void processMAVLinkTelemetry(timeUs_t currentTimeUs) { // is executed @ TELEMETRY_MAVLINK_MAXRATE rate @@ -914,6 +965,11 @@ void processMAVLinkTelemetry(timeUs_t currentTimeUs) if (mavlinkStreamTrigger(MAV_DATA_STREAM_RC_CHANNELS)) { mavlinkSendRCChannelsAndRSSI(); +#if defined(USE_GIMBAL) && defined(USE_GIMBAL_MAVLINK) + if(gimbalCommonIsEnabled() && gimbalConfig()->gimbalType == GIMBAL_DEV_MAVLINK) { + mavlinkSendGimbalAttitude(); + } +#endif } #ifdef USE_GPS @@ -933,7 +989,6 @@ void processMAVLinkTelemetry(timeUs_t currentTimeUs) if (mavlinkStreamTrigger(MAV_DATA_STREAM_EXTRA3)) { mavlinkSendBatteryTemperatureStatusText(); } - } static bool handleIncoming_MISSION_CLEAR_ALL(void) diff --git a/src/main/telemetry/mavlink.h b/src/main/telemetry/mavlink.h index f5b51e1e3ef..45e98bc42e8 100755 --- a/src/main/telemetry/mavlink.h +++ b/src/main/telemetry/mavlink.h @@ -23,3 +23,4 @@ void checkMAVLinkTelemetryState(void); void freeMAVLinkTelemetryPort(void); void configureMAVLinkTelemetryPort(void); +bool isMAVLinkTelemetryEnabled(void); \ No newline at end of file diff --git a/src/test/unit/CMakeLists.txt b/src/test/unit/CMakeLists.txt index 2cb53b64ecb..206f0f050a6 100644 --- a/src/test/unit/CMakeLists.txt +++ b/src/test/unit/CMakeLists.txt @@ -41,7 +41,7 @@ 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" "drivers/headtracker_common.c") -set_property(SOURCE gimbal_serial_unittest.cc PROPERTY definitions USE_SERIAL_GIMBAL GIMBAL_UNIT_TEST USE_HEADTRACKER) +set_property(SOURCE gimbal_serial_unittest.cc PROPERTY definitions USE_GIMBAL_SERIAL USE_GIMBAL GIMBAL_UNIT_TEST USE_HEADTRACKER GIMBAL_UNIT_TEST) function(unit_test src) get_filename_component(basename ${src} NAME)