Add MAVLink receiver support, RADIO_RC_CHANNELS, no III

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

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 <AP_Math/AP_Math.h>
 #include <RC_Channel/RC_Channel.h>
 
@@ -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()
     {

libraries/AP_RCProtocol/AP_RCProtocol.h

@@ -20,6 +20,9 @@
 
 #include <AP_HAL/AP_HAL.h>
 #include <AP_Common/AP_Common.h>
+#if AP_RCPROTOCOL_MAVLINK_RADIO_ENABLED
+#include <GCS_MAVLink/GCS_MAVLink.h>
+#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);
 

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;

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
+

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
+

libraries/AP_RCProtocol/AP_RCProtocol_config.h

@@ -2,6 +2,7 @@
 
 #include <AP_HAL/AP_HAL_Boards.h>
 #include <AP_Frsky_Telem/AP_Frsky_config.h>
+#include <GCS_MAVLink/GCS_config.h>
 
 #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
+

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) { }

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

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