diff --git a/src/include/containers/List.hpp b/src/include/containers/List.hpp index 0aa07effde..69cb747e0e 100644 --- a/src/include/containers/List.hpp +++ b/src/include/containers/List.hpp @@ -62,8 +62,25 @@ public: void add(T newNode) { - newNode->setSibling(getHead()); - _head = newNode; + if (_head == nullptr) { + // list is empty, add as head + _head = newNode; + return; + + } else { + // find last node and add to end + T node = _head; + + while (node != nullptr) { + if (node->getSibling() == nullptr) { + // found last node, now add newNode + node->setSibling(newNode); + return; + } + + node = node->getSibling(); + } + } } bool remove(T removeNode) diff --git a/src/lib/mixer/Mixer.hpp b/src/lib/mixer/Mixer.hpp index c6a37699d4..f08e3d3eda 100644 --- a/src/lib/mixer/Mixer.hpp +++ b/src/lib/mixer/Mixer.hpp @@ -126,13 +126,14 @@ #pragma once +#include #include /** * Abstract class defining a mixer mixing zero or more inputs to * one or more outputs. */ -class Mixer +class Mixer : public ListNode { public: enum class Airmode : int32_t { @@ -141,9 +142,6 @@ public: roll_pitch_yaw = 2 }; - /** next mixer in a list */ - Mixer *_next{nullptr}; - /** * Fetch a control value. * diff --git a/src/lib/mixer/MixerGroup.cpp b/src/lib/mixer/MixerGroup.cpp index 7f69d44edb..52e9a0b314 100644 --- a/src/lib/mixer/MixerGroup.cpp +++ b/src/lib/mixer/MixerGroup.cpp @@ -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 @@ -50,46 +50,17 @@ //#include //#define debug(fmt, args...) syslog(fmt "\n", ##args) -void -MixerGroup::add_mixer(Mixer *mixer) -{ - Mixer **mpp = &_first; - - while (*mpp != nullptr) { - mpp = &((*mpp)->_next); - } - - *mpp = mixer; - mixer->_next = nullptr; -} - -void -MixerGroup::reset() -{ - Mixer *mixer; - Mixer *next = _first; - - /* flag mixer as invalid */ - _first = nullptr; - - /* discard sub-mixers */ - while (next != nullptr) { - mixer = next; - next = mixer->_next; - delete mixer; - mixer = nullptr; - } -} - unsigned MixerGroup::mix(float *outputs, unsigned space) { - Mixer *mixer = _first; unsigned index = 0; - while ((mixer != nullptr) && (index < space)) { + for (auto mixer : _mixers) { index += mixer->mix(outputs + index, space - index); - mixer = mixer->_next; + + if (index >= space) { + break; + } } return index; @@ -104,21 +75,19 @@ MixerGroup::mix(float *outputs, unsigned space) unsigned MixerGroup::set_trims(int16_t *values, unsigned n) { - Mixer *mixer = _first; unsigned index = 0; - while ((mixer != nullptr) && (index < n)) { - /* convert from integer to float */ - float offset = (float)values[index] / 10000; - - /* to be safe, clamp offset to range of [-500, 500] usec */ - if (offset < -1.0f) { offset = -1.0f; } - - if (offset > 1.0f) { offset = 1.0f; } + for (auto mixer : _mixers) { + // convert from integer to float + // to be safe, clamp offset to range of [-500, 500] usec + float offset = math::constrain((float)values[index] / 10000, -1.0f, 1.0f); debug("set trim: %d, offset: %5.3f", values[index], (double)offset); index += mixer->set_trim(offset); - mixer = mixer->_next; + + if (index >= n) { + break; + } } return index; @@ -133,13 +102,11 @@ MixerGroup::set_trims(int16_t *values, unsigned n) unsigned MixerGroup::get_trims(int16_t *values) { - Mixer *mixer = _first; unsigned index_mixer = 0; unsigned index = 0; - float trim; - while (mixer != nullptr) { - trim = 0; + for (auto mixer : _mixers) { + float trim = 0; index_mixer += mixer->get_trim(&trim); // MultirotorMixer returns the number of motors so we @@ -148,8 +115,6 @@ MixerGroup::get_trims(int16_t *values) values[index] = trim * 10000; index++; } - - mixer = mixer->_next; } return index; @@ -158,38 +123,28 @@ MixerGroup::get_trims(int16_t *values) void MixerGroup::set_thrust_factor(float val) { - Mixer *mixer = _first; - - while (mixer != nullptr) { + for (auto mixer : _mixers) { mixer->set_thrust_factor(val); - mixer = mixer->_next; } } void MixerGroup::set_airmode(Mixer::Airmode airmode) { - Mixer *mixer = _first; - - while (mixer != nullptr) { + for (auto mixer : _mixers) { mixer->set_airmode(airmode); - mixer = mixer->_next; } } unsigned MixerGroup::get_multirotor_count() { - Mixer *mixer = _first; - - while (mixer != nullptr) { + for (auto mixer : _mixers) { unsigned rotor_count = mixer->get_multirotor_count(); if (rotor_count > 0) { return rotor_count; } - - mixer = mixer->_next; } return 0; @@ -198,39 +153,20 @@ MixerGroup::get_multirotor_count() uint16_t MixerGroup::get_saturation_status() { - Mixer *mixer = _first; uint16_t sat = 0; - while (mixer != nullptr) { + for (auto mixer : _mixers) { sat |= mixer->get_saturation_status(); - mixer = mixer->_next; } return sat; } -unsigned -MixerGroup::count() -{ - Mixer *mixer = _first; - unsigned index = 0; - - while (mixer != nullptr) { - mixer = mixer->_next; - index++; - } - - return index; -} - void MixerGroup::groups_required(uint32_t &groups) { - Mixer *mixer = _first; - - while (mixer != nullptr) { + for (auto mixer : _mixers) { mixer->groups_required(groups); - mixer = mixer->_next; } } @@ -304,10 +240,7 @@ MixerGroup::load_from_buf(Mixer::ControlCallback control_cb, uintptr_t cb_handle void MixerGroup::set_max_delta_out_once(float delta_out_max) { - Mixer *mixer = _first; - - while (mixer != nullptr) { + for (auto mixer : _mixers) { mixer->set_max_delta_out_once(delta_out_max); - mixer = mixer->_next; } } diff --git a/src/lib/mixer/MixerGroup.hpp b/src/lib/mixer/MixerGroup.hpp index 3f17867e96..2cba61bb0c 100644 --- a/src/lib/mixer/MixerGroup.hpp +++ b/src/lib/mixer/MixerGroup.hpp @@ -66,17 +66,17 @@ public: * * @param mixer The mixer to be added. */ - void add_mixer(Mixer *mixer); + void add_mixer(Mixer *mixer) { _mixers.add(mixer); } /** * Remove all the mixers from the group. */ - void reset(); + void reset() { _mixers.clear(); } /** * Count the mixers in the group. */ - unsigned count(); + unsigned count() const { return _mixers.size(); } /** * Adds mixers to the group based on a text description in a buffer. @@ -166,5 +166,5 @@ public: unsigned get_multirotor_count(); private: - Mixer *_first{nullptr}; /**< linked list of mixers */ + List _mixers; /**< linked list of mixers */ }; diff --git a/src/lib/mixer/MultirotorMixer.hpp b/src/lib/mixer/MultirotorMixer.hpp index 96c375852b..f49cd1a259 100644 --- a/src/lib/mixer/MultirotorMixer.hpp +++ b/src/lib/mixer/MultirotorMixer.hpp @@ -40,7 +40,7 @@ * * Values are generated by the px_generate_mixers.py script and placed to mixer_multirotor_normalized.generated.h */ -typedef unsigned int MultirotorGeometryUnderlyingType; +typedef uint8_t MultirotorGeometryUnderlyingType; enum class MultirotorGeometry : MultirotorGeometryUnderlyingType; /** diff --git a/src/systemcmds/tests/test_List.cpp b/src/systemcmds/tests/test_List.cpp index 23329fe028..a06c5887e2 100644 --- a/src/systemcmds/tests/test_List.cpp +++ b/src/systemcmds/tests/test_List.cpp @@ -88,12 +88,12 @@ bool ListTest::test_add() // verify full size (100) ut_assert_true(list1.size() == 100); - int i = 99; + int i = 0; for (auto t : list1) { // verify all elements were inserted in order ut_compare("stored i", i, t->i); - i--; + i++; } // delete all elements @@ -182,27 +182,27 @@ bool ListTest::test_range_based_for() ut_assert_true(!list1.empty()); } - // first element should be 0 - ut_compare("first 0", list1.getHead()->i, 0); + // first element should be 99 (first added) + ut_compare("first 0", list1.getHead()->i, 99); // verify all elements were inserted in order - int i = 0; + int i = 99; auto t1 = list1.getHead(); while (t1 != nullptr) { ut_compare("check count", i, t1->i); t1 = t1->getSibling(); - i++; + i--; } // verify full size (100) ut_compare("size check", list1.size(), 100); - i = 0; + i = 99; for (auto t2 : list1) { ut_compare("range based for i", i, t2->i); - i++; + i--; } // verify full size (100) diff --git a/src/systemcmds/tests/test_mixer.cpp b/src/systemcmds/tests/test_mixer.cpp index cadc6f4072..8aded51a9e 100644 --- a/src/systemcmds/tests/test_mixer.cpp +++ b/src/systemcmds/tests/test_mixer.cpp @@ -383,10 +383,10 @@ bool MixerTest::mixerTest() output_limit_calc(should_arm, should_prearm, mixed, reverse_pwm_mask, r_page_servo_disarmed, r_page_servo_control_min, r_page_servo_control_max, outputs, r_page_servos, &output_limit); - //warnx("mixed %d outputs (max %d), values:", mixed, output_max); - for (unsigned i = 0; i < mixed; i++) { + //PX4_INFO("mixed %d outputs (max %d), values:", mixed, output_max); - //fprintf(stderr, "pre-arm:\t %d: out: %8.4f, servo: %d \n", i, (double)outputs[i], (int)r_page_servos[i]); + for (unsigned i = 0; i < mixed; i++) { + //PX4_ERR("pre-arm:\t %d: out: %8.4f, servo: %d", i, (double)outputs[i], (int)r_page_servos[i]); if (i != actuator_controls_s::INDEX_THROTTLE) { if (r_page_servos[i] < r_page_servo_control_min[i]) {