AP_CANManager: Add multiCAN interface

libraries/AP_CANManager/AP_CAN.h

@@ -28,7 +28,6 @@ public:
         Benewake = 11,
         Scripting2 = 12,
         TOFSenseP = 13,
-        NanoRadar_NRA24 = 14,
+        NanoRadar = 14,
-        MR72 = 15,
     };
 };

libraries/AP_CANManager/AP_CANDriver.cpp

@@ -30,7 +30,7 @@ const AP_Param::GroupInfo AP_CANManager::CANDriver_Params::var_info[] = {
     // @Param: PROTOCOL
     // @DisplayName: Enable use of specific protocol over virtual driver
     // @Description: Enabling this option starts selected protocol that will use this virtual driver
-    // @Values: 0:Disabled,1:DroneCAN,4:PiccoloCAN,6:EFI_NWPMU,7:USD1,8:KDECAN,10:Scripting,11:Benewake,12:Scripting2,13:TOFSenseP,14:NanoRadar_NRA24
+    // @Values: 0:Disabled,1:DroneCAN,4:PiccoloCAN,6:EFI_NWPMU,7:USD1,8:KDECAN,10:Scripting,11:Benewake,12:Scripting2,13:TOFSenseP,14:NanoRadar
     // @User: Advanced
     // @RebootRequired: True
     AP_GROUPINFO("PROTOCOL", 1, AP_CANManager::CANDriver_Params, _driver_type, float(AP_CAN::Protocol::DroneCAN)),
@@ -54,7 +54,7 @@ const AP_Param::GroupInfo AP_CANManager::CANDriver_Params::var_info[] = {
     // @Param: PROTOCOL2
     // @DisplayName: Secondary protocol with 11 bit CAN addressing
     // @Description: Secondary protocol with 11 bit CAN addressing
-    // @Values: 0:Disabled,7:USD1,10:Scripting,11:Benewake,12:Scripting2,13:TOFSenseP,14:NanoRadar_NRA24
+    // @Values: 0:Disabled,7:USD1,10:Scripting,11:Benewake,12:Scripting2,13:TOFSenseP,14:NanoRadar
     // @User: Advanced
     // @RebootRequired: True
     AP_GROUPINFO("PROTOCOL2", 6, AP_CANManager::CANDriver_Params, _driver_type_11bit, float(AP_CAN::Protocol::None)),

libraries/AP_CANManager/AP_CANSensor.cpp

@@ -21,6 +21,7 @@
 
 #include <AP_Scheduler/AP_Scheduler.h>
 #include "AP_CANSensor.h"
+#include <AP_BoardConfig/AP_BoardConfig.h>
 
 extern const AP_HAL::HAL& hal;
 
@@ -190,5 +191,57 @@ void CANSensor::loop()
     }
 }
 
+MultiCAN::MultiCANLinkedList* MultiCAN::callbacks;
+
+MultiCAN::MultiCAN(ForwardCanFrame cf, AP_CAN::Protocol can_type, const char *driver_name) :
+        CANSensor(driver_name)
+{
+    if (callbacks == nullptr) {
+        callbacks = new MultiCANLinkedList();
+    }
+    if (callbacks == nullptr) {
+        AP_BoardConfig::allocation_error("Failed to create multican callback");
+    }
+
+    // Register new driver
+    register_driver(can_type);
+    callbacks->register_callback(cf);
+}
+
+// handle a received frame from the CAN bus
+void MultiCAN::handle_frame(AP_HAL::CANFrame &frame)
+{
+    if (callbacks != nullptr) {
+        callbacks->handle_frame(frame);
+    }
+
+}
+
+// register a callback for a CAN frame by adding it to the linked list
+void MultiCAN::MultiCANLinkedList::register_callback(ForwardCanFrame callback)
+{
+    CANSensor_Multi* newNode = new CANSensor_Multi();
+    if (newNode == nullptr) {
+        AP_BoardConfig::allocation_error("Failed to create multican node");
+    }
+    WITH_SEMAPHORE(sem);
+    {
+        newNode->_callback = callback;
+        newNode->next = head;
+        head = newNode;
+    }
+}
+
+// distribute the CAN frame to the registered callbacks
+void MultiCAN::MultiCANLinkedList::handle_frame(AP_HAL::CANFrame &frame)
+{
+    WITH_SEMAPHORE(sem);
+    for (CANSensor_Multi* current = head; current != nullptr; current = current->next) {
+        if (current->_callback(frame)) {
+            return;
+        }
+    }
+}
+
 #endif // HAL_MAX_CAN_PROTOCOL_DRIVERS
 

libraries/AP_CANManager/AP_CANSensor.h

@@ -90,5 +90,41 @@ private:
 #endif
 };
 
+// a class to allow for multiple CAN backends with one
+// CANSensor driver. This can be shared among different libraries like rangefinder and proximity
+class MultiCAN : public CANSensor {
+public:
+    // callback functor def for forwarding frames
+    FUNCTOR_TYPEDEF(ForwardCanFrame, bool, AP_HAL::CANFrame &);
+
+    MultiCAN(ForwardCanFrame cf, AP_CAN::Protocol can_type, const char *driver_name);
+
+    // handle a received frame from the CAN bus
+    void handle_frame(AP_HAL::CANFrame &frame) override;
+
+private:
+    // class to allow for multiple callbacks implemented as a linked list
+    class MultiCANLinkedList {
+    public:
+        struct CANSensor_Multi {
+            ForwardCanFrame _callback;
+            CANSensor_Multi* next = nullptr;
+        };
+
+        // register a callback for a CAN frame by adding it to the linked list
+        void register_callback(ForwardCanFrame callback);
+
+        // distribute the CAN frame to the registered callbacks
+        void handle_frame(AP_HAL::CANFrame &frame);
+        HAL_Semaphore sem;
+
+    private:
+        CANSensor_Multi* head = nullptr;
+    };
+
+    // Pointer to static instance of the linked list for persistence across instances
+    static MultiCANLinkedList* callbacks;
+};
+
 #endif // HAL_MAX_CAN_PROTOCOL_DRIVERS