Merge pull request #3005 from PX4/uavcan_footprint_reduction

UAVCAN footprint reduction

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)

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 <pavel.kirienko@gmail.com>
+ */
+
+#pragma once
+
+#include <systemlib/err.h>
+#include <uavcan/uavcan.hpp>
+#include <uavcan/helpers/heap_based_pool_allocator.hpp>
+
+// 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<uavcan::MemPoolBlockSize, AllocatorSynchronizer>
+{
+	static constexpr unsigned CapacitySoftLimit = 250;
+	static constexpr unsigned CapacityHardLimit = 500;
+
+	Allocator() :
+		uavcan::HeapBasedPoolAllocator<uavcan::MemPoolBlockSize, AllocatorSynchronizer>(CapacitySoftLimit, CapacityHardLimit)
+	{ }
+
+	~Allocator()
+	{
+	        if (getNumAllocatedBlocks() > 0)
+	        {
+	        	warnx("UAVCAN LEAKS MEMORY: %u BLOCKS (%u BYTES) LOST",
+	        		getNumAllocatedBlocks(), getNumAllocatedBlocks() * uavcan::MemPoolBlockSize);
+	        }
+	}
+};
+
+}

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);
 }

src/modules/uavcan/sensors/baro.hpp

@@ -76,8 +76,10 @@ private:
 
 	uavcan::Subscriber<uavcan::equipment::air_data::StaticPressure, AirPressureCbBinder> _sub_air_pressure_data;
 	uavcan::Subscriber<uavcan::equipment::air_data::StaticTemperature, AirTemperatureCbBinder> _sub_air_temperature_data;
+
+	ringbuffer::RingBuffer _reports;
+
 	unsigned _msl_pressure = 101325;
-	ringbuffer::RingBuffer	*_reports;
 	float last_temperature_kelvin = 0.0f;
 
 };

src/modules/uavcan/uavcan_main.cpp

@@ -77,7 +77,7 @@
 UavcanNode *UavcanNode::_instance;
 UavcanNode::UavcanNode(uavcan::ICanDriver &can_driver, uavcan::ISystemClock &system_clock) :
 	CDev("uavcan", UAVCAN_DEVICE_PATH),
-	_node(can_driver, system_clock),
+	_node(can_driver, system_clock, _pool_allocator),
 	_node_mutex(),
 	_esc_controller(_node),
 	_time_sync_master(_node),
@@ -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.");
 
@@ -1119,6 +1124,33 @@ 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());
+
+	// 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
+	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("\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
 	printf("ESC actuators control groups: sub: %u / req: %u / fds: %u\n",
 	       (unsigned)_groups_subscribed, (unsigned)_groups_required, _poll_fds_num);
@@ -1169,13 +1201,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[]);
@@ -1251,6 +1290,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);

src/modules/uavcan/uavcan_main.hpp

@@ -36,6 +36,7 @@
 #include <px4_config.h>
 
 #include <uavcan_stm32/uavcan_stm32.hpp>
+#include <uavcan/helpers/heap_based_pool_allocator.hpp>
 #include <uavcan/protocol/global_time_sync_master.hpp>
 #include <uavcan/protocol/global_time_sync_slave.hpp>
 #include <uavcan/protocol/param/GetSet.hpp>
@@ -55,6 +56,7 @@
 #include "sensors/sensor_bridge.hpp"
 
 #include "uavcan_servers.hpp"
+#include "allocator.hpp"
 
 /**
  * @file uavcan_main.hpp
@@ -78,9 +80,8 @@ 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;
 
-	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.
@@ -97,7 +98,6 @@ class UavcanNode : public device::CDev
 	static constexpr unsigned StackSize          = 1800;
 
 public:
-	typedef uavcan::Node<MemPoolSize> Node;
 	typedef uavcan_stm32::CanInitHelper<RxQueueLenPerIface> CanInitHelper;
 	enum eServerAction {None, Start, Stop, CheckFW , Busy};
 
@@ -109,7 +109,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);
@@ -121,6 +121,8 @@ public:
 
 	void		print_info();
 
+	void		shrink();
+
 	static UavcanNode *instance() { return _instance; }
 	static int         getHardwareVersion(uavcan::protocol::HardwareVersion &hwver);
 	int             fw_server(eServerAction action);
@@ -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,9 @@ private:
 
 	static UavcanNode	*_instance;			///< singleton pointer
 
-	Node			_node;				///< library instance
+	uavcan_node::Allocator _pool_allocator;
+
+	uavcan::Node<>		_node;				///< library instance
 	pthread_mutex_t		_node_mutex;
 	px4_sem_t                   _server_command_sem;
 	UavcanEscController	_esc_controller;

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(),
@@ -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;
 }
 

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<MemPoolSize> 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; }
 
@@ -124,6 +110,7 @@ public:
 private:
 	pthread_t         _subnode_thread;
 	pthread_mutex_t   _subnode_mutex;
+	volatile bool     _subnode_thread_should_exit = false;
 
 	int		init();
 
@@ -131,12 +118,10 @@ private:
 
 	static UavcanServers	*_instance;            ///< singleton pointer
 
-	typedef VirtualCanDriver<QueuePoolSize> vCanDriver;
+	VirtualCanDriver _vdriver;
 
-	vCanDriver    _vdriver;
-
-	uavcan::SubNode<MemPoolSize>  _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;

src/modules/uavcan/uavcan_virtual_can_driver.hpp

@@ -114,6 +114,14 @@ public:
 		uavcan::IsDynamicallyAllocatable<Item>::check();
 	}
 
+	~Queue()
+	{
+		while (!isEmpty())
+		{
+			pop();
+		}
+	}
+
 	bool isEmpty() const { return list_.isEmpty(); }
 
 	/**
@@ -329,11 +337,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 <unsigned SharedMemoryPoolSize>
 class VirtualCanDriver : public uavcan::ICanDriver,
 	public uavcan::IRxFrameListener,
 	public ITxQueueInjector,
@@ -400,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::PoolAllocator<SharedMemoryPoolSize, uavcan::MemPoolBlockSize> allocator_;   ///< Shared across all ifaces
 	uavcan::LazyConstructor<VirtualCanIface> ifaces_[uavcan::MaxCanIfaces];
 	const unsigned num_ifaces_;
 	uavcan::ISystemClock &clock_;
@@ -476,7 +479,10 @@ 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) :
 		num_ifaces_(arg_num_ifaces),
 		clock_(system_clock)
 	{
@@ -485,15 +491,12 @@ 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<uavcan::IPoolAllocator &, uavcan::ISystemClock &,
-				pthread_mutex_t &, unsigned>(allocator_, clock_, driver_mutex_, quota_per_queue);
+			ifaces_[i].template
+				construct<uavcan::IPoolAllocator&, uavcan::ISystemClock&, pthread_mutex_t&, unsigned>
+					(allocator, clock_, driver_mutex_, quota_per_queue);
 		}
 	}