List container improvements and testing

- support range based for loops
 - add remove() method to remove a node
 - add clear() to empty entire list and delete nodes
 - add empty() helper

platforms/posix/cmake/sitl_tests.cmake

@@ -17,6 +17,7 @@ set(tests
 	hrt
 	hysteresis
 	int
+	List
 	mathlib
 	matrix
 	microbench_hrt

src/drivers/uavcan/uavcan_main.cpp

@@ -148,13 +148,7 @@ UavcanNode::~UavcanNode()
 	(void)orb_unsubscribe(_actuator_direct_sub);
 
 	// Removing the sensor bridges
-	auto br = _sensor_bridges.getHead();
+	_sensor_bridges.clear();
-
-	while (br != nullptr) {
-		auto next = br->getSibling();
-		delete br;
-		br = next;
-	}
 
 	_instance = nullptr;
 
@@ -651,9 +645,8 @@ int UavcanNode::init(uavcan::NodeID node_id)
 
 	// Sensor bridges
 	IUavcanSensorBridge::make_all(_node, _sensor_bridges);
-	auto br = _sensor_bridges.getHead();
 
-	while (br != nullptr) {
+	for (const auto &br : _sensor_bridges) {
 		ret = br->init();
 
 		if (ret < 0) {
@@ -662,12 +655,9 @@ int UavcanNode::init(uavcan::NodeID node_id)
 		}
 
 		PX4_INFO("sensor bridge '%s' init ok", br->get_name());
-		br = br->getSibling();
 	}
 
-
 	/*  Start the Node   */
-
 	return _node.start();
 }
 
@@ -1228,13 +1218,10 @@ UavcanNode::print_info()
 	}
 
 	// Sensor bridges
-	auto br = _sensor_bridges.getHead();
+	for (const auto &br : _sensor_bridges) {
-
-	while (br != nullptr) {
 		printf("Sensor '%s':\n", br->get_name());
 		br->print_status();
 		printf("\n");
-		br = br->getSibling();
 	}
 
 	// Printing all nodes that are online

src/include/containers/List.hpp

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
+ *   Copyright (C) 2012-2019 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
@@ -34,11 +34,13 @@
 /**
  * @file List.hpp
  *
- * A linked list.
+ * An intrusive linked list.
  */
 
 #pragma once
 
+#include <stdlib.h>
+
 template<class T>
 class ListNode
 {
@@ -64,8 +66,88 @@ public:
 		_head = newNode;
 	}
 
+	bool remove(T removeNode)
+	{
+		// base case
+		if (removeNode == _head) {
+			_head = nullptr;
+			return true;
+		}
+
+		for (T node = getHead(); node != nullptr; node = node->getSibling()) {
+			// is sibling the node to remove?
+			if (node->getSibling() == removeNode) {
+				// replace sibling
+				if (node->getSibling() != nullptr) {
+					node->setSibling(node->getSibling()->getSibling());
+
+				} else {
+					node->setSibling(nullptr);
+				}
+
+				return true;
+			}
+		}
+
+		return false;
+	}
+
+	struct Iterator {
+		T node;
+		Iterator(T v) : node(v) {}
+
+		operator T() const { return node; }
+		operator T &() { return node; }
+		T operator* () const { return node; }
+		Iterator &operator++ ()
+		{
+			if (node) {
+				node = node->getSibling();
+			};
+
+			return *this;
+		}
+	};
+
+	Iterator begin() { return Iterator(getHead()); }
+	Iterator end() { return Iterator(nullptr); }
+
 	const T getHead() const { return _head; }
 
+	bool empty() const { return getHead() == nullptr; }
+
+	size_t size() const
+	{
+		size_t sz = 0;
+
+		for (auto node = getHead(); node != nullptr; node = node->getSibling()) {
+			sz++;
+		}
+
+		return sz;
+	}
+
+	void deleteNode(T node)
+	{
+		if (remove(node)) {
+			// only delete if node was successfully removed
+			delete node;
+		}
+	}
+
+	void clear()
+	{
+		auto node = getHead();
+
+		while (node != nullptr) {
+			auto next = node->getSibling();
+			delete node;
+			node = next;
+		}
+
+		_head = nullptr;
+	}
+
 protected:
 
 	T _head{nullptr};

src/modules/mavlink/mavlink_main.cpp

@@ -218,8 +218,6 @@ Mavlink::Mavlink() :
 	_wait_to_transmit(false),
 	_received_messages(false),
 	_main_loop_delay(1000),
-	_subscriptions(nullptr),
-	_streams(nullptr),
 	_mavlink_shell(nullptr),
 	_mavlink_ulog(nullptr),
 	_mavlink_ulog_stop_requested(false),
@@ -1349,9 +1347,7 @@ MavlinkOrbSubscription *Mavlink::add_orb_subscription(const orb_id_t topic, int
 {
 	if (!disable_sharing) {
 		/* check if already subscribed to this topic */
-		MavlinkOrbSubscription *sub;
+		for (MavlinkOrbSubscription *sub : _subscriptions) {
-
-		LL_FOREACH(_subscriptions, sub) {
 			if (sub->get_topic() == topic && sub->get_instance() == instance) {
 				/* already subscribed */
 				return sub;
@@ -1362,7 +1358,7 @@ MavlinkOrbSubscription *Mavlink::add_orb_subscription(const orb_id_t topic, int
 	/* add new subscription */
 	MavlinkOrbSubscription *sub_new = new MavlinkOrbSubscription(topic, instance);
 
-	LL_APPEND(_subscriptions, sub_new);
+	_subscriptions.add(sub_new);
 
 	return sub_new;
 }
@@ -1382,9 +1378,7 @@ Mavlink::configure_stream(const char *stream_name, const float rate)
 		interval = -1;
 	}
 
-	/* search if stream exists */
+	for (const auto &stream : _streams) {
-	MavlinkStream *stream;
-	LL_FOREACH(_streams, stream) {
 		if (strcmp(stream_name, stream->get_name()) == 0) {
 			if (interval != 0) {
 				/* set new interval */
@@ -1392,8 +1386,8 @@ Mavlink::configure_stream(const char *stream_name, const float rate)
 
 			} else {
 				/* delete stream */
-				LL_DELETE(_streams, stream);
+				_streams.deleteNode(stream);
-				delete stream;
+				return OK; // must finish with loop after node is deleted
 			}
 
 			return OK;
@@ -1407,11 +1401,11 @@ Mavlink::configure_stream(const char *stream_name, const float rate)
 
 	// search for stream with specified name in supported streams list
 	// create new instance if found
-	stream = create_mavlink_stream(stream_name, this);
+	MavlinkStream *stream = create_mavlink_stream(stream_name, this);
 
 	if (stream != nullptr) {
 		stream->set_interval(interval);
-		LL_APPEND(_streams, stream);
+		_streams.add(stream);
 
 		return OK;
 	}
@@ -1455,7 +1449,6 @@ Mavlink::configure_stream_threadsafe(const char *stream_name, const float rate)
 int
 Mavlink::message_buffer_init(int size)
 {
-
 	_message_buffer.size = size;
 	_message_buffer.write_ptr = 0;
 	_message_buffer.read_ptr = 0;
@@ -1607,8 +1600,7 @@ Mavlink::update_rate_mult()
 	float rate = 0.0f;
 
 	/* scale down rates if their theoretical bandwidth is exceeding the link bandwidth */
-	MavlinkStream *stream;
+	for (const auto &stream : _streams) {
-	LL_FOREACH(_streams, stream) {
 		if (stream->const_rate()) {
 			const_rate += (stream->get_interval() > 0) ? stream->get_size_avg() * 1000000.0f / stream->get_interval() : 0;
 
@@ -2467,8 +2459,7 @@ Mavlink::task_main(int argc, char *argv[])
 		}
 
 		/* update streams */
-		MavlinkStream *stream;
+		for (const auto &stream : _streams) {
-		LL_FOREACH(_streams, stream) {
 			stream->update(t);
 
 			if (!_first_heartbeat_sent) {
@@ -2568,28 +2559,10 @@ Mavlink::task_main(int argc, char *argv[])
 	_subscribe_to_stream = nullptr;
 
 	/* delete streams */
-	MavlinkStream *stream_to_del = nullptr;
+	_streams.clear();
-	MavlinkStream *stream_next = _streams;
-
-	while (stream_next != nullptr) {
-		stream_to_del = stream_next;
-		stream_next = stream_to_del->next;
-		delete stream_to_del;
-	}
-
-	_streams = nullptr;
 
 	/* delete subscriptions */
-	MavlinkOrbSubscription *sub_to_del = nullptr;
+	_subscriptions.clear();
-	MavlinkOrbSubscription *sub_next = _subscriptions;
-
-	while (sub_next != nullptr) {
-		sub_to_del = sub_next;
-		sub_next = sub_to_del->next;
-		delete sub_to_del;
-	}
-
-	_subscriptions = nullptr;
 
 	if (_uart_fd >= 0 && !_is_usb_uart) {
 		/* close UART */
@@ -2628,15 +2601,7 @@ void Mavlink::publish_telemetry_status()
 	_tstatus.forwarding = get_forwarding_on();
 	_tstatus.mavlink_v2 = (_protocol_version == 2);
 
-	int num_streams = 0;
+	_tstatus.streams = _streams.size();
-
-	MavlinkStream *stream;
-	LL_FOREACH(_streams, stream) {
-		// count
-		num_streams++;
-	}
-
-	_tstatus.streams = num_streams;
 
 	_tstatus.timestamp = hrt_absolute_time();
 	int instance;
@@ -2873,8 +2838,8 @@ Mavlink::display_status_streams()
 	printf("\t%-20s%-16s %s\n", "Name", "Rate Config (current) [Hz]", "Message Size (if active) [B]");
 
 	const float rate_mult = _rate_mult;
-	MavlinkStream *stream;
+
-	LL_FOREACH(_streams, stream) {
+	for (const auto &stream : _streams) {
 		const int interval = stream->get_interval();
 		const unsigned size = stream->get_size();
 		char rate_str[20];

src/modules/mavlink/mavlink_main.h

@@ -59,6 +59,8 @@
 #include <net/if.h>
 #endif
 
+#include <containers/List.hpp>
+#include <systemlib/uthash/utlist.h>
 #include <parameters/param.h>
 #include <perf/perf_counter.h>
 #include <pthread.h>
@@ -392,7 +394,7 @@ public:
 	 */
 	void			send_protocol_version();
 
-	MavlinkStream 		*get_streams() const { return _streams; }
+	List<MavlinkStream *> &get_streams() { return _streams; }
 
 	float			get_rate_mult() const { return _rate_mult; }
 
@@ -546,8 +548,8 @@ private:
 
 	unsigned		_main_loop_delay;	/**< mainloop delay, depends on data rate */
 
-	MavlinkOrbSubscription	*_subscriptions;
+	List<MavlinkOrbSubscription *>	_subscriptions;
-	MavlinkStream		*_streams;
+	List<MavlinkStream *>		_streams;
 
 	MavlinkShell			*_mavlink_shell;
 	MavlinkULog			*_mavlink_ulog;

src/modules/mavlink/mavlink_orb_subscription.h

@@ -41,14 +41,13 @@
 #ifndef MAVLINK_ORB_SUBSCRIPTION_H_
 #define MAVLINK_ORB_SUBSCRIPTION_H_
 
-#include <systemlib/uthash/utlist.h>
 #include <drivers/drv_hrt.h>
+#include <containers/List.hpp>
 #include "uORB/uORB.h"	// orb_id_t
 
-class MavlinkOrbSubscription
+class MavlinkOrbSubscription : public ListNode<MavlinkOrbSubscription *>
 {
 public:
-	MavlinkOrbSubscription *next{nullptr};	///< pointer to next subscription in list
 
 	MavlinkOrbSubscription(const orb_id_t topic, int instance);
 	~MavlinkOrbSubscription();

src/modules/mavlink/mavlink_receiver.cpp

@@ -1942,8 +1942,7 @@ MavlinkReceiver::get_message_interval(int msgId)
 {
 	unsigned interval = 0;
 
-	MavlinkStream *stream = nullptr;
+	for (const auto &stream : _mavlink->get_streams()) {
-	LL_FOREACH(_mavlink->get_streams(), stream) {
 		if (stream->get_id() == msgId) {
 			interval = stream->get_interval();
 			break;

src/modules/mavlink/mavlink_stream.cpp

@@ -45,7 +45,6 @@
 #include "mavlink_main.h"
 
 MavlinkStream::MavlinkStream(Mavlink *mavlink) :
-	ModuleParams(nullptr),
 	_mavlink(mavlink)
 {
 	_last_sent = hrt_absolute_time();

src/modules/mavlink/mavlink_stream.h

@@ -43,14 +43,14 @@
 
 #include <drivers/drv_hrt.h>
 #include <px4_module_params.h>
+#include <containers/List.hpp>
 
 class Mavlink;
 
-class MavlinkStream : public ModuleParams
+class MavlinkStream : public ListNode<MavlinkStream *>
 {
 
 public:
-	MavlinkStream *next{nullptr};
 
 	MavlinkStream(Mavlink *mavlink);
 	virtual ~MavlinkStream() = default;

src/modules/uORB/uORBDeviceMaster.cpp

@@ -163,7 +163,7 @@ void uORB::DeviceMaster::printStatistics(bool reset)
 
 	lock();
 
-	for (DeviceNode *node = _node_list.getHead(); node != nullptr; node = node->getSibling()) {
+	for (const auto &node : _node_list) {
 		if (node->print_statistics(reset)) {
 			had_print = true;
 		}
@@ -189,7 +189,7 @@ void uORB::DeviceMaster::addNewDeviceNodes(DeviceNodeStatisticsData **first_node
 		}
 	}
 
-	for (DeviceNode *node = _node_list.getHead(); node != nullptr; node = node->getSibling()) {
+	for (const auto &node : _node_list) {
 
 		++num_topics;
 
@@ -262,7 +262,7 @@ void uORB::DeviceMaster::showTop(char **topic_filter, int num_filters)
 
 	lock();
 
-	if (_node_list.getHead() == nullptr) {
+	if (_node_list.empty()) {
 		unlock();
 		PX4_INFO("no active topics");
 		return;
@@ -380,7 +380,7 @@ uORB::DeviceNode *uORB::DeviceMaster::getDeviceNode(const char *nodepath)
 {
 	lock();
 
-	for (DeviceNode *node = _node_list.getHead(); node != nullptr; node = node->getSibling()) {
+	for (uORB::DeviceNode *node : _node_list) {
 		if (strcmp(node->get_devname(), nodepath) == 0) {
 			unlock();
 			return node;
@@ -405,7 +405,7 @@ uORB::DeviceNode *uORB::DeviceMaster::getDeviceNode(const struct orb_metadata *m
 
 uORB::DeviceNode *uORB::DeviceMaster::getDeviceNodeLocked(const struct orb_metadata *meta, const uint8_t instance)
 {
-	for (DeviceNode *node = _node_list.getHead(); node != nullptr; node = node->getSibling()) {
+	for (uORB::DeviceNode *node : _node_list) {
 		if ((strcmp(node->get_name(), meta->o_name) == 0) && (node->get_instance() == instance)) {
 			return node;
 		}

src/platforms/px4_module_params.h

@@ -78,11 +78,8 @@ protected:
 	 */
 	virtual void updateParams()
 	{
-		ModuleParams *child = _children.getHead();
+		for (const auto &child : _children) {
-
-		while (child) {
 			child->updateParams();
-			child = child->getSibling();
 		}
 
 		updateParamsImpl();

src/systemcmds/tests/CMakeLists.txt

@@ -48,6 +48,7 @@ set(srcs
 	test_int.cpp
 	test_jig_voltages.c
 	test_led.c
+	test_List.cpp
 	test_mathlib.cpp
 	test_matrix.cpp
 	test_microbench_hrt.cpp

src/systemcmds/tests/test_List.cpp

@@ -0,0 +1,214 @@
+/****************************************************************************
+ *
+ *  Copyright (C) 2019 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.
+ *
+ ****************************************************************************/
+
+/**
+ * @file test_List.cpp
+ * Tests the List container.
+ */
+
+#include <unit_test.h>
+#include <containers/List.hpp>
+#include <float.h>
+#include <math.h>
+
+class testContainer : public ListNode<testContainer *>
+{
+public:
+	int i{0};
+};
+
+class ListTest : public UnitTest
+{
+public:
+	virtual bool run_tests();
+
+	bool test_add();
+	bool test_remove();
+	bool test_range_based_for();
+
+};
+
+bool ListTest::run_tests()
+{
+	ut_run_test(test_add);
+	ut_run_test(test_remove);
+	ut_run_test(test_range_based_for);
+
+	return (_tests_failed == 0);
+}
+
+bool ListTest::test_add()
+{
+	List<testContainer *> list1;
+
+	// size should be 0 initially
+	ut_compare("size initially 0", list1.size(), 0);
+	ut_assert_true(list1.empty());
+
+	// insert 100
+	for (int i = 0; i < 100; i++) {
+		testContainer *t = new testContainer();
+		t->i = i;
+		list1.add(t);
+
+		ut_compare("size increasing with i", list1.size(), i + 1);
+		ut_assert_true(!list1.empty());
+	}
+
+	// verify full size (100)
+	ut_assert_true(list1.size() == 100);
+
+	int i = 99;
+
+	for (auto t : list1) {
+		// verify all elements were inserted in order
+		ut_compare("stored i", i, t->i);
+		i--;
+	}
+
+	// delete all elements
+	list1.clear();
+
+	// verify list has been cleared
+	ut_assert_true(list1.empty());
+	ut_compare("size 0", list1.size(), 0);
+
+	return true;
+}
+
+bool ListTest::test_remove()
+{
+	List<testContainer *> list1;
+
+	// size should be 0 initially
+	ut_compare("size initially 0", list1.size(), 0);
+	ut_assert_true(list1.empty());
+
+	// insert 100
+	for (int i = 0; i < 100; i++) {
+		testContainer *t = new testContainer();
+		t->i = i;
+		list1.add(t);
+
+		ut_compare("size increasing with i", list1.size(), i + 1);
+		ut_assert_true(!list1.empty());
+	}
+
+	// verify full size (100)
+	ut_assert_true(list1.size() == 100);
+
+	// test removing elements
+	for (int remove_i = 0; remove_i < 100; remove_i++) {
+
+		// find node with i == remove_i
+		for (auto t : list1) {
+			if (t->i == remove_i) {
+				ut_assert_true(list1.remove(t));
+			}
+		}
+
+		// iterate list again to verify removal
+		for (auto t : list1) {
+			ut_assert_true(t->i != remove_i);
+		}
+	}
+
+	// list should now be empty
+	ut_assert_true(list1.empty());
+	ut_compare("size 0", list1.size(), 0);
+
+	// delete all elements (should be safe on empty list)
+	list1.clear();
+
+	// verify list has been cleared
+	ut_assert_true(list1.empty());
+	ut_compare("size 0", list1.size(), 0);
+
+	return true;
+}
+
+bool ListTest::test_range_based_for()
+{
+	List<testContainer *> list1;
+
+	// size should be 0 initially
+	ut_compare("size initially 0", list1.size(), 0);
+	ut_assert_true(list1.empty());
+
+	// insert 100 elements in order
+	for (int i = 99; i >= 0; i--) {
+		testContainer *t = new testContainer();
+		t->i = i;
+
+		list1.add(t);
+
+		ut_assert_true(!list1.empty());
+	}
+
+	// first element should be 0
+	ut_compare("first 0", list1.getHead()->i, 0);
+
+	// verify all elements were inserted in order
+	int i = 0;
+	auto t1 = list1.getHead();
+
+	while (t1 != nullptr) {
+		ut_compare("check count", i, t1->i);
+		t1 = t1->getSibling();
+		i++;
+	}
+
+	// verify full size (100)
+	ut_compare("size check", list1.size(), 100);
+
+	i = 0;
+
+	for (auto t2 : list1) {
+		ut_compare("range based for i", i, t2->i);
+		i++;
+	}
+
+	// verify full size (100)
+	ut_compare("size check", list1.size(), 100);
+
+	// delete all elements
+	list1.clear();
+
+	// verify list has been cleared
+	ut_assert_true(list1.empty());
+	ut_compare("size check", list1.size(), 0);
+
+	return true;
+}
+
+ut_declare_test_c(test_List, ListTest)

src/systemcmds/tests/tests_main.c

@@ -94,9 +94,9 @@ const struct {
 	{"hott_telemetry",	test_hott_telemetry,	OPT_NOJIGTEST | OPT_NOALLTEST},
 	{"hrt",			test_hrt,		OPT_NOJIGTEST | OPT_NOALLTEST},
 	{"hysteresis",		test_hysteresis,	0},
-	{"hysteresis",		test_hysteresis,	0},
 	{"int",			test_int,		0},
 	{"jig_voltages",	test_jig_voltages,	OPT_NOALLTEST},
+	{"List",		test_List,		0},
 	{"mathlib",		test_mathlib,		0},
 	{"matrix",		test_matrix,		0},
 	{"microbench_hrt",	test_microbench_hrt,	0},

src/systemcmds/tests/tests_main.h

@@ -59,6 +59,7 @@ extern int test_hysteresis(int argc, char *argv[]);
 extern int test_int(int argc, char *argv[]);
 extern int test_jig_voltages(int argc, char *argv[]);
 extern int test_led(int argc, char *argv[]);
+extern int test_List(int argc, char *argv[]);
 extern int test_mathlib(int argc, char *argv[]);
 extern int test_matrix(int argc, char *argv[]);
 extern int test_microbench_hrt(int argc, char *argv[]);