From c8172a2fd623d1039adb972731f87a17d1d7ae2d Mon Sep 17 00:00:00 2001 From: olliw42 Date: Thu, 29 Feb 2024 07:43:50 +0100 Subject: [PATCH] AP_RCProtocol: add support for MAVLink receiver, handle RADIO_RC_CHANNELS message --- libraries/AP_RCProtocol/AP_RCProtocol.cpp | 26 ++++++++++++++++ libraries/AP_RCProtocol/AP_RCProtocol.h | 10 ++++++ .../AP_RCProtocol/AP_RCProtocol_Backend.h | 3 ++ .../AP_RCProtocol_MavlinkRadio.cpp | 31 +++++++++++++++++++ .../AP_RCProtocol_MavlinkRadio.h | 21 +++++++++++++ .../AP_RCProtocol/AP_RCProtocol_config.h | 4 +++ 6 files changed, 95 insertions(+) create mode 100644 libraries/AP_RCProtocol/AP_RCProtocol_MavlinkRadio.cpp create mode 100644 libraries/AP_RCProtocol/AP_RCProtocol_MavlinkRadio.h diff --git a/libraries/AP_RCProtocol/AP_RCProtocol.cpp b/libraries/AP_RCProtocol/AP_RCProtocol.cpp index 7e53bda5e79793..f26a1892bc5fea 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() @@ -422,6 +426,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) { @@ -554,6 +561,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; @@ -589,6 +600,21 @@ 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) +{ + // take a shortcut if protocol is known to be MAVLINK_RADIO + if (_detected_protocol == AP_RCProtocol::MAVLINK_RADIO) { + backend[_detected_protocol]->update_radio_rc_channels(packet); + return; + } + + for (uint8_t i = 0; i < ARRAY_SIZE(backend); i++) { + if (backend[i] != nullptr) { + backend[i]->update_radio_rc_channels(packet); + } + } +}; + namespace AP { AP_RCProtocol &RC() { diff --git a/libraries/AP_RCProtocol/AP_RCProtocol.h b/libraries/AP_RCProtocol/AP_RCProtocol.h index aae2a3b1ecc3ff..858c6d00985777 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 @@ -74,6 +75,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 +162,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 +212,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 3768f6dcc07d78..277c6cc5b948a2 100644 --- a/libraries/AP_RCProtocol/AP_RCProtocol_Backend.h +++ b/libraries/AP_RCProtocol/AP_RCProtocol_Backend.h @@ -44,6 +44,9 @@ class AP_RCProtocol_Backend { // allow for backends that need regular polling virtual void update(void) {} + // 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 00000000000000..4ba612142b86b5 --- /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 00000000000000..53e2c70f4e67bc --- /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 7136a381e34ef9..bf7df658946c53 100644 --- a/libraries/AP_RCProtocol/AP_RCProtocol_config.h +++ b/libraries/AP_RCProtocol/AP_RCProtocol_config.h @@ -69,3 +69,7 @@ #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 +#endif