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
This commit is contained in:
Daniel Agar 2019-02-22 13:34:23 -05:00
parent 6c3e79f361
commit e2bf4b1894
15 changed files with 335 additions and 88 deletions

View File

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

View File

@ -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

View File

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

View File

@ -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];

View File

@ -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;
MavlinkStream *_streams;
List<MavlinkOrbSubscription *> _subscriptions;
List<MavlinkStream *> _streams;
MavlinkShell *_mavlink_shell;
MavlinkULog *_mavlink_ulog;

View File

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

View File

@ -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;

View File

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

View File

@ -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;

View File

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

View File

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

View File

@ -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

View File

@ -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)

View File

@ -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},

View File

@ -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[]);