From e227f139a5b362f308a20197e87aa79faec62819 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Fri, 16 Oct 2015 17:24:14 +0300 Subject: [PATCH 01/17] Libuavcan switched to footprint_reduction --- src/modules/uavcan/libuavcan | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/uavcan/libuavcan b/src/modules/uavcan/libuavcan index 0643879922..4e4d9b7854 160000 --- a/src/modules/uavcan/libuavcan +++ b/src/modules/uavcan/libuavcan @@ -1 +1 @@ -Subproject commit 0643879922239930cf7482777356f06891c26616 +Subproject commit 4e4d9b7854b7821f42fd83d89885cc489cc18c33 From 7373d99d72db00d76a6cb2a776d5e3f31d7d90fb Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Fri, 16 Oct 2015 18:56:07 +0300 Subject: [PATCH 02/17] UAVCAN CMakeLists - removed useless definitions --- src/modules/uavcan/CMakeLists.txt | 3 --- 1 file changed, 3 deletions(-) diff --git a/src/modules/uavcan/CMakeLists.txt b/src/modules/uavcan/CMakeLists.txt index b8c899840a..ce348ec910 100644 --- a/src/modules/uavcan/CMakeLists.txt +++ b/src/modules/uavcan/CMakeLists.txt @@ -37,15 +37,12 @@ set(UAVCAN_PLATFORM stm32 CACHE STRING "uavcan platform") string(TOUPPER "${OS}" OS_UPPER) add_definitions( -DUAVCAN_CPP_VERSION=UAVCAN_CPP03 - -DUAVCAN_MAX_NETWORK_SIZE_HINT=16 -DUAVCAN_MEM_POOL_BLOCK_SIZE=48 -DUAVCAN_NO_ASSERTIONS -DUAVCAN_PLATFORM=stm32 -DUAVCAN_STM32_${OS_UPPER}=1 -DUAVCAN_STM32_NUM_IFACES=2 -DUAVCAN_STM32_TIMER_NUMBER=5 - -DUAVCAN_USE_CPP03=ON - -DUAVCAN_USE_EXTERNAL_SNPRINT ) add_subdirectory(libuavcan EXCLUDE_FROM_ALL) From 407191d4ab1a90bb413c3db1241cbba8adb95172 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Fri, 16 Oct 2015 19:37:18 +0300 Subject: [PATCH 03/17] UAVCAN driver transformed to use global memory pool --- src/modules/uavcan/uavcan_main.cpp | 3 ++- src/modules/uavcan/uavcan_main.hpp | 20 ++++++++++---- src/modules/uavcan/uavcan_servers.cpp | 4 +-- src/modules/uavcan/uavcan_servers.hpp | 26 ++++--------------- .../uavcan/uavcan_virtual_can_driver.hpp | 18 +++++-------- 5 files changed, 31 insertions(+), 40 deletions(-) diff --git a/src/modules/uavcan/uavcan_main.cpp b/src/modules/uavcan/uavcan_main.cpp index 1ba9d345fb..871551dd2c 100644 --- a/src/modules/uavcan/uavcan_main.cpp +++ b/src/modules/uavcan/uavcan_main.cpp @@ -77,7 +77,8 @@ UavcanNode *UavcanNode::_instance; UavcanNode::UavcanNode(uavcan::ICanDriver &can_driver, uavcan::ISystemClock &system_clock) : CDev("uavcan", UAVCAN_DEVICE_PATH), - _node(can_driver, system_clock), + _pool_allocator(MemoryPoolBlockCapacitySoftLimit, MemoryPoolBlockCapacityHardLimit), + _node(can_driver, system_clock, _pool_allocator), _node_mutex(), _esc_controller(_node), _time_sync_master(_node), diff --git a/src/modules/uavcan/uavcan_main.hpp b/src/modules/uavcan/uavcan_main.hpp index 6a1d2f391d..05ceca83f6 100644 --- a/src/modules/uavcan/uavcan_main.hpp +++ b/src/modules/uavcan/uavcan_main.hpp @@ -36,6 +36,7 @@ #include #include +#include #include #include #include @@ -80,7 +81,6 @@ class UavcanNode : public device::CDev static constexpr unsigned PollTimeoutMs = 10; - static constexpr unsigned MemPoolSize = 64 * uavcan::MemPoolBlockSize; /* * This memory is reserved for uavcan to use for queuing CAN frames. * At 1Mbit there is approximately one CAN frame every 145 uS. @@ -96,8 +96,10 @@ class UavcanNode : public device::CDev static constexpr unsigned RxQueueLenPerIface = FramePerMSecond * PollTimeoutMs; // At static constexpr unsigned StackSize = 1800; + static constexpr unsigned MemoryPoolBlockCapacitySoftLimit = 250; + static constexpr unsigned MemoryPoolBlockCapacityHardLimit = 500; + public: - typedef uavcan::Node Node; typedef uavcan_stm32::CanInitHelper CanInitHelper; enum eServerAction {None, Start, Stop, CheckFW , Busy}; @@ -109,7 +111,7 @@ public: static int start(uavcan::NodeID node_id, uint32_t bitrate); - Node &get_node() { return _node; } + uavcan::Node<> &get_node() { return _node; } // TODO: move the actuator mixing stuff into the ESC controller class static int control_callback(uintptr_t handle, uint8_t control_group, uint8_t control_index, float &input); @@ -130,8 +132,8 @@ public: int set_param(int remote_node_id, const char *name, char *value); int get_param(int remote_node_id, const char *name); int reset_node(int remote_node_id); -private: +private: void fill_node_info(); int init(uavcan::NodeID node_id); void node_spin_once(); @@ -167,7 +169,15 @@ private: static UavcanNode *_instance; ///< singleton pointer - Node _node; ///< library instance + struct MemoryPoolSynchronizer + { + const ::irqstate_t state = ::irqsave(); + ~MemoryPoolSynchronizer() { ::irqrestore(state); } + }; + + uavcan::HeapBasedPoolAllocator _pool_allocator; + + uavcan::Node<> _node; ///< library instance pthread_mutex_t _node_mutex; px4_sem_t _server_command_sem; UavcanEscController _esc_controller; diff --git a/src/modules/uavcan/uavcan_servers.cpp b/src/modules/uavcan/uavcan_servers.cpp index 713a4e00f7..8ae013ce48 100644 --- a/src/modules/uavcan/uavcan_servers.cpp +++ b/src/modules/uavcan/uavcan_servers.cpp @@ -85,8 +85,8 @@ UavcanServers *UavcanServers::_instance; UavcanServers::UavcanServers(uavcan::INode &main_node) : _subnode_thread(-1), - _vdriver(NumIfaces, uavcan_stm32::SystemClock::instance()), - _subnode(_vdriver, uavcan_stm32::SystemClock::instance()), + _vdriver(NumIfaces, uavcan_stm32::SystemClock::instance(), main_node.getAllocator(), VirtualIfaceBlockAllocationQuota), + _subnode(_vdriver, uavcan_stm32::SystemClock::instance(), main_node.getAllocator()), _main_node(main_node), _tracer(), _storage_backend(), diff --git a/src/modules/uavcan/uavcan_servers.hpp b/src/modules/uavcan/uavcan_servers.hpp index 0b83baa941..9bf9705771 100644 --- a/src/modules/uavcan/uavcan_servers.hpp +++ b/src/modules/uavcan/uavcan_servers.hpp @@ -81,24 +81,10 @@ class UavcanServers { static constexpr unsigned NumIfaces = 1; // UAVCAN_STM32_NUM_IFACES - static constexpr unsigned MemPoolSize = 64 * uavcan::MemPoolBlockSize; - - static constexpr unsigned MaxCanFramesPerTransfer = 63; - - /** - * This number is based on the worst case max number of frames per interface. With - * MemPoolBlockSize set at 48 this is 6048 Bytes. - * - * The servers can be forced to use the primary interface only, this can be achieved simply by passing - * 1 instead of UAVCAN_STM32_NUM_IFACES into the constructor of the virtual CAN driver. - */ - static constexpr unsigned QueuePoolSize = - (NumIfaces * uavcan::MemPoolBlockSize * MaxCanFramesPerTransfer); - static constexpr unsigned StackSize = 6000; static constexpr unsigned Priority = 120; - typedef uavcan::SubNode SubNode; + static constexpr unsigned VirtualIfaceBlockAllocationQuota = 80; public: UavcanServers(uavcan::INode &main_node); @@ -108,7 +94,7 @@ public: static int start(uavcan::INode &main_node); static int stop(); - SubNode &get_node() { return _subnode; } + uavcan::SubNode<> &get_node() { return _subnode; } static UavcanServers *instance() { return _instance; } @@ -131,12 +117,10 @@ private: static UavcanServers *_instance; ///< singleton pointer - typedef VirtualCanDriver vCanDriver; - - vCanDriver _vdriver; + VirtualCanDriver _vdriver; - uavcan::SubNode _subnode; ///< library instance - uavcan::INode &_main_node; ///< library instance + uavcan::SubNode<> _subnode; + uavcan::INode &_main_node; uavcan_posix::dynamic_node_id_server::FileEventTracer _tracer; uavcan_posix::dynamic_node_id_server::FileStorageBackend _storage_backend; diff --git a/src/modules/uavcan/uavcan_virtual_can_driver.hpp b/src/modules/uavcan/uavcan_virtual_can_driver.hpp index 1c861e481c..500750c441 100644 --- a/src/modules/uavcan/uavcan_virtual_can_driver.hpp +++ b/src/modules/uavcan/uavcan_virtual_can_driver.hpp @@ -329,11 +329,7 @@ public: /** * Objects of this class are owned by the sub-node thread. * This class does not use heap memory. - * @tparam SharedMemoryPoolSize Amount of memory, in bytes, that will be statically allocated for the - * memory pool that will be shared across all interfaces for RX/TX queues. - * Typically this value should be no less than 4K per interface. */ -template class VirtualCanDriver : public uavcan::ICanDriver, public uavcan::IRxFrameListener, public ITxQueueInjector, @@ -402,7 +398,7 @@ class VirtualCanDriver : public uavcan::ICanDriver, Event event_; ///< Used to unblock the select() call when IO happens. pthread_mutex_t driver_mutex_; ///< Shared across all ifaces - uavcan::PoolAllocator allocator_; ///< Shared across all ifaces + uavcan::IPoolAllocator& allocator_; ///< Shared across all ifaces uavcan::LazyConstructor ifaces_[uavcan::MaxCanIfaces]; const unsigned num_ifaces_; uavcan::ISystemClock &clock_; @@ -476,7 +472,11 @@ class VirtualCanDriver : public uavcan::ICanDriver, } public: - VirtualCanDriver(unsigned arg_num_ifaces, uavcan::ISystemClock &system_clock) : + VirtualCanDriver(unsigned arg_num_ifaces, + uavcan::ISystemClock &system_clock, + uavcan::IPoolAllocator& allocator, + unsigned virtual_iface_block_allocation_quota) : + allocator_(allocator), num_ifaces_(arg_num_ifaces), clock_(system_clock) { @@ -485,11 +485,7 @@ public: assert(num_ifaces_ > 0 && num_ifaces_ <= uavcan::MaxCanIfaces); - const unsigned quota_per_iface = allocator_.getNumBlocks() / num_ifaces_; - const unsigned quota_per_queue = quota_per_iface; // 2x overcommit - - UAVCAN_TRACE("VirtualCanDriver", "Total blocks: %u, quota per queue: %u", - unsigned(allocator_.getNumBlocks()), unsigned(quota_per_queue)); + const unsigned quota_per_queue = virtual_iface_block_allocation_quota; // 2x overcommit for (unsigned i = 0; i < num_ifaces_; i++) { ifaces_[i].template construct Date: Fri, 16 Oct 2015 20:37:47 +0300 Subject: [PATCH 04/17] Libuavcan update --- src/modules/uavcan/libuavcan | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/uavcan/libuavcan b/src/modules/uavcan/libuavcan index 4e4d9b7854..9a432c0323 160000 --- a/src/modules/uavcan/libuavcan +++ b/src/modules/uavcan/libuavcan @@ -1 +1 @@ -Subproject commit 4e4d9b7854b7821f42fd83d89885cc489cc18c33 +Subproject commit 9a432c03231781403f2a4808846183623cbfc49e From a570d1de7d20f6d0817fd6210473a58db9c3aa13 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Fri, 16 Oct 2015 22:59:17 +0300 Subject: [PATCH 05/17] UAVCAN memory usage status and shrink --- src/modules/uavcan/uavcan_main.cpp | 21 ++++++++++++++++++++- src/modules/uavcan/uavcan_main.hpp | 2 ++ 2 files changed, 22 insertions(+), 1 deletion(-) diff --git a/src/modules/uavcan/uavcan_main.cpp b/src/modules/uavcan/uavcan_main.cpp index 871551dd2c..6508e339f0 100644 --- a/src/modules/uavcan/uavcan_main.cpp +++ b/src/modules/uavcan/uavcan_main.cpp @@ -1120,6 +1120,13 @@ UavcanNode::print_info() (void)pthread_mutex_lock(&_node_mutex); + // Memory status + printf("Pool allocator status:\n"); + printf("\tCapacity hard/soft: %u/%u blocks\n", + _pool_allocator.getBlockCapacityHardLimit(), _pool_allocator.getBlockCapacity()); + printf("\tReserved: %u blocks\n", _pool_allocator.getNumReservedBlocks()); + printf("\tAllocated: %u blocks\n", _pool_allocator.getNumAllocatedBlocks()); + // ESC mixer status printf("ESC actuators control groups: sub: %u / req: %u / fds: %u\n", (unsigned)_groups_subscribed, (unsigned)_groups_required, _poll_fds_num); @@ -1170,13 +1177,20 @@ UavcanNode::print_info() (void)pthread_mutex_unlock(&_node_mutex); } +void UavcanNode::shrink() +{ + (void)pthread_mutex_lock(&_node_mutex); + _pool_allocator.shrink(); + (void)pthread_mutex_unlock(&_node_mutex); +} + /* * App entry point */ static void print_usage() { warnx("usage: \n" - "\tuavcan {start [fw]|status|stop [all|fw]|arm|disarm|update fw|param [set|get|list|save] nodeid [name] [value]|reset nodeid}"); + "\tuavcan {start [fw]|status|stop [all|fw]|shrink|arm|disarm|update fw|param [set|get|list|save] nodeid [name] [value]|reset nodeid}"); } extern "C" __EXPORT int uavcan_main(int argc, char *argv[]); @@ -1252,6 +1266,11 @@ int uavcan_main(int argc, char *argv[]) ::exit(0); } + if (!std::strcmp(argv[1], "shrink")) { + inst->shrink(); + ::exit(0); + } + if (!std::strcmp(argv[1], "arm")) { inst->arm_actuators(true); ::exit(0); diff --git a/src/modules/uavcan/uavcan_main.hpp b/src/modules/uavcan/uavcan_main.hpp index 05ceca83f6..00cdaf418f 100644 --- a/src/modules/uavcan/uavcan_main.hpp +++ b/src/modules/uavcan/uavcan_main.hpp @@ -123,6 +123,8 @@ public: void print_info(); + void shrink(); + static UavcanNode *instance() { return _instance; } static int getHardwareVersion(uavcan::protocol::HardwareVersion &hwver); int fw_server(eServerAction action); From 1ace017cb88319829c80e9571d71c903a4bc78a4 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Fri, 16 Oct 2015 23:17:07 +0300 Subject: [PATCH 06/17] Deallocating memory used by UAVCAN virtual iface on destruction --- src/modules/uavcan/uavcan_virtual_can_driver.hpp | 8 ++++++++ 1 file changed, 8 insertions(+) diff --git a/src/modules/uavcan/uavcan_virtual_can_driver.hpp b/src/modules/uavcan/uavcan_virtual_can_driver.hpp index 500750c441..57b664068c 100644 --- a/src/modules/uavcan/uavcan_virtual_can_driver.hpp +++ b/src/modules/uavcan/uavcan_virtual_can_driver.hpp @@ -114,6 +114,14 @@ public: uavcan::IsDynamicallyAllocatable::check(); } + ~Queue() + { + while (!isEmpty()) + { + pop(); + } + } + bool isEmpty() const { return list_.isEmpty(); } /** From edfb19d9afa3bcba28dbf28227ee11a13a3dc250 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Sat, 17 Oct 2015 01:06:35 +0300 Subject: [PATCH 07/17] Libuavcan update --- src/modules/uavcan/libuavcan | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/uavcan/libuavcan b/src/modules/uavcan/libuavcan index 9a432c0323..9b092509c9 160000 --- a/src/modules/uavcan/libuavcan +++ b/src/modules/uavcan/libuavcan @@ -1 +1 @@ -Subproject commit 9a432c03231781403f2a4808846183623cbfc49e +Subproject commit 9b092509c93794ec60484b2864be7a2995aefac6 From ca4e55fec301bb80af20a94fdfcdbf8d9551a83d Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Sat, 17 Oct 2015 03:24:59 +0300 Subject: [PATCH 08/17] UAVCAN allocator as a dedicated type; reporting a warning if memory leak is deetcted upon destruction --- src/modules/uavcan/allocator.hpp | 73 ++++++++++++++++++++++++++++++ src/modules/uavcan/uavcan_main.cpp | 1 - src/modules/uavcan/uavcan_main.hpp | 12 +---- 3 files changed, 75 insertions(+), 11 deletions(-) create mode 100644 src/modules/uavcan/allocator.hpp diff --git a/src/modules/uavcan/allocator.hpp b/src/modules/uavcan/allocator.hpp new file mode 100644 index 0000000000..db34aa04fb --- /dev/null +++ b/src/modules/uavcan/allocator.hpp @@ -0,0 +1,73 @@ +/**************************************************************************** + * + * Copyright (C) 2015 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @author Pavel Kirienko + */ + +#pragma once + +#include +#include +#include + +// TODO: Entire UAVCAN application should be moved into a namespace later; this is the first step. +namespace uavcan_node +{ + +struct AllocatorSynchronizer +{ + const ::irqstate_t state = ::irqsave(); + ~AllocatorSynchronizer() { ::irqrestore(state); } +}; + +struct Allocator : public uavcan::HeapBasedPoolAllocator +{ + static constexpr unsigned CapacitySoftLimit = 250; + static constexpr unsigned CapacityHardLimit = 500; + + Allocator() : + uavcan::HeapBasedPoolAllocator(CapacitySoftLimit, CapacityHardLimit) + { } + + ~Allocator() + { + if (getNumAllocatedBlocks() > 0) + { + warnx("UAVCAN LEAKS MEMORY: %u BLOCKS (%u BYTES) LOST", + getNumAllocatedBlocks(), getNumAllocatedBlocks() * uavcan::MemPoolBlockSize); + } + } +}; + +} diff --git a/src/modules/uavcan/uavcan_main.cpp b/src/modules/uavcan/uavcan_main.cpp index 6508e339f0..affaa36b0a 100644 --- a/src/modules/uavcan/uavcan_main.cpp +++ b/src/modules/uavcan/uavcan_main.cpp @@ -77,7 +77,6 @@ UavcanNode *UavcanNode::_instance; UavcanNode::UavcanNode(uavcan::ICanDriver &can_driver, uavcan::ISystemClock &system_clock) : CDev("uavcan", UAVCAN_DEVICE_PATH), - _pool_allocator(MemoryPoolBlockCapacitySoftLimit, MemoryPoolBlockCapacityHardLimit), _node(can_driver, system_clock, _pool_allocator), _node_mutex(), _esc_controller(_node), diff --git a/src/modules/uavcan/uavcan_main.hpp b/src/modules/uavcan/uavcan_main.hpp index 00cdaf418f..b91765591d 100644 --- a/src/modules/uavcan/uavcan_main.hpp +++ b/src/modules/uavcan/uavcan_main.hpp @@ -56,6 +56,7 @@ #include "sensors/sensor_bridge.hpp" #include "uavcan_servers.hpp" +#include "allocator.hpp" /** * @file uavcan_main.hpp @@ -96,9 +97,6 @@ class UavcanNode : public device::CDev static constexpr unsigned RxQueueLenPerIface = FramePerMSecond * PollTimeoutMs; // At static constexpr unsigned StackSize = 1800; - static constexpr unsigned MemoryPoolBlockCapacitySoftLimit = 250; - static constexpr unsigned MemoryPoolBlockCapacityHardLimit = 500; - public: typedef uavcan_stm32::CanInitHelper CanInitHelper; enum eServerAction {None, Start, Stop, CheckFW , Busy}; @@ -171,13 +169,7 @@ private: static UavcanNode *_instance; ///< singleton pointer - struct MemoryPoolSynchronizer - { - const ::irqstate_t state = ::irqsave(); - ~MemoryPoolSynchronizer() { ::irqrestore(state); } - }; - - uavcan::HeapBasedPoolAllocator _pool_allocator; + uavcan_node::Allocator _pool_allocator; uavcan::Node<> _node; ///< library instance pthread_mutex_t _node_mutex; From 109bee855b3d734b63fe88bd3eec5fdc8d443b58 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Sat, 17 Oct 2015 03:49:02 +0300 Subject: [PATCH 09/17] Node on leaked memory in UAVCAN driver --- src/modules/uavcan/uavcan_main.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/src/modules/uavcan/uavcan_main.cpp b/src/modules/uavcan/uavcan_main.cpp index affaa36b0a..ed5814a3f4 100644 --- a/src/modules/uavcan/uavcan_main.cpp +++ b/src/modules/uavcan/uavcan_main.cpp @@ -1069,6 +1069,7 @@ UavcanNode::ioctl(file *filp, int cmd, unsigned long arg) unsigned buflen = strnlen(buf, 1024); if (_mixers == nullptr) { + // TODO: Do we have to delete it when stopping? _mixers = new MixerGroup(control_callback, (uintptr_t)_controls); } From 6c4f09c0e4fe3a243bb68c9bcac03110b1c2a4d1 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Sat, 17 Oct 2015 04:05:35 +0300 Subject: [PATCH 10/17] Fixed memory leak in UAVCAN baro driver --- src/modules/uavcan/sensors/baro.cpp | 15 ++++----------- src/modules/uavcan/sensors/baro.hpp | 4 +++- 2 files changed, 7 insertions(+), 12 deletions(-) diff --git a/src/modules/uavcan/sensors/baro.cpp b/src/modules/uavcan/sensors/baro.cpp index 694a3988a1..36f28a423c 100644 --- a/src/modules/uavcan/sensors/baro.cpp +++ b/src/modules/uavcan/sensors/baro.cpp @@ -44,7 +44,7 @@ UavcanBarometerBridge::UavcanBarometerBridge(uavcan::INode &node) : UavcanCDevSensorBridgeBase("uavcan_baro", "/dev/uavcan/baro", BARO_BASE_DEVICE_PATH, ORB_ID(sensor_baro)), _sub_air_pressure_data(node), _sub_air_temperature_data(node), - _reports(nullptr) + _reports(2, sizeof(baro_report)) { } int UavcanBarometerBridge::init() @@ -55,13 +55,6 @@ int UavcanBarometerBridge::init() return res; } - /* allocate basic report buffers */ - _reports = new ringbuffer::RingBuffer(2, sizeof(baro_report)); - - if (_reports == nullptr) { - return -1; - } - res = _sub_air_pressure_data.start(AirPressureCbBinder(this, &UavcanBarometerBridge::air_pressure_sub_cb)); if (res < 0) { @@ -91,7 +84,7 @@ ssize_t UavcanBarometerBridge::read(struct file *filp, char *buffer, size_t bufl } while (count--) { - if (_reports->get(baro_buf)) { + if (_reports.get(baro_buf)) { ret += sizeof(*baro_buf); baro_buf++; } @@ -132,7 +125,7 @@ int UavcanBarometerBridge::ioctl(struct file *filp, int cmd, unsigned long arg) irqstate_t flags = irqsave(); - if (!_reports->resize(arg)) { + if (!_reports.resize(arg)) { irqrestore(flags); return -ENOMEM; } @@ -186,7 +179,7 @@ void UavcanBarometerBridge::air_pressure_sub_cb(const report.altitude = (((std::pow((p / p1), (-(a * R) / g))) * T1) - T1) / a; // add to the ring buffer - _reports->force(&report); + _reports.force(&report); publish(msg.getSrcNodeID().get(), &report); } diff --git a/src/modules/uavcan/sensors/baro.hpp b/src/modules/uavcan/sensors/baro.hpp index 38cf6ab89a..ed1e308515 100644 --- a/src/modules/uavcan/sensors/baro.hpp +++ b/src/modules/uavcan/sensors/baro.hpp @@ -76,8 +76,10 @@ private: uavcan::Subscriber _sub_air_pressure_data; uavcan::Subscriber _sub_air_temperature_data; + + ringbuffer::RingBuffer _reports; + unsigned _msl_pressure = 101325; - ringbuffer::RingBuffer *_reports; float last_temperature_kelvin = 0.0f; }; From 9d86dbb6a1c3625f912ffc9470c0184c274d0da2 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Sat, 17 Oct 2015 19:50:01 +0300 Subject: [PATCH 11/17] Fixed memory leaks in the primary UAVCAN thread --- src/modules/uavcan/uavcan_main.cpp | 14 +++++++++----- 1 file changed, 9 insertions(+), 5 deletions(-) diff --git a/src/modules/uavcan/uavcan_main.cpp b/src/modules/uavcan/uavcan_main.cpp index ed5814a3f4..3d7580b534 100644 --- a/src/modules/uavcan/uavcan_main.cpp +++ b/src/modules/uavcan/uavcan_main.cpp @@ -144,10 +144,9 @@ UavcanNode::~UavcanNode() } while (_task != -1); } - /* clean up the alternate device node */ - // unregister_driver(PWM_OUTPUT_DEVICE_PATH); - - ::close(_armed_sub); + (void)::close(_armed_sub); + (void)::close(_test_motor_sub); + (void)::close(_actuator_direct_sub); // Removing the sensor bridges auto br = _sensor_bridges.getHead(); @@ -166,6 +165,10 @@ UavcanNode::~UavcanNode() pthread_mutex_destroy(&_node_mutex); px4_sem_destroy(&_server_command_sem); + // Is it allowed to delete it like that? + if (_mixers != nullptr) { + delete _mixers; + } } int UavcanNode::getHardwareVersion(uavcan::protocol::HardwareVersion &hwver) @@ -965,6 +968,8 @@ int UavcanNode::run() } } + (void)::close(busevent_fd); + teardown(); warnx("exiting."); @@ -1069,7 +1074,6 @@ UavcanNode::ioctl(file *filp, int cmd, unsigned long arg) unsigned buflen = strnlen(buf, 1024); if (_mixers == nullptr) { - // TODO: Do we have to delete it when stopping? _mixers = new MixerGroup(control_callback, (uintptr_t)_controls); } From 5b9fc2d9fa16a7ac9f6ddfb4f0a29d8ef4b3ebfb Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Sat, 17 Oct 2015 20:29:49 +0300 Subject: [PATCH 12/17] Proper termination of UAVACN server thread --- src/modules/uavcan/uavcan_servers.cpp | 17 ++++++++++------- src/modules/uavcan/uavcan_servers.hpp | 1 + 2 files changed, 11 insertions(+), 7 deletions(-) diff --git a/src/modules/uavcan/uavcan_servers.cpp b/src/modules/uavcan/uavcan_servers.cpp index 8ae013ce48..ca5bcede66 100644 --- a/src/modules/uavcan/uavcan_servers.cpp +++ b/src/modules/uavcan/uavcan_servers.cpp @@ -141,13 +141,14 @@ int UavcanServers::stop() return -1; } - _instance = nullptr; - - if (server->_subnode_thread != -1) { - pthread_cancel(server->_subnode_thread); - pthread_join(server->_subnode_thread, NULL); + if (server->_subnode_thread) { + warnx("stopping fw srv thread..."); + server->_subnode_thread_should_exit = true; + (void)pthread_join(server->_subnode_thread, NULL); } + _instance = nullptr; + server->_main_node.getDispatcher().removeRxFrameListener(); delete server; @@ -334,7 +335,7 @@ pthread_addr_t UavcanServers::run(pthread_addr_t) memset(_esc_enumeration_ids, 0, sizeof(_esc_enumeration_ids)); _esc_enumeration_index = 0; - while (1) { + while (!_subnode_thread_should_exit) { if (_check_fw == true) { _check_fw = false; @@ -554,7 +555,9 @@ pthread_addr_t UavcanServers::run(pthread_addr_t) } } - warnx("exiting."); + _subnode_thread_should_exit = false; + + warnx("exiting"); return (pthread_addr_t) 0; } diff --git a/src/modules/uavcan/uavcan_servers.hpp b/src/modules/uavcan/uavcan_servers.hpp index 9bf9705771..3cf4cbb4da 100644 --- a/src/modules/uavcan/uavcan_servers.hpp +++ b/src/modules/uavcan/uavcan_servers.hpp @@ -110,6 +110,7 @@ public: private: pthread_t _subnode_thread; pthread_mutex_t _subnode_mutex; + volatile bool _subnode_thread_should_exit = false; int init(); From 53ed0425daee0acaea4179af848f9763bfb0b11e Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Sat, 17 Oct 2015 20:49:08 +0300 Subject: [PATCH 13/17] Libuavcan set to master --- src/modules/uavcan/libuavcan | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/uavcan/libuavcan b/src/modules/uavcan/libuavcan index 9b092509c9..ed1d71e639 160000 --- a/src/modules/uavcan/libuavcan +++ b/src/modules/uavcan/libuavcan @@ -1 +1 @@ -Subproject commit 9b092509c93794ec60484b2864be7a2995aefac6 +Subproject commit ed1d71e639543ea5743a839c313c53237ab3a27d From 72e651bedac613c1b3e96b47c7d4feffc8aef10d Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Sat, 17 Oct 2015 21:08:03 +0300 Subject: [PATCH 14/17] UAVCAN virtual iface: removed a useless class field --- src/modules/uavcan/uavcan_virtual_can_driver.hpp | 9 ++++----- 1 file changed, 4 insertions(+), 5 deletions(-) diff --git a/src/modules/uavcan/uavcan_virtual_can_driver.hpp b/src/modules/uavcan/uavcan_virtual_can_driver.hpp index 57b664068c..956caf11ee 100644 --- a/src/modules/uavcan/uavcan_virtual_can_driver.hpp +++ b/src/modules/uavcan/uavcan_virtual_can_driver.hpp @@ -404,9 +404,8 @@ class VirtualCanDriver : public uavcan::ICanDriver, } }; - Event event_; ///< Used to unblock the select() call when IO happens. + Event event_; ///< Used to unblock the select() call when IO happens. pthread_mutex_t driver_mutex_; ///< Shared across all ifaces - uavcan::IPoolAllocator& allocator_; ///< Shared across all ifaces uavcan::LazyConstructor ifaces_[uavcan::MaxCanIfaces]; const unsigned num_ifaces_; uavcan::ISystemClock &clock_; @@ -484,7 +483,6 @@ public: uavcan::ISystemClock &system_clock, uavcan::IPoolAllocator& allocator, unsigned virtual_iface_block_allocation_quota) : - allocator_(allocator), num_ifaces_(arg_num_ifaces), clock_(system_clock) { @@ -496,8 +494,9 @@ public: const unsigned quota_per_queue = virtual_iface_block_allocation_quota; // 2x overcommit for (unsigned i = 0; i < num_ifaces_; i++) { - ifaces_[i].template construct(allocator_, clock_, driver_mutex_, quota_per_queue); + ifaces_[i].template + construct + (allocator, clock_, driver_mutex_, quota_per_queue); } } From 635bfb6a7b4df37add20af8f5582b23ed992e03d Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Sun, 18 Oct 2015 03:55:47 +0300 Subject: [PATCH 15/17] CAN driver RX queue reduced to 21 frames per interface; poll() timeout set to 3 ms --- src/modules/uavcan/uavcan_main.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/uavcan/uavcan_main.hpp b/src/modules/uavcan/uavcan_main.hpp index b91765591d..b236946e24 100644 --- a/src/modules/uavcan/uavcan_main.hpp +++ b/src/modules/uavcan/uavcan_main.hpp @@ -80,7 +80,7 @@ class UavcanNode : public device::CDev static constexpr unsigned FramePerSecond = MaxBitRatePerSec / bitPerFrame; static constexpr unsigned FramePerMSecond = ((FramePerSecond / 1000) + 1); - static constexpr unsigned PollTimeoutMs = 10; + static constexpr unsigned PollTimeoutMs = 3; /* * This memory is reserved for uavcan to use for queuing CAN frames. From e06c46da03d75dd01423485f91c0502724f1aa30 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Sun, 18 Oct 2015 03:56:09 +0300 Subject: [PATCH 16/17] uavcan status output extended with CAN error reporting --- src/modules/uavcan/uavcan_main.cpp | 7 +++++++ 1 file changed, 7 insertions(+) diff --git a/src/modules/uavcan/uavcan_main.cpp b/src/modules/uavcan/uavcan_main.cpp index 3d7580b534..4b77c91648 100644 --- a/src/modules/uavcan/uavcan_main.cpp +++ b/src/modules/uavcan/uavcan_main.cpp @@ -1131,6 +1131,13 @@ UavcanNode::print_info() printf("\tReserved: %u blocks\n", _pool_allocator.getNumReservedBlocks()); printf("\tAllocated: %u blocks\n", _pool_allocator.getNumAllocatedBlocks()); + // CAN driver status + printf("CAN status:\n"); + for (unsigned i = 0; i < _node.getDispatcher().getCanIOManager().getCanDriver().getNumIfaces(); i++) { + auto iface = _node.getDispatcher().getCanIOManager().getCanDriver().getIface(i); + printf("\tCAN%u: errors: %llu\n", unsigned(i + 1), iface->getErrorCount()); + } + // ESC mixer status printf("ESC actuators control groups: sub: %u / req: %u / fds: %u\n", (unsigned)_groups_subscribed, (unsigned)_groups_required, _poll_fds_num); From 96a12a60278f9c2aae7a46d0cafa7b2cf3fad457 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Sun, 18 Oct 2015 04:13:25 +0300 Subject: [PATCH 17/17] UAVCAN extended status reporting --- src/modules/uavcan/uavcan_main.cpp | 17 +++++++++++++++-- 1 file changed, 15 insertions(+), 2 deletions(-) diff --git a/src/modules/uavcan/uavcan_main.cpp b/src/modules/uavcan/uavcan_main.cpp index 4b77c91648..00671decd2 100644 --- a/src/modules/uavcan/uavcan_main.cpp +++ b/src/modules/uavcan/uavcan_main.cpp @@ -1131,11 +1131,24 @@ UavcanNode::print_info() printf("\tReserved: %u blocks\n", _pool_allocator.getNumReservedBlocks()); printf("\tAllocated: %u blocks\n", _pool_allocator.getNumAllocatedBlocks()); + // UAVCAN node perfcounters + printf("UAVCAN node status:\n"); + printf("\tInternal failures: %llu\n", _node.getInternalFailureCount()); + printf("\tTransfer errors: %llu\n", _node.getDispatcher().getTransferPerfCounter().getErrorCount()); + printf("\tRX transfers: %llu\n", _node.getDispatcher().getTransferPerfCounter().getRxTransferCount()); + printf("\tTX transfers: %llu\n", _node.getDispatcher().getTransferPerfCounter().getTxTransferCount()); + // CAN driver status - printf("CAN status:\n"); for (unsigned i = 0; i < _node.getDispatcher().getCanIOManager().getCanDriver().getNumIfaces(); i++) { + printf("CAN%u status:\n", unsigned(i + 1)); + auto iface = _node.getDispatcher().getCanIOManager().getCanDriver().getIface(i); - printf("\tCAN%u: errors: %llu\n", unsigned(i + 1), iface->getErrorCount()); + printf("\tHW errors: %llu\n", iface->getErrorCount()); + + auto iface_perf_cnt = _node.getDispatcher().getCanIOManager().getIfacePerfCounters(i); + printf("\tIO errors: %llu\n", iface_perf_cnt.errors); + printf("\tRX frames: %llu\n", iface_perf_cnt.frames_rx); + printf("\tTX frames: %llu\n", iface_perf_cnt.frames_tx); } // ESC mixer status