diff --git a/libraries/AP_UAVCAN/AP_UAVCAN.cpp b/libraries/AP_UAVCAN/AP_UAVCAN.cpp index 6b2c20a87c..ef8894e798 100644 --- a/libraries/AP_UAVCAN/AP_UAVCAN.cpp +++ b/libraries/AP_UAVCAN/AP_UAVCAN.cpp @@ -58,11 +58,13 @@ #include "AP_UAVCAN_DNA_Server.h" #include #include +#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 #if HAL_CANFD_SUPPORTED #define UAVCAN_NODE_POOL_SIZE 16384 @@ -77,15 +79,6 @@ extern const AP_HAL::HAL& hal; #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 @@ -146,6 +139,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 }; @@ -196,9 +196,6 @@ static uavcan::Subscriber *esc_stat UC_REGISTRY_BINDER(DebugCb, uavcan::protocol::debug::LogMessage); static uavcan::Subscriber *debug_listener[HAL_MAX_CAN_PROTOCOL_DRIVERS]; -static uavcan::PoolAllocator _node_allocator[HAL_MAX_CAN_PROTOCOL_DRIVERS]; - - AP_UAVCAN::AP_UAVCAN() { AP_Param::setup_object_defaults(this, var_info); @@ -258,7 +255,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[driver_index]); + _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"); diff --git a/libraries/AP_UAVCAN/AP_UAVCAN.h b/libraries/AP_UAVCAN/AP_UAVCAN.h index 15f9d6c2ea..a2908d2038 100644 --- a/libraries/AP_UAVCAN/AP_UAVCAN.h +++ b/libraries/AP_UAVCAN/AP_UAVCAN.h @@ -30,7 +30,6 @@ #include #include #include -#include #include @@ -54,6 +53,7 @@ class ESCStatusCb; class DebugCb; class ParamGetSetCb; class ParamExecuteOpcodeCb; +class AP_PoolAllocator; #if defined(__GNUC__) && (__GNUC__ > 8) #define DISABLE_W_CAST_FUNCTION_TYPE_PUSH \ @@ -264,7 +264,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; diff --git a/libraries/AP_UAVCAN/AP_UAVCAN_pool.cpp b/libraries/AP_UAVCAN/AP_UAVCAN_pool.cpp new file mode 100644 index 0000000000..3fda97c1f2 --- /dev/null +++ b/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 . + * + */ +/* + based on dynamic_memory.hpp which is: + Copyright (C) 2014 Pavel Kirienko + */ + +#include + +#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(const_cast(ptr)); + p->next = free_list; + free_list = p; + + used--; +} + +#endif // HAL_ENABLE_LIBUAVCAN_DRIVERS + diff --git a/libraries/AP_UAVCAN/AP_UAVCAN_pool.h b/libraries/AP_UAVCAN/AP_UAVCAN_pool.h new file mode 100644 index 0000000000..2549526bb7 --- /dev/null +++ b/libraries/AP_UAVCAN/AP_UAVCAN_pool.h @@ -0,0 +1,60 @@ +/* + * 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 . + * + */ +/* + based on dynamic_memory.hpp which is: + Copyright (C) 2014 Pavel Kirienko + */ + +#pragma once + +#include "AP_UAVCAN.h" + +#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 + +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; +};