diff --git a/platforms/posix/cmake/sitl_tests.cmake b/platforms/posix/cmake/sitl_tests.cmake index 0b5f23ca6b..1b329e6e05 100644 --- a/platforms/posix/cmake/sitl_tests.cmake +++ b/platforms/posix/cmake/sitl_tests.cmake @@ -17,6 +17,7 @@ set(tests hrt hysteresis int + List mathlib matrix microbench_hrt diff --git a/src/drivers/uavcan/uavcan_main.cpp b/src/drivers/uavcan/uavcan_main.cpp index ce714d73f0..4d480ca50b 100644 --- a/src/drivers/uavcan/uavcan_main.cpp +++ b/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(); - - while (br != nullptr) { - auto next = br->getSibling(); - delete br; - br = next; - } + _sensor_bridges.clear(); _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(); - - while (br != nullptr) { + for (const auto &br : _sensor_bridges) { printf("Sensor '%s':\n", br->get_name()); br->print_status(); printf("\n"); - br = br->getSibling(); } // Printing all nodes that are online diff --git a/src/include/containers/List.hpp b/src/include/containers/List.hpp index 33af53276f..bfd875e491 100644 --- a/src/include/containers/List.hpp +++ b/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 + template 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}; diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 7a9adab7f7..35fc137ecc 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/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; - - LL_FOREACH(_subscriptions, sub) { + for (MavlinkOrbSubscription *sub : _subscriptions) { 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 */ - MavlinkStream *stream; - LL_FOREACH(_streams, stream) { + for (const auto &stream : _streams) { 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); - delete stream; + _streams.deleteNode(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; - LL_FOREACH(_streams, stream) { + for (const auto &stream : _streams) { 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; - LL_FOREACH(_streams, stream) { + for (const auto &stream : _streams) { 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; - 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; + _streams.clear(); /* delete subscriptions */ - MavlinkOrbSubscription *sub_to_del = nullptr; - 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; + _subscriptions.clear(); 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; - - MavlinkStream *stream; - LL_FOREACH(_streams, stream) { - // count - num_streams++; - } - - _tstatus.streams = num_streams; + _tstatus.streams = _streams.size(); _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]; diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h index 89215e1eca..600319c82b 100644 --- a/src/modules/mavlink/mavlink_main.h +++ b/src/modules/mavlink/mavlink_main.h @@ -59,6 +59,8 @@ #include #endif +#include +#include #include #include #include @@ -392,7 +394,7 @@ public: */ void send_protocol_version(); - MavlinkStream *get_streams() const { return _streams; } + List &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; - MavlinkStream *_streams; + List _subscriptions; + List _streams; MavlinkShell *_mavlink_shell; MavlinkULog *_mavlink_ulog; diff --git a/src/modules/mavlink/mavlink_orb_subscription.h b/src/modules/mavlink/mavlink_orb_subscription.h index d527deb900..900d86ec3c 100644 --- a/src/modules/mavlink/mavlink_orb_subscription.h +++ b/src/modules/mavlink/mavlink_orb_subscription.h @@ -41,14 +41,13 @@ #ifndef MAVLINK_ORB_SUBSCRIPTION_H_ #define MAVLINK_ORB_SUBSCRIPTION_H_ -#include #include +#include #include "uORB/uORB.h" // orb_id_t -class MavlinkOrbSubscription +class MavlinkOrbSubscription : public ListNode { public: - MavlinkOrbSubscription *next{nullptr}; ///< pointer to next subscription in list MavlinkOrbSubscription(const orb_id_t topic, int instance); ~MavlinkOrbSubscription(); diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index cee0b0239a..4350641df9 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -1942,8 +1942,7 @@ MavlinkReceiver::get_message_interval(int msgId) { unsigned interval = 0; - MavlinkStream *stream = nullptr; - LL_FOREACH(_mavlink->get_streams(), stream) { + for (const auto &stream : _mavlink->get_streams()) { if (stream->get_id() == msgId) { interval = stream->get_interval(); break; diff --git a/src/modules/mavlink/mavlink_stream.cpp b/src/modules/mavlink/mavlink_stream.cpp index f95587c833..af7c9fb3fd 100644 --- a/src/modules/mavlink/mavlink_stream.cpp +++ b/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(); diff --git a/src/modules/mavlink/mavlink_stream.h b/src/modules/mavlink/mavlink_stream.h index 327911f633..8e9bcfffc9 100644 --- a/src/modules/mavlink/mavlink_stream.h +++ b/src/modules/mavlink/mavlink_stream.h @@ -43,14 +43,14 @@ #include #include +#include class Mavlink; -class MavlinkStream : public ModuleParams +class MavlinkStream : public ListNode { public: - MavlinkStream *next{nullptr}; MavlinkStream(Mavlink *mavlink); virtual ~MavlinkStream() = default; diff --git a/src/modules/uORB/uORBDeviceMaster.cpp b/src/modules/uORB/uORBDeviceMaster.cpp index 160aec9584..c761ee6131 100644 --- a/src/modules/uORB/uORBDeviceMaster.cpp +++ b/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; } diff --git a/src/platforms/px4_module_params.h b/src/platforms/px4_module_params.h index 50d2a08a90..48f3ce9cf7 100644 --- a/src/platforms/px4_module_params.h +++ b/src/platforms/px4_module_params.h @@ -78,11 +78,8 @@ protected: */ virtual void updateParams() { - ModuleParams *child = _children.getHead(); - - while (child) { + for (const auto &child : _children) { child->updateParams(); - child = child->getSibling(); } updateParamsImpl(); diff --git a/src/systemcmds/tests/CMakeLists.txt b/src/systemcmds/tests/CMakeLists.txt index 0010a5778c..d62920cb75 100644 --- a/src/systemcmds/tests/CMakeLists.txt +++ b/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 diff --git a/src/systemcmds/tests/test_List.cpp b/src/systemcmds/tests/test_List.cpp new file mode 100644 index 0000000000..5baabde5d9 --- /dev/null +++ b/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 +#include +#include +#include + +class testContainer : public ListNode +{ +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 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 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 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) diff --git a/src/systemcmds/tests/tests_main.c b/src/systemcmds/tests/tests_main.c index 84cd6878ff..b1dfd86c80 100644 --- a/src/systemcmds/tests/tests_main.c +++ b/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}, diff --git a/src/systemcmds/tests/tests_main.h b/src/systemcmds/tests/tests_main.h index c5d1928ed0..5d9d0fc3e8 100644 --- a/src/systemcmds/tests/tests_main.h +++ b/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[]);