diff --git a/src/main/CMakeLists.txt b/src/main/CMakeLists.txt index d37646abc4..3972b6ec0c 100644 --- a/src/main/CMakeLists.txt +++ b/src/main/CMakeLists.txt @@ -386,6 +386,8 @@ main_sources(COMMON_SRC rx/ibus.h rx/jetiexbus.c rx/jetiexbus.h + rx/mavlink.c + rx/mavlink.h rx/msp.c rx/msp.h rx/msp_override.c diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index d462d72cc8..de05765f53 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -25,7 +25,7 @@ tables: values: ["NONE", "PPM", "SERIAL", "MSP", "SPI", "UNUSED"] enum: rxReceiverType_e - name: serial_rx - values: ["SPEK1024", "SPEK2048", "SBUS", "SUMD", "SUMH", "XB-B", "XB-B-RJ01", "IBUS", "JETIEXBUS", "CRSF", "FPORT", "SBUS_FAST", "FPORT2", "SRXL2", "GHST"] + values: ["SPEK1024", "SPEK2048", "SBUS", "SUMD", "SUMH", "XB-B", "XB-B-RJ01", "IBUS", "JETIEXBUS", "CRSF", "FPORT", "SBUS_FAST", "FPORT2", "SRXL2", "GHST", "MAVLINK"] - name: rx_spi_protocol values: ["ELERES"] enum: rx_spi_protocol_e diff --git a/src/main/rx/mavlink.c b/src/main/rx/mavlink.c new file mode 100644 index 0000000000..5af1acaced --- /dev/null +++ b/src/main/rx/mavlink.c @@ -0,0 +1,51 @@ +/* + * This file is part of Cleanflight. + * + * 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 Cleanflight. If not, see . + */ + +#include +#include +#include +#include + +#include "platform.h" +FILE_COMPILE_FOR_SPEED +#ifdef USE_SERIALRX_MAVLINK + +#include "build/build_config.h" +#include "build/debug.h" + +#include "common/crc.h" +#include "common/maths.h" +#include "common/utils.h" + +#include "drivers/time.h" +#include "drivers/serial.h" +#include "drivers/serial_uart.h" + +#include "io/serial.h" + +#include "rx/rx.h" +#include "rx/mavlink.h" + +#define MAVLINK_MAX_CHANNEL 18 + +bool mavlinkRxInit(const rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig) +{ + // TODO + return false; +} + +#endif diff --git a/src/main/rx/mavlink.h b/src/main/rx/mavlink.h new file mode 100644 index 0000000000..3311b15413 --- /dev/null +++ b/src/main/rx/mavlink.h @@ -0,0 +1,20 @@ +/* + * This file is part of Cleanflight. + * + * 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 Cleanflight. If not, see . + */ + +#pragma once + +bool mavlinkRxInit(const struct rxConfig_s *initialRxConfig, struct rxRuntimeConfig_s *rxRuntimeConfig); diff --git a/src/main/rx/rx.c b/src/main/rx/rx.c index f028e73330..b01c9e4539 100755 --- a/src/main/rx/rx.c +++ b/src/main/rx/rx.c @@ -68,6 +68,7 @@ #include "rx/sumh.h" #include "rx/xbus.h" #include "rx/ghst.h" +#include "rx/mavlink.h" //#define DEBUG_RX_SIGNAL_LOSS @@ -255,6 +256,11 @@ bool serialRxInit(const rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig case SERIALRX_GHST: enabled = ghstRxInit(rxConfig, rxRuntimeConfig); break; +#endif +#ifdef USE_SERIALRX_MAVLINK + case SERIALRX_MAVLINK: + enabled = mavlinkRxInit(rxConfig, rxRuntimeConfig); + break; #endif default: enabled = false; diff --git a/src/main/rx/rx.h b/src/main/rx/rx.h index 0aee1e8353..f35f7bc1f8 100644 --- a/src/main/rx/rx.h +++ b/src/main/rx/rx.h @@ -84,6 +84,7 @@ typedef enum { SERIALRX_FPORT2 = 12, SERIALRX_SRXL2 = 13, SERIALRX_GHST = 14, + SERIALRX_MAVLINK = 15, } rxSerialReceiverType_e; #define MAX_SUPPORTED_RC_PPM_CHANNEL_COUNT 16 diff --git a/src/main/target/common.h b/src/main/target/common.h index 9d0ddc3346..cce269cd7e 100755 --- a/src/main/target/common.h +++ b/src/main/target/common.h @@ -177,6 +177,7 @@ #define USE_SERIALRX_SUMH #define USE_SERIALRX_XBUS #define USE_SERIALRX_JETIEXBUS +#define USE_SERIALRX_MAVLINK #define USE_SERIALRX_CRSF #define USE_PWM_SERVO_DRIVER #define USE_SERIAL_PASSTHROUGH diff --git a/src/main/telemetry/mavlink.c b/src/main/telemetry/mavlink.c index b5725a19b2..272d9c6b06 100644 --- a/src/main/telemetry/mavlink.c +++ b/src/main/telemetry/mavlink.c @@ -1010,6 +1010,10 @@ static bool handleIncoming_MISSION_REQUEST(void) return false; } +static bool handleIncoming_RC_CHANNELS_OVERRIDE(void) { + +} + static bool processMAVLinkIncomingTelemetry(void) { while (serialRxBytesWaiting(mavlinkPort) > 0) { @@ -1030,6 +1034,8 @@ static bool processMAVLinkIncomingTelemetry(void) return handleIncoming_MISSION_REQUEST_LIST(); case MAVLINK_MSG_ID_MISSION_REQUEST: return handleIncoming_MISSION_REQUEST(); + case MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE: + return handleIncoming_RC_CHANNELS_OVERRIDE(); default: return false; }