From a570d1de7d20f6d0817fd6210473a58db9c3aa13 Mon Sep 17 00:00:00 2001
From: Pavel Kirienko <pavel.kirienko@gmail.com>
Date: Fri, 16 Oct 2015 22:59:17 +0300
Subject: [PATCH] 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);