AP_RCProtocol: add support for MAVLink receiver, handle RADIO_RC_CHANNELS message

Author: olliw42

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

libraries/AP_RCProtocol/AP_RCProtocol.h

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

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;

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
+

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
+

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