From 586568007057077986a22c5a6620b9fa18c25b71 Mon Sep 17 00:00:00 2001
From: bugobliterator <siddharthbharatpurohit@gmail.com>
Date: Tue, 1 Mar 2022 22:17:00 +0530
Subject: [PATCH] AP_UAVCAN: move pool allocator to cpp

---
 libraries/AP_UAVCAN/AP_UAVCAN.cpp | 30 +++++++++++++++++++++++++++---
 libraries/AP_UAVCAN/AP_UAVCAN.h   | 25 +------------------------
 2 files changed, 28 insertions(+), 27 deletions(-)

diff --git a/libraries/AP_UAVCAN/AP_UAVCAN.cpp b/libraries/AP_UAVCAN/AP_UAVCAN.cpp
index 26e62ae14d..2653d92896 100644
--- a/libraries/AP_UAVCAN/AP_UAVCAN.cpp
+++ b/libraries/AP_UAVCAN/AP_UAVCAN.cpp
@@ -62,6 +62,29 @@
 
 extern const AP_HAL::HAL& hal;
 
+#ifndef UAVCAN_NODE_POOL_SIZE
+#if HAL_CANFD_SUPPORTED
+#define UAVCAN_NODE_POOL_SIZE 16384
+#else
+#define UAVCAN_NODE_POOL_SIZE 8192
+#endif
+#endif
+
+#if HAL_CANFD_SUPPORTED
+#define UAVCAN_STACK_SIZE     8192
+#else
+#define UAVCAN_STACK_SIZE     4096
+#endif
+
+
+#ifndef UAVCAN_NODE_POOL_BLOCK_SIZE
+#if HAL_CANFD_SUPPORTED
+#define UAVCAN_NODE_POOL_BLOCK_SIZE 128
+#else
+#define UAVCAN_NODE_POOL_BLOCK_SIZE 64
+#endif
+#endif
+
 #define debug_uavcan(level_debug, fmt, args...) do { AP::can().log_text(level_debug, "UAVCAN", fmt, ##args); } while (0)
 
 // Translation of all messages from UAVCAN structures into AP structures is done
@@ -164,9 +187,10 @@ static uavcan::Subscriber<uavcan::equipment::esc::Status, ESCStatusCb> *esc_stat
 UC_REGISTRY_BINDER(DebugCb, uavcan::protocol::debug::LogMessage);
 static uavcan::Subscriber<uavcan::protocol::debug::LogMessage, DebugCb> *debug_listener[HAL_MAX_CAN_PROTOCOL_DRIVERS];
 
+static uavcan::PoolAllocator<UAVCAN_NODE_POOL_SIZE, UAVCAN_NODE_POOL_BLOCK_SIZE, AP_UAVCAN::RaiiSynchronizer> _node_allocator[HAL_MAX_CAN_PROTOCOL_DRIVERS];
 
-AP_UAVCAN::AP_UAVCAN() :
-    _node_allocator()
+
+AP_UAVCAN::AP_UAVCAN()
 {
     AP_Param::setup_object_defaults(this, var_info);
 
@@ -225,7 +249,7 @@ void AP_UAVCAN::init(uint8_t driver_index, bool enable_filters)
         return;
     }
 
-    _node = new uavcan::Node<0>(*_iface_mgr, uavcan::SystemClock::instance(), _node_allocator);
+    _node = new uavcan::Node<0>(*_iface_mgr, uavcan::SystemClock::instance(), _node_allocator[driver_index]);
 
     if (_node == nullptr) {
         debug_uavcan(AP_CANManager::LOG_ERROR, "UAVCAN: couldn't allocate node\n\r");
diff --git a/libraries/AP_UAVCAN/AP_UAVCAN.h b/libraries/AP_UAVCAN/AP_UAVCAN.h
index 8ea923c336..a285afa92f 100644
--- a/libraries/AP_UAVCAN/AP_UAVCAN.h
+++ b/libraries/AP_UAVCAN/AP_UAVCAN.h
@@ -33,27 +33,6 @@
 #include <uavcan/helpers/heap_based_pool_allocator.hpp>
 
 
-#ifndef UAVCAN_NODE_POOL_SIZE
-#if HAL_CANFD_SUPPORTED
-#define UAVCAN_NODE_POOL_SIZE 16384
-#else
-#define UAVCAN_NODE_POOL_SIZE 8192
-#endif
-#endif
-
-#if HAL_CANFD_SUPPORTED
-#define UAVCAN_STACK_SIZE     8192
-#else
-#define UAVCAN_STACK_SIZE     4096
-#endif
-
-#ifndef UAVCAN_NODE_POOL_BLOCK_SIZE
-#if HAL_CANFD_SUPPORTED
-#define UAVCAN_NODE_POOL_BLOCK_SIZE 128
-#else
-#define UAVCAN_NODE_POOL_BLOCK_SIZE 64
-#endif
-#endif
 
 #ifndef UAVCAN_SRV_NUMBER
 #define UAVCAN_SRV_NUMBER 18
@@ -232,11 +211,11 @@ public:
     // 0. return true if it was set
     bool check_and_reset_option(Options option);
 
-private:
     // This will be needed to implement if UAVCAN is used with multithreading
     // Such cases will be firmware update, etc.
     class RaiiSynchronizer {};
 
+private:
     void loop(void);
 
     ///// SRV output /////
@@ -277,8 +256,6 @@ private:
     HAL_Semaphore _param_save_sem;
     uint8_t param_save_request_node_id;
 
-    uavcan::PoolAllocator<UAVCAN_NODE_POOL_SIZE, UAVCAN_NODE_POOL_BLOCK_SIZE, AP_UAVCAN::RaiiSynchronizer> _node_allocator;
-
     // UAVCAN parameters
     AP_Int8 _uavcan_node;
     AP_Int32 _servo_bm;