AP_CANManager: Add support for multiple libraries to share a CAN Protocol

libraries/AP_Arming/AP_Arming.cpp

@@ -1198,9 +1198,8 @@ bool AP_Arming::can_checks(bool report)
                 }
                 case AP_CAN::Protocol::USD1:
                 case AP_CAN::Protocol::TOFSenseP:
-                case AP_CAN::Protocol::NanoRadar_NRA24:
+                case AP_CAN::Protocol::NanoRadar:
                 case AP_CAN::Protocol::Benewake:
-                case AP_CAN::Protocol::MR72:
                 {
                     for (uint8_t j = i; j; j--) {
                         if (AP::can().get_driver_type(i) == AP::can().get_driver_type(j-1)) {

libraries/AP_CANManager/AP_CAN.h

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

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 @@ class CANSensor : public AP_CANDriver {
 #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
 

libraries/AP_Proximity/AP_Proximity_MR72_CAN.cpp

@@ -32,26 +32,14 @@ const AP_Param::GroupInfo AP_Proximity_MR72_CAN::var_info[] = {
     AP_GROUPEND
 };
 
-MR72_MultiCAN *AP_Proximity_MR72_CAN::multican;
-
 AP_Proximity_MR72_CAN::AP_Proximity_MR72_CAN(AP_Proximity &_frontend,
                                      AP_Proximity::Proximity_State &_state,
                                      AP_Proximity_Params& _params):
     AP_Proximity_Backend(_frontend, _state, _params)
 {
-    if (multican == nullptr) {
-        multican = new MR72_MultiCAN();
-        if (multican == nullptr) {
-            AP_BoardConfig::allocation_error("MR72_CAN");
-        }
-    }
-
-    {
-        // add to linked list of drivers
-        WITH_SEMAPHORE(multican->sem);
-        auto *prev = multican->drivers;
-        next = prev;
-        multican->drivers = this;
+    multican_MR72 = new MultiCAN{FUNCTOR_BIND_MEMBER(&AP_Proximity_MR72_CAN::handle_frame, bool, AP_HAL::CANFrame &), AP_CAN::Protocol::NanoRadar, "MR72 MultiCAN"};
+    if (multican_MR72 == nullptr) {
+        AP_BoardConfig::allocation_error("Failed to create proximity multican");
     }
 
     AP_Param::setup_object_defaults(this, var_info);
@@ -132,15 +120,4 @@ bool AP_Proximity_MR72_CAN::parse_distance_message(AP_HAL::CANFrame &frame)
     return true;
 }
 
-// handle frames from CANSensor, passing to the drivers
-void MR72_MultiCAN::handle_frame(AP_HAL::CANFrame &frame)
-{
-    WITH_SEMAPHORE(sem);
-    for (auto *d = drivers; d; d=d->next) {
-        if (d->handle_frame(frame)) {
-            break;
-        }
-    }
-}
-
 #endif // HAL_PROXIMITY_ENABLED

libraries/AP_Proximity/AP_Proximity_MR72_CAN.h

@@ -45,24 +45,7 @@ class AP_Proximity_MR72_CAN : public AP_Proximity_Backend {
 
     AP_Int32 receive_id;                // ID of the sensor
 
-    static MR72_MultiCAN *multican;     // linked list
-    AP_Proximity_MR72_CAN *next;
-};
-
-// a class to allow for multiple MR_72 backends with one
-// CANSensor driver
-class MR72_MultiCAN : public CANSensor {
-public:
-    MR72_MultiCAN() : CANSensor("MR72") {
-        register_driver(AP_CAN::Protocol::MR72);
-    }
-
-    // handler for incoming frames
-    void handle_frame(AP_HAL::CANFrame &frame) override;
-
-    HAL_Semaphore sem;
-    AP_Proximity_MR72_CAN *drivers;
-
+    MultiCAN* multican_MR72;            // Allows for multiple CAN rangefinders on a single bus
 };
 
 #endif // HAL_PROXIMITY_ENABLED

libraries/AP_RangeFinder/AP_RangeFinder_Backend_CAN.cpp

@@ -39,11 +39,16 @@ const AP_Param::GroupInfo AP_RangeFinder_Backend_CAN::var_info[] = {
 
 // constructor
 AP_RangeFinder_Backend_CAN::AP_RangeFinder_Backend_CAN(
-    RangeFinder::RangeFinder_State &_state, AP_RangeFinder_Params &_params) :
+    RangeFinder::RangeFinder_State &_state, AP_RangeFinder_Params &_params, AP_CAN::Protocol can_type,
+                                    const char *driver_name) :
     AP_RangeFinder_Backend(_state, _params)
 {
     AP_Param::setup_object_defaults(this, var_info);
     state.var_info = var_info;
+    multican_rangefinder = new MultiCAN{FUNCTOR_BIND_MEMBER(&AP_RangeFinder_Backend_CAN::handle_frame, bool, AP_HAL::CANFrame &), can_type, driver_name};
+    if (multican_rangefinder == nullptr) {
+        AP_BoardConfig::allocation_error("Failed to create rangefinder multican");
+    }
 }
 
 // update the state of the sensor
@@ -82,15 +87,4 @@ bool AP_RangeFinder_Backend_CAN::is_correct_id(uint32_t id) const
     return true;
 }
 
-// handle frames from CANSensor, passing to the drivers
-void RangeFinder_MultiCAN::handle_frame(AP_HAL::CANFrame &frame)
-{
-    WITH_SEMAPHORE(sem);
-    for (auto *d = drivers; d != nullptr; d=d->next) {
-        if (d->handle_frame(frame)) {
-            break;
-        }
-    }
-}
-
 #endif // HAL_MAX_CAN_PROTOCOL_DRIVERS

libraries/AP_RangeFinder/AP_RangeFinder_Backend_CAN.h

@@ -14,7 +14,8 @@ class AP_RangeFinder_Backend_CAN : public AP_RangeFinder_Backend
 public:
     // constructor
     AP_RangeFinder_Backend_CAN(RangeFinder::RangeFinder_State &_state,
-                                  AP_RangeFinder_Params &_params);
+                                AP_RangeFinder_Params &_params, AP_CAN::Protocol can_type,
+                                const char *driver_name);
 
     friend class RangeFinder_MultiCAN;
 
@@ -53,26 +54,11 @@ class AP_RangeFinder_Backend_CAN : public AP_RangeFinder_Backend
     AP_Int32 receive_id; // CAN ID to receive for this backend
     AP_Int32 snr_min; // minimum signal strength to accept packet
 
+    MultiCAN* multican_rangefinder; // Allows for multiple CAN rangefinders on a single bus
 private:
 
     float _distance_sum; // meters
     uint32_t _distance_count;
 };
 
-// a class to allow for multiple CAN backends with one
-// CANSensor driver
-class RangeFinder_MultiCAN : public CANSensor {
-public:
-    RangeFinder_MultiCAN(AP_CAN::Protocol can_type, const char *driver_name) : CANSensor(driver_name) {
-        register_driver(can_type);
-    }
-
-    // handler for incoming frames
-    void handle_frame(AP_HAL::CANFrame &frame) override;
-
-    // Semaphore for access to shared backend data
-    HAL_Semaphore sem;
-    AP_RangeFinder_Backend_CAN *drivers;
-};
-
 #endif // HAL_MAX_CAN_PROTOCOL_DRIVERS

libraries/AP_RangeFinder/AP_RangeFinder_Benewake_CAN.cpp

@@ -5,30 +5,6 @@
 
 #if AP_RANGEFINDER_BENEWAKE_CAN_ENABLED
 
-RangeFinder_MultiCAN *AP_RangeFinder_Benewake_CAN::multican;
-
-/*
-  constructor
- */
-AP_RangeFinder_Benewake_CAN::AP_RangeFinder_Benewake_CAN(RangeFinder::RangeFinder_State &_state, AP_RangeFinder_Params &_params) :
-    AP_RangeFinder_Backend_CAN(_state, _params)
-{
-    if (multican == nullptr) {
-        multican = new RangeFinder_MultiCAN(AP_CAN::Protocol::Benewake, "Benewake MultiCAN");
-        if (multican == nullptr) {
-            AP_BoardConfig::allocation_error("Benewake_CAN");
-        }
-    }
-
-    {
-        // add to linked list of drivers
-        WITH_SEMAPHORE(multican->sem);
-        auto *prev = multican->drivers;
-        next = prev;
-        multican->drivers = this;
-    }
-}
-
 // handler for incoming frames for H30 radar
 bool AP_RangeFinder_Benewake_CAN::handle_frame_H30(AP_HAL::CANFrame &frame)
 {

libraries/AP_RangeFinder/AP_RangeFinder_Benewake_CAN.h

@@ -7,14 +7,14 @@
 
 class AP_RangeFinder_Benewake_CAN : public AP_RangeFinder_Backend_CAN {
 public:
-    AP_RangeFinder_Benewake_CAN(RangeFinder::RangeFinder_State &_state, AP_RangeFinder_Params &_params);
+    AP_RangeFinder_Benewake_CAN(RangeFinder::RangeFinder_State &_state, AP_RangeFinder_Params &_params) :
+        AP_RangeFinder_Backend_CAN(_state, _params, AP_CAN::Protocol::Benewake, "benewake")
+    {
+    }
 
     // handler for incoming frames. Return true if consumed
     bool handle_frame(AP_HAL::CANFrame &frame) override;
     bool handle_frame_H30(AP_HAL::CANFrame &frame);
-
-private:
-    static RangeFinder_MultiCAN *multican;
 };
 
 #endif  // AP_RANGEFINDER_BENEWAKE_CAN_ENABLED

libraries/AP_RangeFinder/AP_RangeFinder_NRA24_CAN.cpp

@@ -6,28 +6,6 @@
 #include <AP_BoardConfig/AP_BoardConfig.h>
 #include <AP_HAL/AP_HAL.h>
 
-RangeFinder_MultiCAN *AP_RangeFinder_NRA24_CAN::multican_NRA24;
-
-// constructor
-AP_RangeFinder_NRA24_CAN::AP_RangeFinder_NRA24_CAN(RangeFinder::RangeFinder_State &_state, AP_RangeFinder_Params &_params) :
-    AP_RangeFinder_Backend_CAN(_state, _params)
-{
-    if (multican_NRA24 == nullptr) {
-        multican_NRA24 = new RangeFinder_MultiCAN(AP_CAN::Protocol::NanoRadar_NRA24, "NRA24 MultiCAN");
-        if (multican_NRA24 == nullptr) {
-            AP_BoardConfig::allocation_error("Rangefinder_MultiCAN");
-        }
-    }
-
-    {
-        // add to linked list of drivers
-        WITH_SEMAPHORE(multican_NRA24->sem);
-        auto *prev = multican_NRA24->drivers;
-        next = prev;
-        multican_NRA24->drivers = this;
-    }
-}
-
 // update the state of the sensor
 void AP_RangeFinder_NRA24_CAN::update(void)
 {

libraries/AP_RangeFinder/AP_RangeFinder_NRA24_CAN.h

@@ -6,7 +6,10 @@
 
 class AP_RangeFinder_NRA24_CAN : public AP_RangeFinder_Backend_CAN {
 public:
-    AP_RangeFinder_NRA24_CAN(RangeFinder::RangeFinder_State &_state, AP_RangeFinder_Params &_params);
+    AP_RangeFinder_NRA24_CAN(RangeFinder::RangeFinder_State &_state, AP_RangeFinder_Params &_params) :
+        AP_RangeFinder_Backend_CAN(_state, _params, AP_CAN::Protocol::NanoRadar, "nra24")
+    {
+    }
 
     void update(void) override;
 
@@ -18,9 +21,6 @@ class AP_RangeFinder_NRA24_CAN : public AP_RangeFinder_Backend_CAN {
 private:
 
     uint32_t get_radar_id(uint32_t id) const { return ((id & 0xF0U) >> 4U); }
-
-    static RangeFinder_MultiCAN *multican_NRA24;
-
     uint32_t last_heartbeat_ms; // last status message received from the sensor
 };