mixer: update to List<> container

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)

src/lib/mixer/Mixer.hpp

@@ -126,13 +126,14 @@
 
 #pragma once
 
+#include <containers/List.hpp>
 #include <mathlib/mathlib.h>
 
 /**
  * Abstract class defining a mixer mixing zero or more inputs to
  * one or more outputs.
  */
-class Mixer
+class Mixer : public ListNode<Mixer *>
 {
 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.
 	 *

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 <debug.h>
 //#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;
 	}
 }

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<Mixer *>			_mixers;	/**< linked list of mixers */
 };

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;
 
 /**

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)

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]) {