AP_CANManager: support mavcan message forwarding

libraries/AP_CANManager/AP_CANManager.cpp

@@ -167,11 +167,6 @@ void AP_CANManager::init()
             // we have slcan bridge setup pass that on as can iface
             can_initialised = hal.can[i]->init(_interfaces[i]._bitrate, AP_HAL::CANIface::NormalMode);
             iface = &_slcan_interface;
-#ifdef HAL_BUILD_AP_PERIPH
-        } else if(drv_type[drv_num] == Driver_Type_UAVCAN) {
-            // setup for filtering on AP_Periph if using UAVCAN
-            can_initialised = hal.can[i]->init(_interfaces[i]._bitrate, AP_HAL::CANIface::FilteredMode);
-#endif
         } else {
             can_initialised = hal.can[i]->init(_interfaces[i]._bitrate, AP_HAL::CANIface::NormalMode);
         }
@@ -386,6 +381,62 @@ void AP_CANManager::log_retrieve(ExpandingString &str) const
     str.append(_log_buf, _log_pos);
 }
 
+#if HAL_GCS_ENABLED
+/*
+  handle MAV_CMD_CAN_FORWARD mavlink long command
+ */
+bool AP_CANManager::handle_can_forward(mavlink_channel_t chan, const mavlink_command_long_t &packet, const mavlink_message_t &msg)
+{
+    const int8_t bus = int8_t(packet.param1)-1;
+    if (bus == -1) {
+        for (auto can_iface : hal.can) {
+            if (can_iface) {
+                can_iface->register_frame_callback(nullptr);
+            }
+        }
+        return true;
+    }
+    if (bus >= HAL_NUM_CAN_IFACES || hal.can[bus] == nullptr) {
+        return false;
+    }
+    if (!hal.can[bus]->register_frame_callback(
+            FUNCTOR_BIND_MEMBER(&AP_CANManager::can_frame_callback, void, uint8_t, const AP_HAL::CANFrame &))) {
+        return false;
+    }
+    can_forward.chan = chan;
+    can_forward.system_id = msg.sysid;
+    can_forward.component_id = msg.compid;
+    return true;
+}
+
+/*
+  handle a CAN_FRAME packet
+ */
+void AP_CANManager::handle_can_frame(const mavlink_message_t &msg) const
+{
+    mavlink_can_frame_t p;
+    mavlink_msg_can_frame_decode(&msg, &p);
+    if (p.bus >= HAL_NUM_CAN_IFACES || hal.can[p.bus] == nullptr) {
+        return;
+    }
+    const uint16_t timeout_us = 2000;
+    AP_HAL::CANFrame frame{p.id, p.data, p.len};
+    hal.can[p.bus]->send(frame, AP_HAL::native_micros64() + timeout_us, AP_HAL::CANIface::IsMAVCAN);
+}
+
+/*
+  handler for CAN frames from the registered callback, sending frames
+  out as CAN_FRAME messages
+ */
+void AP_CANManager::can_frame_callback(uint8_t bus, const AP_HAL::CANFrame &frame)
+{
+    if (HAVE_PAYLOAD_SPACE(can_forward.chan, CAN_FRAME)) {
+        mavlink_msg_can_frame_send(can_forward.chan, can_forward.system_id, can_forward.component_id,
+                                   bus, frame.dlc, frame.id, const_cast<uint8_t*>(frame.data));
+    }
+}
+#endif // HAL_GCS_ENABLED
+
 AP_CANManager& AP::can()
 {
     return *AP_CANManager::get_singleton();

libraries/AP_CANManager/AP_CANManager.h

@@ -24,6 +24,8 @@
 #include <AP_Param/AP_Param.h>
 #include "AP_SLCANIface.h"
 #include "AP_CANDriver.h"
+#include <GCS_MAVLink/GCS.h>
+//#include <GCS_MAVLink/GCS_MAVLink.h>
 
 class AP_CANManager
 {
@@ -107,6 +109,11 @@ public:
 
     static const struct AP_Param::GroupInfo var_info[];
 
+#if HAL_GCS_ENABLED
+    bool handle_can_forward(mavlink_channel_t chan, const mavlink_command_long_t &packet, const mavlink_message_t &msg);
+    void handle_can_frame(const mavlink_message_t &msg) const;
+#endif
+
 private:
 
     // Parameter interface for CANIfaces
@@ -161,6 +168,20 @@ private:
     uint32_t _log_pos;
 
     HAL_Semaphore _sem;
+
+#if HAL_GCS_ENABLED
+    /*
+      handler for CAN frames from the registered callback, sending frames
+      out as CAN_FRAME messages
+    */
+    void can_frame_callback(uint8_t bus, const AP_HAL::CANFrame &frame);
+
+    struct {
+        mavlink_channel_t chan;
+        uint8_t system_id;
+        uint8_t component_id;
+    } can_forward;
+#endif // HAL_GCS_ENABLED
 };
 
 namespace AP

libraries/AP_CANManager/AP_SLCANIface.cpp

@@ -112,7 +112,7 @@ bool SLCAN::CANIface::push_Frame(AP_HAL::CANFrame &frame)
     frm.frame = frame;
     frm.flags = 0;
     frm.timestamp_us = AP_HAL::native_micros64();
-    return rx_queue_.push(frm);
+    return add_to_rx_queue(frm);
 }
 
 /**

libraries/AP_CANManager/AP_SLCANIface.h

@@ -100,10 +100,6 @@ public:
     bool init_passthrough(uint8_t i);
 
     void reset_params();
-    int8_t get_iface_num() const
-    {
-        return _iface_num;
-    }
 
     // Overriden methods
     bool set_event_handle(AP_HAL::EventHandle* evt_handle) override;
@@ -123,6 +119,15 @@ public:
 
     int16_t receive(AP_HAL::CANFrame& out_frame, uint64_t& rx_time,
                     AP_HAL::CANIface::CanIOFlags& out_flags) override;
+
+protected:
+    int8_t get_iface_num() const override {
+        return _iface_num;
+    }
+
+    bool add_to_rx_queue(const AP_HAL::CANIface::CanRxItem &frm) override {
+        return rx_queue_.push(frm);
+    }
 };
 
 }