mixer: organize by type

src/lib/mixer/MultirotorMixer/CMakeLists.txt

@@ -0,0 +1,113 @@
+############################################################################
+#
+#   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.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 Mixer)
+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 Mixer)
+
+	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()

src/lib/mixer/MultirotorMixer/MultirotorMixer.cpp

@@ -0,0 +1,517 @@
+/****************************************************************************
+ *
+ *   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,
+				 float roll_scale, float pitch_scale, float yaw_scale, float idle_speed) :
+	MultirotorMixer(control_cb, cb_handle, _config_index[(int)geometry], _config_rotor_count[(int)geometry])
+{
+	_roll_scale = roll_scale;
+	_pitch_scale = pitch_scale;
+	_yaw_scale = yaw_scale;
+	_idle_speed = -1.0f + idle_speed * 2.0f;	/* shift to output range here to avoid runtime calculation */
+}
+
+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] = _idle_speed;
+	}
+}
+
+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[8];
+	int s[4];
+	int used;
+
+	/* enforce that the mixer ends with a new line */
+	if (!string_well_formed(buf, buflen)) {
+		return nullptr;
+	}
+
+	if (sscanf(buf, "R: %7s %d %d %d %d%n", geomname, &s[0], &s[1], &s[2], &s[3], &used) != 5) {
+		debug("multirotor parse failed on '%s'", buf);
+		return nullptr;
+	}
+
+	if (used > (int)buflen) {
+		debug("OVERFLOW: multirotor 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;
+	}
+
+	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,
+		       s[0] / 10000.0f,
+		       s[1] / 10000.0f,
+		       s[2] / 10000.0f,
+		       s[3] / 10000.0f);
+}
+
+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) * _roll_scale, -1.0f, 1.0f);
+	float pitch   = math::constrain(get_control(0, 1) * _pitch_scale, -1.0f, 1.0f);
+	float yaw     = math::constrain(get_control(0, 2) * _yaw_scale, -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(_idle_speed + (outputs[i] * (1.0f - _idle_speed)), _idle_speed, 1.0f);
+	}
+
+	// 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] < _idle_speed + 0.01f) {
+			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;
+}

src/lib/mixer/MultirotorMixer/MultirotorMixer.hpp

@@ -0,0 +1,260 @@
+/****************************************************************************
+ *
+ *   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/Mixer/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,
+			float roll_scale, float pitch_scale, float yaw_scale, float idle_speed);
+
+	/**
+	 * 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				_roll_scale{1.0f};
+	float				_pitch_scale{1.0f};
+	float				_yaw_scale{1.0f};
+	float				_idle_speed{0.0f};
+	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};
+};

src/lib/mixer/MultirotorMixer/geometries/dodeca_bottom_cox.toml

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

src/lib/mixer/MultirotorMixer/geometries/dodeca_top_cox.toml

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

src/lib/mixer/MultirotorMixer/geometries/hex_cox.toml

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

src/lib/mixer/MultirotorMixer/geometries/hex_plus.toml

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

src/lib/mixer/MultirotorMixer/geometries/hex_t.toml

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

src/lib/mixer/MultirotorMixer/geometries/hex_x.toml

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

src/lib/mixer/MultirotorMixer/geometries/octa_cox.toml

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

src/lib/mixer/MultirotorMixer/geometries/octa_cox_wide.toml

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

src/lib/mixer/MultirotorMixer/geometries/octa_plus.toml

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

src/lib/mixer/MultirotorMixer/geometries/octa_x.toml

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

src/lib/mixer/MultirotorMixer/geometries/quad_deadcat.toml

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

src/lib/mixer/MultirotorMixer/geometries/quad_h.toml

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

src/lib/mixer/MultirotorMixer/geometries/quad_plus.toml

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

src/lib/mixer/MultirotorMixer/geometries/quad_s250aq.toml

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

src/lib/mixer/MultirotorMixer/geometries/quad_vtail.toml

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

src/lib/mixer/MultirotorMixer/geometries/quad_wide.toml

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

src/lib/mixer/MultirotorMixer/geometries/quad_x.toml

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

src/lib/mixer/MultirotorMixer/geometries/quad_x_pusher.toml

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

src/lib/mixer/MultirotorMixer/geometries/quad_y.toml

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

src/lib/mixer/MultirotorMixer/geometries/tools/px_generate_mixers.py

@@ -0,0 +1,392 @@
+#!/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)
+"""
+
+try:
+    import toml
+    import numpy as np
+except ImportError as e:
+    print("python import error: ", e)
+    print('''
+Required python packages not installed.
+
+On a GNU/Linux or MacOS system please run:
+  sudo pip install numpy toml
+
+On Windows please run:
+  easy_install numpy toml
+''')
+    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)

src/lib/mixer/MultirotorMixer/geometries/tri_y.toml

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

src/lib/mixer/MultirotorMixer/geometries/twin_engine.toml

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

src/lib/mixer/MultirotorMixer/mixer_multirotor.py

@@ -0,0 +1,389 @@
+#!/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.71,  0.71,  1.,   1.  ],
+    [ 0.71, -0.71,  1.,   1.  ],
+    [ 0.71,  0.71, -1.,   1.  ],
+    [-0.71, -0.71, -1.,   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, P2, P3, P4, P5 ]
+
+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
+    if failed:
+        print("Error: test failed")
+        print("B:\n{}".format(B))
+        print("P:\n{}".format(P))
+        print(result)
+        raise Exception('Test failed')
+
+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(0)
+
+# --------------------------------------------------
+# 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))

src/lib/mixer/MultirotorMixer/test_mixer_multirotor.cpp

@@ -0,0 +1,168 @@
+/****************************************************************************
+ *
+ *   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 <cmath>
+
+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;
+		}
+
+		// 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;
+		}
+
+		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;
+}