initial control allocation support

- control allocation module with multirotor, VTOL standard, and tiltrotor support
 - angular_velocity_controller
 - See https://github.com/PX4/PX4-Autopilot/pull/13351 for details

Co-authored-by: Silvan Fuhrer <silvan@auterion.com>
Co-authored-by: Roman Bapst <bapstroman@gmail.com>

src/lib/mathlib/math/filter/LowPassFilter2pVector3f.cpp

@@ -43,6 +43,7 @@ namespace math
 void LowPassFilter2pVector3f::set_cutoff_frequency(float sample_freq, float cutoff_freq)
 {
 	_cutoff_freq = cutoff_freq;
+	_sample_freq = sample_freq;
 
 	// reset delay elements on filter change
 	_delay_element_1.zero();

src/lib/mathlib/math/filter/LowPassFilter2pVector3f.hpp

@@ -74,12 +74,16 @@ public:
 	// Return the cutoff frequency
 	float get_cutoff_freq() const { return _cutoff_freq; }
 
+	// Return the sample frequency
+	float get_sample_freq() const { return _sample_freq; }
+
 	// Reset the filter state to this value
 	matrix::Vector3f reset(const matrix::Vector3f &sample);
 
 private:
 
 	float _cutoff_freq{0.0f};
+	float _sample_freq{0.0f};
 
 	float _a1{0.0f};
 	float _a2{0.0f};

src/lib/mixer/AllocatedActuatorMixer/AllocatedActuatorMixer.cpp

@@ -0,0 +1,149 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2020 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 mixer_AllocatedActuatorMixer.cpp
+ *
+ * Mixer for allocated actuators.
+ *
+ * @author Julien Lecoeur <julien.lecoeur@gmail.com>
+ */
+
+#include "AllocatedActuatorMixer.hpp"
+
+#include <mathlib/mathlib.h>
+#include <cstdio>
+#include <px4_platform_common/defines.h>
+
+// #define debug(fmt, args...)	do { } while(0)
+#define debug(fmt, args...)	do { printf("[mixer] " fmt "\n", ##args); } while(0)
+
+AllocatedActuatorMixer::AllocatedActuatorMixer(ControlCallback control_cb,
+		uintptr_t cb_handle,
+		uint8_t index) :
+	Mixer(control_cb, cb_handle)
+{
+	if (index < 8) {
+		_control_group = 4;
+		_control_index = index;
+
+	} else if (index < 16) {
+		_control_group = 5;
+		_control_index = index - 8;
+
+	} else {
+		debug("'A:' invalid index");
+	}
+}
+
+unsigned AllocatedActuatorMixer::set_trim(float trim)
+{
+	return 1;
+}
+
+unsigned AllocatedActuatorMixer::get_trim(float *trim)
+{
+	*trim = 0.0f;
+	return 1;
+}
+
+int
+AllocatedActuatorMixer::parse(const char *buf, unsigned &buflen, uint8_t &index)
+{
+	int ret;
+	int i;
+
+	// enforce that the mixer ends with a new line
+	if (!string_well_formed(buf, buflen)) {
+		return -1;
+	}
+
+	// parse line
+	if ((ret = sscanf(buf, "A: %d", &i)) != 1) {
+		debug("'A:' parser: failed on '%s'", buf);
+		return -1;
+	}
+
+	buf = skipline(buf, buflen);
+
+	if (buf == nullptr) {
+		debug("'A:' parser: no line ending, line is incomplete");
+		return -1;
+	}
+
+	// check parsed index
+	if (i < 16) {
+		index = i;
+
+	} else {
+		debug("'A:' parser: invalid index");
+		return -1;
+	}
+
+	return 0;
+}
+
+AllocatedActuatorMixer *
+AllocatedActuatorMixer::from_text(Mixer::ControlCallback control_cb, uintptr_t cb_handle, const char *buf,
+				  unsigned &buflen)
+{
+	uint8_t index;
+
+	if (parse(buf, buflen, index) == 0) {
+		return new AllocatedActuatorMixer(control_cb, cb_handle, index);
+
+	} else {
+		return nullptr;
+	}
+}
+
+unsigned
+AllocatedActuatorMixer::mix(float *outputs, unsigned space)
+{
+	if (space < 1) {
+		return 0;
+	}
+
+	_control_cb(_cb_handle,
+		    _control_group,
+		    _control_index,
+		    *outputs);
+
+	return 1;
+}
+
+void
+AllocatedActuatorMixer::groups_required(uint32_t &groups)
+{
+	groups |= 1 << _control_group;
+}

src/lib/mixer/AllocatedActuatorMixer/AllocatedActuatorMixer.hpp

@@ -0,0 +1,103 @@
+/****************************************************************************
+ *
+ *   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
+ * 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 mixer_AllocatedActuatorMixer.hpp
+ *
+ * Mixer for allocated actuators.
+ *
+ * @author Julien Lecoeur <julien.lecoeur@gmail.com>
+ */
+
+#pragma once
+
+#include <lib/mixer/MixerBase/Mixer.hpp>
+
+/**
+ * Mixer for allocated actuators.
+ *
+ * Copies a single actuator to a single output.
+ */
+class AllocatedActuatorMixer : public Mixer
+{
+public:
+	/**
+	 * Constructor
+	 *
+	 * @param index	Actuator index (0..15)
+	 */
+	AllocatedActuatorMixer(ControlCallback control_cb,
+			       uintptr_t cb_handle,
+			       uint8_t index);
+	virtual ~AllocatedActuatorMixer() = default;
+
+	// no copy, assignment, move, move assignment
+	AllocatedActuatorMixer(const AllocatedActuatorMixer &) = delete;
+	AllocatedActuatorMixer &operator=(const AllocatedActuatorMixer &) = delete;
+	AllocatedActuatorMixer(AllocatedActuatorMixer &&) = delete;
+	AllocatedActuatorMixer &operator=(AllocatedActuatorMixer &&) = delete;
+
+	/**
+	 * Factory method with full external configuration.
+	 *
+	 * Given a pointer to a buffer containing a text description of the mixer,
+	 * returns a pointer to a new instance of the mixer.
+	 *
+	 * @param control_cb		The callback to invoke when fetching a
+	 *				control value.
+	 * @param cb_handle		Handle passed to the control callback.
+	 * @param buf			Buffer containing a text description of
+	 *				the mixer.
+	 * @param buflen		Length of the buffer in bytes, adjusted
+	 *				to reflect the bytes consumed.
+	 * @return			A new AllocatedActuatorMixer instance, or nullptr
+	 *				if the text format is bad.
+	 */
+	static AllocatedActuatorMixer *from_text(Mixer::ControlCallback control_cb,
+			uintptr_t cb_handle,
+			const char *buf,
+			unsigned &buflen);
+
+	unsigned mix(float *outputs, unsigned space) override;
+	void groups_required(uint32_t &groups) override;
+	unsigned set_trim(float trim) override;
+	unsigned get_trim(float *trim) override;
+
+private:
+	uint8_t	_control_group;	/**< group from which the input reads */
+	uint8_t	_control_index;	/**< index within the control group */
+
+	static int parse(const char *buf,
+			 unsigned &buflen,
+			 uint8_t &index);
+};

src/lib/mixer/AllocatedActuatorMixer/CMakeLists.txt

@@ -0,0 +1,39 @@
+############################################################################
+#
+#   Copyright (c) 2015-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.
+#
+############################################################################
+
+add_library(AllocatedActuatorMixer
+	AllocatedActuatorMixer.cpp
+	AllocatedActuatorMixer.hpp
+)
+target_link_libraries(AllocatedActuatorMixer PRIVATE MixerBase)
+add_dependencies(AllocatedActuatorMixer prebuild_targets)

src/lib/mixer/CMakeLists.txt

@@ -34,6 +34,7 @@
 # required by other mixers
 add_subdirectory(MixerBase)
 
+add_subdirectory(AllocatedActuatorMixer)
 add_subdirectory(HelicopterMixer)
 add_subdirectory(MultirotorMixer)
 add_subdirectory(NullMixer)
@@ -48,6 +49,7 @@ add_library(mixer
 
 target_link_libraries(mixer
 	PRIVATE
+		AllocatedActuatorMixer
 		HelicopterMixer
 		MultirotorMixer
 		NullMixer

src/lib/mixer/MixerGroup.cpp

@@ -39,6 +39,7 @@
 
 #include "MixerGroup.hpp"
 
+#include "AllocatedActuatorMixer/AllocatedActuatorMixer.hpp"
 #include "HelicopterMixer/HelicopterMixer.hpp"
 #include "MultirotorMixer/MultirotorMixer.hpp"
 #include "NullMixer/NullMixer.hpp"
@@ -192,6 +193,10 @@ MixerGroup::load_from_buf(Mixer::ControlCallback control_cb, uintptr_t cb_handle
 			m = NullMixer::from_text(p, resid);
 			break;
 
+		case 'A':
+			m = AllocatedActuatorMixer::from_text(control_cb, cb_handle, p, resid);
+			break;
+
 		case 'M':
 			m = SimpleMixer::from_text(control_cb, cb_handle, p, resid);
 			break;

src/lib/mixer_module/mixer_module.cpp

@@ -49,7 +49,9 @@ MixingOutput::MixingOutput(uint8_t max_num_outputs, OutputModuleInterface &inter
 	{&interface, ORB_ID(actuator_controls_0)},
 	{&interface, ORB_ID(actuator_controls_1)},
 	{&interface, ORB_ID(actuator_controls_2)},
-	{&interface, ORB_ID(actuator_controls_3)}
+	{&interface, ORB_ID(actuator_controls_3)},
+	{&interface, ORB_ID(actuator_controls_4)},
+	{&interface, ORB_ID(actuator_controls_5)},
 },
 _scheduling_policy(scheduling_policy),
 _support_esc_calibration(support_esc_calibration),
@@ -536,9 +538,11 @@ int MixingOutput::controlCallback(uintptr_t handle, uint8_t control_group, uint8
 
 	/* motor spinup phase - lock throttle to zero */
 	if (output->_output_limit.state == OUTPUT_LIMIT_STATE_RAMP) {
-		if ((control_group == actuator_controls_s::GROUP_INDEX_ATTITUDE ||
-		     control_group == actuator_controls_s::GROUP_INDEX_ATTITUDE_ALTERNATE) &&
-		    control_index == actuator_controls_s::INDEX_THROTTLE) {
+		if (((control_group == actuator_controls_s::GROUP_INDEX_ATTITUDE ||
+		      control_group == actuator_controls_s::GROUP_INDEX_ATTITUDE_ALTERNATE) &&
+		     control_index == actuator_controls_s::INDEX_THROTTLE) ||
+		    (control_group == actuator_controls_s::GROUP_INDEX_ALLOCATED_PART1 ||
+		     control_group == actuator_controls_s::GROUP_INDEX_ALLOCATED_PART2)) {
 			/* limit the throttle output to zero during motor spinup,
 			 * as the motors cannot follow any demand yet
 			 */
@@ -548,9 +552,11 @@ int MixingOutput::controlCallback(uintptr_t handle, uint8_t control_group, uint8
 
 	/* throttle not arming - mark throttle input as invalid */
 	if (output->armNoThrottle() && !output->_armed.in_esc_calibration_mode) {
-		if ((control_group == actuator_controls_s::GROUP_INDEX_ATTITUDE ||
-		     control_group == actuator_controls_s::GROUP_INDEX_ATTITUDE_ALTERNATE) &&
-		    control_index == actuator_controls_s::INDEX_THROTTLE) {
+		if (((control_group == actuator_controls_s::GROUP_INDEX_ATTITUDE ||
+		      control_group == actuator_controls_s::GROUP_INDEX_ATTITUDE_ALTERNATE) &&
+		     control_index == actuator_controls_s::INDEX_THROTTLE) ||
+		    (control_group == actuator_controls_s::GROUP_INDEX_ALLOCATED_PART1 ||
+		     control_group == actuator_controls_s::GROUP_INDEX_ALLOCATED_PART2)) {
 			/* set the throttle to an invalid value */
 			input = NAN;
 		}

src/lib/parameters/px4params/srcparser.py

@@ -333,7 +333,7 @@ class SourceParser(object):
                     self.param_groups[group].AddParameter(param)
                 state = None
         return True
-    
+
     def IsNumber(self, numberString):
         try:
             float(numberString)
@@ -348,8 +348,8 @@ class SourceParser(object):
         seenParamNames = []
         #allowedUnits should match set defined in /Firmware/validation/module_schema.yaml
         allowedUnits = set ([
-                                '%', 'Hz', 'mAh',
-                                'rad', '%/rad', 'rad/s', 'rad/s^2', '%/rad/s',  'rad s^2/m','rad s/m',
+                                '%', 'Hz', '1/s', 'mAh',
+                                'rad', '%/rad', 'rad/s', 'rad/s^2', '%/rad/s', 'rad s^2/m', 'rad s/m',
                                 'bit/s', 'B/s',
                                 'deg', 'deg*1e7', 'deg/s',
                                 'celcius', 'gauss', 'gauss/s', 'gauss^2',
@@ -357,9 +357,9 @@ class SourceParser(object):
                                 'mm', 'm', 'm/s', 'm^2', 'm/s^2', 'm/s^3', 'm/s^2/sqrt(Hz)', 'm/s/rad',
                                 'Ohm', 'V',
                                 'us', 'ms', 's',
-                                'S', 'A/%', '(m/s^2)^2',  'm/m',  'tan(rad)^2', '(m/s)^2', 'm/rad',
-                                'm/s^3/sqrt(Hz)', 'm/s/sqrt(Hz)', 's/(1000*PWM)', '%m/s', 'min', 'us/C', 
-                                'N/(m/s)', 'Nm/(rad/s)', 'Nm', 'N',
+                                'S', 'A/%', '(m/s^2)^2', 'm/m',  'tan(rad)^2', '(m/s)^2', 'm/rad',
+                                'm/s^3/sqrt(Hz)', 'm/s/sqrt(Hz)', 's/(1000*PWM)', '%m/s', 'min', 'us/C',
+                                'N/(m/s)', 'Nm/rad', 'Nm/(rad/s)', 'Nm', 'N',
                                 'normalized_thrust/s', 'normalized_thrust', 'norm', 'SD',''])
         for group in self.GetParamGroups():
             for param in group.GetParams():
@@ -380,7 +380,7 @@ class SourceParser(object):
                 min = param.GetFieldValue("min")
                 max = param.GetFieldValue("max")
                 units = param.GetFieldValue("unit")
-                if units not in allowedUnits: 
+                if units not in allowedUnits:
                     sys.stderr.write("Invalid unit in {0}: {1}\n".format(name, units))
                     return False
                 #sys.stderr.write("{0} default:{1} min:{2} max:{3}\n".format(name, default, min, max))