diff --git a/Tools/scripts/build_options.py b/Tools/scripts/build_options.py index d63cf9f49bc07..bba07f7534f7b 100644 --- a/Tools/scripts/build_options.py +++ b/Tools/scripts/build_options.py @@ -207,6 +207,7 @@ def __init__(self, Feature('RC', 'RC_ST24', 'AP_RCPROTOCOL_ST24_ENABLED', "Enable ST24 Protocol", 0, "RC_Protocol"), # NOQA: E501 Feature('RC', 'RC_SUMD', 'AP_RCPROTOCOL_SUMD_ENABLED', "Enable SUMD RC Protocol", 0, "RC_Protocol"), # NOQA: E501 Feature('RC', 'RC_GHST', 'AP_RCPROTOCOL_GHST_ENABLED', "Enable Ghost RC Protocol", 0, "RC_Protocol"), # NOQA: E501 + Feature('RC', 'RC_MAVLINK_RADIO', 'AP_RCPROTOCOL_MAVLINK_RADIO_ENABLED', "Enable MAVLink RC Protocol", 0, "RC_Protocol"), # NOQA: E501 Feature('Rangefinder', 'RANGEFINDER', 'AP_RANGEFINDER_ENABLED', "Enable Rangefinders", 0, None), # NOQA: E501 Feature('Rangefinder', 'RANGEFINDER_ANALOG', 'AP_RANGEFINDER_ANALOG_ENABLED', "Enable Rangefinder - Analog", 0, "RANGEFINDER"), # NOQA: E501 diff --git a/libraries/AP_RCProtocol/AP_RCProtocol.cpp b/libraries/AP_RCProtocol/AP_RCProtocol.cpp index 0141968f7e2a2..0a34615a52937 100644 --- a/libraries/AP_RCProtocol/AP_RCProtocol.cpp +++ b/libraries/AP_RCProtocol/AP_RCProtocol.cpp @@ -34,6 +34,7 @@ #include "AP_RCProtocol_FPort2.h" #include "AP_RCProtocol_DroneCAN.h" #include "AP_RCProtocol_GHST.h" +#include "AP_RCProtocol_MAVLinkRadio.h" #include #include @@ -88,6 +89,9 @@ void AP_RCProtocol::init() #if AP_RCPROTOCOL_GHST_ENABLED backend[AP_RCProtocol::GHST] = new AP_RCProtocol_GHST(*this); #endif +#if AP_RCPROTOCOL_MAVLINK_RADIO_ENABLED + backend[AP_RCProtocol::MAVLINK_RADIO] = new AP_RCProtocol_MAVLinkRadio(*this); +#endif } AP_RCProtocol::~AP_RCProtocol() @@ -430,6 +434,9 @@ bool AP_RCProtocol::new_input() const rcprotocol_t pollable[] { #if AP_RCPROTOCOL_DRONECAN_ENABLED AP_RCProtocol::DRONECAN, +#endif +#if AP_RCPROTOCOL_MAVLINK_RADIO_ENABLED + AP_RCProtocol::MAVLINK_RADIO, #endif }; for (const auto protocol : pollable) { @@ -562,6 +569,10 @@ const char *AP_RCProtocol::protocol_name_from_protocol(rcprotocol_t protocol) #if AP_RCPROTOCOL_GHST_ENABLED case GHST: return "GHST"; +#endif +#if AP_RCPROTOCOL_MAVLINK_RADIO_ENABLED + case MAVLINK_RADIO: + return "MAVRadio"; #endif case NONE: break; @@ -597,6 +608,17 @@ bool AP_RCProtocol::protocol_enabled(rcprotocol_t protocol) const return ((1U<<(uint8_t(protocol)+1)) & rc_protocols_mask) != 0; } +#if AP_RCPROTOCOL_MAVLINK_RADIO_ENABLED +void AP_RCProtocol::handle_radio_rc_channels(const mavlink_radio_rc_channels_t* packet) +{ + if (backend[AP_RCProtocol::MAVLINK_RADIO] == nullptr) { + return; + } + + backend[AP_RCProtocol::MAVLINK_RADIO]->update_radio_rc_channels(packet); +}; +#endif // AP_RCPROTOCOL_MAVLINK_RADIO_ENABLED + namespace AP { AP_RCProtocol &RC() { diff --git a/libraries/AP_RCProtocol/AP_RCProtocol.h b/libraries/AP_RCProtocol/AP_RCProtocol.h index aae2a3b1ecc3f..9aa9ee0959c6f 100644 --- a/libraries/AP_RCProtocol/AP_RCProtocol.h +++ b/libraries/AP_RCProtocol/AP_RCProtocol.h @@ -20,6 +20,9 @@ #include #include +#if AP_RCPROTOCOL_MAVLINK_RADIO_ENABLED +#include +#endif #define MAX_RCIN_CHANNELS 18 #define MIN_RCIN_CHANNELS 5 @@ -74,6 +77,9 @@ class AP_RCProtocol { #endif #if AP_RCPROTOCOL_GHST_ENABLED GHST = 14, +#endif +#if AP_RCPROTOCOL_MAVLINK_RADIO_ENABLED + MAVLINK_RADIO = 15, #endif NONE //last enum always is None }; @@ -158,6 +164,9 @@ class AP_RCProtocol { #endif #if AP_RCPROTOCOL_DRONECAN_ENABLED case DRONECAN: +#endif +#if AP_RCPROTOCOL_MAVLINK_RADIO_ENABLED + case MAVLINK_RADIO: #endif case NONE: return false; @@ -205,6 +214,11 @@ class AP_RCProtocol { return _detected_with_bytes; } + // handle mavlink radio +#if AP_RCPROTOCOL_MAVLINK_RADIO_ENABLED + void handle_radio_rc_channels(const mavlink_radio_rc_channels_t* packet); +#endif + private: void check_added_uart(void); diff --git a/libraries/AP_RCProtocol/AP_RCProtocol_Backend.h b/libraries/AP_RCProtocol/AP_RCProtocol_Backend.h index 3768f6dcc07d7..e050db2dc1d97 100644 --- a/libraries/AP_RCProtocol/AP_RCProtocol_Backend.h +++ b/libraries/AP_RCProtocol/AP_RCProtocol_Backend.h @@ -44,6 +44,11 @@ class AP_RCProtocol_Backend { // allow for backends that need regular polling virtual void update(void) {} + // update from mavlink messages +#if AP_RCPROTOCOL_MAVLINK_RADIO_ENABLED + virtual void update_radio_rc_channels(const mavlink_radio_rc_channels_t* packet) {} +#endif + // get number of frames, ignoring failsafe uint32_t get_rc_frame_count(void) const { return rc_frame_count; diff --git a/libraries/AP_RCProtocol/AP_RCProtocol_MAVLinkRadio.cpp b/libraries/AP_RCProtocol/AP_RCProtocol_MAVLinkRadio.cpp new file mode 100644 index 0000000000000..c11fca5bc0858 --- /dev/null +++ b/libraries/AP_RCProtocol/AP_RCProtocol_MAVLinkRadio.cpp @@ -0,0 +1,25 @@ + +#include "AP_RCProtocol_config.h" + +#if AP_RCPROTOCOL_MAVLINK_RADIO_ENABLED + +#include "AP_RCProtocol_MAVLinkRadio.h" + +void AP_RCProtocol_MAVLinkRadio::update_radio_rc_channels(const mavlink_radio_rc_channels_t* packet) +{ + const uint8_t count = MIN(packet->count, MAX_RCIN_CHANNELS); + + uint16_t rc_chan[MAX_RCIN_CHANNELS]; + for (uint8_t i = 0; i < count; i++) { + // The channel values are in centered 13 bit format. Range is [-4096,4096], center is 0. + // According to specification, the conversion to PWM is x * 5/32 + 1500. + rc_chan[i] = ((int32_t)packet->channels[i] * 5) / 32 + 1500; + } + + bool failsafe = (packet->flags & RADIO_RC_CHANNELS_FLAGS_FAILSAFE); + + add_input(count, rc_chan, failsafe); +} + +#endif // AP_RCPROTOCOL_MAVLINK_RADIO_ENABLED + diff --git a/libraries/AP_RCProtocol/AP_RCProtocol_MAVLinkRadio.h b/libraries/AP_RCProtocol/AP_RCProtocol_MAVLinkRadio.h new file mode 100644 index 0000000000000..cd80f502a3f87 --- /dev/null +++ b/libraries/AP_RCProtocol/AP_RCProtocol_MAVLinkRadio.h @@ -0,0 +1,21 @@ + +#pragma once + +#include "AP_RCProtocol_config.h" + +#if AP_RCPROTOCOL_MAVLINK_RADIO_ENABLED + +#include "AP_RCProtocol.h" + + +class AP_RCProtocol_MAVLinkRadio : public AP_RCProtocol_Backend { +public: + + using AP_RCProtocol_Backend::AP_RCProtocol_Backend; + + // update from mavlink messages + void update_radio_rc_channels(const mavlink_radio_rc_channels_t* packet) override; +}; + +#endif // AP_RCPROTOCOL_MAVLINK_RADIO_ENABLED + diff --git a/libraries/AP_RCProtocol/AP_RCProtocol_config.h b/libraries/AP_RCProtocol/AP_RCProtocol_config.h index 7136a381e34ef..e886c8635f129 100644 --- a/libraries/AP_RCProtocol/AP_RCProtocol_config.h +++ b/libraries/AP_RCProtocol/AP_RCProtocol_config.h @@ -2,6 +2,7 @@ #include #include +#include #ifndef AP_RCPROTOCOL_ENABLED #define AP_RCPROTOCOL_ENABLED 1 @@ -69,3 +70,8 @@ #ifndef AP_RCPROTOCOL_GHST_ENABLED #define AP_RCPROTOCOL_GHST_ENABLED AP_RCPROTOCOL_BACKEND_DEFAULT_ENABLED #endif + +#ifndef AP_RCPROTOCOL_MAVLINK_RADIO_ENABLED +#define AP_RCPROTOCOL_MAVLINK_RADIO_ENABLED AP_RCPROTOCOL_BACKEND_DEFAULT_ENABLED && BOARD_FLASH_SIZE > 1024 && HAL_GCS_ENABLED +#endif + diff --git a/libraries/GCS_MAVLink/GCS.h b/libraries/GCS_MAVLink/GCS.h index 52f728c9d293f..efd5fbbd1ac3a 100644 --- a/libraries/GCS_MAVLink/GCS.h +++ b/libraries/GCS_MAVLink/GCS.h @@ -690,6 +690,7 @@ class GCS_MAVLINK void handle_optical_flow(const mavlink_message_t &msg); void handle_manual_control(const mavlink_message_t &msg); + void handle_radio_rc_channels(const mavlink_message_t &msg); // default empty handling of LANDING_TARGET virtual void handle_landing_target(const mavlink_landing_target_t &packet, uint32_t timestamp_ms) { } diff --git a/libraries/GCS_MAVLink/GCS_Common.cpp b/libraries/GCS_MAVLink/GCS_Common.cpp index 0ad5337c31103..fbdfcce5ca67e 100644 --- a/libraries/GCS_MAVLink/GCS_Common.cpp +++ b/libraries/GCS_MAVLink/GCS_Common.cpp @@ -4193,6 +4193,11 @@ void GCS_MAVLINK::handle_message(const mavlink_message_t &msg) case MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE: handle_rc_channels_override(msg); break; +#if AP_RCPROTOCOL_MAVLINK_RADIO_ENABLED + case MAVLINK_MSG_ID_RADIO_RC_CHANNELS: + handle_radio_rc_channels(msg); + break; +#endif #endif #if AP_OPTICALFLOW_ENABLED @@ -6976,4 +6981,14 @@ MAV_RESULT GCS_MAVLINK::handle_control_high_latency(const mavlink_command_int_t } #endif // HAL_HIGH_LATENCY2_ENABLED +#if AP_RCPROTOCOL_MAVLINK_RADIO_ENABLED +void GCS_MAVLINK::handle_radio_rc_channels(const mavlink_message_t &msg) +{ + mavlink_radio_rc_channels_t packet; + mavlink_msg_radio_rc_channels_decode(&msg, &packet); + + AP::RC().handle_radio_rc_channels(&packet); +} +#endif // AP_RCPROTOCOL_MAVLINK_RADIO_ENABLED + #endif // HAL_GCS_ENABLED diff --git a/libraries/RC_Channel/RC_Channels_VarInfo.h b/libraries/RC_Channel/RC_Channels_VarInfo.h index a824cc1ececc3..2419033b4087d 100644 --- a/libraries/RC_Channel/RC_Channels_VarInfo.h +++ b/libraries/RC_Channel/RC_Channels_VarInfo.h @@ -101,7 +101,7 @@ const AP_Param::GroupInfo RC_Channels::var_info[] = { // @DisplayName: RC protocols enabled // @Description: Bitmask of enabled RC protocols. Allows narrowing the protocol detection to only specific types of RC receivers which can avoid issues with incorrect detection. Set to 1 to enable all protocols. // @User: Advanced - // @Bitmask: 0:All,1:PPM,2:IBUS,3:SBUS,4:SBUS_NI,5:DSM,6:SUMD,7:SRXL,8:SRXL2,9:CRSF,10:ST24,11:FPORT,12:FPORT2,13:FastSBUS,14:DroneCAN,15:Ghost + // @Bitmask: 0:All,1:PPM,2:IBUS,3:SBUS,4:SBUS_NI,5:DSM,6:SUMD,7:SRXL,8:SRXL2,9:CRSF,10:ST24,11:FPORT,12:FPORT2,13:FastSBUS,14:DroneCAN,15:Ghost,16:MAVRadio AP_GROUPINFO("_PROTOCOLS", 34, RC_CHANNELS_SUBCLASS, _protocols, 1), // @Param: _FS_TIMEOUT