AP_RCProtocol: add support for DroneCAN RCInput packets

libraries/AP_RCProtocol/AP_RCProtocol.cpp

@@ -32,6 +32,7 @@
 #include "AP_RCProtocol_ST24.h"
 #include "AP_RCProtocol_FPort.h"
 #include "AP_RCProtocol_FPort2.h"
+#include "AP_RCProtocol_DroneCAN.h"
 #include <AP_Math/AP_Math.h>
 #include <RC_Channel/RC_Channel.h>
 
@@ -78,6 +79,9 @@ void AP_RCProtocol::init()
 #if AP_RCPROTOCOL_FPORT_ENABLED
     backend[AP_RCProtocol::FPORT] = new AP_RCProtocol_FPort(*this, true);
 #endif
+#if AP_RCPROTOCOL_DRONECAN_ENABLED
+    backend[AP_RCProtocol::DRONECAN] = new AP_RCProtocol_DroneCAN(*this);
+#endif
 }
 
 AP_RCProtocol::~AP_RCProtocol()
@@ -359,9 +363,6 @@ void AP_RCProtocol::update()
 
 bool AP_RCProtocol::new_input()
 {
-    bool ret = _new_input;
-    _new_input = false;
-
     // if we have an extra UART from a SERIALn_PROTOCOL then check it for data
     check_added_uart();
 
@@ -371,6 +372,25 @@ bool AP_RCProtocol::new_input()
             backend[i]->update();
         }
     }
+
+#if AP_RCPROTOCOL_DRONECAN_ENABLED
+    uint32_t now = AP_HAL::millis();
+    if (should_search(now)) {
+        if (backend[AP_RCProtocol::DRONECAN] != nullptr &&
+            backend[AP_RCProtocol::DRONECAN]->new_input()) {
+            _detected_protocol = AP_RCProtocol::DRONECAN;
+            _last_input_ms = now;
+        }
+    } else if (_detected_protocol == AP_RCProtocol::DRONECAN) {
+        _new_input = backend[AP_RCProtocol::DRONECAN]->new_input();
+        if (_new_input) {
+            _last_input_ms = now;
+        }
+    }
+#endif
+
+    bool ret = _new_input;
+    _new_input = false;
     return ret;
 }
 
@@ -480,6 +500,10 @@ const char *AP_RCProtocol::protocol_name_from_protocol(rcprotocol_t protocol)
 #if AP_RCPROTOCOL_FPORT2_ENABLED
     case FPORT2:
         return "FPORT2";
+#endif
+#if AP_RCPROTOCOL_DRONECAN_ENABLED
+    case DRONECAN:
+        return "DroneCAN";
 #endif
     case NONE:
         break;

libraries/AP_RCProtocol/AP_RCProtocol.h

@@ -66,6 +66,9 @@ public:
 #endif
 #if AP_RCPROTOCOL_FASTSBUS_ENABLED
         FASTSBUS   = 12,
+#endif
+#if AP_RCPROTOCOL_DRONECAN_ENABLED
+        DRONECAN   = 13,
 #endif
         NONE    //last enum always is None
     };
@@ -142,6 +145,9 @@ public:
 #endif
 #if AP_RCPROTOCOL_ST24_ENABLED
         case ST24:
+#endif
+#if AP_RCPROTOCOL_DRONECAN_ENABLED
+        case DRONECAN:
 #endif
         case NONE:
             return false;

libraries/AP_RCProtocol/AP_RCProtocol_DroneCAN.cpp

@@ -0,0 +1,103 @@
+#include "AP_RCProtocol_config.h"
+
+#if AP_RCPROTOCOL_DRONECAN_ENABLED
+
+#include <AP_CANManager/AP_CANManager.h>
+#include <AP_DroneCAN/AP_DroneCAN.h>
+#include <AP_BoardConfig/AP_BoardConfig.h>
+#include "AP_RCProtocol_DroneCAN.h"
+
+extern const AP_HAL::HAL& hal;
+
+#define LOG_TAG "RCInput"
+
+AP_RCProtocol_DroneCAN::Registry AP_RCProtocol_DroneCAN::registry;
+AP_RCProtocol_DroneCAN *AP_RCProtocol_DroneCAN::_singleton;
+
+void AP_RCProtocol_DroneCAN::subscribe_msgs(AP_DroneCAN* ap_dronecan)
+{
+    if (ap_dronecan == nullptr) {
+        return;
+    }
+
+    if (Canard::allocate_sub_arg_callback(ap_dronecan, &handle_rcinput, ap_dronecan->get_driver_index()) == nullptr) {
+        AP_BoardConfig::allocation_error("rc_sub");
+    }
+}
+
+AP_RCProtocol_DroneCAN* AP_RCProtocol_DroneCAN::get_dronecan_backend(AP_DroneCAN* ap_dronecan, uint8_t node_id)
+{
+    if (_singleton == nullptr) {
+        return nullptr;
+    }
+
+    if (ap_dronecan == nullptr) {
+        return nullptr;
+    }
+
+    for (auto &device : registry.detected_devices) {
+        if (device.driver == nullptr) {
+            continue;
+        }
+        if (device.ap_dronecan != ap_dronecan) {
+            continue;
+        }
+        if (device.node_id != node_id ) {
+            continue;
+        }
+        return device.driver;
+    }
+
+    // not found in registry; add it if possible.
+    for (auto &device : registry.detected_devices) {
+        if (device.ap_dronecan == nullptr) {
+            device.ap_dronecan = ap_dronecan;
+            device.node_id = node_id;
+            device.driver = _singleton;
+            return device.driver;
+        }
+    }
+
+    return nullptr;
+}
+
+void AP_RCProtocol_DroneCAN::handle_rcinput(AP_DroneCAN *ap_dronecan, const CanardRxTransfer& transfer, const dronecan_sensors_rc_RCInput &msg)
+{
+    AP_RCProtocol_DroneCAN* driver = get_dronecan_backend(ap_dronecan, transfer.source_node_id);
+    if (driver == nullptr) {
+        return;
+    }
+
+    auto &rcin = driver->rcin;
+    WITH_SEMAPHORE(rcin.sem);
+    rcin.quality = msg.quality;
+    rcin.status = msg.status;
+    rcin.num_channels = MIN(msg.rcin.len, ARRAY_SIZE(rcin.channels));
+    for (auto i=0; i<rcin.num_channels; i++) {
+        rcin.channels[i] = msg.rcin.data[i];
+    }
+
+    rcin.last_sample_time_ms = AP_HAL::millis();
+}
+
+void AP_RCProtocol_DroneCAN::update()
+{
+    {
+        WITH_SEMAPHORE(rcin.sem);
+        if (rcin.last_sample_time_ms == last_receive_ms) {
+            // no new data
+            return;
+        }
+        last_receive_ms = rcin.last_sample_time_ms;
+
+        add_input(
+            rcin.num_channels,
+            rcin.channels,
+            rcin.bits.FAILSAFE,
+            rcin.bits.QUALITY_VALID ? rcin.quality : 0,  // CHECK ME
+            0  // link quality
+            );
+    }
+}
+
+#endif // AP_RCPROTOCOL_DRONECAN_ENABLED

libraries/AP_RCProtocol/AP_RCProtocol_DroneCAN.h

@@ -0,0 +1,63 @@
+#pragma once
+
+#include "AP_RCProtocol_config.h"
+
+#if AP_RCPROTOCOL_DRONECAN_ENABLED
+
+#include "AP_RCProtocol_Backend.h"
+
+#include <AP_DroneCAN/AP_DroneCAN.h>
+
+#include <AP_Common/missing/endian.h>
+
+class AP_RCProtocol_DroneCAN : public AP_RCProtocol_Backend {
+public:
+
+    AP_RCProtocol_DroneCAN(AP_RCProtocol &_frontend) :
+        AP_RCProtocol_Backend(_frontend) {
+        _singleton = this;
+    }
+
+    static void subscribe_msgs(AP_DroneCAN* ap_dronecan);
+
+    void update() override;
+
+private:
+
+    static class AP_RCProtocol_DroneCAN *_singleton;
+
+    static void handle_rcinput(AP_DroneCAN *ap_dronecan, const CanardRxTransfer& transfer, const dronecan_sensors_rc_RCInput &msg);
+
+    static AP_RCProtocol_DroneCAN* get_dronecan_backend(AP_DroneCAN* ap_dronecan, uint8_t node_id);
+
+    struct {
+        uint8_t quality;
+        union {
+            uint16_t status;
+            struct {
+                uint8_t QUALITY_VALID : 1;
+                uint8_t FAILSAFE : 1;
+            } bits;
+        };
+        uint8_t num_channels;
+        uint16_t channels[MAX_RCIN_CHANNELS];
+
+        uint32_t last_sample_time_ms;
+        HAL_Semaphore sem;
+    } rcin;
+
+    // Module Detection Registry
+    static struct Registry {
+        struct DetectedDevice {
+            AP_DroneCAN* ap_dronecan;
+            uint8_t node_id;
+            AP_RCProtocol_DroneCAN *driver;
+        } detected_devices[1];
+        HAL_Semaphore sem;
+    } registry;
+
+    uint32_t last_receive_ms;
+};
+
+
+#endif  // AP_RCPROTOCOL_DRONECAN_ENABLED

libraries/AP_RCProtocol/AP_RCProtocol_config.h

@@ -15,6 +15,10 @@
 #define AP_RCPROTOCOL_CRSF_ENABLED AP_RCPROTOCOL_BACKEND_DEFAULT_ENABLED
 #endif
 
+#ifndef AP_RCPROTOCOL_DRONECAN_ENABLED
+#define AP_RCPROTOCOL_DRONECAN_ENABLED AP_RCPROTOCOL_BACKEND_DEFAULT_ENABLED && HAL_ENABLE_DRONECAN_DRIVERS
+#endif
+
 #ifndef AP_RCPROTOCOL_FPORT_ENABLED
 #define AP_RCPROTOCOL_FPORT_ENABLED AP_RCPROTOCOL_BACKEND_DEFAULT_ENABLED && AP_FRSKY_SPORT_TELEM_ENABLED
 #endif