From 73a3fdd2011ade73f5d0a5bbc38833c81111ef8b Mon Sep 17 00:00:00 2001 From: rishabsingh3003 Date: Thu, 22 Feb 2024 20:59:50 -0500 Subject: [PATCH] AP_CANManager: Add multiCAN interface --- libraries/AP_CANManager/AP_CAN.h | 3 +- libraries/AP_CANManager/AP_CANDriver.cpp | 4 +- libraries/AP_CANManager/AP_CANSensor.cpp | 53 ++++++++++++++++++++++++ libraries/AP_CANManager/AP_CANSensor.h | 36 ++++++++++++++++ 4 files changed, 92 insertions(+), 4 deletions(-) diff --git a/libraries/AP_CANManager/AP_CAN.h b/libraries/AP_CANManager/AP_CAN.h index dff85215c1..c9befd5b0b 100644 --- a/libraries/AP_CANManager/AP_CAN.h +++ b/libraries/AP_CANManager/AP_CAN.h @@ -28,7 +28,6 @@ public: Benewake = 11, Scripting2 = 12, TOFSenseP = 13, - NanoRadar_NRA24 = 14, - MR72 = 15, + NanoRadar = 14, }; }; diff --git a/libraries/AP_CANManager/AP_CANDriver.cpp b/libraries/AP_CANManager/AP_CANDriver.cpp index 0ec7ebbceb..782eea0e04 100644 --- a/libraries/AP_CANManager/AP_CANDriver.cpp +++ b/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)), diff --git a/libraries/AP_CANManager/AP_CANSensor.cpp b/libraries/AP_CANManager/AP_CANSensor.cpp index 8e300eba09..17b46d1d30 100644 --- a/libraries/AP_CANManager/AP_CANSensor.cpp +++ b/libraries/AP_CANManager/AP_CANSensor.cpp @@ -21,6 +21,7 @@ #include #include "AP_CANSensor.h" +#include 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 diff --git a/libraries/AP_CANManager/AP_CANSensor.h b/libraries/AP_CANManager/AP_CANSensor.h index 2cd775f4c3..7ba0021bb1 100644 --- a/libraries/AP_CANManager/AP_CANSensor.h +++ b/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