AP_UAVCAN: added dynamically allocated pool size param

allow for smaller pool size to save memory

libraries/AP_UAVCAN/AP_UAVCAN.cpp

@@ -57,11 +57,19 @@
 #include <AP_ADSB/AP_ADSB.h>
 #include "AP_UAVCAN_DNA_Server.h"
 #include <AP_Logger/AP_Logger.h>
+#include "AP_UAVCAN_pool.h"
 
 #define LED_DELAY_US 50000
 
 extern const AP_HAL::HAL& hal;
 
+// setup default pool size
+#ifndef UAVCAN_NODE_POOL_SIZE
+#define UAVCAN_NODE_POOL_SIZE 8192
+#endif
+
+#define UAVCAN_STACK_SIZE     4096
+
 #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
@@ -121,6 +129,13 @@ const AP_Param::GroupInfo AP_UAVCAN::var_info[] = {
     // @Range: 0 18
     // @User: Advanced
     AP_GROUPINFO("ESC_OF", 7, AP_UAVCAN, _esc_offset, 0),
+
+    // @Param: POOL
+    // @DisplayName: CAN pool size
+    // @Description: Amount of memory in bytes to allocate for the DroneCAN memory pool. More memory is needed for higher CAN bus loads
+    // @Range: 1024 16384
+    // @User: Advanced
+    AP_GROUPINFO("POOL", 8, AP_UAVCAN, _pool_size, UAVCAN_NODE_POOL_SIZE),
     
     AP_GROUPEND
 };
@@ -171,9 +186,7 @@ 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];
 
-
-AP_UAVCAN::AP_UAVCAN() :
-    _node_allocator()
+AP_UAVCAN::AP_UAVCAN()
 {
     AP_Param::setup_object_defaults(this, var_info);
 
@@ -232,7 +245,14 @@ void AP_UAVCAN::init(uint8_t driver_index, bool enable_filters)
         return;
     }
 
-    _node = new uavcan::Node<0>(*_iface_mgr, uavcan::SystemClock::instance(), _node_allocator);
+    _allocator = new AP_PoolAllocator(_pool_size);
+
+    if (_allocator == nullptr || !_allocator->init()) {
+        debug_uavcan(AP_CANManager::LOG_ERROR, "UAVCAN: couldn't allocate node pool\n");
+        return;
+    }
+
+    _node = new uavcan::Node<0>(*_iface_mgr, uavcan::SystemClock::instance(), *_allocator);
 
     if (_node == nullptr) {
         debug_uavcan(AP_CANManager::LOG_ERROR, "UAVCAN: couldn't allocate node\n\r");

libraries/AP_UAVCAN/AP_UAVCAN.h

@@ -30,8 +30,6 @@
 #include <AP_ESC_Telem/AP_ESC_Telem_Backend.h>
 #include <uavcan/protocol/param/GetSet.hpp>
 #include <uavcan/protocol/param/ExecuteOpcode.hpp>
-#include <uavcan/helpers/heap_based_pool_allocator.hpp>
-
 
 #ifndef UAVCAN_NODE_POOL_SIZE
 #define UAVCAN_NODE_POOL_SIZE 8192
@@ -61,6 +59,7 @@ class ESCStatusCb;
 class DebugCb;
 class ParamGetSetCb;
 class ParamExecuteOpcodeCb;
+class AP_PoolAllocator;
 
 #if defined(__GNUC__) && (__GNUC__ > 8)
 #define DISABLE_W_CAST_FUNCTION_TYPE_PUSH \
@@ -272,7 +271,9 @@ private:
     AP_Int16 _servo_rate_hz;
     AP_Int16 _options;
     AP_Int16 _notify_state_hz;
+    AP_Int16 _pool_size;
 
+    AP_PoolAllocator *_allocator;
     uavcan::Node<0> *_node;
 
     uint8_t _driver_index;

libraries/AP_UAVCAN/AP_UAVCAN_pool.cpp

@@ -0,0 +1,79 @@
+/*
+ * This file is free software: you can redistribute it and/or modify it
+ * under the terms of the GNU General Public License as published by the
+ * Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This file is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
+ * See the GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License along
+ * with this program.  If not, see <http://www.gnu.org/licenses/>.
+ *
+ */
+/*
+  based on dynamic_memory.hpp which is:
+  Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>
+ */
+
+#include <AP_HAL/AP_HAL.h>
+
+#if HAL_ENABLE_LIBUAVCAN_DRIVERS
+
+#include "AP_UAVCAN.h"
+#include "AP_UAVCAN_pool.h"
+
+AP_PoolAllocator::AP_PoolAllocator(uint16_t _pool_size) :
+    num_blocks(_pool_size / UAVCAN_NODE_POOL_BLOCK_SIZE)
+{
+}
+
+bool AP_PoolAllocator::init(void)
+{
+    WITH_SEMAPHORE(sem);
+    pool_nodes = (Node *)calloc(num_blocks, UAVCAN_NODE_POOL_BLOCK_SIZE);
+    if (pool_nodes == nullptr) {
+        return false;
+    }
+    for (uint16_t i=0; i<(num_blocks-1); i++) {
+        pool_nodes[i].next = &pool_nodes[i+1];
+    }
+    free_list = &pool_nodes[0];
+    return true;
+}
+
+void* AP_PoolAllocator::allocate(std::size_t size)
+{
+    WITH_SEMAPHORE(sem);
+    if (free_list == nullptr || size > UAVCAN_NODE_POOL_BLOCK_SIZE) {
+        return nullptr;
+    }
+    Node *ret = free_list;
+    free_list = free_list->next;
+
+    used++;
+    if (used > max_used) {
+        max_used = used;
+    }
+
+    return ret;
+}
+
+void AP_PoolAllocator::deallocate(const void* ptr)
+{
+    if (ptr == nullptr) {
+        return;
+    }
+    WITH_SEMAPHORE(sem);
+
+    Node *p = reinterpret_cast<Node*>(const_cast<void*>(ptr));
+    p->next = free_list;
+    free_list = p;
+
+    used--;
+}
+
+#endif // HAL_ENABLE_LIBUAVCAN_DRIVERS
+

libraries/AP_UAVCAN/AP_UAVCAN_pool.h

@@ -0,0 +1,56 @@
+/*
+ * This file is free software: you can redistribute it and/or modify it
+ * under the terms of the GNU General Public License as published by the
+ * Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This file is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
+ * See the GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License along
+ * with this program.  If not, see <http://www.gnu.org/licenses/>.
+ *
+ */
+/*
+  based on dynamic_memory.hpp which is:
+  Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>
+ */
+
+#pragma once
+
+#include "AP_UAVCAN.h"
+
+#ifndef UAVCAN_NODE_POOL_BLOCK_SIZE
+#define UAVCAN_NODE_POOL_BLOCK_SIZE 64
+#endif
+
+class AP_PoolAllocator : public uavcan::IPoolAllocator
+{
+public:
+    AP_PoolAllocator(uint16_t _pool_size);
+
+    bool init(void);
+    void *allocate(std::size_t size) override;
+    void deallocate(const void* ptr) override;
+
+    uint16_t getBlockCapacity() const override {
+        return num_blocks;
+    }
+
+private:
+    const uint16_t num_blocks;
+
+    union Node {
+        uint8_t data[UAVCAN_NODE_POOL_BLOCK_SIZE];
+        Node* next;
+    };
+
+    Node *free_list;
+    Node *pool_nodes;
+    HAL_Semaphore sem;
+
+    uint16_t used;
+    uint16_t max_used;
+};