Temporarily add old multirotor mixer test to take over the exact cases into new unit tests

2026-10-14 12:43:34 +08:00 · 2025-04-30 14:59:10 +02:00
parent 80f4f6123e
commit 20c863c9d6
51 changed files with 5160 additions and 0 deletions
@@ -420,6 +420,7 @@ if(BUILD_TESTING)
 			DEPENDS
 				px4
 				examples__dyn_hello
+				test_mixer_multirotor
 			USES_TERMINAL
 			COMMENT "Running tests"
 			WORKING_DIRECTORY ${PX4_BINARY_DIR})
@@ -57,6 +57,7 @@ add_subdirectory(lat_lon_alt EXCLUDE_FROM_ALL)
 add_subdirectory(led EXCLUDE_FROM_ALL)
 add_subdirectory(matrix EXCLUDE_FROM_ALL)
 add_subdirectory(mathlib EXCLUDE_FROM_ALL)
+add_subdirectory(mixer)
 add_subdirectory(mixer_module EXCLUDE_FROM_ALL)
 add_subdirectory(motion_planning EXCLUDE_FROM_ALL)
 add_subdirectory(npfg EXCLUDE_FROM_ALL)
@@ -0,0 +1,2 @@
+
+/test_mixer_multirotor
@@ -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;
+}
@@ -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);
+};
@@ -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)
@@ -0,0 +1,58 @@
+############################################################################
+#
+#   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.
+#
+############################################################################
+
+# required by other mixers
+add_subdirectory(MixerBase)
+
+add_subdirectory(AllocatedActuatorMixer)
+add_subdirectory(HelicopterMixer)
+add_subdirectory(MultirotorMixer)
+add_subdirectory(NullMixer)
+add_subdirectory(SimpleMixer)
+
+add_library(mixer
+	MixerGroup.cpp
+	MixerGroup.hpp
+	load_mixer_file.cpp
+	mixer_load.h
+)
+
+target_link_libraries(mixer
+	PRIVATE
+		AllocatedActuatorMixer
+		HelicopterMixer
+		MultirotorMixer
+		NullMixer
+		SimpleMixer
+	)
+add_dependencies(mixer prebuild_targets)
@@ -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(HelicopterMixer
+	HelicopterMixer.cpp
+	HelicopterMixer.hpp
+)
+target_link_libraries(HelicopterMixer PRIVATE MixerBase)
+add_dependencies(HelicopterMixer prebuild_targets)
@@ -0,0 +1,240 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2012-2017 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_helicopter.cpp
+ *
+ * Helicopter mixers.
+ */
+
+#include "HelicopterMixer.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)
+//#include <debug.h>
+//#define debug(fmt, args...)	lowsyslog(fmt "\n", ##args)
+
+using math::constrain;
+
+HelicopterMixer::HelicopterMixer(ControlCallback control_cb, uintptr_t cb_handle, mixer_heli_s mixer_info) :
+	Mixer(control_cb, cb_handle),
+	_mixer_info(mixer_info)
+{
+}
+
+HelicopterMixer *
+HelicopterMixer::from_text(Mixer::ControlCallback control_cb, uintptr_t cb_handle, const char *buf, unsigned &buflen)
+{
+	mixer_heli_s mixer_info;
+	unsigned swash_plate_servo_count = 0;
+	unsigned u[5];
+	int s[5];
+	int used;
+
+	/* enforce that the mixer ends with a new line */
+	if (!string_well_formed(buf, buflen)) {
+		return nullptr;
+	}
+
+	if (sscanf(buf, "H: %u%n", &swash_plate_servo_count, &used) != 1) {
+		debug("helicopter parse failed on '%s'", buf);
+		return nullptr;
+	}
+
+	if (swash_plate_servo_count < 3 || swash_plate_servo_count > 4) {
+		debug("only supporting swash plate with 3 or 4 servos");
+		return nullptr;
+	}
+
+	if (used > (int)buflen) {
+		debug("OVERFLOW: helicopter spec used %d of %u", used, buflen);
+		return nullptr;
+	}
+
+	buf = skipline(buf, buflen);
+
+	if (buf == nullptr) {
+		debug("no line ending, line is incomplete");
+		return nullptr;
+	}
+
+	buf = findtag(buf, buflen, 'T');
+
+	if ((buf == nullptr) || (buflen < 12)) {
+		debug("control parser failed finding tag, ret: '%s'", buf);
+		return nullptr;
+	}
+
+	if (sscanf(buf, "T: %u %u %u %u %u",
+		   &u[0], &u[1], &u[2], &u[3], &u[4]) != 5) {
+		debug("control parse failed on '%s'", buf);
+		return nullptr;
+	}
+
+	for (unsigned i = 0; i < HELI_CURVES_NR_POINTS; i++) {
+		mixer_info.throttle_curve[i] = ((float) u[i]) / 10000.0f;
+	}
+
+	buf = skipline(buf, buflen);
+
+	if (buf == nullptr) {
+		debug("no line ending, line is incomplete");
+		return nullptr;
+	}
+
+	buf = findtag(buf, buflen, 'P');
+
+	if ((buf == nullptr) || (buflen < 12)) {
+		debug("control parser failed finding tag, ret: '%s'", buf);
+		return nullptr;
+	}
+
+	if (sscanf(buf, "P: %d %d %d %d %d",
+		   &s[0], &s[1], &s[2], &s[3], &s[4]) != 5) {
+		debug("control parse failed on '%s'", buf);
+		return nullptr;
+	}
+
+	for (unsigned i = 0; i < HELI_CURVES_NR_POINTS; i++) {
+		mixer_info.pitch_curve[i] = ((float) s[i]) / 10000.0f;
+	}
+
+	buf = skipline(buf, buflen);
+
+	if (buf == nullptr) {
+		debug("no line ending, line is incomplete");
+		return nullptr;
+	}
+
+	mixer_info.control_count = swash_plate_servo_count;
+
+	/* Now loop through the servos */
+	for (unsigned i = 0; i < mixer_info.control_count; i++) {
+
+		buf = findtag(buf, buflen, 'S');
+
+		if ((buf == nullptr) || (buflen < 12)) {
+			debug("control parser failed finding tag, ret: '%s'", buf);
+			return nullptr;
+		}
+
+		if (sscanf(buf, "S: %u %u %d %d %d %d",
+			   &u[0],
+			   &u[1],
+			   &s[0],
+			   &s[1],
+			   &s[2],
+			   &s[3]) != 6) {
+			debug("control parse failed on '%s'", buf);
+			return nullptr;
+		}
+
+		mixer_info.servos[i].angle = ((float) u[0]) * M_PI_F / 180.0f;
+		mixer_info.servos[i].arm_length = ((float) u[1]) / 10000.0f;
+		mixer_info.servos[i].scale = ((float) s[0]) / 10000.0f;
+		mixer_info.servos[i].offset = ((float) s[1]) / 10000.0f;
+		mixer_info.servos[i].min_output = ((float) s[2]) / 10000.0f;
+		mixer_info.servos[i].max_output = ((float) s[3]) / 10000.0f;
+
+		buf = skipline(buf, buflen);
+
+		if (buf == nullptr) {
+			debug("no line ending, line is incomplete");
+			return nullptr;
+		}
+	}
+
+	debug("remaining in buf: %d, first char: %c", buflen, buf[0]);
+
+	HelicopterMixer *hm = new HelicopterMixer(control_cb, cb_handle, mixer_info);
+
+	if (hm != nullptr) {
+		debug("loaded heli mixer with %d swash plate input(s)", mixer_info.control_count);
+
+	} else {
+		debug("could not allocate memory for mixer");
+	}
+
+	return hm;
+}
+
+unsigned
+HelicopterMixer::mix(float *outputs, unsigned space)
+{
+	if (space < _mixer_info.control_count + 1u) {
+		return 0;
+	}
+
+	/* Find index to use for curves */
+	float thrust_cmd = get_control(0, 3);
+	int idx = (thrust_cmd / 0.25f);
+
+	/* Make sure idx is in range */
+	if (idx < 0) {
+		idx = 0;
+
+	} else if (idx > HELI_CURVES_NR_POINTS - 2) {
+		/* We access idx + 1 below, so max legal index is (size - 2) */
+		idx = HELI_CURVES_NR_POINTS - 2;
+	}
+
+	/* Local throttle curve gradient and offset */
+	float tg = (_mixer_info.throttle_curve[idx + 1] - _mixer_info.throttle_curve[idx]) / 0.25f;
+	float to = (_mixer_info.throttle_curve[idx]) - (tg * idx * 0.25f);
+	float throttle = constrain(2.0f * (tg * thrust_cmd + to) - 1.0f, -1.0f, 1.0f);
+
+	/* Local pitch curve gradient and offset */
+	float pg = (_mixer_info.pitch_curve[idx + 1] - _mixer_info.pitch_curve[idx]) / 0.25f;
+	float po = (_mixer_info.pitch_curve[idx]) - (pg * idx * 0.25f);
+	float collective_pitch = constrain((pg * thrust_cmd + po), -0.5f, 0.5f);
+
+	float roll_cmd = get_control(0, 0);
+	float pitch_cmd = get_control(0, 1);
+
+	outputs[0] = throttle;
+
+	for (unsigned i = 0; i < _mixer_info.control_count; i++) {
+		outputs[i + 1] = collective_pitch
+				 + cosf(_mixer_info.servos[i].angle) * pitch_cmd * _mixer_info.servos[i].arm_length
+				 - sinf(_mixer_info.servos[i].angle) * roll_cmd * _mixer_info.servos[i].arm_length;
+		outputs[i + 1] *= _mixer_info.servos[i].scale;
+		outputs[i + 1] += _mixer_info.servos[i].offset;
+		outputs[i + 1] = constrain(outputs[i + 1], _mixer_info.servos[i].min_output, _mixer_info.servos[i].max_output);
+	}
+
+	return _mixer_info.control_count + 1;
+}
@@ -0,0 +1,111 @@
+/****************************************************************************
+ *
+ *   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.
+ *
+ ****************************************************************************/
+
+#pragma once
+
+#include <mixer/MixerBase/Mixer.hpp>
+
+/** helicopter swash servo mixer */
+struct mixer_heli_servo_s {
+	float angle;
+	float arm_length;
+	float scale;
+	float offset;
+	float min_output;
+	float max_output;
+};
+
+#define HELI_CURVES_NR_POINTS 5
+
+/** helicopter swash plate mixer */
+struct mixer_heli_s {
+	uint8_t				control_count;	/**< number of inputs */
+	float				throttle_curve[HELI_CURVES_NR_POINTS];
+	float				pitch_curve[HELI_CURVES_NR_POINTS];
+	mixer_heli_servo_s		servos[4];	/**< up to four inputs */
+};
+
+/**
+ * Generic helicopter mixer for helicopters with swash plate.
+ *
+ * Collects four inputs (roll, pitch, yaw, thrust) and mixes them to servo commands
+ * for swash plate tilting and throttle- and pitch curves.
+ */
+class HelicopterMixer : public Mixer
+{
+public:
+	/**
+	 * Constructor.
+	 *
+	 * @param control_cb		Callback invoked to read inputs.
+	 * @param cb_handle		Passed to control_cb.
+	 * @param mixer_info		Pointer to heli mixer configuration
+	 */
+	HelicopterMixer(ControlCallback control_cb, uintptr_t cb_handle, mixer_heli_s mixer_info);
+	virtual ~HelicopterMixer() = default;
+
+	// no copy, assignment, move, move assignment
+	HelicopterMixer(const HelicopterMixer &) = delete;
+	HelicopterMixer &operator=(const HelicopterMixer &) = delete;
+	HelicopterMixer(HelicopterMixer &&) = delete;
+	HelicopterMixer &operator=(HelicopterMixer &&) = delete;
+
+	/**
+	 * Factory method.
+	 *
+	 * 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 HelicopterMixer instance, or nullptr
+	 *				if the text format is bad.
+	 */
+	static HelicopterMixer		*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 { groups |= (1 << 0); }
+
+	unsigned			set_trim(float trim) override { return 4; }
+	unsigned			get_trim(float *trim) override { return 4; }
+
+private:
+	mixer_heli_s			_mixer_info;
+};
@@ -0,0 +1,38 @@
+############################################################################
+#
+#   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(MixerBase
+	Mixer.cpp
+	Mixer.hpp
+)
+add_dependencies(MixerBase prebuild_targets)
@@ -0,0 +1,124 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2012 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.cpp
+ *
+ * Programmable multi-channel mixer library.
+ */
+
+#include "Mixer.hpp"
+
+#include <math.h>
+#include <cstring>
+#include <ctype.h>
+
+#define debug(fmt, args...)	do { } while(0)
+//#define debug(fmt, args...)	do { printf("[mixer] " fmt "\n", ##args); } while(0)
+
+float
+Mixer::get_control(uint8_t group, uint8_t index)
+{
+	float value;
+
+	_control_cb(_cb_handle, group, index, value);
+
+	return value;
+}
+
+const char *
+Mixer::findtag(const char *buf, unsigned &buflen, char tag)
+{
+	while (buflen >= 2) {
+		if ((buf[0] == tag) && (buf[1] == ':')) {
+			return buf;
+		}
+
+		buf++;
+		buflen--;
+	}
+
+	return nullptr;
+}
+
+char
+Mixer::findnexttag(const char *buf, unsigned buflen)
+{
+	while (buflen >= 2) {
+		if (isupper(buf[0]) && buf[1] == ':') {
+			return buf[0];
+		}
+
+		buf++;
+		buflen--;
+	}
+
+	return 0;
+}
+
+const char *
+Mixer::skipline(const char *buf, unsigned &buflen)
+{
+	const char *p;
+
+	/* if we can find a CR or NL in the buffer, skip up to it */
+	if ((p = (const char *)memchr(buf, '\r', buflen)) || (p = (const char *)memchr(buf, '\n', buflen))) {
+		/* skip up to it AND one beyond - could be on the NUL symbol now */
+		buflen -= (p - buf) + 1;
+		return p + 1;
+	}
+
+	return nullptr;
+}
+
+bool
+Mixer::string_well_formed(const char *buf, unsigned &buflen)
+{
+	/* enforce that the mixer ends with a new line */
+	for (int i = buflen - 1; i >= 0; i--) {
+		if (buf[i] == '\0') {
+			continue;
+		}
+
+		/* require a space or newline at the end of the buffer, fail on printable chars */
+		if (buf[i] == '\n' || buf[i] == '\r') {
+			/* found a line ending, so no split symbols / numbers. good. */
+			return true;
+		}
+
+	}
+
+	debug("pre-parser rejected: No newline in buf");
+
+	return false;
+}
@@ -0,0 +1,279 @@
+/****************************************************************************
+ *
+ *   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.hpp
+ *
+ * Generic, programmable, procedural control signal mixers.
+ *
+ * This library implements a generic mixer interface that can be used
+ * by any driver or subsytem that wants to combine several control signals
+ * into a single output.
+ *
+ * Terminology
+ * ===========
+ *
+ * control value
+ *	A mixer input value, typically provided by some controlling
+ *	component of the system.
+ *
+ * control group
+ * 	A collection of controls provided by a single controlling component.
+ *
+ * actuator
+ *	The mixer output value.
+ *
+ *
+ * Mixing basics
+ * =============
+ *
+ * An actuator derives its value from the combination of one or more
+ * control values. Each of the control values is scaled according to
+ * the actuator's configuration and then combined to produce the
+ * actuator value, which may then be further scaled to suit the specific
+ * output type.
+ *
+ * Internally, all scaling is performed using floating point values.
+ * Inputs and outputs are clamped to the range -1.0 to 1.0.
+ *
+ * control    control   control
+ *    |          |         |
+ *    v          v         v
+ *  scale      scale     scale
+ *    |          |         |
+ *    |          v         |
+ *    +-------> mix <------+
+ *               |
+ *             scale
+ *               |
+ *               v
+ *              out
+ *
+ * Scaling
+ * -------
+ *
+ * Each scaler allows the input value to be scaled independently for
+ * inputs greater/less than zero. An offset can be applied to the output,
+ * as well as lower and upper boundary constraints.
+ * Negative scaling factors cause the output to be inverted (negative input
+ * produces positive output).
+ *
+ * Scaler pseudocode:
+ *
+ * if (input < 0)
+ *     output = (input * NEGATIVE_SCALE) + OFFSET
+ * else
+ *     output = (input * POSITIVE_SCALE) + OFFSET
+ *
+ * if (output < LOWER_LIMIT)
+ *     output = LOWER_LIMIT
+ * if (output > UPPER_LIMIT)
+ *     output = UPPER_LIMIT
+ *
+ *
+ * Mixing
+ * ------
+ *
+ * Mixing behaviour varies based on the specific mixer class; each
+ * mixer class describes its behaviour in more detail.
+ *
+ *
+ * Controls
+ * --------
+ *
+ * The precise assignment of controls may vary depending on the
+ * application, but the following assignments should be used
+ * when appropriate.  Some mixer classes have specific assumptions
+ * about the assignment of controls.
+ *
+ * control | standard meaning
+ * --------+-----------------------
+ *     0   | roll
+ *     1   | pitch
+ *     2   | yaw
+ *     3   | primary thrust
+ */
+
+#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 : public ListNode<Mixer *>
+{
+public:
+	enum class Airmode : int32_t {
+		disabled = 0,
+		roll_pitch = 1,
+		roll_pitch_yaw = 2
+	};
+
+	/**
+	 * Fetch a control value.
+	 *
+	 * @param handle		Token passed when the callback is registered.
+	 * @param control_group		The group to fetch the control from.
+	 * @param control_index		The group-relative index to fetch the control from.
+	 * @param control		The returned control
+	 * @return			Zero if the value was fetched, nonzero otherwise.
+	 */
+	typedef int	(* ControlCallback)(uintptr_t handle, uint8_t control_group, uint8_t control_index, float &control);
+
+	/**
+	 * Constructor.
+	 *
+	 * @param control_cb		Callback invoked when reading controls.
+	 */
+	Mixer(ControlCallback control_cb, uintptr_t cb_handle) : _control_cb(control_cb), _cb_handle(cb_handle) {}
+	virtual ~Mixer() = default;
+
+	// no copy, assignment, move, move assignment
+	Mixer(const Mixer &) = delete;
+	Mixer &operator=(const Mixer &) = delete;
+	Mixer(Mixer &&) = delete;
+	Mixer &operator=(Mixer &&) = delete;
+
+	/**
+	 * Perform the mixing function.
+	 *
+	 * @param outputs		Array into which mixed output(s) should be placed.
+	 * @param space			The number of available entries in the output array;
+	 * @return			The number of entries in the output array that were populated.
+	 */
+	virtual unsigned		mix(float *outputs, unsigned space) = 0;
+
+	/**
+	 * Get the saturation status.
+	 *
+	 * @return			Integer bitmask containing saturation_status from multirotor_motor_limits.msg.
+	 */
+	virtual uint16_t		get_saturation_status() { return 0; }
+
+	/**
+	 * Analyses the mix configuration and updates a bitmask of groups
+	 * that are required.
+	 *
+	 * @param groups		A bitmask of groups (0-31) that the mixer requires.
+	 */
+	virtual void			groups_required(uint32_t &groups) {};
+
+	/**
+	 * @brief      Empty method, only implemented for MultirotorMixer and MixerGroup class.
+	 *
+	 * @param[in]  delta_out_max  Maximum delta output.
+	 *
+	 */
+	virtual void 			set_max_delta_out_once(float delta_out_max) {}
+
+	/**
+	 * @brief Set trim offset for this mixer
+	 *
+	 * @return the number of outputs this mixer feeds to
+	 */
+	virtual unsigned		set_trim(float trim) { return 0; }
+
+	/**
+	 * @brief Get trim offset for this mixer
+	 *
+	 * @return the number of outputs this mixer feeds to
+	 */
+	virtual unsigned		get_trim(float *trim) { return 0; }
+
+	/*
+	 * @brief      Sets the thrust factor used to calculate mapping from desired thrust to motor control signal output.
+	 *
+	 * @param[in]  val   The value
+	 */
+	virtual void 			set_thrust_factor(float val) {}
+
+	/**
+	 * @brief Set airmode. Airmode allows the mixer to increase the total thrust in order to unsaturate the motors.
+	 *
+	 * @param[in]  airmode   Select airmode type (0 = disabled, 1 = roll/pitch, 2 = roll/pitch/yaw)
+	 */
+	virtual void			set_airmode(Airmode airmode) {};
+
+	virtual unsigned		get_multirotor_count()  { return 0; }
+
+	virtual void 			set_dt_once(float dt) {}
+
+protected:
+
+	/** client-supplied callback used when fetching control values */
+	ControlCallback			_control_cb;
+	uintptr_t			_cb_handle;
+
+	/**
+	 * Invoke the client callback to fetch a control value.
+	 *
+	 * @param group			Control group to fetch from.
+	 * @param index			Control index to fetch.
+	 * @return			The control value.
+	 */
+	float				get_control(uint8_t group, uint8_t index);
+
+	/**
+	 * Find a tag
+	 *
+	 * @param buf			The buffer to operate on.
+	 * @param buflen		length of the buffer.
+	 * @param tag			character to search for.
+	 */
+	static const char 		*findtag(const char *buf, unsigned &buflen, char tag);
+
+	/**
+	 * Find next tag and return it (0 is returned if no tag is found)
+	 *
+	 * @param buf			The buffer to operate on.
+	 * @param buflen		length of the buffer.
+	 */
+	static char 			findnexttag(const char *buf, unsigned buflen);
+
+	/**
+	 * Skip a line
+	 *
+	 * @param buf			The buffer to operate on.
+	 * @param buflen		length of the buffer.
+	 * @return			0 / OK if a line could be skipped, 1 else
+	 */
+	static const char 		*skipline(const char *buf, unsigned &buflen);
+
+	/**
+	 * Check wether the string is well formed and suitable for parsing
+	 */
+	static bool			string_well_formed(const char *buf, unsigned &buflen);
+};
@@ -0,0 +1,258 @@
+/****************************************************************************
+ *
+ *   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_group.cpp
+ *
+ * Mixer collection.
+ */
+
+#include "MixerGroup.hpp"
+
+#include "AllocatedActuatorMixer/AllocatedActuatorMixer.hpp"
+#include "HelicopterMixer/HelicopterMixer.hpp"
+#include "MultirotorMixer/MultirotorMixer.hpp"
+#include "NullMixer/NullMixer.hpp"
+#include "SimpleMixer/SimpleMixer.hpp"
+
+#define debug(fmt, args...)	do { } while(0)
+//#define debug(fmt, args...)	do { printf("[mixer] " fmt "\n", ##args); } while(0)
+//#include <debug.h>
+//#define debug(fmt, args...)	syslog(fmt "\n", ##args)
+
+unsigned
+MixerGroup::mix(float *outputs, unsigned space)
+{
+	unsigned index = 0;
+
+	for (auto mixer : _mixers) {
+		index += mixer->mix(outputs + index, space - index);
+
+		if (index >= space) {
+			break;
+		}
+	}
+
+	return index;
+}
+
+/*
+ * set_trims() has no effect except for the SimpleMixer implementation for which set_trim()
+ * always returns the value one.
+ * The only other existing implementation is MultirotorMixer, which ignores the trim value
+ * and returns _rotor_count.
+ */
+unsigned
+MixerGroup::set_trims(int16_t *values, unsigned n)
+{
+	unsigned index = 0;
+
+	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);
+
+		if (index >= n) {
+			break;
+		}
+	}
+
+	return index;
+}
+
+/*
+ * get_trims() has no effect except for the SimpleMixer implementation for which get_trim()
+ * always returns the value one and sets the trim value.
+ * The only other existing implementation is MultirotorMixer, which ignores the trim value
+ * and returns _rotor_count.
+ */
+unsigned
+MixerGroup::get_trims(int16_t *values)
+{
+	unsigned index_mixer = 0;
+	unsigned index = 0;
+
+	for (auto mixer : _mixers) {
+		float trim = 0;
+		index_mixer += mixer->get_trim(&trim);
+
+		// MultirotorMixer returns the number of motors so we
+		// loop through index_mixer and set the same trim value for all motors
+		while (index < index_mixer) {
+			values[index] = trim * 10000;
+			index++;
+		}
+	}
+
+	return index;
+}
+
+void
+MixerGroup::set_thrust_factor(float val)
+{
+	for (auto mixer : _mixers) {
+		mixer->set_thrust_factor(val);
+	}
+}
+
+void
+MixerGroup::set_airmode(Mixer::Airmode airmode)
+{
+	for (auto mixer : _mixers) {
+		mixer->set_airmode(airmode);
+	}
+}
+
+unsigned
+MixerGroup::get_multirotor_count()
+{
+	for (auto mixer : _mixers) {
+		unsigned rotor_count = mixer->get_multirotor_count();
+
+		if (rotor_count > 0) {
+			return rotor_count;
+		}
+	}
+
+	return 0;
+}
+
+uint16_t
+MixerGroup::get_saturation_status()
+{
+	uint16_t sat = 0;
+
+	for (auto mixer : _mixers) {
+		sat |= mixer->get_saturation_status();
+	}
+
+	return sat;
+}
+
+void
+MixerGroup::groups_required(uint32_t &groups)
+{
+	for (auto mixer : _mixers) {
+		mixer->groups_required(groups);
+	}
+}
+
+int
+MixerGroup::load_from_buf(Mixer::ControlCallback control_cb, uintptr_t cb_handle, const char *buf, unsigned &buflen)
+{
+	int ret = -1;
+	const char *end = buf + buflen;
+
+	/*
+	 * Loop until either we have emptied the buffer, or we have failed to
+	 * allocate something when we expected to.
+	 */
+	while (buflen > 0) {
+		Mixer *m = nullptr;
+		const char *p = end - buflen;
+		unsigned resid = buflen;
+
+		/*
+		 * Use the next character as a hint to decide which mixer class to construct.
+		 */
+		switch (*p) {
+		case 'Z':
+			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;
+
+		case 'R':
+			m = MultirotorMixer::from_text(control_cb, cb_handle, p, resid);
+			break;
+
+		case 'H':
+			m = HelicopterMixer::from_text(control_cb, cb_handle, p, resid);
+			break;
+
+		default:
+			/* it's probably junk or whitespace, skip a byte and retry */
+			buflen--;
+			continue;
+		}
+
+		/*
+		 * If we constructed something, add it to the group.
+		 */
+		if (m != nullptr) {
+			add_mixer(m);
+
+			/* we constructed something */
+			ret = 0;
+
+			/* only adjust buflen if parsing was successful */
+			buflen = resid;
+			debug("SUCCESS - buflen: %d", buflen);
+
+		} else {
+
+			/*
+			 * There is data in the buffer that we expected to parse, but it didn't,
+			 * so give up for now.
+			 */
+			break;
+		}
+	}
+
+	/* nothing more in the buffer for us now */
+	return ret;
+}
+
+void MixerGroup::set_max_delta_out_once(float delta_out_max)
+{
+	for (auto mixer : _mixers) {
+		mixer->set_max_delta_out_once(delta_out_max);
+	}
+}
+
+void
+MixerGroup::set_dt_once(float dt)
+{
+	for (auto mixer : _mixers) {
+		mixer->set_dt_once(dt);
+	}
+}
@@ -0,0 +1,172 @@
+/****************************************************************************
+ *
+ *   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.
+ *
+ ****************************************************************************/
+
+#pragma once
+
+#include "MixerBase/Mixer.hpp"
+
+/**
+ * Group of mixers, built up from single mixers and processed
+ * in order when mixing.
+ */
+class MixerGroup
+{
+public:
+	MixerGroup() = default;
+
+	~MixerGroup()
+	{
+		reset();
+	}
+
+	// no copy, assignment, move, move assignment
+	MixerGroup(const MixerGroup &) = delete;
+	MixerGroup &operator=(const MixerGroup &) = delete;
+	MixerGroup(MixerGroup &&) = delete;
+	MixerGroup &operator=(MixerGroup &&) = delete;
+
+	unsigned			mix(float *outputs, unsigned space);
+
+	uint16_t			get_saturation_status();
+
+	void				groups_required(uint32_t &groups);
+
+	/**
+	 * Add a mixer to the group.
+	 *
+	 * @param mixer			The mixer to be added.
+	 */
+	void				add_mixer(Mixer *mixer) { _mixers.add(mixer); }
+
+	/**
+	 * Remove all the mixers from the group.
+	 */
+	void				reset() { _mixers.clear(); }
+
+	/**
+	 * Count the mixers in the group.
+	 */
+	unsigned			count() const { return _mixers.size(); }
+
+	/**
+	 * Adds mixers to the group based on a text description in a buffer.
+	 *
+	 * Mixer definitions begin with a single capital letter and a colon.
+	 * The actual format of the mixer definition varies with the individual
+	 * mixers; they are summarised here, but see ROMFS/mixers/README for
+	 * more details.
+	 *
+	 * Null Mixer
+	 * ..........
+	 *
+	 * The null mixer definition has the form:
+	 *
+	 *   Z:
+	 *
+	 * Simple Mixer
+	 * ............
+	 *
+	 * A simple mixer definition begins with:
+	 *
+	 *   M: <control count>
+	 *   O: <-ve scale> <+ve scale> <offset> <lower limit> <upper limit>
+	 *
+	 * The second line O: can be omitted. In that case 'O: 10000 10000 0 -10000 10000' is used.
+	 * The definition continues with <control count> entries describing the control
+	 * inputs and their scaling, in the form:
+	 *
+	 *   S: <group> <index> <-ve scale> <+ve scale> <offset> <lower limit> <upper limit>
+	 *
+	 * Multirotor Mixer
+	 * ................
+	 *
+	 * The multirotor mixer definition is a single line of the form:
+	 *
+	 * R: <geometry> <roll scale> <pitch scale> <yaw scale> <deadband>
+	 *
+	 * Helicopter Mixer
+	 * ................
+	 *
+	 * The helicopter mixer includes throttle and pitch curves
+	 *
+	 * H: <swash plate servo count>
+	 * T: <0> <2500> <5000> <7500> <10000>
+	 * P: <-10000> <-5000> <0> <5000> <10000>
+	 *
+	 * The definition continues with <swash plate servo count> entries describing
+	 * the position of the servo, in the following form:
+	 *
+	 *   S: <angle (deg)> <normalized arm length> <scale> <offset> <lower limit> <upper limit>
+	 *
+	 * @param buf			The mixer configuration buffer.
+	 * @param buflen		The length of the buffer, updated to reflect
+	 *				bytes as they are consumed.
+	 * @return			Zero on successful load, nonzero otherwise.
+	 */
+	int				load_from_buf(Mixer::ControlCallback control_cb, uintptr_t cb_handle, const char *buf, unsigned &buflen);
+
+	/**
+	 * @brief      Update slew rate parameter. This tells instances of the class MultirotorMixer
+	 *             the maximum allowed change of the output values per cycle.
+	 *             The value is only valid for one cycle, in order to have continuous
+	 *             slew rate limiting this function needs to be called before every call
+	 *             to mix().
+	 *
+	 * @param[in]  delta_out_max  Maximum delta output.
+	 *
+	 */
+	void 				set_max_delta_out_once(float delta_out_max);
+
+	/*
+	 * Invoke the set_offset method of each mixer in the group
+	 * for each value in page r_page_servo_control_trim
+	 */
+	unsigned			set_trims(int16_t *v, unsigned n);
+	unsigned			get_trims(int16_t *values);
+
+	/**
+	 * @brief      Sets the thrust factor used to calculate mapping from desired thrust to motor control signal output.
+	 *
+	 * @param[in]  val   The value
+	 */
+	void				set_thrust_factor(float val);
+
+	void				set_airmode(Mixer::Airmode airmode);
+
+	unsigned			get_multirotor_count();
+
+	void 				set_dt_once(float dt);
+
+private:
+	List<Mixer *>			_mixers;	/**< linked list of mixers */
+};
@@ -0,0 +1,114 @@
+############################################################################
+#
+#   Copyright (c) 2015 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.
+#
+############################################################################
+
+set(geometry_files
+	dodeca_bottom_cox.toml
+	dodeca_top_cox.toml
+	hex_cox.toml
+	hex_plus.toml
+	hex_t.toml
+	hex_x.toml
+	octa_cox.toml
+	octa_cox_wide.toml
+	octa_plus.toml
+	octa_x.toml
+	quad_deadcat.toml
+	quad_h.toml
+	quad_plus.toml
+	quad_s250aq.toml
+	quad_vtail.toml
+	quad_wide.toml
+	quad_x_cw.toml
+	quad_x.toml
+	quad_x_pusher.toml
+	quad_y.toml
+	tri_y.toml
+	twin_engine.toml
+)
+
+set(geometries_list)
+foreach(geom_file ${geometry_files})
+	list(APPEND geometries_list ${CMAKE_CURRENT_SOURCE_DIR}/geometries/${geom_file})
+endforeach()
+
+set(MIXER_TOOLS ${CMAKE_CURRENT_SOURCE_DIR}/geometries/tools)
+
+# generate mixers and normalize
+add_custom_command(
+	OUTPUT ${CMAKE_CURRENT_BINARY_DIR}/mixer_multirotor.generated.h
+	COMMAND ${PYTHON_EXECUTABLE} ${MIXER_TOOLS}/px_generate_mixers.py -f ${geometries_list} -o mixer_multirotor.generated.h
+	DEPENDS ${MIXER_TOOLS}/px_generate_mixers.py ${geometries_list}
+	)
+add_custom_command(
+	OUTPUT ${CMAKE_CURRENT_BINARY_DIR}/mixer_multirotor_normalized.generated.h
+	COMMAND ${PYTHON_EXECUTABLE} ${MIXER_TOOLS}/px_generate_mixers.py --normalize -f ${geometries_list} -o mixer_multirotor_normalized.generated.h
+	DEPENDS ${MIXER_TOOLS}/px_generate_mixers.py ${geometries_list}
+	)
+add_custom_target(mixer_gen DEPENDS mixer_multirotor.generated.h ${CMAKE_CURRENT_BINARY_DIR}/mixer_multirotor_normalized.generated.h)
+
+# 6DOF mixers
+add_custom_command(
+	OUTPUT ${CMAKE_CURRENT_BINARY_DIR}/mixer_multirotor_6dof.generated.h
+	COMMAND ${PYTHON_EXECUTABLE} ${MIXER_TOOLS}/px_generate_mixers.py --sixdof -f ${geometries_list} -o mixer_multirotor_6dof.generated.h
+	DEPENDS ${MIXER_TOOLS}/px_generate_mixers.py ${geometries_list}
+	)
+add_custom_target(mixer_gen_6dof DEPENDS mixer_multirotor_6dof.generated.h)
+
+add_library(MultirotorMixer
+	MultirotorMixer.cpp
+	MultirotorMixer.hpp
+
+	${CMAKE_CURRENT_BINARY_DIR}/mixer_multirotor.generated.h
+	${CMAKE_CURRENT_BINARY_DIR}/mixer_multirotor_normalized.generated.h
+	${CMAKE_CURRENT_BINARY_DIR}/mixer_multirotor_6dof.generated.h
+)
+target_link_libraries(MultirotorMixer PRIVATE MixerBase)
+target_include_directories(MultirotorMixer PRIVATE ${CMAKE_CURRENT_BINARY_DIR})
+add_dependencies(MultirotorMixer mixer_gen mixer_gen_6dof prebuild_targets)
+
+if(BUILD_TESTING)
+
+	add_executable(test_mixer_multirotor
+		test_mixer_multirotor.cpp
+		MultirotorMixer.cpp
+	)
+	target_compile_definitions(test_mixer_multirotor PRIVATE MIXER_MULTIROTOR_USE_MOCK_GEOMETRY)
+	target_compile_options(test_mixer_multirotor PRIVATE -Wno-unused-result)
+	target_link_libraries(test_mixer_multirotor PRIVATE MixerBase)
+
+	add_test(NAME mixer_multirotor
+		COMMAND ${PYTHON_EXECUTABLE} ${CMAKE_CURRENT_SOURCE_DIR}/mixer_multirotor.py --test --mixer-multirotor-binary $<TARGET_FILE:test_mixer_multirotor>
+		WORKING_DIRECTORY ${CMAKE_CURRENT_BINARY_DIR}
+	)
+
+endif()
@@ -0,0 +1,498 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2012-2018 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_multirotor.cpp
+ *
+ * Multi-rotor mixers.
+ */
+
+#include "MultirotorMixer.hpp"
+
+#include <float.h>
+#include <cstring>
+#include <cstdio>
+
+#include <mathlib/mathlib.h>
+
+#ifdef MIXER_MULTIROTOR_USE_MOCK_GEOMETRY
+enum class MultirotorGeometry : MultirotorGeometryUnderlyingType {
+	QUAD_X,
+	MAX_GEOMETRY
+};
+namespace
+{
+const MultirotorMixer::Rotor _config_quad_x[] = {
+	{ -0.707107,  0.707107,  1.000000,  1.000000 },
+	{  0.707107, -0.707107,  1.000000,  1.000000 },
+	{  0.707107,  0.707107, -1.000000,  1.000000 },
+	{ -0.707107, -0.707107, -1.000000,  1.000000 },
+};
+const MultirotorMixer::Rotor *_config_index[] = {
+	&_config_quad_x[0]
+};
+const unsigned _config_rotor_count[] = {4};
+const char *_config_key[] = {"4x"};
+}
+
+#else
+
+// This file is generated by the px_generate_mixers.py script which is invoked during the build process
+// #include "mixer_multirotor.generated.h"
+#include "mixer_multirotor_normalized.generated.h"
+
+#endif /* MIXER_MULTIROTOR_USE_MOCK_GEOMETRY */
+
+
+#define debug(fmt, args...)	do { } while(0)
+//#define debug(fmt, args...)	do { printf("[mixer] " fmt "\n", ##args); } while(0)
+//#include <debug.h>
+//#define debug(fmt, args...)	syslog(fmt "\n", ##args)
+
+MultirotorMixer::MultirotorMixer(ControlCallback control_cb, uintptr_t cb_handle, MultirotorGeometry geometry) :
+	MultirotorMixer(control_cb, cb_handle, _config_index[(int)geometry], _config_rotor_count[(int)geometry])
+{
+}
+
+MultirotorMixer::MultirotorMixer(ControlCallback control_cb, uintptr_t cb_handle, const Rotor *rotors,
+				 unsigned rotor_count) :
+	Mixer(control_cb, cb_handle),
+	_rotor_count(rotor_count),
+	_rotors(rotors),
+	_outputs_prev(new float[_rotor_count]),
+	_tmp_array(new float[_rotor_count])
+{
+	for (unsigned i = 0; i < _rotor_count; ++i) {
+		_outputs_prev[i] = -1.f;
+	}
+}
+
+MultirotorMixer::~MultirotorMixer()
+{
+	delete[] _outputs_prev;
+	delete[] _tmp_array;
+}
+
+MultirotorMixer *
+MultirotorMixer::from_text(Mixer::ControlCallback control_cb, uintptr_t cb_handle, const char *buf, unsigned &buflen)
+{
+	MultirotorGeometry geometry = MultirotorGeometry::MAX_GEOMETRY;
+	char geomname[16];
+
+	/* enforce that the mixer ends with a new line */
+	if (!string_well_formed(buf, buflen)) {
+		return nullptr;
+	}
+
+	if (sscanf(buf, "R: %15s", geomname) != 1) {
+		debug("multirotor parse failed on '%s'", buf);
+		return nullptr;
+	}
+
+	buf = skipline(buf, buflen);
+
+	if (buf == nullptr) {
+		debug("no line ending, line is incomplete");
+		return nullptr;
+	}
+
+	debug("remaining in buf: %d, first char: %c", buflen, buf[0]);
+
+	for (MultirotorGeometryUnderlyingType i = 0; i < (MultirotorGeometryUnderlyingType)MultirotorGeometry::MAX_GEOMETRY;
+	     i++) {
+		if (!strcmp(geomname, _config_key[i])) {
+			geometry = (MultirotorGeometry)i;
+			break;
+		}
+	}
+
+	if (geometry == MultirotorGeometry::MAX_GEOMETRY) {
+		debug("unrecognised geometry '%s'", geomname);
+		return nullptr;
+	}
+
+	debug("adding multirotor mixer '%s'", geomname);
+
+	return new MultirotorMixer(control_cb, cb_handle, geometry);
+}
+
+float
+MultirotorMixer::compute_desaturation_gain(const float *desaturation_vector, const float *outputs,
+		saturation_status &sat_status, float min_output, float max_output) const
+{
+	float k_min = 0.f;
+	float k_max = 0.f;
+
+	for (unsigned i = 0; i < _rotor_count; i++) {
+		// Avoid division by zero. If desaturation_vector[i] is zero, there's nothing we can do to unsaturate anyway
+		if (fabsf(desaturation_vector[i]) < FLT_EPSILON) {
+			continue;
+		}
+
+		if (outputs[i] < min_output) {
+			float k = (min_output - outputs[i]) / desaturation_vector[i];
+
+			if (k < k_min) { k_min = k; }
+
+			if (k > k_max) { k_max = k; }
+
+			sat_status.flags.motor_neg = true;
+		}
+
+		if (outputs[i] > max_output) {
+			float k = (max_output - outputs[i]) / desaturation_vector[i];
+
+			if (k < k_min) { k_min = k; }
+
+			if (k > k_max) { k_max = k; }
+
+			sat_status.flags.motor_pos = true;
+		}
+	}
+
+	// Reduce the saturation as much as possible
+	return k_min + k_max;
+}
+
+void
+MultirotorMixer::minimize_saturation(const float *desaturation_vector, float *outputs,
+				     saturation_status &sat_status, float min_output, float max_output, bool reduce_only) const
+{
+	float k1 = compute_desaturation_gain(desaturation_vector, outputs, sat_status, min_output, max_output);
+
+	if (reduce_only && k1 > 0.f) {
+		return;
+	}
+
+	for (unsigned i = 0; i < _rotor_count; i++) {
+		outputs[i] += k1 * desaturation_vector[i];
+	}
+
+	// Compute the desaturation gain again based on the updated outputs.
+	// In most cases it will be zero. It won't be if max(outputs) - min(outputs) > max_output - min_output.
+	// In that case adding 0.5 of the gain will equilibrate saturations.
+	float k2 = 0.5f * compute_desaturation_gain(desaturation_vector, outputs, sat_status, min_output, max_output);
+
+	for (unsigned i = 0; i < _rotor_count; i++) {
+		outputs[i] += k2 * desaturation_vector[i];
+	}
+}
+
+void
+MultirotorMixer::mix_airmode_rp(float roll, float pitch, float yaw, float thrust, float *outputs)
+{
+	// Airmode for roll and pitch, but not yaw
+
+	// Mix without yaw
+	for (unsigned i = 0; i < _rotor_count; i++) {
+		outputs[i] = roll * _rotors[i].roll_scale +
+			     pitch * _rotors[i].pitch_scale +
+			     thrust * _rotors[i].thrust_scale;
+
+		// Thrust will be used to unsaturate if needed
+		_tmp_array[i] = _rotors[i].thrust_scale;
+	}
+
+	minimize_saturation(_tmp_array, outputs, _saturation_status);
+
+	// Mix yaw independently
+	mix_yaw(yaw, outputs);
+}
+
+void
+MultirotorMixer::mix_airmode_rpy(float roll, float pitch, float yaw, float thrust, float *outputs)
+{
+	// Airmode for roll, pitch and yaw
+
+	// Do full mixing
+	for (unsigned i = 0; i < _rotor_count; i++) {
+		outputs[i] = roll * _rotors[i].roll_scale +
+			     pitch * _rotors[i].pitch_scale +
+			     yaw * _rotors[i].yaw_scale +
+			     thrust * _rotors[i].thrust_scale;
+
+		// Thrust will be used to unsaturate if needed
+		_tmp_array[i] = _rotors[i].thrust_scale;
+	}
+
+	minimize_saturation(_tmp_array, outputs, _saturation_status);
+
+	// Unsaturate yaw (in case upper and lower bounds are exceeded)
+	// to prioritize roll/pitch over yaw.
+	for (unsigned i = 0; i < _rotor_count; i++) {
+		_tmp_array[i] = _rotors[i].yaw_scale;
+	}
+
+	minimize_saturation(_tmp_array, outputs, _saturation_status);
+}
+
+void
+MultirotorMixer::mix_airmode_disabled(float roll, float pitch, float yaw, float thrust, float *outputs)
+{
+	// Airmode disabled: never allow to increase the thrust to unsaturate a motor
+
+	// Mix without yaw
+	for (unsigned i = 0; i < _rotor_count; i++) {
+		outputs[i] = roll * _rotors[i].roll_scale +
+			     pitch * _rotors[i].pitch_scale +
+			     thrust * _rotors[i].thrust_scale;
+
+		// Thrust will be used to unsaturate if needed
+		_tmp_array[i] = _rotors[i].thrust_scale;
+	}
+
+	// only reduce thrust
+	minimize_saturation(_tmp_array, outputs, _saturation_status, 0.f, 1.f, true);
+
+	// Reduce roll/pitch acceleration if needed to unsaturate
+	for (unsigned i = 0; i < _rotor_count; i++) {
+		_tmp_array[i] = _rotors[i].roll_scale;
+	}
+
+	minimize_saturation(_tmp_array, outputs, _saturation_status);
+
+	for (unsigned i = 0; i < _rotor_count; i++) {
+		_tmp_array[i] = _rotors[i].pitch_scale;
+	}
+
+	minimize_saturation(_tmp_array, outputs, _saturation_status);
+
+	// Mix yaw independently
+	mix_yaw(yaw, outputs);
+}
+
+void MultirotorMixer::mix_yaw(float yaw, float *outputs)
+{
+	// Add yaw to outputs
+	for (unsigned i = 0; i < _rotor_count; i++) {
+		outputs[i] += yaw * _rotors[i].yaw_scale;
+
+		// Yaw will be used to unsaturate if needed
+		_tmp_array[i] = _rotors[i].yaw_scale;
+	}
+
+	// Change yaw acceleration to unsaturate the outputs if needed (do not change roll/pitch),
+	// and allow some yaw response at maximum thrust
+	minimize_saturation(_tmp_array, outputs, _saturation_status, 0.f, 1.15f);
+
+	for (unsigned i = 0; i < _rotor_count; i++) {
+		_tmp_array[i] = _rotors[i].thrust_scale;
+	}
+
+	// reduce thrust only
+	minimize_saturation(_tmp_array, outputs, _saturation_status, 0.f, 1.f, true);
+}
+
+unsigned
+MultirotorMixer::mix(float *outputs, unsigned space)
+{
+	if (space < _rotor_count) {
+		return 0;
+	}
+
+	float roll    = math::constrain(get_control(0, 0), -1.0f, 1.0f);
+	float pitch   = math::constrain(get_control(0, 1), -1.0f, 1.0f);
+	float yaw     = math::constrain(get_control(0, 2), -1.0f, 1.0f);
+	float thrust  = math::constrain(get_control(0, 3), 0.0f, 1.0f);
+
+	// clean out class variable used to capture saturation
+	_saturation_status.value = 0;
+
+	// Do the mixing using the strategy given by the current Airmode configuration
+	switch (_airmode) {
+	case Airmode::roll_pitch:
+		mix_airmode_rp(roll, pitch, yaw, thrust, outputs);
+		break;
+
+	case Airmode::roll_pitch_yaw:
+		mix_airmode_rpy(roll, pitch, yaw, thrust, outputs);
+		break;
+
+	case Airmode::disabled:
+	default: // just in case: default to disabled
+		mix_airmode_disabled(roll, pitch, yaw, thrust, outputs);
+		break;
+	}
+
+	// Apply thrust model and scale outputs to range [idle_speed, 1].
+	// At this point the outputs are expected to be in [0, 1], but they can be outside, for example
+	// if a roll command exceeds the motor band limit.
+	for (unsigned i = 0; i < _rotor_count; i++) {
+		// Implement simple model for static relationship between applied motor pwm and motor thrust
+		// model: thrust = (1 - _thrust_factor) * PWM + _thrust_factor * PWM^2
+		if (_thrust_factor > 0.0f) {
+			outputs[i] = -(1.0f - _thrust_factor) / (2.0f * _thrust_factor) + sqrtf((1.0f - _thrust_factor) *
+					(1.0f - _thrust_factor) / (4.0f * _thrust_factor * _thrust_factor) + (outputs[i] < 0.0f ? 0.0f : outputs[i] /
+							_thrust_factor));
+		}
+
+		outputs[i] = math::constrain((2.f * outputs[i] - 1.f), -1.f, 1.f);
+	}
+
+	// Slew rate limiting and saturation checking
+	for (unsigned i = 0; i < _rotor_count; i++) {
+		bool clipping_high = false;
+		bool clipping_low_roll_pitch = false;
+		bool clipping_low_yaw = false;
+
+		// Check for saturation against static limits.
+		// We only check for low clipping if airmode is disabled (or yaw
+		// clipping if airmode==roll/pitch), since in all other cases thrust will
+		// be reduced or boosted and we can keep the integrators enabled, which
+		// leads to better tracking performance.
+		if (outputs[i] < -0.99f) {
+			if (_airmode == Airmode::disabled) {
+				clipping_low_roll_pitch = true;
+				clipping_low_yaw = true;
+
+			} else if (_airmode == Airmode::roll_pitch) {
+				clipping_low_yaw = true;
+			}
+		}
+
+		// check for saturation against slew rate limits
+		if (_delta_out_max > 0.0f) {
+			float delta_out = outputs[i] - _outputs_prev[i];
+
+			if (delta_out > _delta_out_max) {
+				outputs[i] = _outputs_prev[i] + _delta_out_max;
+				clipping_high = true;
+
+			} else if (delta_out < -_delta_out_max) {
+				outputs[i] = _outputs_prev[i] - _delta_out_max;
+				clipping_low_roll_pitch = true;
+				clipping_low_yaw = true;
+			}
+		}
+
+		_outputs_prev[i] = outputs[i];
+
+		// update the saturation status report
+		update_saturation_status(i, clipping_high, clipping_low_roll_pitch, clipping_low_yaw);
+	}
+
+	// this will force the caller of the mixer to always supply new slew rate values, otherwise no slew rate limiting will happen
+	_delta_out_max = 0.0f;
+
+	return _rotor_count;
+}
+
+/*
+ * This function update the control saturation status report using the following inputs:
+ *
+ * index: 0 based index identifying the motor that is saturating
+ * clipping_high: true if the motor demand is being limited in the positive direction
+ * clipping_low_roll_pitch: true if the motor demand is being limited in the negative direction (roll/pitch)
+ * clipping_low_yaw: true if the motor demand is being limited in the negative direction (yaw)
+*/
+void
+MultirotorMixer::update_saturation_status(unsigned index, bool clipping_high, bool clipping_low_roll_pitch,
+		bool clipping_low_yaw)
+{
+	// The motor is saturated at the upper limit
+	// check which control axes and which directions are contributing
+	if (clipping_high) {
+		if (_rotors[index].roll_scale > 0.0f) {
+			// A positive change in roll will increase saturation
+			_saturation_status.flags.roll_pos = true;
+
+		} else if (_rotors[index].roll_scale < 0.0f) {
+			// A negative change in roll will increase saturation
+			_saturation_status.flags.roll_neg = true;
+		}
+
+		// check if the pitch input is saturating
+		if (_rotors[index].pitch_scale > 0.0f) {
+			// A positive change in pitch will increase saturation
+			_saturation_status.flags.pitch_pos = true;
+
+		} else if (_rotors[index].pitch_scale < 0.0f) {
+			// A negative change in pitch will increase saturation
+			_saturation_status.flags.pitch_neg = true;
+		}
+
+		// check if the yaw input is saturating
+		if (_rotors[index].yaw_scale > 0.0f) {
+			// A positive change in yaw will increase saturation
+			_saturation_status.flags.yaw_pos = true;
+
+		} else if (_rotors[index].yaw_scale < 0.0f) {
+			// A negative change in yaw will increase saturation
+			_saturation_status.flags.yaw_neg = true;
+		}
+
+		// A positive change in thrust will increase saturation
+		_saturation_status.flags.thrust_pos = true;
+	}
+
+	// The motor is saturated at the lower limit
+	// check which control axes and which directions are contributing
+	if (clipping_low_roll_pitch) {
+		// check if the roll input is saturating
+		if (_rotors[index].roll_scale > 0.0f) {
+			// A negative change in roll will increase saturation
+			_saturation_status.flags.roll_neg = true;
+
+		} else if (_rotors[index].roll_scale < 0.0f) {
+			// A positive change in roll will increase saturation
+			_saturation_status.flags.roll_pos = true;
+		}
+
+		// check if the pitch input is saturating
+		if (_rotors[index].pitch_scale > 0.0f) {
+			// A negative change in pitch will increase saturation
+			_saturation_status.flags.pitch_neg = true;
+
+		} else if (_rotors[index].pitch_scale < 0.0f) {
+			// A positive change in pitch will increase saturation
+			_saturation_status.flags.pitch_pos = true;
+		}
+
+		// A negative change in thrust will increase saturation
+		_saturation_status.flags.thrust_neg = true;
+	}
+
+	if (clipping_low_yaw) {
+		// check if the yaw input is saturating
+		if (_rotors[index].yaw_scale > 0.0f) {
+			// A negative change in yaw will increase saturation
+			_saturation_status.flags.yaw_neg = true;
+
+		} else if (_rotors[index].yaw_scale < 0.0f) {
+			// A positive change in yaw will increase saturation
+			_saturation_status.flags.yaw_pos = true;
+		}
+	}
+
+	_saturation_status.flags.valid = true;
+}
@@ -0,0 +1,255 @@
+/****************************************************************************
+ *
+ *   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.
+ *
+ ****************************************************************************/
+
+#pragma once
+
+#include <mixer/MixerBase/Mixer.hpp>
+
+/**
+ * Supported multirotor geometries.
+ *
+ * Values are generated by the px_generate_mixers.py script and placed to mixer_multirotor_normalized.generated.h
+ */
+typedef uint8_t MultirotorGeometryUnderlyingType;
+enum class MultirotorGeometry : MultirotorGeometryUnderlyingType;
+
+/**
+ * Multi-rotor mixer for pre-defined vehicle geometries.
+ *
+ * Collects four inputs (roll, pitch, yaw, thrust) and mixes them to
+ * a set of outputs based on the configured geometry.
+ */
+class MultirotorMixer : public Mixer
+{
+public:
+	/**
+
+	 * Precalculated rotor mix.
+	 */
+	struct Rotor {
+		float	roll_scale;		/**< scales roll for this rotor */
+		float	pitch_scale;	/**< scales pitch for this rotor */
+		float	yaw_scale;		/**< scales yaw for this rotor */
+		float	thrust_scale;	/**< scales thrust for this rotor */
+	};
+
+	/**
+	 * Constructor.
+	 *
+	 * @param control_cb		Callback invoked to read inputs.
+	 * @param cb_handle		Passed to control_cb.
+	 * @param geometry		The selected geometry.
+	 * @param roll_scale		Scaling factor applied to roll inputs
+	 *				compared to thrust.
+	 * @param pitch_scale		Scaling factor applied to pitch inputs
+	 *				compared to thrust.
+	 * @param yaw_wcale		Scaling factor applied to yaw inputs compared
+	 *				to thrust.
+	 * @param idle_speed		Minimum rotor control output value; usually
+	 *				tuned to ensure that rotors never stall at the
+	 * 				low end of their control range.
+	 */
+	MultirotorMixer(ControlCallback control_cb, uintptr_t cb_handle, MultirotorGeometry geometry);
+
+	/**
+	 * Constructor (for testing).
+	 *
+	 * @param control_cb		Callback invoked to read inputs.
+	 * @param cb_handle		Passed to control_cb.
+	 * @param rotors		control allocation matrix
+	 * @param rotor_count		length of rotors array (= number of motors)
+	 */
+	MultirotorMixer(ControlCallback control_cb, uintptr_t cb_handle, const Rotor *rotors, unsigned rotor_count);
+	virtual ~MultirotorMixer();
+
+	// no copy, assignment, move, move assignment
+	MultirotorMixer(const MultirotorMixer &) = delete;
+	MultirotorMixer &operator=(const MultirotorMixer &) = delete;
+	MultirotorMixer(MultirotorMixer &&) = delete;
+	MultirotorMixer &operator=(MultirotorMixer &&) = delete;
+
+	/**
+	 * Factory method.
+	 *
+	 * 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 MultirotorMixer instance, or nullptr
+	 *				if the text format is bad.
+	 */
+	static MultirotorMixer *from_text(Mixer::ControlCallback control_cb, uintptr_t cb_handle, const char *buf,
+					  unsigned &buflen);
+
+	unsigned		mix(float *outputs, unsigned space) override;
+
+	uint16_t		get_saturation_status() override { return _saturation_status.value; }
+
+	void			groups_required(uint32_t &groups) override { groups |= (1 << 0); }
+
+	/**
+	 * @brief      Update slew rate parameter. This tells the multicopter mixer
+	 *             the maximum allowed change of the output values per cycle.
+	 *             The value is only valid for one cycle, in order to have continuous
+	 *             slew rate limiting this function needs to be called before every call
+	 *             to mix().
+	 *
+	 * @param[in]  delta_out_max  Maximum delta output.
+	 *
+	 */
+	void 			set_max_delta_out_once(float delta_out_max) override { _delta_out_max = delta_out_max; }
+
+	unsigned		set_trim(float trim) override { return _rotor_count; }
+	unsigned		get_trim(float *trim) override { return _rotor_count; }
+
+	/**
+	 * @brief      Sets the thrust factor used to calculate mapping from desired thrust to motor control signal output.
+	 *
+	 * @param[in]  val   The value
+	 */
+	void			set_thrust_factor(float val) override { _thrust_factor = math::constrain(val, 0.0f, 1.0f); }
+
+	void 			set_airmode(Airmode airmode) override { _airmode = airmode; }
+
+	unsigned		get_multirotor_count() override { return _rotor_count; }
+
+	union saturation_status {
+		struct {
+			uint16_t valid		: 1; // 0 - true when the saturation status is used
+			uint16_t motor_pos	: 1; // 1 - true when any motor has saturated in the positive direction
+			uint16_t motor_neg	: 1; // 2 - true when any motor has saturated in the negative direction
+			uint16_t roll_pos	: 1; // 3 - true when a positive roll demand change will increase saturation
+			uint16_t roll_neg	: 1; // 4 - true when a negative roll demand change will increase saturation
+			uint16_t pitch_pos	: 1; // 5 - true when a positive pitch demand change will increase saturation
+			uint16_t pitch_neg	: 1; // 6 - true when a negative pitch demand change will increase saturation
+			uint16_t yaw_pos	: 1; // 7 - true when a positive yaw demand change will increase saturation
+			uint16_t yaw_neg	: 1; // 8 - true when a negative yaw demand change will increase saturation
+			uint16_t thrust_pos	: 1; // 9 - true when a positive thrust demand change will increase saturation
+			uint16_t thrust_neg	: 1; //10 - true when a negative thrust demand change will increase saturation
+		} flags;
+		uint16_t value;
+	};
+
+private:
+	/**
+	 * Computes the gain k by which desaturation_vector has to be multiplied
+	 * in order to unsaturate the output that has the greatest saturation.
+	 * @see also minimize_saturation().
+	 *
+	 * @return desaturation gain
+	 */
+	float compute_desaturation_gain(const float *desaturation_vector, const float *outputs, saturation_status &sat_status,
+					float min_output, float max_output) const;
+
+	/**
+	 * Minimize the saturation of the actuators by adding or substracting a fraction of desaturation_vector.
+	 * desaturation_vector is the vector that added to the output outputs, modifies the thrust or angular
+	 * acceleration on a specific axis.
+	 * For example, if desaturation_vector is given to slide along the vertical thrust axis (thrust_scale), the
+	 * saturation will be minimized by shifting the vertical thrust setpoint, without changing the
+	 * roll/pitch/yaw accelerations.
+	 *
+	 * Note that as we only slide along the given axis, in extreme cases outputs can still contain values
+	 * outside of [min_output, max_output].
+	 *
+	 * @param desaturation_vector vector that is added to the outputs, e.g. thrust_scale
+	 * @param outputs output vector that is modified
+	 * @param sat_status saturation status output
+	 * @param min_output minimum desired value in outputs
+	 * @param max_output maximum desired value in outputs
+	 * @param reduce_only if true, only allow to reduce (substract) a fraction of desaturation_vector
+	 */
+	void minimize_saturation(const float *desaturation_vector, float *outputs, saturation_status &sat_status,
+				 float min_output = 0.f, float max_output = 1.f, bool reduce_only = false) const;
+
+	/**
+	 * Mix roll, pitch, yaw, thrust and set the outputs vector.
+	 *
+	 * Desaturation behavior: airmode for roll/pitch:
+	 * thrust is increased/decreased as much as required to meet the demanded roll/pitch.
+	 * Yaw is not allowed to increase the thrust, @see mix_yaw() for the exact behavior.
+	 */
+	inline void mix_airmode_rp(float roll, float pitch, float yaw, float thrust, float *outputs);
+
+	/**
+	 * Mix roll, pitch, yaw, thrust and set the outputs vector.
+	 *
+	 * Desaturation behavior: full airmode for roll/pitch/yaw:
+	 * thrust is increased/decreased as much as required to meet demanded the roll/pitch/yaw,
+	 * while giving priority to roll and pitch over yaw.
+	 */
+	inline void mix_airmode_rpy(float roll, float pitch, float yaw, float thrust, float *outputs);
+
+	/**
+	 * Mix roll, pitch, yaw, thrust and set the outputs vector.
+	 *
+	 * Desaturation behavior: no airmode, thrust is NEVER increased to meet the demanded
+	 * roll/pitch/yaw. Instead roll/pitch/yaw is reduced as much as needed.
+	 * Thrust can be reduced to unsaturate the upper side.
+	 * @see mix_yaw() for the exact yaw behavior.
+	 */
+	inline void mix_airmode_disabled(float roll, float pitch, float yaw, float thrust, float *outputs);
+
+	/**
+	 * Mix yaw by updating an existing output vector (that already contains roll/pitch/thrust).
+	 *
+	 * Desaturation behavior: thrust is allowed to be decreased up to 15% in order to allow
+	 * some yaw control on the upper end. On the lower end thrust will never be increased,
+	 * but yaw is decreased as much as required.
+	 *
+	 * @param yaw demanded yaw
+	 * @param outputs output vector that is updated
+	 */
+	inline void mix_yaw(float yaw, float *outputs);
+
+	void update_saturation_status(unsigned index, bool clipping_high, bool clipping_low_roll_pitch, bool clipping_low_yaw);
+
+	float 				_delta_out_max{0.0f};
+	float 				_thrust_factor{0.0f};
+
+	Airmode				_airmode{Airmode::disabled};
+
+	saturation_status		_saturation_status{};
+
+	unsigned			_rotor_count;
+	const Rotor			*_rotors;
+
+	float 				*_outputs_prev{nullptr};
+	float 				*_tmp_array{nullptr};
+};
@@ -0,0 +1,40 @@
+# Generic Dodecacopter in X coax configuration, bottom half
+
+[info]
+key = "6a"
+description = "Generic Dodecacopter in X coax configuration, bottom half"
+
+[rotor_default]
+axis      = [0.0, 0.0, -1.0]
+Ct        = 1.0
+Cm        = 0.05
+
+[[rotors]]
+name      = "mid_right_bottom"
+position  = [0.0, 1.0, 0.1]
+direction = "CCW"
+
+[[rotors]]
+name      = "mid_left_bottom"
+position  = [0.0, -1.0, 0.1]
+direction = "CW"
+
+[[rotors]]
+name      = "front_left_bottom"
+position  = [0.866025, -0.5, 0.1]
+direction = "CCW"
+
+[[rotors]]
+name      = "rear_right_bottom"
+position  = [-0.866025, 0.5, 0.1]
+direction = "CW"
+
+[[rotors]]
+name      = "front_right_bottom"
+position  = [0.866025, 0.5, 0.1]
+direction = "CW"
+
+[[rotors]]
+name      = "rear_left_bottom"
+position  = [-0.866025, -0.5, 0.1]
+direction = "CCW"
@@ -0,0 +1,40 @@
+# Generic Dodecacopter in X coax configuration, top half
+
+[info]
+key = "6m"
+description = "Generic Dodecacopter in X coax configuration, top half"
+
+[rotor_default]
+axis      = [0.0, 0.0, -1.0]
+Ct        = 1.0
+Cm        = 0.05
+
+[[rotors]]
+name      = "mid_right_top"
+position  = [0.0, 1.0, -0.1]
+direction = "CW"
+
+[[rotors]]
+name      = "mid_left_top"
+position  = [0.0, -1.0, -0.1]
+direction = "CCW"
+
+[[rotors]]
+name      = "front_left_top"
+position  = [0.866025, -0.5, -0.1]
+direction = "CW"
+
+[[rotors]]
+name      = "rear_right_top"
+position  = [-0.866025, 0.5, -0.1]
+direction = "CCW"
+
+[[rotors]]
+name      = "front_right_top"
+position  = [0.866025, 0.5, -0.1]
+direction = "CCW"
+
+[[rotors]]
+name      = "rear_left_top"
+position  = [-0.866025, -0.5, -0.1]
+direction = "CW"
@@ -0,0 +1,40 @@
+# Generic Hexacopter in coaxial configuration
+
+[info]
+key = "6c"
+description = "Generic Hexacopter in coaxial configuration"
+
+[rotor_default]
+axis      = [0.0, 0.0, -1.0]
+Ct        = 1.0
+Cm        = 0.05
+
+[[rotors]]
+name      = "front_right_top"
+position  = [0.5, 0.866, -0.1]
+direction = "CW"
+
+[[rotors]]
+name      = "front_right_bottom"
+position  = [0.5, 0.866, 0.1]
+direction = "CCW"
+
+[[rotors]]
+name      = "rear_top"
+position  = [-1.0, 0.0, -0.1]
+direction = "CW"
+
+[[rotors]]
+name      = "rear_bottom"
+position  = [-1.0, 0.0, 0.1]
+direction = "CCW"
+
+[[rotors]]
+name      = "front_left_top"
+position  = [0.5, -0.866, -0.1]
+direction = "CW"
+
+[[rotors]]
+name      = "front_left_bottom"
+position  = [0.5, -0.866, 0.1]
+direction = "CCW"
@@ -0,0 +1,40 @@
+# Generic Hexacopter in + configuration
+
+[info]
+key = "6+"
+description = "Generic Hexacopter in + configuration"
+
+[rotor_default]
+axis      = [0.0, 0.0, -1.0]
+Ct        = 1.0
+Cm        = 0.05
+
+[[rotors]]
+name      = "front"
+position  = [1.0, 0.0, 0.0]
+direction = "CW"
+
+[[rotors]]
+name      = "rear"
+position  = [-1.0, 0.0, 0.0]
+direction = "CCW"
+
+[[rotors]]
+name      = "rear_left"
+position  = [-0.5, -0.866025, 0.0]
+direction = "CW"
+
+[[rotors]]
+name      = "front_right"
+position  = [0.5, 0.866025, 0.0]
+direction = "CCW"
+
+[[rotors]]
+name      = "front_left"
+position  = [0.5, -0.866025, 0.0]
+direction = "CCW"
+
+[[rotors]]
+name      = "rear_right"
+position  = [-0.5, 0.866025, 0.0]
+direction = "CW"
@@ -0,0 +1,40 @@
+# Generic Hexacopter in T configuration
+
+[info]
+key = "6t"
+description = "Generic Hexacopter in T configuration"
+
+[rotor_default]
+axis      = [0.0, 0.0, -1.0]
+Ct        = 1.0
+Cm        = 0.05
+
+[[rotors]]
+name      = "front_right_top"
+position  = [0.729, 0.684, 0.1]
+direction = "CW"
+
+[[rotors]]
+name      = "front_right_bottom"
+position  = [0.729, 0.684, -0.1]
+direction = "CCW"
+
+[[rotors]]
+name      = "rear_top"
+position  = [-1.0, 0.0, 0.1]
+direction = "CW"
+
+[[rotors]]
+name      = "rear_bottom"
+position  = [-1.0, 0.0, -0.1]
+direction = "CCW"
+
+[[rotors]]
+name      = "front_left_top"
+position  = [0.729, -0.684, 0.1]
+direction = "CW"
+
+[[rotors]]
+name      = "front_left_bottom"
+position  = [0.729, -0.684, -0.1]
+direction = "CCW"
@@ -0,0 +1,40 @@
+# Generic Hexacopter in X configuration
+
+[info]
+key = "6x"
+description = "Generic Hexacopter in X configuration"
+
+[rotor_default]
+axis      = [0.0, 0.0, -1.0]
+Ct        = 1.0
+Cm        = 0.05
+
+[[rotors]]
+name      = "mid_right"
+position  = [0.0, 1.0, 0.0]
+direction = "CW"
+
+[[rotors]]
+name      = "mid_left"
+position  = [0.0, -1.0, 0.0]
+direction = "CCW"
+
+[[rotors]]
+name      = "front_left"
+position  = [0.866025, -0.5, 0.0]
+direction = "CW"
+
+[[rotors]]
+name      = "rear_right"
+position  = [-0.866025, 0.5, 0.0]
+direction = "CCW"
+
+[[rotors]]
+name      = "front_right"
+position  = [0.866025, 0.5, 0.0]
+direction = "CCW"
+
+[[rotors]]
+name      = "rear_left"
+position  = [-0.866025, -0.5, 0.0]
+direction = "CW"
@@ -0,0 +1,50 @@
+# Generic Octacopter in coax configuration
+
+[info]
+key = "8c"
+description = "GenericOctacopter in coax configuration"
+
+[rotor_default]
+axis      = [0.0, 0.0, -1.0]
+Ct        = 1.0
+Cm        = 0.05
+
+[[rotors]]
+name      = "front_right_top"
+position  = [0.707107, 0.707107, -0.1]
+direction = "CCW"
+
+[[rotors]]
+name     = "front_left_top"
+position = [0.707107, -0.707107, -0.1]
+direction = "CW"
+
+[[rotors]]
+name      = "rear_left_top"
+position  = [-0.707107, -0.707107, -0.1]
+direction = "CCW"
+
+[[rotors]]
+name     = "rear_right_top"
+position = [-0.707107, 0.707107, -0.1]
+direction = "CW"
+
+[[rotors]]
+name     = "front_left_bottom"
+position = [0.707107, -0.707107, 0.1]
+direction = "CCW"
+
+[[rotors]]
+name      = "front_right_bottom"
+position  = [0.707107, 0.707107, 0.1]
+direction = "CW"
+
+[[rotors]]
+name     = "rear_right_bottom"
+position = [-0.707107, 0.707107, 0.1]
+direction = "CCW"
+
+[[rotors]]
+name      = "rear_left_bottom"
+position  = [-0.707107, -0.707107, 0.1]
+direction = "CW"
@@ -0,0 +1,50 @@
+# Generic Octacopter in wide coax configuration
+
+[info]
+key = "8cw"
+description = "Generic Octacopter in wide coax configuration"
+
+[rotor_default]
+axis      = [0.0, 0.0, -1.0]
+Ct        = 1.0
+Cm        = 0.05
+
+[[rotors]]
+name      = "front_right_top"
+position  = [0.3746066, 0.927184, -0.1]
+direction = "CCW"
+
+[[rotors]]
+name     = "front_left_top"
+position = [0.3746066, -0.927184, -0.1]
+direction = "CW"
+
+[[rotors]]
+name      = "rear_left_top"
+position  = [-0.62932, -0.777146, -0.1]
+direction = "CCW"
+
+[[rotors]]
+name     = "rear_right_top"
+position = [-0.62932, 0.777146, -0.1]
+direction = "CW"
+
+[[rotors]]
+name     = "front_left_bottom"
+position = [0.3746066, -0.927184, 0.1]
+direction = "CCW"
+
+[[rotors]]
+name      = "front_right_bottom"
+position  = [0.3746066, 0.927184, 0.1]
+direction = "CW"
+
+[[rotors]]
+name     = "rear_right_bottom"
+position = [-0.62932, 0.777146, 0.1]
+direction = "CCW"
+
+[[rotors]]
+name      = "rear_left_bottom"
+position  = [-0.62932, -0.777146, 0.1]
+direction = "CW"
@@ -0,0 +1,50 @@
+# Generic Octacopter in + configuration
+
+[info]
+key = "8+"
+description = "Generic Octacopter in + configuration"
+
+[rotor_default]
+axis      = [0.0, 0.0, -1.0]
+Ct        = 1.0
+Cm        = 0.05
+
+[[rotors]]
+name      = "front"
+position  = [1.0, 0.0, 0.0]
+direction = "CW"
+
+[[rotors]]
+name      = "rear"
+position  = [-1.0, 0.0, 0.0]
+direction = "CW"
+
+[[rotors]]
+name      = "front_right"
+position  = [0.707107, 0.707107, 0.0]
+direction = "CCW"
+
+[[rotors]]
+name      = "rear_right"
+position  = [-0.707107, 0.707107, 0.0]
+direction = "CCW"
+
+[[rotors]]
+name      = "front_left"
+position  = [0.707107, -0.707107, 0.0]
+direction = "CCW"
+
+[[rotors]]
+name      = "rear_left"
+position  = [-0.707107, -0.707107, 0.0]
+direction = "CCW"
+
+[[rotors]]
+name      = "left"
+position  = [0.0, -1.0, 0.0]
+direction = "CW"
+
+[[rotors]]
+name      = "right"
+position  = [0.0, 1.0, 0.0]
+direction = "CW"
@@ -0,0 +1,50 @@
+# Generic Octacopter in X configuration
+
+[info]
+key = "8x"
+description = "Generic Octacopter in X configuration"
+
+[rotor_default]
+axis      = [0.0, 0.0, -1.0]
+Ct        = 1.0
+Cm        = 0.05
+
+[[rotors]]
+name      = "front_right"
+position  = [0.9238795, 0.3826834, 0.0]
+direction = "CW"
+
+[[rotors]]
+name      = "rear_left"
+position  = [-0.9238795, -0.3826834, 0.0]
+direction = "CW"
+
+[[rotors]]
+name      = "mid_front_right"
+position  = [0.3826834, 0.9238795, 0.0]
+direction = "CCW"
+
+[[rotors]]
+name      = "rear_right"
+position  = [-0.9238795, 0.3826834, 0.0]
+direction = "CCW"
+
+[[rotors]]
+name      = "front_left"
+position  = [0.9238795, -0.3826834, 0.0]
+direction = "CCW"
+
+[[rotors]]
+name      = "mid_rear_left"
+position  = [-0.3826834, -0.9238795, 0.0]
+direction = "CCW"
+
+[[rotors]]
+name      = "mid_front_left"
+position  = [0.3826834, -0.9238795, 0.0]
+direction = "CW"
+
+[[rotors]]
+name      = "mid_rear_right"
+position  = [-0.3826834, 0.9238795, 0.0]
+direction = "CW"
@@ -0,0 +1,30 @@
+# SK450 DeadCat Quadcopter.
+# Same geometry as quad_wide, except CG is located at intersection of rear arms, so front motors are more loaded.
+
+[info]
+key = "4dc"
+description = "SK450 DeadCat Quadcopter, CG at intersection of rear arms"
+
+[rotor_default]
+direction = "CW"
+axis      = [0.0, 0.0, -1.0]
+Ct        = 1.0
+Cm        = 0.01
+
+[[rotors]]
+name      = "front_right"
+position  = [0.1155, 0.245, 0.0]
+direction = "CCW"
+
+[[rotors]]
+name      = "rear_left"
+position  = [-0.1875, -0.1875, 0.0]
+direction = "CCW"
+
+[[rotors]]
+name     = "front_left"
+position = [0.1155, -0.245, 0.0]
+
+[[rotors]]
+name     = "rear_right"
+position = [-0.1875, 0.1875, 0.0]
@@ -0,0 +1,30 @@
+# Generic Quadcopter in H configuration
+
+[info]
+key = "4h"
+description = "Generic Quadcopter in H configuration"
+
+[rotor_default]
+axis      = [0.0, 0.0, -1.0]
+Ct        = 1.0
+Cm        = 0.05
+
+[[rotors]]
+name      = "front_right"
+position  = [0.707107, 0.707107, 0.0]
+direction = "CW"
+
+[[rotors]]
+name      = "rear_left"
+position  = [-0.707107, -0.707107, 0.0]
+direction = "CW"
+
+[[rotors]]
+name     = "front_left"
+position = [0.707107, -0.707107, 0.0]
+direction = "CCW"
+
+[[rotors]]
+name     = "rear_right"
+position = [-0.707107, 0.707107, 0.0]
+direction = "CCW"
@@ -0,0 +1,29 @@
+# Generic Quadcopter in + configuration
+
+[info]
+key = "4+"
+description = "Generic Quadcopter in + configuration"
+
+[rotor_default]
+direction = "CW"
+axis      = [0.0, 0.0, -1.0]
+Ct        = 1.0
+Cm        = 0.05
+
+[[rotors]]
+name      = "right"
+position  = [0.0, 1.0, 0.0]
+direction = "CCW"
+
+[[rotors]]
+name      = "left"
+position  = [0.0, -1.0, 0.0]
+direction = "CCW"
+
+[[rotors]]
+name     = "front"
+position = [1.0, 0.0, 0.0]
+
+[[rotors]]
+name     = "rear"
+position = [-1.0, 0.0, 0.0]
@@ -0,0 +1,30 @@
+# Spedix S250AQ Quadcopter
+# Note: Rotor positions computed from specifications: length:210mm, width:260mm, wheel base:280mm
+
+[info]
+key = "4s"
+description = "Spedix S250AQ Quadcopter"
+
+[rotor_default]
+direction = "CW"
+axis      = [0.0, 0.0, -1.0]
+Ct        = 1.0
+Cm        = 0.01
+
+[[rotors]]
+name      = "front_right"
+position  = [0.105, 0.130, 0.0]
+direction = "CCW"
+
+[[rotors]]
+name      = "rear_left"
+position  = [-0.105, -0.0552, 0.0]
+direction = "CCW"
+
+[[rotors]]
+name     = "front_left"
+position = [0.105, -0.130, 0.0]
+
+[[rotors]]
+name     = "rear_right"
+position = [-0.105, 0.0552, 0.0]
@@ -0,0 +1,33 @@
+# Quadcopter in Y configuration with rear props tilted at 45 degrees
+
+[info]
+key = "4vt"
+description = "Quadcopter in Y configuration with rear props tilted at 45 degrees"
+
+[rotor_default]
+direction = "CW"
+axis      = [0.0, 0.0, -1.0]
+Ct        = 1.0
+Cm        = 0.05
+
+[[rotors]]
+name      = "front_right"
+position  = [0.2, 0.2, 0.0]
+direction = "CW"
+
+[[rotors]]
+name      = "rear_left"
+position  = [-0.3, -0.1, -0.1]
+axis      = [0.0, 0.707106, -0.707106]
+direction = "CW"
+
+[[rotors]]
+name     = "front_left"
+position = [0.2, -0.2, 0.0]
+direction = "CCW"
+
+[[rotors]]
+name     = "rear_right"
+position = [-0.3, 0.1, -0.1]
+axis      = [0.0, -0.707106, -0.707106]
+direction = "CCW"
@@ -0,0 +1,30 @@
+# Generic Quadcopter in wide configuration
+
+[info]
+key = "4w"
+description = "Quadcopter in wide configuration. Same geometry as SK450 Deadcat except the CG is moved backward to load all motors equally"
+
+[rotor_default]
+axis      = [0.0, 0.0, -1.0]
+Ct        = 1.0
+Cm        = 0.05
+
+[[rotors]]
+name      = "front_right"
+position  = [0.1515, 0.245, 0.0]
+direction = "CCW"
+
+[[rotors]]
+name      = "rear_left"
+position  = [-0.1515, -0.1875, 0.0]
+direction = "CCW"
+
+[[rotors]]
+name     = "front_left"
+position = [0.1515, -0.245, 0.0]
+direction = "CW"
+
+[[rotors]]
+name     = "rear_right"
+position = [-0.1515, 0.1875, 0.0]
+direction = "CW"
@@ -0,0 +1,29 @@
+# Generic Quadcopter in X configuration
+
+[info]
+key = "4x"
+description = "Generic Quadcopter in X configuration"
+
+[rotor_default]
+direction = "CW"
+axis      = [0.0, 0.0, -1.0]
+Ct        = 1.0
+Cm        = 0.05
+
+[[rotors]]
+name      = "front_right"
+position  = [0.707107, 0.707107, 0.0]
+direction = "CCW"
+
+[[rotors]]
+name      = "rear_left"
+position  = [-0.707107, -0.707107, 0.0]
+direction = "CCW"
+
+[[rotors]]
+name     = "front_left"
+position = [0.707107, -0.707107, 0.0]
+
+[[rotors]]
+name     = "rear_right"
+position = [-0.707107, 0.707107, 0.0]
@@ -0,0 +1,30 @@
+# Generic Quadcopter in X configuration
+# with clock-wise motor numbering
+
+[info]
+key = "4xcw"
+description =  "Quadcopter in X configuration with clock-wise motor numbering"
+
+[rotor_default]
+direction = "CW"
+axis      = [0.0, 0.0, -1.0]
+Ct        = 1.0
+Cm        = 0.05
+
+[[rotors]]
+name      = "front_right"
+position  = [0.707107, 0.707107, 0.0]
+direction = "CCW"
+
+[[rotors]]
+name     = "rear_right"
+position = [-0.707107, 0.707107, 0.0]
+
+[[rotors]]
+name      = "rear_left"
+position  = [-0.707107, -0.707107, 0.0]
+direction = "CCW"
+
+[[rotors]]
+name     = "front_left"
+position = [0.707107, -0.707107, 0.0]
@@ -0,0 +1,37 @@
+# Quadcopter in X configuration,
+# with added pusher motor in the back
+
+[info]
+key = "4x1p"
+description = "Quadcopter in X configuration, with added pusher motor in the back"
+
+[rotor_default]
+direction = "CW"
+axis      = [0.0, 0.0, -1.0]
+Ct        = 1.0
+Cm        = 0.05
+
+[[rotors]]
+name      = "front_right"
+position  = [1.0, 1.0, 0.0]
+direction = "CCW"
+
+[[rotors]]
+name      = "rear_left"
+position  = [-1.0, -1.0, 0.0]
+direction = "CCW"
+
+[[rotors]]
+name     = "front_left"
+position = [1.0, -1.0, 0.0]
+
+[[rotors]]
+name     = "rear_right"
+position = [-1.0, 1.0, 0.0]
+
+[[rotors]]
+name     = "pusher"
+position = [-1.0, 0.0, 0.0]
+axis     = [1.0, 0.0, 0.0]
+Ct       = 2.0
+Cm       = 0.0
@@ -0,0 +1,31 @@
+# Quadcopter in Y configuration with coax rear props
+
+[info]
+key = "4y"
+description = "Quadcopter in Y configuration with coax rear props"
+
+[rotor_default]
+direction = "CW"
+axis      = [0.0, 0.0, -1.0]
+Ct        = 1.0
+Cm        = 0.05
+
+[[rotors]]
+name      = "front_right"
+position  = [0.2, 0.2, 0.0]
+direction = "CCW"
+
+[[rotors]]
+name      = "rear_top"
+position  = [-0.2, 0.0, -0.1]
+direction = "CCW"
+
+[[rotors]]
+name     = "front_left"
+position = [0.2, -0.2, 0.0]
+direction = "CW"
+
+[[rotors]]
+name     = "rear_bottom"
+position = [-0.2, 0.0, 0.1]
+direction = "CW"
@@ -0,0 +1,398 @@
+#!/usr/bin/env python
+#############################################################################
+#
+#   Copyright (C) 2013-2016 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.
+#
+#############################################################################
+
+"""
+px_generate_mixers.py
+Generates c/cpp header/source files for multirotor mixers
+from geometry descriptions files (.toml format)
+"""
+import sys
+
+try:
+    import toml
+except ImportError as e:
+    print("Failed to import toml: " + str(e))
+    print("")
+    print("You may need to install it using:")
+    print("    pip3 install --user toml")
+    print("")
+    sys.exit(1)
+
+try:
+    import numpy as np
+except ImportError as e:
+    print("Failed to import numpy: " + str(e))
+    print("")
+    print("You may need to install it using:")
+    print("    pip3 install --user numpy")
+    print("")
+    sys.exit(1)
+
+__author__ = "Julien Lecoeur"
+__copyright__ = "Copyright (C) 2013-2017 PX4 Development Team."
+__license__ = "BSD"
+__email__ = "julien.lecoeur@gmail.com"
+
+
+def parse_geometry_toml(filename):
+    '''
+    Parses toml geometry file and returns a dictionary with curated list of rotors
+    '''
+    import os
+
+    # Load toml file
+    d = toml.load(filename)
+
+    # Check info section
+    if 'info' not in d:
+        raise AttributeError('{}: Error, missing info section'.format(filename))
+
+    # Check info section
+    for field in ['key', 'description']:
+        if field not in d['info']:
+            raise AttributeError('{}: Error, unspecified info field "{}"'.format(filename, field))
+
+    # Use filename as mixer name
+    d['info']['name'] = os.path.basename(filename).split('.')[0].lower()
+
+    # Store filename
+    d['info']['filename'] = filename
+
+    # Check default rotor config
+    if 'rotor_default' in d:
+        default = d['rotor_default']
+    else:
+        default = {}
+
+    # Convert rotors
+    rotor_list = []
+    if 'rotors' in d:
+        for r in d['rotors']:
+            # Make sure all fields are defined, fill missing with default
+            for field in ['name', 'position', 'axis', 'direction', 'Ct', 'Cm']:
+                if field not in r:
+                    if field in default:
+                        r[field] = default[field]
+                    else:
+                        raise AttributeError('{}: Error, unspecified field "{}" for rotor "{}"'
+                                             .format(filename, field, r['name']))
+
+            # Check direction field
+            r['direction'] = r['direction'].upper()
+            if r['direction'] not in ['CW', 'CCW']:
+                raise AttributeError('{}: Error, invalid direction value "{}" for rotor "{}"'
+                                     .format(filename, r['direction'], r['name']))
+
+            # Check vector3 fields
+            for field in ['position', 'axis']:
+                if len(r[field]) != 3:
+                    raise AttributeError('{}: Error, field "{}" for rotor "{}"'
+                                         .format(filename, field, r['name']) +
+                                         ' must be an array of length 3')
+
+            # Add rotor to list
+            rotor_list.append(r)
+
+    # Clean dictionary
+    geometry = {'info': d['info'],
+            'rotors': rotor_list}
+
+    return geometry
+
+def torque_matrix(center, axis, dirs, Ct, Cm):
+    '''
+    Compute torque generated by rotors
+    '''
+    # normalize rotor axis
+    ax = axis / np.linalg.norm(axis, axis=1)[:, np.newaxis]
+    torque = Ct * np.cross(center, ax) - Cm * ax * dirs
+    return torque
+
+def geometry_to_torque_matrix(geometry):
+    '''
+    Compute torque matrix Am and Bm from geometry dictionnary
+    Am is a 3xN matrix where N is the number of rotors
+    Each column is the torque generated by one rotor
+    '''
+    Am = torque_matrix(center=np.array([rotor['position'] for rotor in geometry['rotors']]),
+                       axis=np.array([rotor['axis'] for rotor in geometry['rotors']]),
+                       dirs=np.array([[1.0 if rotor['direction'] == 'CCW' else -1.0]
+                                      for rotor in geometry['rotors']]),
+                       Ct=np.array([[rotor['Ct']] for rotor in geometry['rotors']]),
+                       Cm=np.array([[rotor['Cm']] for rotor in geometry['rotors']])).T
+    return Am
+
+def thrust_matrix(axis, Ct):
+    '''
+    Compute thrust generated by rotors
+    '''
+    # Normalize rotor axis
+    ax = axis / np.linalg.norm(axis, axis=1)[:, np.newaxis]
+    thrust = Ct * ax
+    return thrust
+
+def geometry_to_thrust_matrix(geometry):
+    '''
+    Compute thrust matrix At from geometry dictionnary
+    At is a 3xN matrix where N is the number of rotors
+    Each column is the thrust generated by one rotor
+    '''
+    At = thrust_matrix(axis=np.array([rotor['axis'] for rotor in geometry['rotors']]),
+                       Ct=np.array([[rotor['Ct']] for rotor in geometry['rotors']])).T
+
+    return At
+
+def geometry_to_mix(geometry):
+    '''
+    Compute combined torque & thrust matrix A and mix matrix B from geometry dictionnary
+
+    A is a 6xN matrix where N is the number of rotors
+    Each column is the torque and thrust generated by one rotor
+
+    B is a Nx6 matrix where N is the number of rotors
+    Each column is the command to apply to the servos to get
+    roll torque, pitch torque, yaw torque, x thrust, y thrust, z thrust
+    '''
+    # Combined torque & thrust matrix
+    At = geometry_to_thrust_matrix(geometry)
+    Am = geometry_to_torque_matrix(geometry)
+    A = np.vstack([Am, At])
+
+    # Mix matrix computed as pseudoinverse of A
+    B = np.linalg.pinv(A)
+
+    return A, B
+
+def normalize_mix_px4(B):
+    '''
+    Normalize mix for PX4
+    This is for compatibility only and should ideally not be used
+    '''
+    B_norm = np.linalg.norm(B, axis=0)
+    B_max = np.abs(B).max(axis=0)
+    B_sum = np.sum(B, axis=0)
+
+    # Same scale on roll and pitch
+    B_norm[0] = max(B_norm[0], B_norm[1]) / np.sqrt(B.shape[0] / 2.0)
+    B_norm[1] = B_norm[0]
+
+    # Scale yaw separately
+    B_norm[2] = B_max[2]
+
+    # Same scale on x, y
+    B_norm[3] = max(B_max[3], B_max[4])
+    B_norm[4] = B_norm[3]
+
+    # Scale z thrust separately
+    B_norm[5] = - B_sum[5] / np.count_nonzero(B[:,5])
+
+    # Normalize
+    B_norm[np.abs(B_norm) < 1e-3] = 1
+    B_px = (B / B_norm)
+
+    return B_px
+
+def generate_mixer_multirotor_header(geometries_list, use_normalized_mix=False, use_6dof=False):
+    '''
+    Generate C header file with same format as multi_tables.py
+    TODO: rewrite using templates (see generation of uORB headers)
+    '''
+    from io import StringIO
+    buf = StringIO()
+
+    # Print Header
+    buf.write(u"/*\n")
+    buf.write(u"* This file is automatically generated by px_generate_mixers.py - do not edit.\n")
+    buf.write(u"*/\n")
+    buf.write(u"\n")
+    buf.write(u"#ifndef _MIXER_MULTI_TABLES\n")
+    buf.write(u"#define _MIXER_MULTI_TABLES\n")
+    buf.write(u"\n")
+
+    # Print enum
+    buf.write(u"enum class MultirotorGeometry : MultirotorGeometryUnderlyingType {\n")
+    for i, geometry in enumerate(geometries_list):
+        buf.write(u"\t{},{}// {} (text key {})\n".format(
+            geometry['info']['name'].upper(), ' ' * (max(0, 30 - len(geometry['info']['name']))),
+            geometry['info']['description'], geometry['info']['key']))
+    buf.write(u"\n\tMAX_GEOMETRY\n")
+    buf.write(u"}; // enum class MultirotorGeometry\n\n")
+
+    # Print mixer gains
+    buf.write(u"namespace {\n")
+    for geometry in geometries_list:
+        # Get desired mix matrix
+        if use_normalized_mix:
+            mix = geometry['mix']['B_px']
+        else:
+            mix = geometry['mix']['B']
+
+        buf.write(u"static constexpr MultirotorMixer::Rotor _config_{}[] {{\n".format(geometry['info']['name']))
+
+        for row in mix:
+            if use_6dof:
+            # 6dof mixer
+                buf.write(u"\t{{ {:9f}, {:9f}, {:9f}, {:9f}, {:9f}, {:9f} }},\n".format(
+                    row[0], row[1], row[2],
+                    row[3], row[4], row[5]))
+            else:
+            # 4dof mixer
+                buf.write(u"\t{{ {:9f}, {:9f}, {:9f}, {:9f} }},\n".format(
+                    row[0], row[1], row[2],
+                    -row[5]))  # Upward thrust is positive TODO: to remove this, adapt PX4 to use NED correctly
+
+        buf.write(u"};\n\n")
+
+    # Print geometry indeces
+    buf.write(u"static constexpr const MultirotorMixer::Rotor *_config_index[] {\n")
+    for geometry in geometries_list:
+        buf.write(u"\t&_config_{}[0],\n".format(geometry['info']['name']))
+    buf.write(u"};\n\n")
+
+    # Print geometry rotor counts
+    buf.write(u"static constexpr unsigned _config_rotor_count[] {\n")
+    for geometry in geometries_list:
+        buf.write(u"\t{}, /* {} */\n".format(len(geometry['rotors']), geometry['info']['name']))
+    buf.write(u"};\n\n")
+
+    # Print geometry key
+    buf.write(u"const char* _config_key[] {\n")
+    for geometry in geometries_list:
+        buf.write(u"\t\"{}\",\t/* {} */\n".format(geometry['info']['key'], geometry['info']['name']))
+    buf.write(u"};\n\n")
+
+    # Print footer
+    buf.write(u"} // anonymous namespace\n\n")
+    buf.write(u"#endif /* _MIXER_MULTI_TABLES */\n\n")
+
+    return buf.getvalue()
+
+
+if __name__ == '__main__':
+    import argparse
+    import glob
+    import os
+
+    # Parse arguments
+    parser = argparse.ArgumentParser(
+        description='Convert geometry .toml files to mixer headers')
+    parser.add_argument('-d', dest='dir',
+                        help='directory with geometry files')
+    parser.add_argument('-f', dest='files',
+                        help="files to convert (use only without -d)",
+                        nargs="+")
+    parser.add_argument('-o', dest='outputfile',
+                        help='output header file')
+    parser.add_argument('--verbose', help='Print details on standard output',
+                        action='store_true')
+    parser.add_argument('--normalize', help='Use normalized mixers (compatibility mode)',
+                        action='store_true')
+    parser.add_argument('--sixdof', help='Use 6dof mixers',
+                        action='store_true')
+    args = parser.parse_args()
+
+    # Find toml files
+    if args.files is not None:
+        filenames = args.files
+    elif args.dir is not None:
+        filenames = glob.glob(os.path.join(args.dir, '*.toml'))
+    else:
+        parser.print_usage()
+        raise Exception("Missing input directory (-d) or list of geometry files (-f)")
+
+    # List of geometries
+    geometries_list = []
+
+    for filename in filenames:
+        # Parse geometry file
+        geometry = parse_geometry_toml(filename)
+
+        # Compute torque and thrust matrices
+        A, B = geometry_to_mix(geometry)
+
+        # Normalize mixer
+        B_px = normalize_mix_px4(B)
+
+        # Store matrices in geometry
+        geometry['mix'] = {'A': A, 'B': B, 'B_px': B_px}
+
+        # Add to list
+        geometries_list.append(geometry)
+
+        if args.verbose:
+            print('\nFilename')
+            print(filename)
+            print('\nGeometry')
+            print(geometry)
+            print('\nA:')
+            print(A.round(2))
+            print('\nB:')
+            print(B.round(2))
+            print('\nNormalized Mix (as in PX4):')
+            print(B_px.round(2))
+            print('\n-----------------------------')
+
+    # Check that there are no duplicated mixer names or keys
+    for i in range(len(geometries_list)):
+        name_i = geometries_list[i]['info']['name']
+        key_i = geometries_list[i]['info']['key']
+
+        for j in range(i + 1, len(geometries_list)):
+            name_j = geometries_list[j]['info']['name']
+            key_j = geometries_list[j]['info']['key']
+
+            # Mixers cannot share the same name
+            if name_i == name_j:
+                raise ValueError('Duplicated mixer name "{}" in files {} and {}'.format(
+                    name_i,
+                    geometries_list[i]['info']['filename'],
+                    geometries_list[j]['info']['filename']))
+
+            # Mixers cannot share the same key
+            if key_i == key_j:
+                raise ValueError('Duplicated mixer key "{}" for mixers "{}" and "{}"'.format(
+                    key_i, name_i, name_j))
+
+    # Generate header file
+    header = generate_mixer_multirotor_header(geometries_list,
+                                              use_normalized_mix=args.normalize,
+                                              use_6dof=args.sixdof)
+
+    if args.outputfile is not None:
+        # Write header file
+        with open(args.outputfile, 'w') as fd:
+            fd.write(header)
+    else:
+        # Print to standard output
+        print(header)
@@ -0,0 +1,23 @@
+# Tri Y
+
+[info]
+key = "3y"
+description = "Tri Y"
+
+[rotor_default]
+axis      = [0.0, 0.0, -1.0]
+Ct        = 1.0
+Cm        = 0.0
+direction = "CW"
+
+[[rotors]]
+name      = "front_right"
+position  = [0.5, 0.866025, 0.0]
+
+[[rotors]]
+name      = "front_left"
+position  = [0.5, -0.866025, 0.0]
+
+[[rotors]]
+name      = "rear"
+position  = [-1.0, 0.0, 0.0]
@@ -0,0 +1,19 @@
+# Twin engine
+
+[info]
+key = "2-"
+description = "Twin engine"
+
+[rotor_default]
+axis      = [0.0, 0.0, -1.0]
+Ct        = 1.0
+Cm        = 0.0
+direction = "CW"
+
+[[rotors]]
+name      = "right"
+position  = [0.0, 1.0, 0.0]
+
+[[rotors]]
+name      = "left"
+position  = [0.0, -1.0, 0.0]
@@ -0,0 +1,386 @@
+#!/usr/bin/env python
+# -*- coding: utf-8 -*-
+
+"""
+Mixer multirotor test and prototyping script.
+
+Author: Mathieu Bresciani <brescianimathieu@gmail.com>, Beat Kueng <beat-kueng@gmx.net>
+Description: This script can be used to prototype new mixer algorithms and test
+it against the C++ implementation.
+"""
+
+
+from __future__ import print_function
+
+from argparse import ArgumentParser
+import numpy as np
+import numpy.matlib
+import subprocess
+
+
+# --------------------------------------------------
+# mixing algorithms
+# --------------------------------------------------
+
+def compute_desaturation_gain(u, u_min, u_max, desaturation_vector):
+    """
+    Computes the gain k by which desaturation_vector has to be multiplied
+    in order to unsaturate the output that has the greatest saturation
+    """
+    d_u_sat_plus = u_max - u
+    d_u_sat_minus = u_min - u
+    k = np.zeros(u.size*2)
+    for i in range(u.size):
+        if abs(desaturation_vector[i]) < 0.000001:
+            # avoid division by zero
+            continue
+
+        if d_u_sat_minus[i] > 0.0:
+            k[2*i] = d_u_sat_minus[i] / desaturation_vector[i]
+        if d_u_sat_plus[i] < 0.0:
+            k[2*i+1] = d_u_sat_plus[i] / desaturation_vector[i]
+
+    k_min = min(k)
+    k_max = max(k)
+
+    # Reduce the saturation as much as possible
+    k = k_min + k_max
+    return k
+
+
+def minimize_sat(u, u_min, u_max, desaturation_vector):
+    """
+    Minimize the saturation of the actuators by
+    adding or substracting a fraction of desaturation_vector.
+    desaturation_vector is the vector that added to the output u,
+    modifies the thrust or angular acceleration on a
+    specific axis.
+    For example, if desaturation_vector is given
+    to slide along the vertical thrust axis, the saturation will
+    be minimized by shifting the vertical thrust setpoint,
+    without changing the roll/pitch/yaw accelerations.
+    """
+    k_1 = compute_desaturation_gain(u, u_min, u_max, desaturation_vector)
+    u_1 = u + k_1 * desaturation_vector # Try to unsaturate
+
+
+    # Compute the desaturation gain again based on the updated outputs.
+    # In most cases it will be zero. It won't be if max(outputs) - min(outputs)
+    # > max_output - min_output.
+    # In that case adding 0.5 of the gain will equilibrate saturations.
+    k_2 = compute_desaturation_gain(u_1, u_min, u_max, desaturation_vector)
+
+    k_opt = k_1 + 0.5 * k_2
+
+    u_prime = u + k_opt * desaturation_vector
+    return u_prime
+
+def mix_yaw(m_sp, u, P, u_min, u_max):
+    """
+    Mix yaw by adding it to an existing output vector u
+
+    Desaturation behavior: thrust is allowed to be decreased up to 15% in order to allow
+    some yaw control on the upper end. On the lower end thrust will never be increased,
+    but yaw is decreased as much as required.
+    """
+    m_sp_yaw_only = np.matlib.zeros(m_sp.size).T
+    m_sp_yaw_only[2, 0] = m_sp[2, 0]
+    u_p = u + P * m_sp_yaw_only
+
+    # Change yaw acceleration to unsaturate the outputs if needed (do not change roll/pitch),
+    # and allow some yaw response at maximum thrust
+    u_r_dot = P[:,2]
+    u_pp = minimize_sat(u_p, u_min, u_max+0.15, u_r_dot)
+    u_T = P[:, 3]
+    u_ppp = minimize_sat(u_pp, 0, u_max, u_T)
+    # reduce thrust only
+    if (u_ppp > (u_pp)).any():
+        u_ppp = u_pp
+    return u_ppp
+
+def airmode_rp(m_sp, P, u_min, u_max):
+    """
+    Mix roll, pitch, yaw and thrust.
+
+    Desaturation behavior: airmode for roll/pitch:
+    thrust is increased/decreased as much as required to meet the demanded roll/pitch.
+    Yaw is not allowed to increase the thrust, @see mix_yaw() for the exact behavior.
+    """
+    # Mix without yaw
+    m_sp_no_yaw = m_sp.copy()
+    m_sp_no_yaw[2, 0] = 0.0
+    u = P * m_sp_no_yaw
+
+    # Use thrust to unsaturate the outputs if needed
+    u_T = P[:, 3]
+    u_prime = minimize_sat(u, u_min, u_max, u_T)
+
+    # Mix yaw axis independently
+    u_final = mix_yaw(m_sp, u_prime, P, u_min, u_max)
+
+    return (u, u_final)
+
+
+def airmode_rpy(m_sp, P, u_min, u_max):
+    """
+    Mix roll, pitch, yaw and thrust.
+
+    Desaturation behavior: full airmode for roll/pitch/yaw:
+    thrust is increased/decreased as much as required to meet demanded the roll/pitch/yaw.
+    """
+    # Mix with yaw
+    u = P * m_sp
+
+    # Use thrust to unsaturate the outputs if needed
+    u_T = P[:, 3]
+    u_prime = minimize_sat(u, u_min, u_max, u_T)
+
+    # Unsaturate yaw (in case upper and lower bounds are exceeded)
+    # to prioritize roll/pitch over yaw.
+    u_T = P[:, 2]
+    u_prime_yaw = minimize_sat(u_prime, u_min, u_max, u_T)
+
+    return (u, u_prime_yaw)
+
+
+def normal_mode(m_sp, P, u_min, u_max):
+    """
+    Mix roll, pitch, yaw and thrust.
+
+    Desaturation behavior: no airmode, thrust is NEVER increased to meet the demanded
+    roll/pitch/yaw. Instead roll/pitch/yaw is reduced as much as needed.
+    Thrust can be reduced to unsaturate the upper side.
+    @see mix_yaw() for the exact yaw behavior.
+    """
+    # Mix without yaw
+    m_sp_no_yaw = m_sp.copy()
+    m_sp_no_yaw[2, 0] = 0.0
+    u = P * m_sp_no_yaw
+
+    # Use thrust to unsaturate the outputs if needed
+    # by reducing the thrust only
+    u_T = P[:, 3]
+    u_prime = minimize_sat(u, u_min, u_max, u_T)
+    if (u_prime > (u)).any():
+        u_prime = u
+
+    # Reduce roll/pitch acceleration if needed to unsaturate
+    u_p_dot = P[:, 0]
+    u_p2 = minimize_sat(u_prime, u_min, u_max, u_p_dot)
+    u_q_dot = P[:, 1]
+    u_p3 = minimize_sat(u_p2, u_min, u_max, u_q_dot)
+
+    # Mix yaw axis independently
+    u_final = mix_yaw(m_sp, u_p3, P, u_min, u_max)
+    return (u, u_final)
+
+
+# --------------------------------------------------
+# test cases
+# --------------------------------------------------
+
+# normalized control allocation test matrices (B_px from px_generate_mixers.py)
+# quad_x
+P1 = np.matrix([
+    [-0.25, 0.25, 0.25, 1],
+    [-0.25, -0.25, -0.25, 1],
+    [0.25, -0.25, 0.25, 1],
+    [0.25, 0.25, -0.25, 1]])
+# quad_wide
+P2 = np.matrix([
+    [-0.5,   0.71,  0.77,  1.  ],
+    [ 0.5,  -0.71,  1.,    1.  ],
+    [ 0.5,   0.71, -0.77,  1.  ],
+    [-0.5,  -0.71, -1.,    1.  ]])
+# hex_x
+P3 = np.matrix([
+    [-1.,    0.,   -1.,    1.  ],
+    [ 1.,   -0.,    1.,    1.  ],
+    [ 0.5,   0.87, -1.,    1.  ],
+    [-0.5,  -0.87,  1.,    1.  ],
+    [-0.5,   0.87,  1.,    1.  ],
+    [ 0.5,  -0.87, -1.,    1.  ]])
+# hex_cox
+P4 = np.matrix([
+    [-0.87,  0.5,  -1.,    1.  ],
+    [-0.87,  0.5,   1.,    1.  ],
+    [ 0.,   -1.,   -1.,    1.  ],
+    [ 0.,   -1.,    1.,    1.  ],
+    [ 0.87,  0.5,  -1.,    1.  ],
+    [ 0.87,  0.5,   1.,    1.  ]])
+# octa_plus
+P5 = np.matrix([
+    [-0.,    1.,   -1.,    1.  ],
+    [ 0.,   -1.,   -1.,    1.  ],
+    [-0.71,  0.71,  1.,    1.  ],
+    [-0.71, -0.71,  1.,    1.  ],
+    [ 0.71,  0.71,  1.,    1.  ],
+    [ 0.71, -0.71,  1.,    1.  ],
+    [ 1.,    0.,   -1.,    1.  ],
+    [-1.,   -0.,   -1.,    1.  ]])
+
+P_tests = [ P1 ]
+
+test_cases_input = np.matrix([
+    # desired accelerations (must be within [-1, 1]):
+    #roll pitch yaw thrust
+    [ 0.0,   0.0,  0.0,    0.0],
+    [-0.05,  0.0,  0.0,    0.0],
+    [ 0.05, -0.05, 0.0,    0.0],
+    [ 0.05,  0.05, -0.025, 0.0],
+    [ 0.0,   0.2,  -0.025, 0.0],
+    [ 0.2,   0.05, 0.09,   0.0],
+    [-0.125, 0.02, 0.04,   0.0],
+
+    # extreme cases
+    [ 1.0,  0.0,  0.0,  0.0],
+    [ 0.0, -1.0,  0.0,  0.0],
+    [ 0.0,  0.0,  1.0,  0.0],
+    [ 1.0,  1.0, -1.0,  0.0],
+    [-1.0,  0.9, -0.9,  0.0],
+    [-1.0,  0.9,  0.0,  0.0],
+    ])
+
+# use the following thrust values for all test cases (must be within [0, 1])
+thrust_values = [0, 0.1, 0.45, 0.9, 1.0]
+test_cases = np.zeros((test_cases_input.shape[0] * len(thrust_values), 4))
+for i in range(test_cases_input.shape[0]):
+    for k in range(len(thrust_values)):
+        test_case = test_cases_input[i]
+        test_case[0, 3] = thrust_values[k]
+        test_cases[i * len(thrust_values) + k, :] = test_case
+
+
+def run_tests(mixer_cb, P, test_mixer_binary, test_index=None):
+    """
+    Run all (or a specific) tests for a certain mixer method an control
+    allocation matrix P
+    """
+    B = np.linalg.pinv(P)
+    proc = subprocess.Popen(
+        test_mixer_binary,
+        #'cat > /tmp/test_'+str(mode_idx), shell=True, # just to test the output
+        #stdout=subprocess.PIPE,
+        stdin=subprocess.PIPE)
+    proc.stdin.write("{:}\n".format(mode_idx).encode('utf-8')) # airmode
+    motor_count = P.shape[0]
+    proc.stdin.write("{:}\n".format(motor_count).encode('utf-8')) # motor count
+    # control allocation matrix
+    for row in P.getA():
+        for col in row:
+            proc.stdin.write("{:.8f} ".format(col).encode('utf-8'))
+        proc.stdin.write("\n".encode('utf-8'))
+    proc.stdin.write("\n".encode('utf-8'))
+
+    failed = False
+    try:
+        if test_index is None:
+            # go through all test cases
+            test_indices = range(test_cases.shape[0])
+        else:
+            test_indices = [test_index]
+        for i in test_indices:
+            actuator_controls = test_cases[[i], :].T
+            proc.stdin.write("{:.8f} {:.8f} {:.8f} {:.8f}\n"
+                    .format(actuator_controls[0, 0], actuator_controls[1, 0],
+                        actuator_controls[2, 0], actuator_controls[3, 0]).encode('utf-8'))
+
+            (u, u_new) = mixer_cb(actuator_controls, P, 0.0, 1.0)
+            # Saturate the outputs between 0 and 1
+            u_new_sat = np.maximum(u_new, np.matlib.zeros(u.size).T)
+            u_new_sat = np.minimum(u_new_sat, np.matlib.ones(u.size).T)
+            # write expected outputs
+            for j in range(motor_count):
+                proc.stdin.write("{:.8f} ".format(u_new_sat[j, 0]).encode('utf-8'))
+            proc.stdin.write("\n".encode('utf-8'))
+
+        proc.stdin.close()
+    except IOError as e:
+        failed = True
+    #result = proc.stdout.read()
+    proc.wait()
+    if proc.returncode != 0: failed = True
+    print("B:\n{}".format(B))
+    print("P:\n{}".format(P))
+    #print(result)
+
+parser = ArgumentParser(description=__doc__)
+parser.add_argument('--test', action='store_true', default=False, help='Run tests')
+parser.add_argument("--mixer-multirotor-binary",
+                  help="select test_mixer_multirotor binary file name",
+                  default='./test_mixer_multirotor')
+parser.add_argument("--mode", "-m", dest="mode",
+                help="mixer mode: none, rp, rpy", default=None)
+parser.add_argument("-i", dest="index", type=int,
+                  help="Select a single test to run (starting at 1)", default=None)
+
+args = parser.parse_args()
+mixer_mode = args.mode
+
+if args.test:
+    mixer_binary = args.mixer_multirotor_binary
+    test_index = args.index
+    if test_index is not None: test_index -= 1
+    for mode_idx, (airmode, mixer_cb) in enumerate([
+            ('none', normal_mode),
+            ('rp', airmode_rp),
+            ('rpy', airmode_rpy)]):
+        if mixer_mode is not None and mixer_mode != airmode:
+            continue
+        print('Testing mode: '+airmode)
+        for P in P_tests:
+            run_tests(mixer_cb, P, mixer_binary, test_index)
+    exit(1)
+
+# --------------------------------------------------
+# Prototyping and corner case testing playground
+# --------------------------------------------------
+
+# Compute the control allocation matrix
+# u = P * m
+P = P1 # normal quad
+#P = P2 # wide quad
+
+# Normalized actuator effectiveness matrix using the pseudo inverse of P
+# m = B * u
+B = np.linalg.pinv(P)
+
+# Desired accelerations (actuator controls, in [-1, 1])
+p_dot_sp = 0.0 # roll acceleration (p is the roll rate)
+q_dot_sp = 0.1 # pitch acceleration
+r_dot_sp = 0.1 # yaw acceleration
+T_sp = 0.0 # vertical thrust
+m_sp = np.matrix([p_dot_sp, q_dot_sp, r_dot_sp, T_sp]).T # Vector of desired "accelerations"
+
+# Airmode type (none/rp/rpy)
+airmode = mixer_mode
+if airmode is None: airmode = "none"
+
+# Actuators output saturations
+u_max = 1.0
+u_min = 0.0
+
+if airmode == "none":
+    (u, u_new) = normal_mode(m_sp, P, u_min, u_max)
+elif airmode == "rp":
+    (u, u_new) = airmode_rp(m_sp, P, u_min, u_max)
+elif airmode == "rpy":
+    (u, u_new) = airmode_rpy(m_sp, P, u_min, u_max)
+else:
+    u = 0.0
+    u_new = 0.0
+
+# Saturate the outputs between 0 and 1
+u_new_sat = np.maximum(u_new, np.matlib.zeros(u.size).T)
+u_new_sat = np.minimum(u_new_sat, np.matlib.ones(u.size).T)
+
+np.set_printoptions(suppress=True)
+
+# Display some results
+print("u = \n{}\n".format(u))
+print("u_new = \n{}\n".format(u_new))
+print("u_new_sat = \n{}\n".format(u_new_sat))
+print("Desired accelerations = \n{}\n".format(m_sp))
+# Compute back the allocated accelerations
+m_new = B * u_new_sat
+print("Allocated accelerations = \n{}\n".format(m_new))
@@ -0,0 +1,178 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2018 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.
+ *
+ ****************************************************************************/
+
+/**
+ * testing binary that runs the multirotor mixer through test cases given
+ * via file or stdin and compares the mixer output against expected values.
+ */
+
+#include "MultirotorMixer.hpp"
+
+#include <cstdio>
+#include <math.h>
+
+static const unsigned output_max = 16;
+static float actuator_controls[output_max] {};
+
+static int	mixer_callback(uintptr_t handle,
+			       uint8_t control_group,
+			       uint8_t control_index,
+			       float &control);
+
+static int
+mixer_callback(uintptr_t handle, uint8_t control_group, uint8_t control_index, float &control)
+{
+	control = actuator_controls[control_index];
+	return 0;
+}
+
+int main(int argc, char *argv[])
+{
+	FILE *file_in = stdin;
+
+	if (argc > 1) {
+		file_in = fopen(argv[1], "r");
+	}
+
+	unsigned rotor_count = 0;
+	MultirotorMixer::Rotor rotors[output_max];
+	float actuator_outputs[output_max];
+
+	// read airmode
+	int airmode;
+	fscanf(file_in, "%i", &airmode);
+
+	// read the motor count & control allocation matrix
+	fscanf(file_in, "%i", &rotor_count);
+
+	if (rotor_count > output_max) {
+		return -1;
+	}
+
+	for (unsigned i = 0; i < rotor_count; ++i) {
+		fscanf(file_in, "%f %f %f %f", &rotors[i].roll_scale, &rotors[i].pitch_scale,
+		       &rotors[i].yaw_scale, &rotors[i].thrust_scale);
+	}
+
+	MultirotorMixer mixer(mixer_callback, 0, rotors, rotor_count);
+	mixer.set_airmode((Mixer::Airmode)airmode);
+
+	int test_counter = 0;
+	int num_failed = 0;
+
+	while (!feof(file_in)) {
+
+		// read actuator controls
+		unsigned count = 0;
+
+		while (count < 4 && fscanf(file_in, "%f", &actuator_controls[count]) == 1) {
+			++count;
+		}
+
+		if (count < 4) {
+			break;
+		}
+
+		// do the mixing
+		if (mixer.mix(actuator_outputs, output_max) != rotor_count) {
+			return -1;
+		}
+
+		// Account for MultirotorMixer outputing [-1,1] and python script [0,1]
+		for (unsigned i = 0; i < rotor_count; i++) {
+			actuator_outputs[i] = (actuator_outputs[i] + 1.f) * .5f;
+		}
+
+		// read expected outputs
+		count = 0;
+		float expected_output[output_max];
+		bool failed = false;
+
+		while (count < rotor_count && fscanf(file_in, "%f", &expected_output[count]) == 1) {
+			if (fabsf(expected_output[count] - actuator_outputs[count]) > 0.00001f) {
+				failed = true;
+			}
+
+			++count;
+		}
+
+		if (count < rotor_count) {
+			break;
+		}
+
+		printf("EXPECT_EQ(allocate(%.3ff, %.3ff, %.3ff, -%.3ff), Vector4f(%.6ff, %.6ff, %.6ff, %.6ff)); // %i\n",
+		       (double)actuator_controls[0], (double)actuator_controls[1], (double)actuator_controls[2], (double)actuator_controls[3],
+		       (double)expected_output[0], (double)expected_output[1], (double)expected_output[2], (double)expected_output[3],
+		       test_counter + 1);
+
+		if (failed) {
+			printf("test %i failed:\n", test_counter + 1);
+			printf("control input  : %.3f %.3f %.3f %.3f\n", (double)actuator_controls[0], (double)actuator_controls[1],
+			       (double)actuator_controls[2], (double)actuator_controls[3]);
+			printf("mixer output   : ");
+
+			for (unsigned i = 0; i < rotor_count; ++i) {
+				printf("%.3f ", (double)actuator_outputs[i]);
+			}
+
+			printf("\n");
+			printf("expected output: ");
+
+			for (unsigned i = 0; i < rotor_count; ++i) {
+				printf("%.3f ", (double)expected_output[i]);
+			}
+
+			printf("\n");
+			printf("diff           : ");
+
+			for (unsigned i = 0; i < rotor_count; ++i) {
+				printf("%.3f ", (double)(expected_output[i] - actuator_outputs[i]));
+			}
+
+			printf("\n");
+			++num_failed;
+		}
+
+		++test_counter;
+	}
+
+	printf("tested %i cases: %i success, %i failed\n", test_counter,
+	       test_counter - num_failed, num_failed);
+
+
+	if (file_in != stdin) {
+		fclose(file_in);
+	}
+
+	return num_failed > 0 ? -1 : 0;
+}
@@ -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(NullMixer
+	NullMixer.cpp
+	NullMixer.hpp
+)
+target_link_libraries(NullMixer PRIVATE MixerBase)
+add_dependencies(NullMixer prebuild_targets)
@@ -0,0 +1,67 @@
+/****************************************************************************
+ *
+ *   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.
+ *
+ ****************************************************************************/
+
+#include "NullMixer.hpp"
+
+#include <math.h>
+#include <cstring>
+#include <ctype.h>
+
+unsigned
+NullMixer::mix(float *outputs, unsigned space)
+{
+	if (space > 0) {
+		*outputs = NAN;
+		return 1;
+	}
+
+	return 0;
+}
+
+NullMixer *
+NullMixer::from_text(const char *buf, unsigned &buflen)
+{
+	NullMixer *nm = nullptr;
+
+	/* enforce that the mixer ends with a new line */
+	if (!string_well_formed(buf, buflen)) {
+		return nullptr;
+	}
+
+	if ((buflen >= 2) && (buf[0] == 'Z') && (buf[1] == ':')) {
+		nm = new NullMixer;
+		buflen -= 2;
+	}
+
+	return nm;
+}
@@ -0,0 +1,75 @@
+/****************************************************************************
+ *
+ *   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.
+ *
+ ****************************************************************************/
+
+#pragma once
+
+#include <mixer/MixerBase/Mixer.hpp>
+
+/**
+ * Null mixer; returns zero.
+ *
+ * Used as a placeholder for output channels that are unassigned in groups.
+ */
+class NullMixer : public Mixer
+{
+public:
+	NullMixer() : Mixer(nullptr, 0) {}
+	virtual ~NullMixer() = default;
+
+	// no copy, assignment, move, move assignment
+	NullMixer(const NullMixer &) = delete;
+	NullMixer &operator=(const NullMixer &) = delete;
+	NullMixer(NullMixer &&) = delete;
+	NullMixer &operator=(NullMixer &&) = delete;
+
+	/**
+	 * Factory method.
+	 *
+	 * Given a pointer to a buffer containing a text description of the mixer,
+	 * returns a pointer to a new instance of the mixer.
+	 *
+	 * @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 NullMixer instance, or nullptr
+	 *				if the text format is bad.
+	 */
+	static NullMixer		*from_text(const char *buf, unsigned &buflen);
+
+	unsigned			mix(float *outputs, unsigned space) override;
+
+	unsigned			set_trim(float trim) override { return 1; }
+	unsigned			get_trim(float *trim) override { return 1; }
+
+};
@@ -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(SimpleMixer
+	SimpleMixer.cpp
+	SimpleMixer.hpp
+)
+target_link_libraries(SimpleMixer PRIVATE MixerBase)
+add_dependencies(SimpleMixer prebuild_targets)
@@ -0,0 +1,390 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2012-2015 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_simple.cpp
+ *
+ * Simple summing mixer.
+ */
+
+#include "SimpleMixer.hpp"
+
+#include <stdio.h>
+#include <stdlib.h>
+
+#define debug(fmt, args...)	do { } while(0)
+//#define debug(fmt, args...)	do { printf("[mixer] " fmt "\n", ##args); } while(0)
+
+SimpleMixer::SimpleMixer(ControlCallback control_cb, uintptr_t cb_handle, mixer_simple_s *mixinfo) :
+	Mixer(control_cb, cb_handle),
+	_pinfo(mixinfo)
+{
+}
+
+SimpleMixer::~SimpleMixer()
+{
+	if (_pinfo != nullptr) {
+		free(_pinfo);
+	}
+}
+
+unsigned SimpleMixer::set_trim(float trim)
+{
+	_pinfo->output_scaler.offset = trim;
+	return 1;
+}
+
+unsigned SimpleMixer::get_trim(float *trim)
+{
+	*trim = _pinfo->output_scaler.offset;
+	return 1;
+}
+
+void
+SimpleMixer::set_dt_once(float dt)
+{
+	_dt = dt;
+}
+
+int
+SimpleMixer::parse_output_scaler(const char *buf, unsigned &buflen, mixer_scaler_s &scaler, float &slew_rate_rise_time)
+{
+	int ret;
+	int s[6];
+	int n = -1;
+
+	buf = findtag(buf, buflen, 'O');
+
+	if ((buf == nullptr) || (buflen < 12)) {
+		debug("output parser failed finding tag, ret: '%s'", buf);
+		return -1;
+	}
+
+	if ((ret = sscanf(buf, "O: %d %d %d %d %d %d %n",
+			  &s[0], &s[1], &s[2], &s[3], &s[4], &s[5], &n)) < 5) {
+		debug("out scaler parse failed on '%s' (got %d, consumed %d)", buf, ret, n);
+		return -1;
+	}
+
+	// set slew rate limit to 0 if no 6th number is specified in mixer file
+	if (ret == 5) {
+		s[5] = 0;
+	}
+
+	buf = skipline(buf, buflen);
+
+	if (buf == nullptr) {
+		debug("no line ending, line is incomplete");
+		return -1;
+	}
+
+	scaler.negative_scale	= s[0] / 10000.0f;
+	scaler.positive_scale	= s[1] / 10000.0f;
+	scaler.offset		= s[2] / 10000.0f;
+	scaler.min_output	= s[3] / 10000.0f;
+	scaler.max_output	= s[4] / 10000.0f;
+	slew_rate_rise_time	= s[5] / 10000.0f;
+
+	return 0;
+}
+
+int
+SimpleMixer::parse_control_scaler(const char *buf, unsigned &buflen, mixer_scaler_s &scaler, uint8_t &control_group,
+				  uint8_t &control_index)
+{
+	unsigned u[2];
+	int s[5];
+
+	buf = findtag(buf, buflen, 'S');
+
+	if ((buf == nullptr) || (buflen < 16)) {
+		debug("control parser failed finding tag, ret: '%s'", buf);
+		return -1;
+	}
+
+	if (sscanf(buf, "S: %u %u %d %d %d %d %d",
+		   &u[0], &u[1], &s[0], &s[1], &s[2], &s[3], &s[4]) != 7) {
+		debug("control parse failed on '%s'", buf);
+		return -1;
+	}
+
+	buf = skipline(buf, buflen);
+
+	if (buf == nullptr) {
+		debug("no line ending, line is incomplete");
+		return -1;
+	}
+
+	control_group		= u[0];
+	control_index		= u[1];
+	scaler.negative_scale	= s[0] / 10000.0f;
+	scaler.positive_scale	= s[1] / 10000.0f;
+	scaler.offset		= s[2] / 10000.0f;
+	scaler.min_output	= s[3] / 10000.0f;
+	scaler.max_output	= s[4] / 10000.0f;
+
+	return 0;
+}
+
+SimpleMixer *
+SimpleMixer::from_text(Mixer::ControlCallback control_cb, uintptr_t cb_handle, const char *buf, unsigned &buflen)
+{
+	SimpleMixer *sm = nullptr;
+	mixer_simple_s *mixinfo = nullptr;
+	unsigned inputs;
+	int used;
+	const char *end = buf + buflen;
+	char next_tag;
+
+	/* enforce that the mixer ends with a new line */
+	if (!string_well_formed(buf, buflen)) {
+		return nullptr;
+	}
+
+	/* get the base info for the mixer */
+	if (sscanf(buf, "M: %u%n", &inputs, &used) != 1) {
+		debug("simple parse failed on '%s'", buf);
+		goto out;
+	}
+
+	/* at least 1 input is required */
+	if (inputs == 0) {
+		debug("simple parse got 0 inputs");
+		goto out;
+	}
+
+	buf = skipline(buf, buflen);
+
+	if (buf == nullptr) {
+		debug("no line ending, line is incomplete");
+		goto out;
+	}
+
+	mixinfo = (mixer_simple_s *)malloc(MIXER_SIMPLE_SIZE(inputs));
+
+	if (mixinfo == nullptr) {
+		debug("could not allocate memory for mixer info");
+		goto out;
+	}
+
+	mixinfo->control_count = inputs;
+
+	/* find the next tag */
+	next_tag = findnexttag(end - buflen, buflen);
+
+	if (next_tag == 'S') {
+		/* No output scalers specified. Use default values.
+		 * Corresponds to:
+		 * O:      10000  10000      0 -10000  10000 0
+		 */
+		mixinfo->output_scaler.negative_scale	= 1.0f;
+		mixinfo->output_scaler.positive_scale	= 1.0f;
+		mixinfo->output_scaler.offset		= 0.f;
+		mixinfo->output_scaler.min_output	= -1.0f;
+		mixinfo->output_scaler.max_output	= 1.0f;
+		mixinfo->slew_rate_rise_time		= 0.0f;
+
+	} else {
+		if (parse_output_scaler(end - buflen, buflen, mixinfo->output_scaler, mixinfo->slew_rate_rise_time)) {
+			debug("simple mixer parser failed parsing out scaler tag, ret: '%s'", buf);
+			goto out;
+		}
+	}
+
+	for (unsigned i = 0; i < inputs; i++) {
+		if (parse_control_scaler(end - buflen, buflen,
+					 mixinfo->controls[i].scaler,
+					 mixinfo->controls[i].control_group,
+					 mixinfo->controls[i].control_index)) {
+			debug("simple mixer parser failed parsing ctrl scaler tag, ret: '%s'", buf);
+			goto out;
+		}
+	}
+
+	sm = new SimpleMixer(control_cb, cb_handle, mixinfo);
+
+	if (sm != nullptr) {
+		mixinfo = nullptr;
+		debug("loaded mixer with %d input(s)", inputs);
+
+	} else {
+		debug("could not allocate memory for mixer");
+	}
+
+out:
+
+	if (mixinfo != nullptr) {
+		free(mixinfo);
+	}
+
+	return sm;
+}
+
+unsigned
+SimpleMixer::mix(float *outputs, unsigned space)
+{
+	float sum = 0.0f;
+
+	if (_pinfo == nullptr) {
+		return 0;
+	}
+
+	if (space < 1) {
+		return 0;
+	}
+
+	for (unsigned i = 0; i < _pinfo->control_count; i++) {
+		float input = 0.0f;
+
+		_control_cb(_cb_handle,
+			    _pinfo->controls[i].control_group,
+			    _pinfo->controls[i].control_index,
+			    input);
+
+		sum += scale(_pinfo->controls[i].scaler, input);
+	}
+
+	*outputs = scale(_pinfo->output_scaler, sum);
+
+	if (_dt > FLT_EPSILON && _pinfo->slew_rate_rise_time > FLT_EPSILON) {
+
+		// factor 2 is needed because actuator outputs are in the range [-1,1]
+		const float output_delta_max = 2.0f * _dt / _pinfo->slew_rate_rise_time;
+
+		float delta_out = *outputs - _output_prev;
+
+		if (delta_out > output_delta_max) {
+			*outputs = _output_prev + output_delta_max;
+
+		} else if (delta_out < -output_delta_max) {
+			*outputs = _output_prev - output_delta_max;
+		}
+
+	}
+
+	// this will force the caller of the mixer to always supply dt values, otherwise no slew rate limiting will happen
+	_dt = 0.f;
+
+	_output_prev = *outputs;
+
+	return 1;
+}
+
+void
+SimpleMixer::groups_required(uint32_t &groups)
+{
+	for (unsigned i = 0; i < _pinfo->control_count; i++) {
+		groups |= 1 << _pinfo->controls[i].control_group;
+	}
+}
+
+int
+SimpleMixer::check()
+{
+	float junk;
+
+	/* sanity that presumes that a mixer includes a control no more than once */
+	/* max of 32 groups due to groups_required API */
+	if (_pinfo->control_count > 32) {
+		return -2;
+	}
+
+	/* validate the output scaler */
+	int ret = scale_check(_pinfo->output_scaler);
+
+	if (ret != 0) {
+		return ret;
+	}
+
+	/* validate input scalers */
+	for (unsigned i = 0; i < _pinfo->control_count; i++) {
+
+		/* verify that we can fetch the control */
+		if (_control_cb(_cb_handle,
+				_pinfo->controls[i].control_group,
+				_pinfo->controls[i].control_index,
+				junk) != 0) {
+			return -3;
+		}
+
+		/* validate the scaler */
+		ret = scale_check(_pinfo->controls[i].scaler);
+
+		if (ret != 0) {
+			return (10 * i + ret);
+		}
+	}
+
+	return 0;
+}
+
+float
+SimpleMixer::scale(const mixer_scaler_s &scaler, float input)
+{
+	float output;
+
+	if (input < 0.0f) {
+		output = (input * scaler.negative_scale) + scaler.offset;
+
+	} else {
+		output = (input * scaler.positive_scale) + scaler.offset;
+	}
+
+	return math::constrain(output, scaler.min_output, scaler.max_output);
+}
+
+int
+SimpleMixer::scale_check(mixer_scaler_s &scaler)
+{
+	if (scaler.offset > 1.001f) {
+		return 1;
+	}
+
+	if (scaler.offset < -1.001f) {
+		return 2;
+	}
+
+	if (scaler.min_output > scaler.max_output) {
+		return 3;
+	}
+
+	if (scaler.min_output < -1.001f) {
+		return 4;
+	}
+
+	if (scaler.max_output > 1.001f) {
+		return 5;
+	}
+
+	return 0;
+}
@@ -0,0 +1,153 @@
+/****************************************************************************
+ *
+ *   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.
+ *
+ ****************************************************************************/
+
+#pragma once
+
+#include <mixer/MixerBase/Mixer.hpp>
+
+/** simple channel scaler */
+struct mixer_scaler_s {
+	float negative_scale{1.0f};
+	float positive_scale{1.0f};
+	float offset{0.0f};
+	float min_output{-1.0f};
+	float max_output{1.0f};
+};
+
+/** mixer input */
+struct mixer_control_s {
+	uint8_t			control_group;	/**< group from which the input reads */
+	uint8_t			control_index;	/**< index within the control group */
+	mixer_scaler_s		scaler;		/**< scaling applied to the input before use */
+};
+
+#define MIXER_SIMPLE_SIZE(_icount)	(sizeof(struct mixer_simple_s) + (_icount) * sizeof(struct mixer_control_s))
+
+/** simple mixer */
+struct mixer_simple_s {
+	uint8_t			control_count;	/**< number of inputs */
+	mixer_scaler_s		output_scaler;	/**< scaling for the output */
+	float 			slew_rate_rise_time{0.0f}; /**< output max rise time (slew rate limit)*/
+	mixer_control_s		controls[];	/**< actual size of the array is set by control_count */
+};
+
+/**
+ * Simple summing mixer.
+ *
+ * Collects zero or more inputs and mixes them to a single output.
+ */
+class SimpleMixer : public Mixer
+{
+public:
+	/**
+	 * Constructor
+	 *
+	 * @param mixinfo		Mixer configuration.  The pointer passed
+	 *				becomes the property of the mixer and
+	 *				will be freed when the mixer is deleted.
+	 */
+	SimpleMixer(ControlCallback control_cb, uintptr_t cb_handle, mixer_simple_s *mixinfo);
+	virtual ~SimpleMixer();
+
+	// no copy, assignment, move, move assignment
+	SimpleMixer(const SimpleMixer &) = delete;
+	SimpleMixer &operator=(const SimpleMixer &) = delete;
+	SimpleMixer(SimpleMixer &&) = delete;
+	SimpleMixer &operator=(SimpleMixer &&) = 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 SimpleMixer instance, or nullptr
+	 *				if the text format is bad.
+	 */
+	static SimpleMixer		*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;
+
+	/**
+	 * Check that the mixer configuration as loaded is sensible.
+	 *
+	 * Note that this function will call control_cb, but only cares about
+	 * error returns, not the input value.
+	 *
+	 * @return			Zero if the mixer makes sense, nonzero otherwise.
+	 */
+	int				check();
+
+	unsigned			set_trim(float trim) override;
+	unsigned			get_trim(float *trim) override;
+	void				set_dt_once(float dt) override;
+
+private:
+
+	/**
+	 * Perform simpler linear scaling.
+	 *
+	 * @param scaler		The scaler configuration.
+	 * @param input			The value to be scaled.
+	 * @return			The scaled value.
+	 */
+	static float			scale(const mixer_scaler_s &scaler, float input);
+
+	/**
+	 * Validate a scaler
+	 *
+	 * @param scaler		The scaler to be validated.
+	 * @return			Zero if good, nonzero otherwise.
+	 */
+	static int			scale_check(struct mixer_scaler_s &scaler);
+
+	static int parse_output_scaler(const char *buf, unsigned &buflen, mixer_scaler_s &scaler, float &slew_rate_rise_time);
+	static int parse_control_scaler(const char *buf, unsigned &buflen, mixer_scaler_s &scaler, uint8_t &control_group,
+					uint8_t &control_index);
+
+	float 				_output_prev{0.f};
+	float				_dt{0.f};
+
+	mixer_simple_s			*_pinfo;
+
+};
@@ -0,0 +1,112 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2012 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_load.c
+ *
+ * Programmable multi-channel mixer library.
+ */
+
+#include <string.h>
+#include <stdio.h>
+#include <ctype.h>
+
+#include "mixer_load.h"
+
+int load_mixer_file(const char *fname, char *buf, unsigned maxlen)
+{
+	FILE *fp;
+	char line[120];
+
+	/* open the mixer definition file */
+	fp = fopen(fname, "r");
+
+	if (fp == nullptr) {
+		printf("file not found\n");
+		return -1;
+	}
+
+	/* read valid lines from the file into a buffer */
+	buf[0] = '\0';
+
+	for (;;) {
+
+		/* get a line, bail on error/EOF */
+		line[0] = '\0';
+
+		if (fgets(line, sizeof(line), fp) == nullptr) {
+			break;
+		}
+
+		/* if the line doesn't look like a mixer definition line, skip it */
+		if ((strlen(line) < 2) || !isupper(line[0]) || (line[1] != ':')) {
+			continue;
+		}
+
+		/* compact whitespace in the buffer */
+		char *t, *f;
+
+		for (f = line; *f != '\0'; f++) {
+			/* scan for space characters */
+			if (*f == ' ') {
+				/* look for additional spaces */
+				t = f + 1;
+
+				while (*t == ' ') {
+					t++;
+				}
+
+				if (*t == '\0') {
+					/* strip trailing whitespace */
+					*f = '\0';
+
+				} else if (t > (f + 1)) {
+					memmove(f + 1, t, strlen(t) + 1);
+				}
+			}
+		}
+
+		/* if the line is too long to fit in the buffer, bail */
+		if ((strlen(line) + strlen(buf) + 1) >= maxlen) {
+			printf("line too long\n");
+			fclose(fp);
+			return -1;
+		}
+
+		/* add the line to the buffer */
+		strcat(buf, line);
+	}
+
+	fclose(fp);
+	return 0;
+}
@@ -0,0 +1,51 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2012 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_load.h
+ *
+ */
+
+
+#ifndef _SYSTEMLIB_MIXER_LOAD_H
+#define _SYSTEMLIB_MIXER_LOAD_H value
+
+#include <px4_platform_common/px4_config.h>
+
+__BEGIN_DECLS
+
+__EXPORT int load_mixer_file(const char *fname, char *buf, unsigned maxlen);
+
+__END_DECLS
+
+#endif