diff --git a/libraries/AP_RCProtocol/AP_RCProtocol.cpp b/libraries/AP_RCProtocol/AP_RCProtocol.cpp index 9c8ef679a1643..1bc5bc35b8c4c 100644 --- a/libraries/AP_RCProtocol/AP_RCProtocol.cpp +++ b/libraries/AP_RCProtocol/AP_RCProtocol.cpp @@ -33,6 +33,7 @@ #include "AP_RCProtocol_FPort.h" #include "AP_RCProtocol_FPort2.h" #include "AP_RCProtocol_DroneCAN.h" +#include "AP_RCProtocol_MAVLinkRadio.h" #include #include @@ -82,6 +83,9 @@ void AP_RCProtocol::init() #if AP_RCPROTOCOL_DRONECAN_ENABLED backend[AP_RCProtocol::DRONECAN] = new AP_RCProtocol_DroneCAN(*this); #endif +#if AP_RCPROTOCOL_MAVLINK_RADIO_ENABLED + backend[AP_RCProtocol::MAVLINK_RADIO] = new AP_RCProtocol_MAVLinkRadio(*this); +#endif } AP_RCProtocol::~AP_RCProtocol() @@ -504,6 +508,10 @@ const char *AP_RCProtocol::protocol_name_from_protocol(rcprotocol_t protocol) #if AP_RCPROTOCOL_DRONECAN_ENABLED case DRONECAN: return "DroneCAN"; +#endif +#if AP_RCPROTOCOL_MAVLINK_RADIO_ENABLED + case MAVLINK_RADIO: + return "MAVRadio"; #endif case NONE: break; @@ -539,6 +547,36 @@ bool AP_RCProtocol::protocol_enabled(rcprotocol_t protocol) const return ((1U<<(uint8_t(protocol)+1)) & rc_protocols_mask) != 0; } +void AP_RCProtocol::handle_radio_rc_channels(const mavlink_radio_rc_channels_t* packet) +{ + // receiving this message is also used to check if the receiver is present + // so let's first do the receiver detection + if (_detected_protocol == AP_RCProtocol::NONE) { // still searching +#if AP_RC_CHANNEL_ENABLED + rc_protocols_mask = rc().enabled_protocols(); +#endif + if (!protocol_enabled(MAVLINK_RADIO)) { + return; // not our turn + } + _detected_protocol = AP_RCProtocol::MAVLINK_RADIO; + } + + // here now comes the message handling itself + + if (_detected_protocol != AP_RCProtocol::MAVLINK_RADIO) { + return; + } + + // now update the backend + backend[_detected_protocol]->update_radio_rc_channels(packet); + + // now we can ask the backend if it got a new input + if (backend[_detected_protocol]->new_input()) { + _new_input = true; + _last_input_ms = AP_HAL::millis(); + } +}; + namespace AP { AP_RCProtocol &RC() { diff --git a/libraries/AP_RCProtocol/AP_RCProtocol.h b/libraries/AP_RCProtocol/AP_RCProtocol.h index eeacb6fa0a0b6..bf0b4d696c4c3 100644 --- a/libraries/AP_RCProtocol/AP_RCProtocol.h +++ b/libraries/AP_RCProtocol/AP_RCProtocol.h @@ -20,6 +20,7 @@ #include #include +#include #define MAX_RCIN_CHANNELS 18 #define MIN_RCIN_CHANNELS 5 @@ -69,6 +70,9 @@ class AP_RCProtocol { #endif #if AP_RCPROTOCOL_DRONECAN_ENABLED DRONECAN = 13, +#endif +#if AP_RCPROTOCOL_MAVLINK_RADIO_ENABLED + MAVLINK_RADIO = 14, #endif NONE //last enum always is None }; @@ -148,6 +152,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; @@ -195,6 +202,9 @@ class AP_RCProtocol { return _detected_with_bytes; } + // handle mavlink radio + void handle_radio_rc_channels(const mavlink_radio_rc_channels_t* packet); + 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 cbb0b1135be71..b8e1876b9eb04 100644 --- a/libraries/AP_RCProtocol/AP_RCProtocol_Backend.h +++ b/libraries/AP_RCProtocol/AP_RCProtocol_Backend.h @@ -48,6 +48,9 @@ class AP_RCProtocol_Backend { PARSE_TYPE_SERIAL }; + // update from mavlink messages + virtual void update_radio_rc_channels(const mavlink_radio_rc_channels_t* packet) {} + // 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..4ba612142b86b --- /dev/null +++ b/libraries/AP_RCProtocol/AP_RCProtocol_MavlinkRadio.cpp @@ -0,0 +1,31 @@ + +#include "AP_RCProtocol_config.h" + +#if AP_RCPROTOCOL_MAVLINK_RADIO_ENABLED + +#include "AP_RCProtocol_MAVLinkRadio.h" + +// constructor +AP_RCProtocol_MAVLinkRadio::AP_RCProtocol_MAVLinkRadio(AP_RCProtocol &_frontend) : + AP_RCProtocol_Backend(_frontend) +{} + +void AP_RCProtocol_MAVLinkRadio::update_radio_rc_channels(const mavlink_radio_rc_channels_t* packet) +{ + uint8_t count = packet->count; + if (count >= MAX_RCIN_CHANNELS) 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..53e2c70f4e67b --- /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: + + AP_RCProtocol_MAVLinkRadio(AP_RCProtocol &_frontend); + + // 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 5619c5373c081..b1c2187e0430e 100644 --- a/libraries/AP_RCProtocol/AP_RCProtocol_config.h +++ b/libraries/AP_RCProtocol/AP_RCProtocol_config.h @@ -61,3 +61,7 @@ #ifndef AP_RCPROTOCOL_FASTSBUS_ENABLED #define AP_RCPROTOCOL_FASTSBUS_ENABLED AP_RCPROTOCOL_BACKEND_DEFAULT_ENABLED && AP_RCPROTOCOL_SBUS_ENABLED #endif + +#ifndef AP_RCPROTOCOL_MAVLINK_RADIO_ENABLED +#define AP_RCPROTOCOL_MAVLINK_RADIO_ENABLED AP_RCPROTOCOL_BACKEND_DEFAULT_ENABLED && BOARD_FLASH_SIZE > 1024 +#endif diff --git a/libraries/GCS_MAVLink/GCS.h b/libraries/GCS_MAVLink/GCS.h index 867ec88903108..d46671c507a90 100644 --- a/libraries/GCS_MAVLink/GCS.h +++ b/libraries/GCS_MAVLink/GCS.h @@ -683,6 +683,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 f2fb1c5bb04c3..d05c6620e6470 100644 --- a/libraries/GCS_MAVLink/GCS_Common.cpp +++ b/libraries/GCS_MAVLink/GCS_Common.cpp @@ -4130,6 +4130,11 @@ void GCS_MAVLINK::handle_common_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 #if AP_OPTICALFLOW_ENABLED case MAVLINK_MSG_ID_OPTICAL_FLOW: @@ -6812,4 +6817,16 @@ 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); + +#if AP_RCPROTOCOL_ENABLED + AP::RC().handle_radio_rc_channels(&packet); +#endif +} +#endif + #endif // HAL_GCS_ENABLED diff --git a/libraries/RC_Channel/RC_Channels_VarInfo.h b/libraries/RC_Channel/RC_Channels_VarInfo.h index 8302be931d6a5..e0bf91fa03170 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 + // @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:MAVRadio AP_GROUPINFO("_PROTOCOLS", 34, RC_CHANNELS_SUBCLASS, _protocols, 1), // @Param: _FS_TIMEOUT