Merge remote-tracking branch 'origin' into pr-metric-allocation

Remove subdirectories

Remove

src/lib/CMakeLists.txt

@@ -31,6 +31,7 @@
 #
 ############################################################################
 
+add_subdirectory(actuator_effectiveness EXCLUDE_FROM_ALL)
 add_subdirectory(adsb EXCLUDE_FROM_ALL)
 add_subdirectory(airspeed EXCLUDE_FROM_ALL)
 add_subdirectory(atmosphere EXCLUDE_FROM_ALL)

src/lib/actuator_effectiveness/ActuatorEffectiveness.cpp

@@ -0,0 +1,103 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2021 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 "ActuatorEffectiveness.hpp"
+
+#include <px4_platform_common/log.h>
+
+int ActuatorEffectiveness::Configuration::addActuator(ActuatorType type, const matrix::Vector3f &torque,
+		const matrix::Vector3f &thrust)
+{
+	int actuator_idx = num_actuators_matrix[selected_matrix];
+
+	if (actuator_idx >= NUM_ACTUATORS) {
+		PX4_ERR("Too many actuators");
+		return -1;
+	}
+
+	if ((int)type < (int)ActuatorType::COUNT - 1 && num_actuators[(int)type + 1] > 0) {
+		PX4_ERR("Trying to add actuators in the wrong order (add motors first, then servos)");
+		return -1;
+	}
+
+	effectiveness_matrices[selected_matrix](ActuatorEffectiveness::ControlAxis::ROLL, actuator_idx) = torque(0);
+	effectiveness_matrices[selected_matrix](ActuatorEffectiveness::ControlAxis::PITCH, actuator_idx) = torque(1);
+	effectiveness_matrices[selected_matrix](ActuatorEffectiveness::ControlAxis::YAW, actuator_idx) = torque(2);
+	effectiveness_matrices[selected_matrix](ActuatorEffectiveness::ControlAxis::THRUST_X, actuator_idx) = thrust(0);
+	effectiveness_matrices[selected_matrix](ActuatorEffectiveness::ControlAxis::THRUST_Y, actuator_idx) = thrust(1);
+	effectiveness_matrices[selected_matrix](ActuatorEffectiveness::ControlAxis::THRUST_Z, actuator_idx) = thrust(2);
+	matrix_selection_indexes[totalNumActuators()] = selected_matrix;
+	++num_actuators[(int)type];
+	return num_actuators_matrix[selected_matrix]++;
+}
+
+void ActuatorEffectiveness::Configuration::actuatorsAdded(ActuatorType type, int count)
+{
+	int total_count = totalNumActuators();
+
+	for (int i = 0; i < count; ++i) {
+		matrix_selection_indexes[i + total_count] = selected_matrix;
+	}
+
+	num_actuators[(int)type] += count;
+	num_actuators_matrix[selected_matrix] += count;
+}
+
+int ActuatorEffectiveness::Configuration::totalNumActuators() const
+{
+	int total_count = 0;
+
+	for (int i = 0; i < MAX_NUM_MATRICES; ++i) {
+		total_count += num_actuators_matrix[i];
+	}
+
+	return total_count;
+}
+
+void ActuatorEffectiveness::stopMaskedMotorsWithZeroThrust(uint32_t stoppable_motors_mask, ActuatorVector &actuator_sp)
+{
+	for (int actuator_idx = 0; actuator_idx < NUM_ACTUATORS; actuator_idx++) {
+		const uint32_t motor_mask = (1u << actuator_idx);
+
+		if (stoppable_motors_mask & motor_mask) {
+
+			// Stop motor if its setpoint is below 2%. This value was determined empirically (RC stick inaccuracy)
+			if (fabsf(actuator_sp(actuator_idx)) < .02f) {
+				_stopped_motors_mask |= motor_mask;
+
+			} else {
+				_stopped_motors_mask &= ~motor_mask;
+			}
+		}
+	}
+}

src/lib/actuator_effectiveness/ActuatorEffectiveness.hpp

@@ -0,0 +1,226 @@
+/****************************************************************************
+ *
+ *   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 ActuatorEffectiveness.hpp
+ *
+ * Interface for Actuator Effectiveness
+ *
+ * @author Julien Lecoeur <julien.lecoeur@gmail.com>
+ */
+
+#pragma once
+
+#include <cstdint>
+
+#include <matrix/matrix/math.hpp>
+#include <uORB/topics/control_allocator_status.h>
+
+enum class AllocationMethod {
+	NONE = -1,
+	PSEUDO_INVERSE = 0,
+	SEQUENTIAL_DESATURATION = 1,
+	AUTO = 2,
+	METRIC = 3,
+};
+
+enum class ActuatorType {
+	MOTORS = 0,
+	SERVOS,
+	THRUSTERS,
+	COUNT
+};
+
+enum class EffectivenessUpdateReason {
+	NO_EXTERNAL_UPDATE = 0,
+	CONFIGURATION_UPDATE = 1,
+	MOTOR_ACTIVATION_UPDATE = 2,
+};
+
+
+class ActuatorEffectiveness
+{
+public:
+	ActuatorEffectiveness() = default;
+	virtual ~ActuatorEffectiveness() = default;
+
+	static constexpr int NUM_ACTUATORS = 16;
+	static constexpr int NUM_AXES = 6;
+
+	enum ControlAxis {
+		ROLL = 0,
+		PITCH,
+		YAW,
+		THRUST_X,
+		THRUST_Y,
+		THRUST_Z
+	};
+
+	static constexpr int MAX_NUM_MATRICES = 2;
+
+	using EffectivenessMatrix = matrix::Matrix<float, NUM_AXES, NUM_ACTUATORS>;
+	using ActuatorVector = matrix::Vector<float, NUM_ACTUATORS>;
+
+	enum class FlightPhase {
+		HOVER_FLIGHT = 0,
+		FORWARD_FLIGHT = 1,
+		TRANSITION_HF_TO_FF = 2,
+		TRANSITION_FF_TO_HF = 3
+	};
+
+	struct Configuration {
+		/**
+		 * Add an actuator to the selected matrix, returning the index, or -1 on error
+		 */
+		int addActuator(ActuatorType type, const matrix::Vector3f &torque, const matrix::Vector3f &thrust);
+
+		/**
+		 * Call this after manually adding N actuators to the selected matrix
+		 */
+		void actuatorsAdded(ActuatorType type, int count);
+
+		int totalNumActuators() const;
+
+		/// Configured effectiveness matrix. Actuators are expected to be filled in order, motors first, then servos
+		EffectivenessMatrix effectiveness_matrices[MAX_NUM_MATRICES];
+
+		int num_actuators_matrix[MAX_NUM_MATRICES]; ///< current amount, and next actuator index to fill in to effectiveness_matrices
+		ActuatorVector trim[MAX_NUM_MATRICES];
+
+		ActuatorVector linearization_point[MAX_NUM_MATRICES];
+
+		int selected_matrix;
+
+		uint8_t matrix_selection_indexes[NUM_ACTUATORS * MAX_NUM_MATRICES];
+		int num_actuators[(int)ActuatorType::COUNT];
+	};
+
+	/**
+	 * Set the current flight phase
+	 *
+	 * @param Flight phase
+	 */
+	virtual void setFlightPhase(const FlightPhase &flight_phase)
+	{
+		_flight_phase = flight_phase;
+	}
+
+	/**
+	 * Get the number of effectiveness matrices. Must be <= MAX_NUM_MATRICES.
+	 * This is expected to stay constant.
+	 */
+	virtual int numMatrices() const { return 1; }
+
+	/**
+	 * Get the desired allocation method(s) for each matrix, if configured as AUTO
+	 */
+	virtual void getDesiredAllocationMethod(AllocationMethod allocation_method_out[MAX_NUM_MATRICES]) const
+	{
+		for (int i = 0; i < MAX_NUM_MATRICES; ++i) {
+			allocation_method_out[i] = AllocationMethod::PSEUDO_INVERSE;
+		}
+	}
+
+	/**
+	 * Query if the roll, pitch and yaw columns of the mixing matrix should be normalized
+	 */
+	virtual void getNormalizeRPY(bool normalize[MAX_NUM_MATRICES]) const
+	{
+		for (int i = 0; i < MAX_NUM_MATRICES; ++i) {
+			normalize[i] = false;
+		}
+	}
+
+	/**
+	 * Get the control effectiveness matrix if updated
+	 *
+	 * @return true if updated and matrix is set
+	 */
+	virtual bool getEffectivenessMatrix(Configuration &configuration, EffectivenessUpdateReason external_update) { return false;}
+
+	/**
+	 * Get the current flight phase
+	 *
+	 * @return Flight phase
+	 */
+	const FlightPhase &getFlightPhase() const
+	{
+		return _flight_phase;
+	}
+
+	/**
+	 * Display name
+	 */
+	virtual const char *name() const = 0;
+
+	/**
+	 * Callback from the control allocation, allowing to manipulate the setpoint.
+	 * Used to allocate auxiliary controls to actuators (e.g. flaps and spoilers).
+	 *
+	 * @param actuator_sp input & output setpoint
+	 */
+	virtual void allocateAuxilaryControls(const float dt, int matrix_index, ActuatorVector &actuator_sp) {}
+
+	/**
+	 * Callback from the control allocation, allowing to manipulate the setpoint.
+	 * This can be used to e.g. add non-linear or external terms.
+	 * It is called after the matrix multiplication and before final clipping.
+	 * @param actuator_sp input & output setpoint
+	 */
+	virtual void updateSetpoint(const matrix::Vector<float, NUM_AXES> &control_sp,
+				    int matrix_index, ActuatorVector &actuator_sp, const matrix::Vector<float, NUM_ACTUATORS> &actuator_min,
+				    const matrix::Vector<float, NUM_ACTUATORS> &actuator_max) {}
+
+	/**
+	 * Get a bitmask of motors to be stopped
+	 */
+	virtual uint32_t getStoppedMotors() const { return _stopped_motors_mask; }
+
+	/**
+	 * Fill in the unallocated torque and thrust, customized by effectiveness type.
+	 * Can be implemented for every type separately. If not implemented then the effectivenes matrix is used instead.
+	 */
+	virtual void getUnallocatedControl(int matrix_index, control_allocator_status_s &status) {}
+
+	/**
+	 * Stops motors which are masked by stoppable_motors_mask and whose demanded thrust is zero
+	 *
+	 * @param stoppable_motors_mask mask of motors that should be stopped if there's no thrust demand
+	 * @param actuator_sp outcome of the allocation to determine if the motor should be stopped
+	 */
+	virtual void stopMaskedMotorsWithZeroThrust(uint32_t stoppable_motors_mask, ActuatorVector &actuator_sp);
+
+protected:
+	FlightPhase _flight_phase{FlightPhase::HOVER_FLIGHT};
+	uint32_t _stopped_motors_mask{0};
+};

src/lib/actuator_effectiveness/ActuatorEffectivenessRotors.cpp

@@ -0,0 +1,312 @@
+/****************************************************************************
+ *
+ *   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 ActuatorEffectivenessRotors.hpp
+ *
+ * Actuator effectiveness computed from rotors position and orientation
+ *
+ * @author Julien Lecoeur <julien.lecoeur@gmail.com>
+ */
+
+#include "ActuatorEffectivenessRotors.hpp"
+
+// #include "ActuatorEffectivenessTilts.hpp"
+
+using namespace matrix;
+
+ActuatorEffectivenessRotors::ActuatorEffectivenessRotors(ModuleParams *parent, AxisConfiguration axis_config,
+		bool tilt_support)
+	: ModuleParams(parent), _axis_config(axis_config), _tilt_support(tilt_support)
+{
+	for (int i = 0; i < NUM_ROTORS_MAX; ++i) {
+		char buffer[17];
+		snprintf(buffer, sizeof(buffer), "CA_ROTOR%u_PX", i);
+		_param_handles[i].position_x = param_find(buffer);
+		snprintf(buffer, sizeof(buffer), "CA_ROTOR%u_PY", i);
+		_param_handles[i].position_y = param_find(buffer);
+		snprintf(buffer, sizeof(buffer), "CA_ROTOR%u_PZ", i);
+		_param_handles[i].position_z = param_find(buffer);
+
+		if (_axis_config == AxisConfiguration::Configurable) {
+			snprintf(buffer, sizeof(buffer), "CA_ROTOR%u_AX", i);
+			_param_handles[i].axis_x = param_find(buffer);
+			snprintf(buffer, sizeof(buffer), "CA_ROTOR%u_AY", i);
+			_param_handles[i].axis_y = param_find(buffer);
+			snprintf(buffer, sizeof(buffer), "CA_ROTOR%u_AZ", i);
+			_param_handles[i].axis_z = param_find(buffer);
+		}
+
+		snprintf(buffer, sizeof(buffer), "CA_ROTOR%u_CT", i);
+		_param_handles[i].thrust_coef = param_find(buffer);
+
+		snprintf(buffer, sizeof(buffer), "CA_ROTOR%u_KM", i);
+		_param_handles[i].moment_ratio = param_find(buffer);
+
+		if (_tilt_support) {
+			snprintf(buffer, sizeof(buffer), "CA_ROTOR%u_TILT", i);
+			_param_handles[i].tilt_index = param_find(buffer);
+		}
+	}
+
+	updateParams();
+}
+
+void ActuatorEffectivenessRotors::updateParams()
+{
+	ModuleParams::updateParams();
+
+	_geometry.num_rotors = math::min(NUM_ROTORS_MAX, static_cast<int>(_param_ca_rotor_count.get()));
+
+	for (int i = 0; i < _geometry.num_rotors; ++i) {
+		Vector3f &position = _geometry.rotors[i].position;
+		param_get(_param_handles[i].position_x, &position(0));
+		param_get(_param_handles[i].position_y, &position(1));
+		param_get(_param_handles[i].position_z, &position(2));
+
+		Vector3f &axis = _geometry.rotors[i].axis;
+
+		switch (_axis_config) {
+		case AxisConfiguration::Configurable:
+			param_get(_param_handles[i].axis_x, &axis(0));
+			param_get(_param_handles[i].axis_y, &axis(1));
+			param_get(_param_handles[i].axis_z, &axis(2));
+			break;
+
+		case AxisConfiguration::FixedForward:
+			axis = Vector3f(1.f, 0.f, 0.f);
+			break;
+
+		case AxisConfiguration::FixedUpwards:
+			axis = Vector3f(0.f, 0.f, -1.f);
+			break;
+		}
+
+		param_get(_param_handles[i].thrust_coef, &_geometry.rotors[i].thrust_coef);
+		param_get(_param_handles[i].moment_ratio, &_geometry.rotors[i].moment_ratio);
+
+		if (_tilt_support) {
+			int32_t tilt_param{0};
+			param_get(_param_handles[i].tilt_index, &tilt_param);
+			_geometry.rotors[i].tilt_index = tilt_param - 1;
+
+		} else {
+			_geometry.rotors[i].tilt_index = -1;
+		}
+	}
+}
+
+bool
+ActuatorEffectivenessRotors::addActuators(Configuration &configuration)
+{
+	if (configuration.num_actuators[(int)ActuatorType::SERVOS] > 0) {
+		PX4_ERR("Wrong actuator ordering: servos need to be after motors");
+		return false;
+	}
+
+	int num_actuators = computeEffectivenessMatrix(_geometry,
+			    configuration.effectiveness_matrices[configuration.selected_matrix],
+			    configuration.num_actuators_matrix[configuration.selected_matrix]);
+	configuration.actuatorsAdded(ActuatorType::MOTORS, num_actuators);
+	return true;
+}
+
+int
+ActuatorEffectivenessRotors::computeEffectivenessMatrix(const Geometry &geometry,
+		EffectivenessMatrix &effectiveness, int actuator_start_index)
+{
+	int num_actuators = 0;
+
+	for (int i = 0; i < geometry.num_rotors; i++) {
+
+		if (i + actuator_start_index >= NUM_ACTUATORS) {
+			break;
+		}
+
+		++num_actuators;
+
+		// Get rotor axis
+		Vector3f axis = geometry.rotors[i].axis;
+
+		// Normalize axis
+		float axis_norm = axis.norm();
+
+		if (axis_norm > FLT_EPSILON) {
+			axis /= axis_norm;
+
+		} else {
+			// Bad axis definition, ignore this rotor
+			continue;
+		}
+
+		// Get rotor position
+		const Vector3f &position = geometry.rotors[i].position;
+
+		// Get coefficients
+		float ct = geometry.rotors[i].thrust_coef;
+		float km = geometry.rotors[i].moment_ratio;
+
+		if (geometry.propeller_torque_disabled) {
+			km = 0.f;
+		}
+
+		if (geometry.propeller_torque_disabled_non_upwards) {
+			bool upwards = fabsf(axis(0)) < 0.1f && fabsf(axis(1)) < 0.1f && axis(2) < -0.5f;
+
+			if (!upwards) {
+				km = 0.f;
+			}
+		}
+
+		if (fabsf(ct) < FLT_EPSILON) {
+			continue;
+		}
+
+		// Compute thrust generated by this rotor
+		matrix::Vector3f thrust = ct * axis;
+
+		// Compute moment generated by this rotor
+		matrix::Vector3f moment = ct * position.cross(axis) - ct * km * axis;
+
+		// Fill corresponding items in effectiveness matrix
+		for (size_t j = 0; j < 3; j++) {
+			effectiveness(j, i + actuator_start_index) = moment(j);
+			effectiveness(j + 3, i + actuator_start_index) = thrust(j);
+		}
+
+		if (geometry.yaw_by_differential_thrust_disabled) {
+			// set yaw effectiveness to 0 if yaw is controlled by other means (e.g. tilts)
+			effectiveness(2, i + actuator_start_index) = 0.f;
+		}
+
+		if (geometry.three_dimensional_thrust_disabled) {
+			// Special case tiltrotor: instead of passing a 3D thrust vector (that would mostly have a x-component in FW, and z in MC),
+			// pass the vector magnitude as z-component, plus the collective tilt. Passing 3D thrust plus tilt is not feasible as they
+			// can't be allocated independently, and with the current controller it's not possible to have collective tilt calculated
+			// by the allocator directly.
+
+			effectiveness(0 + 3, i + actuator_start_index) = 0.f;
+			effectiveness(1 + 3, i + actuator_start_index) = 0.f;
+			effectiveness(2 + 3, i + actuator_start_index) = -ct;
+		}
+	}
+
+	return num_actuators;
+}
+
+// TODO(@Jaeyoung-Lim): This should not be in the Rotors effectiveness, so we need to move it elsewhere
+/*
+uint32_t ActuatorEffectivenessRotors::updateAxisFromTilts(const ActuatorEffectivenessTilts &tilts,
+		float collective_tilt_control)
+{
+	if (!PX4_ISFINITE(collective_tilt_control)) {
+		collective_tilt_control = -1.f;
+	}
+
+	uint32_t nontilted_motors = 0;
+
+	for (int i = 0; i < _geometry.num_rotors; ++i) {
+		int tilt_index = _geometry.rotors[i].tilt_index;
+
+		if (tilt_index == -1 || tilt_index >= tilts.count()) {
+			nontilted_motors |= 1u << i;
+			continue;
+		}
+
+		const ActuatorEffectivenessTilts::Params &tilt = tilts.config(tilt_index);
+		const float tilt_angle = math::lerp(tilt.min_angle, tilt.max_angle, (collective_tilt_control + 1.f) / 2.f);
+		const float tilt_direction = math::radians((float)tilt.tilt_direction);
+		_geometry.rotors[i].axis = tiltedAxis(tilt_angle, tilt_direction);
+	}
+
+	return nontilted_motors;
+}
+*/
+
+Vector3f ActuatorEffectivenessRotors::tiltedAxis(float tilt_angle, float tilt_direction)
+{
+	Vector3f axis{0.f, 0.f, -1.f};
+	return Dcmf{Eulerf{0.f, -tilt_angle, tilt_direction}} * axis;
+}
+
+uint32_t ActuatorEffectivenessRotors::getMotors() const
+{
+	uint32_t motors = 0;
+
+	for (int i = 0; i < _geometry.num_rotors; ++i) {
+		motors |= 1u << i;
+	}
+
+	return motors;
+}
+
+uint32_t ActuatorEffectivenessRotors::getUpwardsMotors() const
+{
+	uint32_t upwards_motors = 0;
+
+	for (int i = 0; i < _geometry.num_rotors; ++i) {
+		const Vector3f &axis = _geometry.rotors[i].axis;
+
+		if (fabsf(axis(0)) < 0.1f && fabsf(axis(1)) < 0.1f && axis(2) < -0.5f) {
+			upwards_motors |= 1u << i;
+		}
+	}
+
+	return upwards_motors;
+}
+
+uint32_t ActuatorEffectivenessRotors::getForwardsMotors() const
+{
+	uint32_t forward_motors = 0;
+
+	for (int i = 0; i < _geometry.num_rotors; ++i) {
+		const Vector3f &axis = _geometry.rotors[i].axis;
+
+		if (axis(0) > 0.5f && fabsf(axis(1)) < 0.1f && fabsf(axis(2)) < 0.1f) {
+			forward_motors |= 1u << i;
+		}
+	}
+
+	return forward_motors;
+}
+
+bool
+ActuatorEffectivenessRotors::getEffectivenessMatrix(Configuration &configuration,
+		EffectivenessUpdateReason external_update)
+{
+	if (external_update == EffectivenessUpdateReason::NO_EXTERNAL_UPDATE) {
+		return false;
+	}
+
+	return addActuators(configuration);
+}

src/lib/actuator_effectiveness/ActuatorEffectivenessRotors.hpp

@@ -0,0 +1,156 @@
+/****************************************************************************
+ *
+ *   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 ActuatorEffectivenessRotors.hpp
+ *
+ * Actuator effectiveness computed from rotors position and orientation
+ *
+ * @author Julien Lecoeur <julien.lecoeur@gmail.com>
+ */
+
+#pragma once
+
+#include "control_allocation/actuator_effectiveness/ActuatorEffectiveness.hpp"
+
+#include <px4_platform_common/module_params.h>
+#include <uORB/Subscription.hpp>
+#include <uORB/SubscriptionInterval.hpp>
+
+class ActuatorEffectivenessTilts;
+
+using namespace time_literals;
+
+class ActuatorEffectivenessRotors : public ModuleParams, public ActuatorEffectiveness
+{
+public:
+	enum class AxisConfiguration {
+		Configurable, ///< axis can be configured
+		FixedForward, ///< axis is fixed, pointing forwards (positive X)
+		FixedUpwards, ///< axis is fixed, pointing upwards (negative Z)
+	};
+
+	static constexpr int NUM_ROTORS_MAX = 12;
+
+	struct RotorGeometry {
+		matrix::Vector3f position;
+		matrix::Vector3f axis;
+		float thrust_coef;
+		float moment_ratio;
+		int tilt_index;
+	};
+
+	struct Geometry {
+		RotorGeometry rotors[NUM_ROTORS_MAX];
+		int num_rotors{0};
+		bool propeller_torque_disabled{false};
+		bool yaw_by_differential_thrust_disabled{false};
+		bool propeller_torque_disabled_non_upwards{false}; ///< keeps propeller torque enabled for upward facing motors
+		bool three_dimensional_thrust_disabled{false}; ///< for handling of tiltrotor VTOL, as they pass in 1D thrust and collective tilt
+	};
+
+	ActuatorEffectivenessRotors(ModuleParams *parent, AxisConfiguration axis_config = AxisConfiguration::Configurable,
+				    bool tilt_support = false);
+	virtual ~ActuatorEffectivenessRotors() = default;
+
+	bool getEffectivenessMatrix(Configuration &configuration, EffectivenessUpdateReason external_update) override;
+
+	void getDesiredAllocationMethod(AllocationMethod allocation_method_out[MAX_NUM_MATRICES]) const override
+	{
+		allocation_method_out[0] = AllocationMethod::SEQUENTIAL_DESATURATION;
+	}
+
+	void getNormalizeRPY(bool normalize[MAX_NUM_MATRICES]) const override
+	{
+		normalize[0] = true;
+	}
+
+	static int computeEffectivenessMatrix(const Geometry &geometry,
+					      EffectivenessMatrix &effectiveness, int actuator_start_index = 0);
+
+	bool addActuators(Configuration &configuration);
+
+	const char *name() const override { return "Rotors"; }
+
+	/**
+	 * Sets the motor axis from tilt configurations and current tilt control.
+	 * @param tilts configured tilt servos
+	 * @param tilt_control current tilt control in [-1, 1] (can be NAN)
+	 * @return the motors as bitset which are not tiltable
+	 */
+	uint32_t updateAxisFromTilts(const ActuatorEffectivenessTilts &tilts, float tilt_control);
+
+	const Geometry &geometry() const { return _geometry; }
+
+	/**
+	 * Get the tilted axis {0, 0, -1} rotated by -tilt_angle around y, then
+	 * rotated by tilt_direction around z.
+	 */
+	static matrix::Vector3f tiltedAxis(float tilt_angle, float tilt_direction);
+
+	void enablePropellerTorque(bool enable) { _geometry.propeller_torque_disabled = !enable; }
+
+	void enableYawByDifferentialThrust(bool enable) { _geometry.yaw_by_differential_thrust_disabled = !enable; }
+
+	void enablePropellerTorqueNonUpwards(bool enable) { _geometry.propeller_torque_disabled_non_upwards = !enable; }
+
+	void enableThreeDimensionalThrust(bool enable) { _geometry.three_dimensional_thrust_disabled = !enable; }
+
+	uint32_t getMotors() const;
+	uint32_t getUpwardsMotors() const;
+	uint32_t getForwardsMotors() const;
+
+private:
+	void updateParams() override;
+	const AxisConfiguration _axis_config;
+	const bool _tilt_support; ///< if true, tilt servo assignment params are loaded
+
+	struct ParamHandles {
+		param_t position_x;
+		param_t position_y;
+		param_t position_z;
+		param_t axis_x;
+		param_t axis_y;
+		param_t axis_z;
+		param_t thrust_coef;
+		param_t moment_ratio;
+		param_t tilt_index;
+	};
+	ParamHandles _param_handles[NUM_ROTORS_MAX];
+
+	Geometry _geometry{};
+
+	DEFINE_PARAMETERS(
+		(ParamInt<px4::params::CA_ROTOR_COUNT>) _param_ca_rotor_count
+	)
+};

src/lib/actuator_effectiveness/ActuatorEffectivenessRotorsTest.cpp

@@ -0,0 +1,135 @@
+/****************************************************************************
+ *
+ *   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 ActuatorEffectivenessRotors.cpp
+ *
+ * Tests for Control Allocation Algorithms
+ *
+ * @author Julien Lecoeur <julien.lecoeur@gmail.com>
+ */
+
+#include <gtest/gtest.h>
+#include "ActuatorEffectivenessRotors.hpp"
+
+using namespace matrix;
+
+TEST(ActuatorEffectivenessRotors, AllZeroCase)
+{
+	// Quad wide geometry
+	ActuatorEffectivenessRotors::Geometry geometry = {};
+	geometry.rotors[0].position(0) = 1.0f;
+	geometry.rotors[0].position(1) = 1.0f;
+	geometry.rotors[0].position(2) = 0.0f;
+	geometry.rotors[0].axis(0) = 0.0f;
+	geometry.rotors[0].axis(1) = 0.0f;
+	geometry.rotors[0].axis(2) = -1.0f;
+	geometry.rotors[0].thrust_coef = 1.0f;
+	geometry.rotors[0].moment_ratio = 0.05f;
+
+	geometry.rotors[1].position(0) = -1.0f;
+	geometry.rotors[1].position(1) = -1.0f;
+	geometry.rotors[1].position(2) = 0.0f;
+	geometry.rotors[1].axis(0) = 0.0f;
+	geometry.rotors[1].axis(1) = 0.0f;
+	geometry.rotors[1].axis(2) = -1.0f;
+	geometry.rotors[1].thrust_coef = 1.0f;
+	geometry.rotors[1].moment_ratio = 0.05f;
+
+	geometry.rotors[2].position(0) = 1.0f;
+	geometry.rotors[2].position(1) = -1.0f;
+	geometry.rotors[2].position(2) = 0.0f;
+	geometry.rotors[2].axis(0) = 0.0f;
+	geometry.rotors[2].axis(1) = 0.0f;
+	geometry.rotors[2].axis(2) = -1.0f;
+	geometry.rotors[2].thrust_coef = 1.0f;
+	geometry.rotors[2].moment_ratio = -0.05f;
+
+	geometry.rotors[3].position(0) = -1.0f;
+	geometry.rotors[3].position(1) = 1.0f;
+	geometry.rotors[3].position(2) = 0.0f;
+	geometry.rotors[3].axis(0) = 0.0f;
+	geometry.rotors[3].axis(1) = 0.0f;
+	geometry.rotors[3].axis(2) = -1.0f;
+	geometry.rotors[3].thrust_coef = 1.0f;
+	geometry.rotors[3].moment_ratio = -0.05f;
+
+	geometry.num_rotors = 4;
+
+	ActuatorEffectiveness::EffectivenessMatrix effectiveness;
+	effectiveness.setZero();
+	ActuatorEffectivenessRotors::computeEffectivenessMatrix(geometry, effectiveness);
+
+	const float expected[ActuatorEffectiveness::NUM_AXES][ActuatorEffectiveness::NUM_ACTUATORS] = {
+		{-1.0f,   1.0f,   1.0f,  -1.0f,  0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f},
+		{ 1.0f,  -1.0f,   1.0f,  -1.0f,  0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f},
+		{ 0.05f,  0.05f, -0.05f, -0.05f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f},
+		{ 0.f,    0.f,    0.f,    0.f,   0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f},
+		{ 0.f,    0.f,    0.f,    0.f,   0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f},
+		{-1.0f,  -1.0f,  -1.0f,  -1.0f,  0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f}
+	};
+	ActuatorEffectiveness::EffectivenessMatrix effectiveness_expected(expected);
+
+	EXPECT_EQ(effectiveness, effectiveness_expected);
+}
+
+TEST(ActuatorEffectivenessRotors, Tilt)
+{
+	Vector3f axis_expected{0.f, 0.f, -1.f};
+	Vector3f axis = ActuatorEffectivenessRotors::tiltedAxis(0.f, 0.f);
+	EXPECT_EQ(axis, axis_expected);
+
+	axis_expected = Vector3f{1.f, 0.f, 0.f};
+	axis = ActuatorEffectivenessRotors::tiltedAxis(M_PI_F / 2.f, 0.f);
+	EXPECT_EQ(axis, axis_expected);
+
+	axis_expected = Vector3f{1.f / sqrtf(2.f), 0.f, -1.f / sqrtf(2.f)};
+	axis = ActuatorEffectivenessRotors::tiltedAxis(M_PI_F / 2.f / 2.f, 0.f);
+	EXPECT_EQ(axis, axis_expected);
+
+	axis_expected = Vector3f{-1.f, 0.f, 0.f};
+	axis = ActuatorEffectivenessRotors::tiltedAxis(-M_PI_F / 2.f, 0.f);
+	EXPECT_EQ(axis, axis_expected);
+
+	axis_expected = Vector3f{0.f, 0.f, -1.f};
+	axis = ActuatorEffectivenessRotors::tiltedAxis(0.f, M_PI_F / 2.f);
+	EXPECT_EQ(axis, axis_expected);
+
+	axis_expected = Vector3f{0.f, 1.f, 0.f};
+	axis = ActuatorEffectivenessRotors::tiltedAxis(M_PI_F / 2.f, M_PI_F / 2.f);
+	EXPECT_EQ(axis, axis_expected);
+
+	axis_expected = Vector3f{0.f, -1.f, 0.f};
+	axis = ActuatorEffectivenessRotors::tiltedAxis(-M_PI_F / 2.f, M_PI_F / 2.f);
+	EXPECT_EQ(axis, axis_expected);
+}

src/lib/actuator_effectiveness/ActuatorEffectivenessThrusters.cpp

@@ -0,0 +1,205 @@
+/****************************************************************************
+ *
+ *   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 ActuatorEffectivenessThrusters.hpp
+ *
+ * Actuator effectiveness computed from thrusters position and orientation
+ *
+ * @author Pedro Roque, <padr@kth.se>
+ */
+
+#include "ActuatorEffectivenessThrusters.hpp"
+
+using namespace matrix;
+
+ActuatorEffectivenessThrusters::ActuatorEffectivenessThrusters(ModuleParams *parent, AxisConfiguration axis_config,
+		bool tilt_support)
+	: ModuleParams(parent), _axis_config(axis_config), _tilt_support(tilt_support)
+{
+
+	for (int i = 0; i < NUM_THRUSTERS_MAX; ++i) {
+		char buffer[17];
+		snprintf(buffer, sizeof(buffer), "CA_THRUSTER%u_PX", i);
+		_param_handles[i].position_x = param_find(buffer);
+		snprintf(buffer, sizeof(buffer), "CA_THRUSTER%u_PY", i);
+		_param_handles[i].position_y = param_find(buffer);
+		snprintf(buffer, sizeof(buffer), "CA_THRUSTER%u_PZ", i);
+		_param_handles[i].position_z = param_find(buffer);
+
+		if (_axis_config == AxisConfiguration::Configurable) {
+			snprintf(buffer, sizeof(buffer), "CA_THRUSTER%u_AX", i);
+			_param_handles[i].axis_x = param_find(buffer);
+			snprintf(buffer, sizeof(buffer), "CA_THRUSTER%u_AY", i);
+			_param_handles[i].axis_y = param_find(buffer);
+			snprintf(buffer, sizeof(buffer), "CA_THRUSTER%u_AZ", i);
+			_param_handles[i].axis_z = param_find(buffer);
+		}
+
+		snprintf(buffer, sizeof(buffer), "CA_THRUSTER%u_CT", i);
+		_param_handles[i].thrust_coef = param_find(buffer);
+
+		if (_tilt_support) {
+			PX4_ERR("Tilt support not implemented");
+		}
+	}
+
+	_count_handle = param_find("CA_THRUSTER_CNT");
+
+	updateParams();
+}
+
+void ActuatorEffectivenessThrusters::updateParams()
+{
+	ModuleParams::updateParams();
+
+	int32_t count = 0;
+
+	if (param_get(_count_handle, &count) != 0) {
+		PX4_ERR("param_get failed");
+		return;
+	}
+
+	_geometry.num_thrusters = math::min(NUM_THRUSTERS_MAX, (int)count);
+
+	for (int i = 0; i < _geometry.num_thrusters; ++i) {
+		Vector3f &position = _geometry.thrusters[i].position;
+		param_get(_param_handles[i].position_x, &position(0));
+		param_get(_param_handles[i].position_y, &position(1));
+		param_get(_param_handles[i].position_z, &position(2));
+
+		Vector3f &axis = _geometry.thrusters[i].axis;
+
+		switch (_axis_config) {
+		case AxisConfiguration::Configurable:
+			param_get(_param_handles[i].axis_x, &axis(0));
+			param_get(_param_handles[i].axis_y, &axis(1));
+			param_get(_param_handles[i].axis_z, &axis(2));
+			break;
+		}
+
+		param_get(_param_handles[i].thrust_coef, &_geometry.thrusters[i].thrust_coef);
+	}
+}
+
+bool
+ActuatorEffectivenessThrusters::addActuators(Configuration &configuration)
+{
+	if (configuration.num_actuators[(int)ActuatorType::SERVOS] > 0) {
+		PX4_ERR("Wrong actuator ordering: servos need to be after motors");
+		return false;
+	}
+
+	int num_actuators = computeEffectivenessMatrix(_geometry,
+			    configuration.effectiveness_matrices[configuration.selected_matrix],
+			    configuration.num_actuators_matrix[configuration.selected_matrix]);
+	configuration.actuatorsAdded(ActuatorType::THRUSTERS, num_actuators);
+	return true;
+}
+
+int
+ActuatorEffectivenessThrusters::computeEffectivenessMatrix(const Geometry &geometry,
+		EffectivenessMatrix &effectiveness, int actuator_start_index)
+{
+	int num_actuators = 0;
+
+	for (int i = 0; i < geometry.num_thrusters; i++) {
+
+		if (i + actuator_start_index >= NUM_ACTUATORS) {
+			break;
+		}
+
+		++num_actuators;
+
+		// Get rotor axis
+		Vector3f axis = geometry.thrusters[i].axis;
+
+		// Normalize axis
+		float axis_norm = axis.norm();
+
+		if (axis_norm > FLT_EPSILON) {
+			axis /= axis_norm;
+
+		} else {
+			// Bad axis definition, ignore this rotor
+			continue;
+		}
+
+		// Get rotor position
+		const Vector3f &position = geometry.thrusters[i].position;
+
+		// Get coefficients
+		float ct = geometry.thrusters[i].thrust_coef;
+
+		if (fabsf(ct) < FLT_EPSILON) {
+			continue;
+		}
+
+		// Compute thrust generated by this rotor
+		matrix::Vector3f thrust = ct * axis;
+
+		// Compute moment generated by this rotor
+		matrix::Vector3f moment = ct * position.cross(axis);
+
+		// Fill corresponding items in effectiveness matrix
+		for (size_t j = 0; j < 3; j++) {
+			effectiveness(j, i + actuator_start_index) = moment(j);
+			effectiveness(j + 3, i + actuator_start_index) = thrust(j);
+		}
+	}
+
+	return num_actuators;
+}
+
+
+uint32_t ActuatorEffectivenessThrusters::getThrusters() const
+{
+	uint32_t thrusters = 0;
+
+	for (int i = 0; i < _geometry.num_thrusters; ++i) {
+		thrusters |= 1u << i;
+	}
+
+	return thrusters;
+}
+
+bool
+ActuatorEffectivenessThrusters::getEffectivenessMatrix(Configuration &configuration,
+		EffectivenessUpdateReason external_update)
+{
+	if (external_update == EffectivenessUpdateReason::NO_EXTERNAL_UPDATE) {
+		return false;
+	}
+
+	return addActuators(configuration);
+}

src/lib/actuator_effectiveness/ActuatorEffectivenessThrusters.hpp

@@ -0,0 +1,121 @@
+/****************************************************************************
+ *
+ *   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 ActuatorEffectivenessThrusters.hpp
+ *
+ * Actuator effectiveness computed from thrusters position and orientation
+ *
+ * @author Pedro Roque, <padr@kth.se>
+ */
+
+#pragma once
+
+#include "ActuatorEffectiveness.hpp"
+
+#include <px4_platform_common/module_params.h>
+#include <uORB/Subscription.hpp>
+#include <uORB/SubscriptionInterval.hpp>
+
+class ActuatorEffectivenessTilts;
+
+using namespace time_literals;
+
+class ActuatorEffectivenessThrusters : public ModuleParams, public ActuatorEffectiveness
+{
+public:
+	enum class AxisConfiguration {
+		Configurable, ///< axis can be configured
+	};
+
+	static constexpr int NUM_THRUSTERS_MAX = 12;
+
+	struct ThrusterGeometry {
+		matrix::Vector3f position;
+		matrix::Vector3f axis;
+		float thrust_coef;
+	};
+
+	struct Geometry {
+		ThrusterGeometry thrusters[NUM_THRUSTERS_MAX];
+		int num_thrusters{0};
+	};
+
+	ActuatorEffectivenessThrusters(ModuleParams *parent, AxisConfiguration axis_config = AxisConfiguration::Configurable,
+				       bool tilt_support = false);
+	virtual ~ActuatorEffectivenessThrusters() = default;
+
+	bool getEffectivenessMatrix(Configuration &configuration, EffectivenessUpdateReason external_update) override;
+
+	void getDesiredAllocationMethod(AllocationMethod allocation_method_out[MAX_NUM_MATRICES]) const override
+	{
+		allocation_method_out[0] =
+			AllocationMethod::SEQUENTIAL_DESATURATION;  // TODO(@Jaeyoung-Lim): needs to be updated based on metric mixer
+	}
+
+	void getNormalizeRPY(bool normalize[MAX_NUM_MATRICES]) const override
+	{
+		// TODO(@Jaeyoung-Lim): needs to be updated based on metric mixer
+		normalize[0] = true;
+	}
+
+	static int computeEffectivenessMatrix(const Geometry &geometry,
+					      EffectivenessMatrix &effectiveness, int actuator_start_index = 0);
+
+	bool addActuators(Configuration &configuration);
+
+	const char *name() const override { return "Thrusters"; }
+
+	const Geometry &geometry() const { return _geometry; }
+
+	uint32_t getThrusters() const;
+
+private:
+	void updateParams() override;
+	const AxisConfiguration _axis_config;
+	const bool _tilt_support; ///< if true, tilt servo assignment params are loaded
+
+	struct ParamHandles {
+		param_t position_x;
+		param_t position_y;
+		param_t position_z;
+		param_t axis_x;
+		param_t axis_y;
+		param_t axis_z;
+		param_t thrust_coef;
+	};
+	ParamHandles _param_handles[NUM_THRUSTERS_MAX];
+	param_t _count_handle;
+
+	Geometry _geometry{};
+};

src/lib/actuator_effectiveness/ActuatorEffectivenessThrustersTest.cpp

@@ -0,0 +1,135 @@
+/****************************************************************************
+ *
+ *   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 ActuatorEffectivenessRotors.cpp
+ *
+ * Tests for Control Allocation Algorithms
+ *
+ * @author Julien Lecoeur <julien.lecoeur@gmail.com>
+ */
+
+#include <gtest/gtest.h>
+#include "ActuatorEffectivenessRotors.hpp"
+
+using namespace matrix;
+
+TEST(ActuatorEffectivenessRotors, AllZeroCase)
+{
+	// Quad wide geometry
+	ActuatorEffectivenessRotors::Geometry geometry = {};
+	geometry.rotors[0].position(0) = 1.0f;
+	geometry.rotors[0].position(1) = 1.0f;
+	geometry.rotors[0].position(2) = 0.0f;
+	geometry.rotors[0].axis(0) = 0.0f;
+	geometry.rotors[0].axis(1) = 0.0f;
+	geometry.rotors[0].axis(2) = -1.0f;
+	geometry.rotors[0].thrust_coef = 1.0f;
+	geometry.rotors[0].moment_ratio = 0.05f;
+
+	geometry.rotors[1].position(0) = -1.0f;
+	geometry.rotors[1].position(1) = -1.0f;
+	geometry.rotors[1].position(2) = 0.0f;
+	geometry.rotors[1].axis(0) = 0.0f;
+	geometry.rotors[1].axis(1) = 0.0f;
+	geometry.rotors[1].axis(2) = -1.0f;
+	geometry.rotors[1].thrust_coef = 1.0f;
+	geometry.rotors[1].moment_ratio = 0.05f;
+
+	geometry.rotors[2].position(0) = 1.0f;
+	geometry.rotors[2].position(1) = -1.0f;
+	geometry.rotors[2].position(2) = 0.0f;
+	geometry.rotors[2].axis(0) = 0.0f;
+	geometry.rotors[2].axis(1) = 0.0f;
+	geometry.rotors[2].axis(2) = -1.0f;
+	geometry.rotors[2].thrust_coef = 1.0f;
+	geometry.rotors[2].moment_ratio = -0.05f;
+
+	geometry.rotors[3].position(0) = -1.0f;
+	geometry.rotors[3].position(1) = 1.0f;
+	geometry.rotors[3].position(2) = 0.0f;
+	geometry.rotors[3].axis(0) = 0.0f;
+	geometry.rotors[3].axis(1) = 0.0f;
+	geometry.rotors[3].axis(2) = -1.0f;
+	geometry.rotors[3].thrust_coef = 1.0f;
+	geometry.rotors[3].moment_ratio = -0.05f;
+
+	geometry.num_rotors = 4;
+
+	ActuatorEffectiveness::EffectivenessMatrix effectiveness;
+	effectiveness.setZero();
+	ActuatorEffectivenessRotors::computeEffectivenessMatrix(geometry, effectiveness);
+
+	const float expected[ActuatorEffectiveness::NUM_AXES][ActuatorEffectiveness::NUM_ACTUATORS] = {
+		{-1.0f,   1.0f,   1.0f,  -1.0f,  0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f},
+		{ 1.0f,  -1.0f,   1.0f,  -1.0f,  0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f},
+		{ 0.05f,  0.05f, -0.05f, -0.05f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f},
+		{ 0.f,    0.f,    0.f,    0.f,   0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f},
+		{ 0.f,    0.f,    0.f,    0.f,   0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f},
+		{-1.0f,  -1.0f,  -1.0f,  -1.0f,  0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f}
+	};
+	ActuatorEffectiveness::EffectivenessMatrix effectiveness_expected(expected);
+
+	EXPECT_EQ(effectiveness, effectiveness_expected);
+}
+
+TEST(ActuatorEffectivenessRotors, Tilt)
+{
+	Vector3f axis_expected{0.f, 0.f, -1.f};
+	Vector3f axis = ActuatorEffectivenessRotors::tiltedAxis(0.f, 0.f);
+	EXPECT_EQ(axis, axis_expected);
+
+	axis_expected = Vector3f{1.f, 0.f, 0.f};
+	axis = ActuatorEffectivenessRotors::tiltedAxis(M_PI_F / 2.f, 0.f);
+	EXPECT_EQ(axis, axis_expected);
+
+	axis_expected = Vector3f{1.f / sqrtf(2.f), 0.f, -1.f / sqrtf(2.f)};
+	axis = ActuatorEffectivenessRotors::tiltedAxis(M_PI_F / 2.f / 2.f, 0.f);
+	EXPECT_EQ(axis, axis_expected);
+
+	axis_expected = Vector3f{-1.f, 0.f, 0.f};
+	axis = ActuatorEffectivenessRotors::tiltedAxis(-M_PI_F / 2.f, 0.f);
+	EXPECT_EQ(axis, axis_expected);
+
+	axis_expected = Vector3f{0.f, 0.f, -1.f};
+	axis = ActuatorEffectivenessRotors::tiltedAxis(0.f, M_PI_F / 2.f);
+	EXPECT_EQ(axis, axis_expected);
+
+	axis_expected = Vector3f{0.f, 1.f, 0.f};
+	axis = ActuatorEffectivenessRotors::tiltedAxis(M_PI_F / 2.f, M_PI_F / 2.f);
+	EXPECT_EQ(axis, axis_expected);
+
+	axis_expected = Vector3f{0.f, -1.f, 0.f};
+	axis = ActuatorEffectivenessRotors::tiltedAxis(-M_PI_F / 2.f, M_PI_F / 2.f);
+	EXPECT_EQ(axis, axis_expected);
+}

src/lib/actuator_effectiveness/CMakeLists.txt

@@ -0,0 +1,58 @@
+############################################################################
+#
+#   Copyright (c) 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.
+#
+############################################################################
+
+px4_add_library(ActuatorEffectiveness
+	ActuatorEffectiveness.cpp
+	ActuatorEffectiveness.hpp
+<<<<<<<< HEAD:src/lib/actuator_effectiveness/CMakeLists.txt
+	ActuatorEffectivenessRotors.cpp
+	ActuatorEffectivenessRotors.hpp
+	ActuatorEffectivenessThrusters.cpp
+	ActuatorEffectivenessThrusters.hpp
+========
+>>>>>>>> origin:src/lib/control_allocation/actuator_effectiveness/CMakeLists.txt
+)
+
+target_compile_options(ActuatorEffectiveness PRIVATE ${MAX_CUSTOM_OPT_LEVEL})
+target_include_directories(ActuatorEffectiveness PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})
+target_link_libraries(ActuatorEffectiveness
+	PRIVATE
+		mathlib
+		PID
+)
+<<<<<<<< HEAD:src/lib/actuator_effectiveness/CMakeLists.txt
+
+px4_add_functional_gtest(SRC ActuatorEffectivenessRotorsTest.cpp LINKLIBS ActuatorEffectiveness)
+px4_add_functional_gtest(SRC ActuatorEffectivenessThrustersTest.cpp LINKLIBS ActuatorEffectiveness)
+========
+>>>>>>>> origin:src/lib/control_allocation/actuator_effectiveness/CMakeLists.txt

src/lib/control_allocation/CMakeLists.txt

@@ -1,6 +1,10 @@
 ############################################################################
 #
+<<<<<<< HEAD
+#   Copyright (c) 2019 PX4 Development Team. All rights reserved.
+=======
 #   Copyright (c) 2025 PX4 Development Team. All rights reserved.
+>>>>>>> origin
 #
 # Redistribution and use in source and binary forms, with or without
 # modification, are permitted provided that the following conditions
@@ -31,5 +35,22 @@
 #
 ############################################################################
 
+px4_add_library(ControlAllocation
+	ControlAllocation.cpp
+	ControlAllocation.hpp
+	ControlAllocationMetric.cpp
+	ControlAllocationMetric.hpp
+	ControlAllocationPseudoInverse.cpp
+	ControlAllocationPseudoInverse.hpp
+	ControlAllocationSequentialDesaturation.cpp
+	ControlAllocationSequentialDesaturation.hpp
+)
+target_compile_options(ControlAllocation PRIVATE ${MAX_CUSTOM_OPT_LEVEL})
+target_include_directories(ControlAllocation PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})
+target_link_libraries(ControlAllocation PRIVATE mathlib)
+
+px4_add_unit_gtest(SRC ControlAllocationPseudoInverseTest.cpp LINKLIBS ControlAllocation)
+px4_add_unit_gtest(SRC ControlAllocationMetric.cpp LINKLIBS ControlAllocation)
+# px4_add_functional_gtest(SRC ControlAllocationSequentialDesaturationTest.cpp LINKLIBS ControlAllocation ActuatorEffectiveness)
 add_subdirectory(control_allocation)
 add_subdirectory(actuator_effectiveness)

src/lib/control_allocation/ControlAllocation.cpp

@@ -0,0 +1,126 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 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 ControlAllocation.cpp
+ *
+ * Interface for Control Allocation Algorithms
+ *
+ * @author Julien Lecoeur <julien.lecoeur@gmail.com>
+ */
+
+#include "ControlAllocation.hpp"
+
+ControlAllocation::ControlAllocation()
+{
+	_control_allocation_scale.setAll(1.f);
+	_actuator_min.setAll(0.f);
+	_actuator_max.setAll(1.f);
+}
+
+void
+ControlAllocation::setEffectivenessMatrix(
+	const matrix::Matrix<float, ControlAllocation::NUM_AXES, ControlAllocation::NUM_ACTUATORS> &effectiveness,
+	const ActuatorVector &actuator_trim, const ActuatorVector &linearization_point, int num_actuators,
+	bool update_normalization_scale)
+{
+	_effectiveness = effectiveness;
+	ActuatorVector linearization_point_clipped = linearization_point;
+	clipActuatorSetpoint(linearization_point_clipped);
+	_actuator_trim = actuator_trim + linearization_point_clipped;
+	clipActuatorSetpoint(_actuator_trim);
+	_num_actuators = num_actuators;
+	_control_trim = _effectiveness * linearization_point_clipped;
+}
+
+void
+ControlAllocation::setActuatorSetpoint(
+	const matrix::Vector<float, ControlAllocation::NUM_ACTUATORS> &actuator_sp)
+{
+	// Set actuator setpoint
+	_actuator_sp = actuator_sp;
+
+	// Clip
+	clipActuatorSetpoint(_actuator_sp);
+}
+
+void
+ControlAllocation::clipActuatorSetpoint(matrix::Vector<float, ControlAllocation::NUM_ACTUATORS> &actuator) const
+{
+	for (int i = 0; i < _num_actuators; i++) {
+		if (_actuator_max(i) < _actuator_min(i)) {
+			actuator(i) = _actuator_trim(i);
+
+		} else if (actuator(i) < _actuator_min(i)) {
+			actuator(i) = _actuator_min(i);
+
+		} else if (actuator(i) > _actuator_max(i)) {
+			actuator(i) = _actuator_max(i);
+		}
+	}
+}
+
+matrix::Vector<float, ControlAllocation::NUM_ACTUATORS>
+ControlAllocation::normalizeActuatorSetpoint(const matrix::Vector<float, ControlAllocation::NUM_ACTUATORS> &actuator)
+const
+{
+	matrix::Vector<float, ControlAllocation::NUM_ACTUATORS> actuator_normalized;
+
+	for (int i = 0; i < _num_actuators; i++) {
+		if (_actuator_min(i) < _actuator_max(i)) {
+			actuator_normalized(i) = (actuator(i) - _actuator_min(i)) / (_actuator_max(i) - _actuator_min(i));
+
+		} else {
+			actuator_normalized(i) = (_actuator_trim(i) - _actuator_min(i)) / (_actuator_max(i) - _actuator_min(i));
+		}
+	}
+
+	return actuator_normalized;
+}
+
+void ControlAllocation::applySlewRateLimit(float dt)
+{
+	for (int i = 0; i < _num_actuators; i++) {
+		if (_actuator_slew_rate_limit(i) > FLT_EPSILON) {
+			float delta_sp_max = dt * (_actuator_max(i) - _actuator_min(i)) / _actuator_slew_rate_limit(i);
+			float delta_sp = _actuator_sp(i) - _prev_actuator_sp(i);
+
+			if (delta_sp > delta_sp_max) {
+				_actuator_sp(i) = _prev_actuator_sp(i) + delta_sp_max;
+
+			} else if (delta_sp < -delta_sp_max) {
+				_actuator_sp(i) = _prev_actuator_sp(i) - delta_sp_max;
+			}
+		}
+	}
+}

src/lib/control_allocation/ControlAllocation.hpp

@@ -0,0 +1,247 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 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 ControlAllocation.hpp
+ *
+ * Interface for Control Allocation Algorithms
+ *
+ * Implementers of this interface are expected to update the members
+ * of this base class in the `allocate` method.
+ *
+ * Example usage:
+ * ```
+ * [...]
+ * // Initialization
+ * ControlAllocationMethodImpl alloc();
+ * alloc.setEffectivenessMatrix(effectiveness, actuator_trim);
+ * alloc.setActuatorMin(actuator_min);
+ * alloc.setActuatorMin(actuator_max);
+ *
+ * while (1) {
+ * 	[...]
+ *
+ * 	// Set control setpoint, allocate actuator setpoint, retrieve actuator setpoint
+ * 	alloc.setControlSetpoint(control_sp);
+ * 	alloc.allocate();
+ * 	actuator_sp = alloc.getActuatorSetpoint();
+ *
+ * 	// Check if the control setpoint was fully allocated
+ *	unallocated_control = control_sp - alloc.getAllocatedControl()
+ *
+ *	[...]
+ * }
+ * ```
+ *
+ *
+ * @author Julien Lecoeur <julien.lecoeur@gmail.com>
+ */
+
+#pragma once
+
+#include <matrix/matrix/math.hpp>
+
+#include "actuator_effectiveness/ActuatorEffectiveness.hpp"
+
+class ControlAllocation
+{
+public:
+	ControlAllocation();
+	virtual ~ControlAllocation() = default;
+
+	static constexpr uint8_t NUM_ACTUATORS = ActuatorEffectiveness::NUM_ACTUATORS;
+	static constexpr uint8_t NUM_AXES = ActuatorEffectiveness::NUM_AXES;
+
+	typedef matrix::Vector<float, NUM_ACTUATORS> ActuatorVector;
+
+	enum ControlAxis {
+		ROLL = 0,
+		PITCH,
+		YAW,
+		THRUST_X,
+		THRUST_Y,
+		THRUST_Z
+	};
+
+	/**
+	 * Allocate control setpoint to actuators
+	 */
+	virtual void allocate() = 0;
+
+	/**
+	 * Set actuator failure flag
+	 * This prevents a change of the scaling in the matrix normalization step
+	 * in case of a motor failure.
+	 *
+	 * @param failure  Motor failure flag
+	 */
+	void setHadActuatorFailure(bool failure) { _had_actuator_failure = failure; }
+
+	/**
+	 * Set the control effectiveness matrix
+	 *
+	 * @param B Effectiveness matrix
+	 */
+	virtual void setEffectivenessMatrix(const matrix::Matrix<float, NUM_AXES, NUM_ACTUATORS> &effectiveness,
+					    const ActuatorVector &actuator_trim, const ActuatorVector &linearization_point, int num_actuators,
+					    bool update_normalization_scale);
+
+	/**
+	 * Get the allocated actuator vector
+	 *
+	 * @return Actuator vector
+	 */
+	const matrix::Vector<float, NUM_ACTUATORS> &getActuatorSetpoint() const { return _actuator_sp; }
+
+	/**
+	 * Set the desired control vector
+	 *
+	 * @param Control vector
+	 */
+	void setControlSetpoint(const matrix::Vector<float, NUM_AXES> &control) { _control_sp = control; }
+
+	/**
+	 * Get the desired control vector
+	 *
+	 * @return Control vector
+	 */
+	const matrix::Vector<float, NUM_AXES> &getControlSetpoint() const { return _control_sp; }
+
+	/**
+	 * Get the allocated control vector
+	 *
+	 * @return Control vector
+	 */
+	matrix::Vector<float, NUM_AXES> getAllocatedControl() const
+	{ return (_effectiveness * (_actuator_sp - _actuator_trim)).emult(_control_allocation_scale); }
+
+	/**
+	 * Get the control effectiveness matrix
+	 *
+	 * @return Effectiveness matrix
+	 */
+	const matrix::Matrix<float, NUM_AXES, NUM_ACTUATORS> &getEffectivenessMatrix() const { return _effectiveness; }
+
+	/**
+	 * Set the minimum actuator values
+	 *
+	 * @param actuator_min Minimum actuator values
+	 */
+	void setActuatorMin(const matrix::Vector<float, NUM_ACTUATORS> &actuator_min) { _actuator_min = actuator_min; }
+
+	/**
+	 * Get the minimum actuator values
+	 *
+	 * @return Minimum actuator values
+	 */
+	const matrix::Vector<float, NUM_ACTUATORS> &getActuatorMin() const { return _actuator_min; }
+
+	/**
+	 * Set the maximum actuator values
+	 *
+	 * @param actuator_max Maximum actuator values
+	 */
+	void setActuatorMax(const matrix::Vector<float, NUM_ACTUATORS> &actuator_max) { _actuator_max = actuator_max; }
+
+	/**
+	 * Get the maximum actuator values
+	 *
+	 * @return Maximum actuator values
+	 */
+	const matrix::Vector<float, NUM_ACTUATORS> &getActuatorMax() const { return _actuator_max; }
+
+	/**
+	 * Set the current actuator setpoint.
+	 *
+	 * Use this when a new allocation method is started to initialize it properly.
+	 * In most cases, it is not needed to call this method before `allocate()`.
+	 * Indeed the previous actuator setpoint is expected to be stored during calls to `allocate()`.
+	 *
+	 * @param actuator_sp Actuator setpoint
+	 */
+	void setActuatorSetpoint(const matrix::Vector<float, NUM_ACTUATORS> &actuator_sp);
+
+	void setSlewRateLimit(const matrix::Vector<float, NUM_ACTUATORS> &slew_rate_limit)
+	{ _actuator_slew_rate_limit = slew_rate_limit; }
+
+	/**
+	 * Apply slew rate to current actuator setpoint
+	 */
+	void applySlewRateLimit(float dt);
+
+	/**
+	 * Clip the actuator setpoint between minimum and maximum values.
+	 *
+	 * The output is in the range [min; max]
+	 *
+	 * @param actuator Actuator vector to clip
+	 */
+	void clipActuatorSetpoint(matrix::Vector<float, NUM_ACTUATORS> &actuator) const;
+
+	void clipActuatorSetpoint() { clipActuatorSetpoint(_actuator_sp); }
+
+	/**
+	 * Normalize the actuator setpoint between minimum and maximum values.
+	 *
+	 * The output is in the range [-1; +1]
+	 *
+	 * @param actuator Actuator vector to normalize
+	 *
+	 * @return Clipped actuator setpoint
+	 */
+	matrix::Vector<float, NUM_ACTUATORS> normalizeActuatorSetpoint(const matrix::Vector<float, NUM_ACTUATORS> &actuator)
+	const;
+
+	virtual void updateParameters() {}
+
+	int numConfiguredActuators() const { return _num_actuators; }
+
+	void setNormalizeRPY(bool normalize_rpy) { _normalize_rpy = normalize_rpy; }
+
+protected:
+	friend class ControlAllocator; // for _actuator_sp
+
+	matrix::Matrix<float, NUM_AXES, NUM_ACTUATORS> _effectiveness;  ///< Effectiveness matrix
+	matrix::Vector<float, NUM_AXES> _control_allocation_scale;  	///< Scaling applied during allocation
+	matrix::Vector<float, NUM_ACTUATORS> _actuator_trim; 	///< Neutral actuator values
+	matrix::Vector<float, NUM_ACTUATORS> _actuator_min; 	///< Minimum actuator values
+	matrix::Vector<float, NUM_ACTUATORS> _actuator_max; 	///< Maximum actuator values
+	matrix::Vector<float, NUM_ACTUATORS> _actuator_slew_rate_limit; 	///< Slew rate limit
+	matrix::Vector<float, NUM_ACTUATORS> _prev_actuator_sp;  	///< Previous actuator setpoint
+	matrix::Vector<float, NUM_ACTUATORS> _actuator_sp;  	///< Actuator setpoint
+	matrix::Vector<float, NUM_AXES> _control_sp;   		///< Control setpoint
+	matrix::Vector<float, NUM_AXES> _control_trim; 		///< Control at trim actuator values
+	int _num_actuators{0};
+	bool _normalize_rpy{false};				///< if true, normalize roll, pitch and yaw columns
+	bool _had_actuator_failure{false};
+};

src/lib/control_allocation/ControlAllocationMetric.cpp

@@ -0,0 +1,75 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2024 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 ControlAllocationMetric.hpp
+ *
+ * Control allocation with metric units.
+ *
+ * @author Pedro Roque <padr@kth.se>
+ */
+
+#include "ControlAllocationMetric.hpp"
+#include <px4_platform_common/log.h>
+
+void
+ControlAllocationMetric::setEffectivenessMatrix(
+	const matrix::Matrix<float, ControlAllocation::NUM_AXES, ControlAllocation::NUM_ACTUATORS> &effectiveness,
+	const ActuatorVector &actuator_trim, const ActuatorVector &linearization_point, int num_actuators,
+	bool update_normalization_scale)
+{
+	ControlAllocation::setEffectivenessMatrix(effectiveness, actuator_trim, linearization_point, num_actuators,
+			update_normalization_scale);
+	_mix_update_needed = true;
+}
+
+void
+ControlAllocationMetric::updatePseudoInverse()
+{
+	if (_mix_update_needed) {
+		matrix::geninv(_effectiveness, _mix);
+		_mix_update_needed = false;
+	}
+}
+
+void
+ControlAllocationMetric::allocate()
+{
+	//Compute new gains if needed
+	updatePseudoInverse();
+
+	_prev_actuator_sp = _actuator_sp;
+
+	// Allocate
+	_actuator_sp = _actuator_trim + _mix * (_control_sp - _control_trim);
+}

src/lib/control_allocation/ControlAllocationMetric.hpp

@@ -0,0 +1,74 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2024 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 ControlAllocationMetric.hpp
+ *
+ * Control allocation with metric units.
+ *
+ * The metric allocation takes into account maximum thrust and torque
+ * provided by the actuators to calculate setpoints that allow to allocate
+ * specific body force and torque, keeping the scale of the target setpoints.
+ * No normalization is done in this case. Note that, similarly to the PseudoInverse
+ * allocation, saturation is handled by clipping.
+ *
+ * @author Pedro Roque <padr@kth.se>
+ */
+
+#pragma once
+
+#include "ControlAllocation.hpp"
+
+class ControlAllocationMetric: public ControlAllocation
+{
+public:
+	ControlAllocationMetric() = default;
+	virtual ~ControlAllocationMetric() = default;
+
+	void allocate() override;
+	void setEffectivenessMatrix(const matrix::Matrix<float, NUM_AXES, NUM_ACTUATORS> &effectiveness,
+				    const ActuatorVector &actuator_trim, const ActuatorVector &linearization_point, int num_actuators,
+				    bool update_normalization_scale) override;
+
+protected:
+	matrix::Matrix<float, NUM_ACTUATORS, NUM_AXES> _mix;
+
+	bool _mix_update_needed{false};
+
+	/**
+	 * Recalculate pseudo inverse if required.
+	 *
+	 */
+	void updatePseudoInverse();
+
+};

src/lib/control_allocation/ControlAllocationMetricTest.cpp

@@ -0,0 +1,215 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2024 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 ControlAllocationTest.cpp
+ *
+ * Tests for Control Allocation metric class
+ *
+ * @author Pedro Roque <padr@kth.se>
+ */
+
+#include <gtest/gtest.h>
+#include <ControlAllocationMetric.hpp>
+#include <../ActuatorEffectiveness/ActuatorEffectivenessThrusters.hpp>
+#include <iostream>
+
+using namespace matrix;
+
+namespace
+{
+	// Creates a 2D spacecraft with 8 thrusters (one in each corner of the axis, NED Frame):
+	//           1 <--> 0
+	//               x
+	//		 |
+	//      ^6 v7    +----y  ^4 v5
+	//
+	//
+	//           3 <--> 2
+	ActuatorEffectivenessThrusters::Geometry make_spacecraft_x_geometry()
+	{
+		ActuatorEffectivenessThrusters::Geometry geometry = {};
+		geometry.thrusters[0].position(0) = 1.0f;
+		geometry.thrusters[0].position(1) = 0.0f;
+		geometry.thrusters[0].position(2) = 0.0f;
+		geometry.thrusters[0].axis(0) = 0.0f;
+		geometry.thrusters[0].axis(1) = 1.0f;
+		geometry.thrusters[0].axis(2) = 0.0f;
+		geometry.thrusters[0].thrust_coef = 2.0f;
+
+		geometry.thrusters[1].position(0) = 1.0f;
+		geometry.thrusters[1].position(1) = 0.0f;
+		geometry.thrusters[1].position(2) = 0.0f;
+		geometry.thrusters[1].axis(0) = 0.0f;
+		geometry.thrusters[1].axis(1) = -1.0f;
+		geometry.thrusters[1].axis(2) = 0.0f;
+		geometry.thrusters[1].thrust_coef = 2.0f;
+
+		geometry.thrusters[2].position(0) = -1.0f;
+		geometry.thrusters[2].position(1) = 0.0f;
+		geometry.thrusters[2].position(2) = 0.0f;
+		geometry.thrusters[2].axis(0) = 0.0f;
+		geometry.thrusters[2].axis(1) = 1.0f;
+		geometry.thrusters[2].axis(2) = 0.0f;
+		geometry.thrusters[2].thrust_coef = 2.0f;
+
+		geometry.thrusters[3].position(0) = -1.0f;
+		geometry.thrusters[3].position(1) = 0.0f;
+		geometry.thrusters[3].position(2) = 0.0f;
+		geometry.thrusters[3].axis(0) = 0.0f;
+		geometry.thrusters[3].axis(1) = -1.0f;
+		geometry.thrusters[3].axis(2) = 0.0f;
+		geometry.thrusters[3].thrust_coef = 2.0f;
+
+		geometry.thrusters[4].position(0) = 0.0f;
+		geometry.thrusters[4].position(1) = 1.0f;
+		geometry.thrusters[4].position(2) = 0.0f;
+		geometry.thrusters[4].axis(0) = 1.0f;
+		geometry.thrusters[4].axis(1) = 0.0f;
+		geometry.thrusters[4].axis(2) = 0.0f;
+		geometry.thrusters[4].thrust_coef = 2.0f;
+
+		geometry.thrusters[5].position(0) = 0.0f;
+		geometry.thrusters[5].position(1) = 1.0f;
+		geometry.thrusters[5].position(2) = 0.0f;
+		geometry.thrusters[5].axis(0) = -1.0f;
+		geometry.thrusters[5].axis(1) = 0.0f;
+		geometry.thrusters[5].axis(2) = 0.0f;
+		geometry.thrusters[5].thrust_coef = 2.0f;
+
+		geometry.thrusters[6].position(0) = 0.0f;
+		geometry.thrusters[6].position(1) = -1.0f;
+		geometry.thrusters[6].position(2) = 0.0f;
+		geometry.thrusters[6].axis(0) = 1.0f;
+		geometry.thrusters[6].axis(1) = 0.0f;
+		geometry.thrusters[6].axis(2) = 0.0f;
+		geometry.thrusters[6].thrust_coef = 2.0f;
+
+		geometry.thrusters[7].position(0) = 0.0f;
+		geometry.thrusters[7].position(1) = -1.0f;
+		geometry.thrusters[7].position(2) = 0.0f;
+		geometry.thrusters[7].axis(0) = -1.0f;
+		geometry.thrusters[7].axis(1) = 0.0f;
+		geometry.thrusters[7].axis(2) = 0.0f;
+		geometry.thrusters[7].thrust_coef = 2.0f;
+
+		geometry.num_thrusters = 8;
+
+		return geometry;
+	}
+
+	ActuatorEffectiveness::EffectivenessMatrix make_spacecraft_x_effectiveness()
+	{
+		ActuatorEffectiveness::EffectivenessMatrix effectiveness;
+		effectiveness.setZero();
+		const auto geometry = make_spacecraft_x_geometry();
+		ActuatorEffectivenessThrusters::computeEffectivenessMatrix(geometry, effectiveness);
+		PX4_INFO("Effectiveness matrix (transposed): ");
+		effectiveness.T().print();
+		return effectiveness;
+	}
+
+	void setup_spacecraft_allocator(ControlAllocationMetric &allocator)
+	{
+		const auto effectiveness = make_spacecraft_x_effectiveness();
+		matrix::Vector<float, ActuatorEffectiveness::NUM_ACTUATORS> actuator_trim;
+		matrix::Vector<float, ActuatorEffectiveness::NUM_ACTUATORS> linearization_point;
+		constexpr bool UPDATE_NORMALIZATION_SCALE{false};
+		allocator.setEffectivenessMatrix(
+			effectiveness,
+			actuator_trim,
+			linearization_point,
+			ActuatorEffectiveness::NUM_ACTUATORS,
+			UPDATE_NORMALIZATION_SCALE
+		);
+	}
+} // namespace
+
+
+TEST(ControlAllocationMetricTest, AllZeroCase)
+{
+	ControlAllocationMetric method;
+
+	matrix::Vector<float, 6> control_sp;
+	matrix::Vector<float, 6> control_allocated;
+	matrix::Vector<float, 6> control_allocated_expected;
+	matrix::Matrix<float, 6, 16> effectiveness;
+	matrix::Vector<float, 16> actuator_sp;
+	matrix::Vector<float, 16> actuator_trim;
+	matrix::Vector<float, 16> linearization_point;
+	matrix::Vector<float, 16> actuator_sp_expected;
+
+	method.setEffectivenessMatrix(effectiveness, actuator_trim, linearization_point, 16, false);
+	method.setControlSetpoint(control_sp);
+	method.allocate();
+	method.clipActuatorSetpoint();
+	actuator_sp = method.getActuatorSetpoint();
+	control_allocated_expected = method.getAllocatedControl();
+
+	EXPECT_EQ(actuator_sp, actuator_sp_expected);
+	EXPECT_EQ(control_allocated, control_allocated_expected);
+}
+
+
+TEST(ControlAllocationMetricTest, MetricOutputProportional)
+{
+	ControlAllocationMetric allocator;
+	setup_spacecraft_allocator(allocator);
+	matrix::Vector<float, ActuatorEffectiveness::NUM_AXES> control_sp;
+
+	control_sp(ControlAllocation::ControlAxis::ROLL) = 0.f;
+	control_sp(ControlAllocation::ControlAxis::PITCH) = 0.f;
+	control_sp(ControlAllocation::ControlAxis::YAW) = 0.f;
+	control_sp(ControlAllocation::ControlAxis::THRUST_X) = 0.5f; // newton
+	control_sp(ControlAllocation::ControlAxis::THRUST_Y) = 0.f;
+	control_sp(ControlAllocation::ControlAxis::THRUST_Z) = 0.f;
+	allocator.setControlSetpoint(control_sp);
+
+	allocator.allocate();
+
+	const auto &actuator_sp = allocator.getActuatorSetpoint();
+
+	// print each actuator setpoint
+	actuator_sp.print();
+
+	// ensure that outputs match expected
+	EXPECT_NEAR(actuator_sp(0), 0.0f, 1e-4);
+	EXPECT_NEAR(actuator_sp(1), 0.0f, 1e-4);
+	EXPECT_NEAR(actuator_sp(2), 0.0f, 1e-4);
+	EXPECT_NEAR(actuator_sp(3), 0.0f, 1e-4);
+	EXPECT_NEAR(actuator_sp(4), 0.06250f, 1e-4);
+	EXPECT_NEAR(actuator_sp(5), -0.06250f, 1e-4);
+	EXPECT_NEAR(actuator_sp(6), 0.06250f, 1e-4);
+	EXPECT_NEAR(actuator_sp(7), -0.06250f, 1e-4);
+
+}

src/lib/control_allocation/ControlAllocationPseudoInverse.cpp

@@ -0,0 +1,181 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 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 ControlAllocationPseudoInverse.cpp
+ *
+ * Simple Control Allocation Algorithm
+ *
+ * @author Julien Lecoeur <julien.lecoeur@gmail.com>
+ */
+
+#include "ControlAllocationPseudoInverse.hpp"
+
+void
+ControlAllocationPseudoInverse::setEffectivenessMatrix(
+	const matrix::Matrix<float, ControlAllocation::NUM_AXES, ControlAllocation::NUM_ACTUATORS> &effectiveness,
+	const ActuatorVector &actuator_trim, const ActuatorVector &linearization_point, int num_actuators,
+	bool update_normalization_scale)
+{
+	ControlAllocation::setEffectivenessMatrix(effectiveness, actuator_trim, linearization_point, num_actuators,
+			update_normalization_scale);
+	_mix_update_needed = true;
+	_normalization_needs_update = update_normalization_scale;
+}
+
+void
+ControlAllocationPseudoInverse::updatePseudoInverse()
+{
+	if (_mix_update_needed) {
+		matrix::geninv(_effectiveness, _mix);
+
+		if (_normalization_needs_update && !_had_actuator_failure) {
+			updateControlAllocationMatrixScale();
+			_normalization_needs_update = false;
+		}
+
+		normalizeControlAllocationMatrix();
+		_mix_update_needed = false;
+	}
+}
+
+void
+ControlAllocationPseudoInverse::updateControlAllocationMatrixScale()
+{
+	// Same scale on roll and pitch
+	if (_normalize_rpy) {
+
+		int num_non_zero_roll_torque = 0;
+		int num_non_zero_pitch_torque = 0;
+
+		for (int i = 0; i < _num_actuators; i++) {
+
+			if (fabsf(_mix(i, 0)) > 1e-3f) {
+				++num_non_zero_roll_torque;
+			}
+
+			if (fabsf(_mix(i, 1)) > 1e-3f) {
+				++num_non_zero_pitch_torque;
+			}
+		}
+
+		float roll_norm_scale = 1.f;
+
+		if (num_non_zero_roll_torque > 0) {
+			roll_norm_scale = sqrtf(_mix.col(0).norm_squared() / (num_non_zero_roll_torque / 2.f));
+		}
+
+		float pitch_norm_scale = 1.f;
+
+		if (num_non_zero_pitch_torque > 0) {
+			pitch_norm_scale = sqrtf(_mix.col(1).norm_squared() / (num_non_zero_pitch_torque / 2.f));
+		}
+
+		_control_allocation_scale(0) = fmaxf(roll_norm_scale, pitch_norm_scale);
+		_control_allocation_scale(1) = _control_allocation_scale(0);
+
+		// Scale yaw separately
+		_control_allocation_scale(2) = _mix.col(2).max();
+
+	} else {
+		_control_allocation_scale(0) = 1.f;
+		_control_allocation_scale(1) = 1.f;
+		_control_allocation_scale(2) = 1.f;
+	}
+
+	// Scale thrust by the sum of the individual thrust axes, and use the scaling for the Z axis if there's no actuators
+	// (for tilted actuators)
+	_control_allocation_scale(THRUST_Z) = 1.f;
+
+	for (int axis_idx = 2; axis_idx >= 0; --axis_idx) {
+		int num_non_zero_thrust = 0;
+		float norm_sum = 0.f;
+
+		for (int i = 0; i < _num_actuators; i++) {
+			float norm = fabsf(_mix(i, 3 + axis_idx));
+			norm_sum += norm;
+
+			if (norm > FLT_EPSILON) {
+				++num_non_zero_thrust;
+			}
+		}
+
+		if (num_non_zero_thrust > 0) {
+			_control_allocation_scale(3 + axis_idx) = norm_sum / num_non_zero_thrust;
+
+		} else {
+			_control_allocation_scale(3 + axis_idx) = _control_allocation_scale(THRUST_Z);
+		}
+	}
+}
+
+void
+ControlAllocationPseudoInverse::normalizeControlAllocationMatrix()
+{
+	if (_control_allocation_scale(0) > FLT_EPSILON) {
+		_mix.col(0) /= _control_allocation_scale(0);
+		_mix.col(1) /= _control_allocation_scale(1);
+	}
+
+	if (_control_allocation_scale(2) > FLT_EPSILON) {
+		_mix.col(2) /= _control_allocation_scale(2);
+	}
+
+	if (_control_allocation_scale(3) > FLT_EPSILON) {
+		_mix.col(3) /= _control_allocation_scale(3);
+		_mix.col(4) /= _control_allocation_scale(4);
+		_mix.col(5) /= _control_allocation_scale(5);
+	}
+
+	// Set all the small elements to 0 to avoid issues
+	// in the control allocation algorithms
+	for (int i = 0; i < _num_actuators; i++) {
+		for (int j = 0; j < NUM_AXES; j++) {
+			if (fabsf(_mix(i, j)) < 1e-3f) {
+				_mix(i, j) = 0.f;
+			}
+		}
+	}
+}
+
+void
+ControlAllocationPseudoInverse::allocate()
+{
+	//Compute new gains if needed
+	updatePseudoInverse();
+
+	_prev_actuator_sp = _actuator_sp;
+
+	// Allocate
+	_actuator_sp = _actuator_trim + _mix * (_control_sp - _control_trim);
+}

src/lib/control_allocation/ControlAllocationPseudoInverse.hpp

@@ -0,0 +1,76 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 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 ControlAllocationPseudoInverse.hpp
+ *
+ * Simple Control Allocation Algorithm
+ *
+ * It computes the pseudo-inverse of the effectiveness matrix
+ * Actuator saturation is handled by simple clipping, do not
+ * expect good performance in case of actuator saturation.
+ *
+ * @author Julien Lecoeur <julien.lecoeur@gmail.com>
+ */
+
+#pragma once
+
+#include "ControlAllocation.hpp"
+
+class ControlAllocationPseudoInverse: public ControlAllocation
+{
+public:
+	ControlAllocationPseudoInverse() = default;
+	virtual ~ControlAllocationPseudoInverse() = default;
+
+	void allocate() override;
+	void setEffectivenessMatrix(const matrix::Matrix<float, NUM_AXES, NUM_ACTUATORS> &effectiveness,
+				    const ActuatorVector &actuator_trim, const ActuatorVector &linearization_point, int num_actuators,
+				    bool update_normalization_scale) override;
+
+protected:
+	matrix::Matrix<float, NUM_ACTUATORS, NUM_AXES> _mix;
+
+	bool _mix_update_needed{false};
+
+	/**
+	 * Recalculate pseudo inverse if required.
+	 *
+	 */
+	void updatePseudoInverse();
+
+private:
+	void normalizeControlAllocationMatrix();
+	void updateControlAllocationMatrixScale();
+	bool _normalization_needs_update{false};
+};

src/lib/control_allocation/ControlAllocationPseudoInverseTest.cpp

@@ -0,0 +1,69 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 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 ControlAllocationPseudoInverseTest.cpp
+ *
+ * Tests for Control Allocation Algorithms
+ *
+ * @author Julien Lecoeur <julien.lecoeur@gmail.com>
+ */
+
+#include <gtest/gtest.h>
+#include <ControlAllocationPseudoInverse.hpp>
+
+using namespace matrix;
+
+TEST(ControlAllocationTest, AllZeroCase)
+{
+	ControlAllocationPseudoInverse method;
+
+	matrix::Vector<float, 6> control_sp;
+	matrix::Vector<float, 6> control_allocated;
+	matrix::Vector<float, 6> control_allocated_expected;
+	matrix::Matrix<float, 6, 16> effectiveness;
+	matrix::Vector<float, 16> actuator_sp;
+	matrix::Vector<float, 16> actuator_trim;
+	matrix::Vector<float, 16> linearization_point;
+	matrix::Vector<float, 16> actuator_sp_expected;
+
+	method.setEffectivenessMatrix(effectiveness, actuator_trim, linearization_point, 16, false);
+	method.setControlSetpoint(control_sp);
+	method.allocate();
+	method.clipActuatorSetpoint();
+	actuator_sp = method.getActuatorSetpoint();
+	control_allocated_expected = method.getAllocatedControl();
+
+	EXPECT_EQ(actuator_sp, actuator_sp_expected);
+	EXPECT_EQ(control_allocated, control_allocated_expected);
+}

src/lib/control_allocation/ControlAllocationSequentialDesaturation.cpp

@@ -0,0 +1,234 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 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 ControlAllocationSequentialDesaturation.cpp
+ *
+ * @author Roman Bapst <bapstroman@gmail.com>
+ * @author Beat Küng <beat-kueng@gmx.net>
+ */
+
+#include "ControlAllocationSequentialDesaturation.hpp"
+
+
+void
+ControlAllocationSequentialDesaturation::allocate()
+{
+	//Compute new gains if needed
+	updatePseudoInverse();
+
+	_prev_actuator_sp = _actuator_sp;
+
+	switch (_param_mc_airmode.get()) {
+	case 1:
+		mixAirmodeRP();
+		break;
+
+	case 2:
+		mixAirmodeRPY();
+		break;
+
+	default:
+		mixAirmodeDisabled();
+		break;
+	}
+}
+
+void ControlAllocationSequentialDesaturation::desaturateActuators(
+	ActuatorVector &actuator_sp,
+	const ActuatorVector &desaturation_vector, bool increase_only)
+{
+	float gain = computeDesaturationGain(desaturation_vector, actuator_sp);
+
+	if (increase_only && gain < 0.f) {
+		return;
+	}
+
+	for (int i = 0; i < _num_actuators; i++) {
+		actuator_sp(i) += gain * desaturation_vector(i);
+	}
+
+	gain = 0.5f * computeDesaturationGain(desaturation_vector, actuator_sp);
+
+	for (int i = 0; i < _num_actuators; i++) {
+		actuator_sp(i) += gain * desaturation_vector(i);
+	}
+}
+
+float ControlAllocationSequentialDesaturation::computeDesaturationGain(const ActuatorVector &desaturation_vector,
+		const ActuatorVector &actuator_sp)
+{
+	float k_min = 0.f;
+	float k_max = 0.f;
+
+	for (int i = 0; i < _num_actuators; i++) {
+		// Do not use try to desaturate using an actuator with weak effectiveness to avoid large desaturation gains
+		if (fabsf(desaturation_vector(i)) < 0.2f) {
+			continue;
+		}
+
+		if (actuator_sp(i) < _actuator_min(i)) {
+			float k = (_actuator_min(i) - actuator_sp(i)) / desaturation_vector(i);
+
+			if (k < k_min) { k_min = k; }
+
+			if (k > k_max) { k_max = k; }
+		}
+
+		if (actuator_sp(i) > _actuator_max(i)) {
+			float k = (_actuator_max(i) - actuator_sp(i)) / desaturation_vector(i);
+
+			if (k < k_min) { k_min = k; }
+
+			if (k > k_max) { k_max = k; }
+		}
+	}
+
+	// Reduce the saturation as much as possible
+	return k_min + k_max;
+}
+
+void
+ControlAllocationSequentialDesaturation::mixAirmodeRP()
+{
+	// Airmode for roll and pitch, but not yaw
+
+	// Mix without yaw
+	ActuatorVector thrust_z;
+
+	for (int i = 0; i < _num_actuators; i++) {
+		_actuator_sp(i) = _actuator_trim(i) +
+				  _mix(i, ControlAxis::ROLL) * (_control_sp(ControlAxis::ROLL) - _control_trim(ControlAxis::ROLL)) +
+				  _mix(i, ControlAxis::PITCH) * (_control_sp(ControlAxis::PITCH) - _control_trim(ControlAxis::PITCH)) +
+				  _mix(i, ControlAxis::THRUST_X) * (_control_sp(ControlAxis::THRUST_X) - _control_trim(ControlAxis::THRUST_X)) +
+				  _mix(i, ControlAxis::THRUST_Y) * (_control_sp(ControlAxis::THRUST_Y) - _control_trim(ControlAxis::THRUST_Y)) +
+				  _mix(i, ControlAxis::THRUST_Z) * (_control_sp(ControlAxis::THRUST_Z) - _control_trim(ControlAxis::THRUST_Z));
+		thrust_z(i) = _mix(i, ControlAxis::THRUST_Z);
+	}
+
+	desaturateActuators(_actuator_sp, thrust_z);
+
+	// Mix yaw independently
+	mixYaw();
+}
+
+void
+ControlAllocationSequentialDesaturation::mixAirmodeRPY()
+{
+	// Airmode for roll, pitch and yaw
+
+	// Do full mixing
+	ActuatorVector thrust_z;
+	ActuatorVector yaw;
+
+	for (int i = 0; i < _num_actuators; i++) {
+		_actuator_sp(i) = _actuator_trim(i) +
+				  _mix(i, ControlAxis::ROLL) * (_control_sp(ControlAxis::ROLL) - _control_trim(ControlAxis::ROLL)) +
+				  _mix(i, ControlAxis::PITCH) * (_control_sp(ControlAxis::PITCH) - _control_trim(ControlAxis::PITCH)) +
+				  _mix(i, ControlAxis::YAW) * (_control_sp(ControlAxis::YAW) - _control_trim(ControlAxis::YAW)) +
+				  _mix(i, ControlAxis::THRUST_X) * (_control_sp(ControlAxis::THRUST_X) - _control_trim(ControlAxis::THRUST_X)) +
+				  _mix(i, ControlAxis::THRUST_Y) * (_control_sp(ControlAxis::THRUST_Y) - _control_trim(ControlAxis::THRUST_Y)) +
+				  _mix(i, ControlAxis::THRUST_Z) * (_control_sp(ControlAxis::THRUST_Z) - _control_trim(ControlAxis::THRUST_Z));
+		thrust_z(i) = _mix(i, ControlAxis::THRUST_Z);
+		yaw(i) = _mix(i, ControlAxis::YAW);
+	}
+
+	desaturateActuators(_actuator_sp, thrust_z);
+
+	// Unsaturate yaw (in case upper and lower bounds are exceeded)
+	// to prioritize roll/pitch over yaw.
+	desaturateActuators(_actuator_sp, yaw);
+}
+
+void
+ControlAllocationSequentialDesaturation::mixAirmodeDisabled()
+{
+	// Airmode disabled: never allow to increase the thrust to unsaturate a motor
+
+	// Mix without yaw
+	ActuatorVector thrust_z;
+	ActuatorVector roll;
+	ActuatorVector pitch;
+
+	for (int i = 0; i < _num_actuators; i++) {
+		_actuator_sp(i) = _actuator_trim(i) +
+				  _mix(i, ControlAxis::ROLL) * (_control_sp(ControlAxis::ROLL) - _control_trim(ControlAxis::ROLL)) +
+				  _mix(i, ControlAxis::PITCH) * (_control_sp(ControlAxis::PITCH) - _control_trim(ControlAxis::PITCH)) +
+				  _mix(i, ControlAxis::THRUST_X) * (_control_sp(ControlAxis::THRUST_X) - _control_trim(ControlAxis::THRUST_X)) +
+				  _mix(i, ControlAxis::THRUST_Y) * (_control_sp(ControlAxis::THRUST_Y) - _control_trim(ControlAxis::THRUST_Y)) +
+				  _mix(i, ControlAxis::THRUST_Z) * (_control_sp(ControlAxis::THRUST_Z) - _control_trim(ControlAxis::THRUST_Z));
+		thrust_z(i) = _mix(i, ControlAxis::THRUST_Z);
+		roll(i) = _mix(i, ControlAxis::ROLL);
+		pitch(i) = _mix(i, ControlAxis::PITCH);
+	}
+
+	// only reduce thrust
+	desaturateActuators(_actuator_sp, thrust_z, true);
+
+	// Reduce roll/pitch acceleration if needed to unsaturate
+	desaturateActuators(_actuator_sp, roll);
+	desaturateActuators(_actuator_sp, pitch);
+
+	// Mix yaw independently
+	mixYaw();
+}
+
+void
+ControlAllocationSequentialDesaturation::mixYaw()
+{
+	// Add yaw to outputs
+	ActuatorVector yaw;
+	ActuatorVector thrust_z;
+
+	for (int i = 0; i < _num_actuators; i++) {
+		_actuator_sp(i) += _mix(i, ControlAxis::YAW) * (_control_sp(ControlAxis::YAW) - _control_trim(ControlAxis::YAW));
+		yaw(i) = _mix(i, ControlAxis::YAW);
+		thrust_z(i) = _mix(i, ControlAxis::THRUST_Z);
+	}
+
+	// Change yaw acceleration to unsaturate the outputs if needed (do not change roll/pitch),
+	// and allow some yaw response at maximum thrust
+	ActuatorVector max_prev = _actuator_max;
+	_actuator_max += (_actuator_max - _actuator_min) * MINIMUM_YAW_MARGIN;
+	desaturateActuators(_actuator_sp, yaw);
+	_actuator_max = max_prev;
+
+	// reduce thrust only
+	desaturateActuators(_actuator_sp, thrust_z, true);
+}
+
+void
+ControlAllocationSequentialDesaturation::updateParameters()
+{
+	updateParams();
+}

src/lib/control_allocation/ControlAllocationSequentialDesaturation.hpp

@@ -0,0 +1,132 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 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 ControlAllocationSequentialDesaturation.hpp
+ *
+ * Control Allocation Algorithm which sequentially modifies control demands in order to
+ * eliminate the saturation of the actuator setpoint vector.
+ *
+ *
+ * @author Roman Bapst <bapstroman@gmail.com>
+ */
+
+#pragma once
+
+#include "ControlAllocationPseudoInverse.hpp"
+
+#include <px4_platform_common/module_params.h>
+
+class ControlAllocationSequentialDesaturation: public ControlAllocationPseudoInverse, public ModuleParams
+{
+public:
+
+	ControlAllocationSequentialDesaturation() : ModuleParams(nullptr) {}
+	virtual ~ControlAllocationSequentialDesaturation() = default;
+
+	void allocate() override;
+
+	void updateParameters() override;
+
+	// This is the minimum actuator yaw granted when the controller is saturated.
+	// In the yaw-only case where outputs are saturated, thrust is reduced by up to this amount.
+	static constexpr float MINIMUM_YAW_MARGIN{0.15f};
+private:
+
+	/**
+	 * 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 actuator_sp Actuator setpoint, vector that is modified
+	 * @param desaturation_vector vector that is added to the outputs, e.g. thrust_scale
+	 * @param increase_only if true, only allow to increase (add) a fraction of desaturation_vector
+	 */
+	void desaturateActuators(ActuatorVector &actuator_sp, const ActuatorVector &desaturation_vector,
+				 bool increase_only = false);
+
+	/**
+	 * Computes the gain k by which desaturation_vector has to be multiplied
+	 * in order to unsaturate the output that has the greatest saturation.
+	 *
+	 * @return desaturation gain
+	 */
+	float computeDesaturationGain(const ActuatorVector &desaturation_vector, const ActuatorVector &actuator_sp);
+
+	/**
+	 * Mix roll, pitch, yaw, thrust and set the actuator setpoint.
+	 *
+	 * 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.
+	 */
+	void mixAirmodeRP();
+
+	/**
+	 * Mix roll, pitch, yaw, thrust and set the actuator setpoint.
+	 *
+	 * 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.
+	 */
+	void mixAirmodeRPY();
+
+	/**
+	 * Mix roll, pitch, yaw, thrust and set the actuator setpoint.
+	 *
+	 * 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 mixYaw() for the exact yaw behavior.
+	 */
+	void mixAirmodeDisabled();
+
+	/**
+	 * Mix yaw by updating the actuator setpoint (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.
+	 */
+	void mixYaw();
+
+	DEFINE_PARAMETERS(
+		(ParamInt<px4::params::MC_AIRMODE>) _param_mc_airmode   ///< air-mode
+	);
+};

src/lib/control_allocation/ControlAllocationSequentialDesaturationTest.cpp

@@ -0,0 +1,385 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2024 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 ControlAllocationSequentialDesaturationTest.cpp
+ *
+ * Tests for Control Allocation Sequential Desaturation Algorithms
+ *
+ */
+
+#include <gtest/gtest.h>
+#include <ControlAllocationSequentialDesaturation.hpp>
+#include <actuator_effectiveness/ActuatorEffectivenessRotors.hpp>
+
+using namespace matrix;
+
+namespace
+{
+
+// Makes and returns a Geometry object for a "standard" quad-x quadcopter.
+ActuatorEffectivenessRotors::Geometry make_quad_x_geometry()
+{
+	ActuatorEffectivenessRotors::Geometry geometry = {};
+	geometry.rotors[0].position(0) = 1.0f;
+	geometry.rotors[0].position(1) = 1.0f;
+	geometry.rotors[0].position(2) = 0.0f;
+	geometry.rotors[0].axis(0) = 0.0f;
+	geometry.rotors[0].axis(1) = 0.0f;
+	geometry.rotors[0].axis(2) = -1.0f;
+	geometry.rotors[0].thrust_coef = 1.0f;
+	geometry.rotors[0].moment_ratio = 0.05f;
+
+	geometry.rotors[1].position(0) = -1.0f;
+	geometry.rotors[1].position(1) = -1.0f;
+	geometry.rotors[1].position(2) = 0.0f;
+	geometry.rotors[1].axis(0) = 0.0f;
+	geometry.rotors[1].axis(1) = 0.0f;
+	geometry.rotors[1].axis(2) = -1.0f;
+	geometry.rotors[1].thrust_coef = 1.0f;
+	geometry.rotors[1].moment_ratio = 0.05f;
+
+	geometry.rotors[2].position(0) = 1.0f;
+	geometry.rotors[2].position(1) = -1.0f;
+	geometry.rotors[2].position(2) = 0.0f;
+	geometry.rotors[2].axis(0) = 0.0f;
+	geometry.rotors[2].axis(1) = 0.0f;
+	geometry.rotors[2].axis(2) = -1.0f;
+	geometry.rotors[2].thrust_coef = 1.0f;
+	geometry.rotors[2].moment_ratio = -0.05f;
+
+	geometry.rotors[3].position(0) = -1.0f;
+	geometry.rotors[3].position(1) = 1.0f;
+	geometry.rotors[3].position(2) = 0.0f;
+	geometry.rotors[3].axis(0) = 0.0f;
+	geometry.rotors[3].axis(1) = 0.0f;
+	geometry.rotors[3].axis(2) = -1.0f;
+	geometry.rotors[3].thrust_coef = 1.0f;
+	geometry.rotors[3].moment_ratio = -0.05f;
+
+	geometry.num_rotors = 4;
+
+	return geometry;
+}
+
+// Returns an effective matrix for a sample quad-copter configuration.
+ActuatorEffectiveness::EffectivenessMatrix make_quad_x_effectiveness()
+{
+	ActuatorEffectiveness::EffectivenessMatrix effectiveness;
+	effectiveness.setZero();
+	const auto geometry = make_quad_x_geometry();
+	ActuatorEffectivenessRotors::computeEffectivenessMatrix(geometry, effectiveness);
+	return effectiveness;
+}
+
+// Configures a ControlAllocationSequentialDesaturation object for a sample quad-copter.
+void setup_quad_allocator(ControlAllocationSequentialDesaturation &allocator)
+{
+	const auto effectiveness = make_quad_x_effectiveness();
+	matrix::Vector<float, ActuatorEffectiveness::NUM_ACTUATORS> actuator_trim;
+	matrix::Vector<float, ActuatorEffectiveness::NUM_ACTUATORS> linearization_point;
+	constexpr bool UPDATE_NORMALIZATION_SCALE{false};
+	allocator.setEffectivenessMatrix(
+		effectiveness,
+		actuator_trim,
+		linearization_point,
+		ActuatorEffectiveness::NUM_ACTUATORS,
+		UPDATE_NORMALIZATION_SCALE
+	);
+}
+
+static constexpr float EXPECT_NEAR_TOL{1e-4f};
+
+} // namespace
+
+// This tests that yaw-only control setpoint at zero actuator setpoint results in zero actuator
+// allocation.
+TEST(ControlAllocationSequentialDesaturationTest, AirmodeDisabledOnlyYaw)
+{
+	ControlAllocationSequentialDesaturation allocator;
+	setup_quad_allocator(allocator);
+	matrix::Vector<float, ActuatorEffectiveness::NUM_AXES> control_sp;
+	control_sp(ControlAllocation::ControlAxis::ROLL) = 0.f;
+	control_sp(ControlAllocation::ControlAxis::PITCH) = 0.f;
+	control_sp(ControlAllocation::ControlAxis::YAW) = 1.f;
+	control_sp(ControlAllocation::ControlAxis::THRUST_X) = 0.f;
+	control_sp(ControlAllocation::ControlAxis::THRUST_Y) = 0.f;
+	control_sp(ControlAllocation::ControlAxis::THRUST_Z) = 0.f;
+	allocator.setControlSetpoint(control_sp);
+
+	// Since MC_AIRMODE was not set explicitly, assume airmode is disabled.
+	allocator.allocate();
+
+	const auto &actuator_sp = allocator.getActuatorSetpoint();
+	matrix::Vector<float, ActuatorEffectiveness::NUM_ACTUATORS> zero;
+	EXPECT_EQ(actuator_sp, zero);
+}
+
+// This tests that a control setpoint for z-thrust returns the desired actuator setpoint.
+// Each motor should have an actuator setpoint that when summed together should be equal to
+// control setpoint.
+TEST(ControlAllocationSequentialDesaturationTest, AirmodeDisabledThrustZ)
+{
+	ControlAllocationSequentialDesaturation allocator;
+	setup_quad_allocator(allocator);
+	matrix::Vector<float, ActuatorEffectiveness::NUM_AXES> control_sp;
+	// Negative, because +z is "downward".
+	constexpr float THRUST_Z_TOTAL{-0.75f};
+	control_sp(ControlAllocation::ControlAxis::ROLL) = 0.f;
+	control_sp(ControlAllocation::ControlAxis::PITCH) = 0.f;
+	control_sp(ControlAllocation::ControlAxis::YAW) = 0.f;
+	control_sp(ControlAllocation::ControlAxis::THRUST_X) = 0.f;
+	control_sp(ControlAllocation::ControlAxis::THRUST_Y) = 0.f;
+	control_sp(ControlAllocation::ControlAxis::THRUST_Z) = THRUST_Z_TOTAL;
+	allocator.setControlSetpoint(control_sp);
+
+	// Since MC_AIRMODE was not set explicitly, assume airmode is disabled.
+	allocator.allocate();
+
+	const auto &actuator_sp = allocator.getActuatorSetpoint();
+	constexpr int MOTOR_COUNT{4};
+	constexpr float THRUST_Z_PER_MOTOR{-THRUST_Z_TOTAL / MOTOR_COUNT};
+
+	for (int i{0}; i < MOTOR_COUNT; ++i) {
+		EXPECT_NEAR(actuator_sp(i), THRUST_Z_PER_MOTOR, EXPECT_NEAR_TOL);
+	}
+
+	for (int i{MOTOR_COUNT}; i < ActuatorEffectiveness::NUM_ACTUATORS; ++i) {
+		EXPECT_NEAR(actuator_sp(i), 0.f, EXPECT_NEAR_TOL);
+	}
+}
+
+// This tests that a control setpoint for z-thrust + yaw returns the desired actuator setpoint.
+// This test does not saturate the yaw response.
+TEST(ControlAllocationSequentialDesaturationTest, AirmodeDisabledThrustAndYaw)
+{
+	ControlAllocationSequentialDesaturation allocator;
+	setup_quad_allocator(allocator);
+	matrix::Vector<float, ActuatorEffectiveness::NUM_AXES> control_sp;
+	// Negative, because +z is "downward".
+	constexpr float THRUST_Z_TOTAL{-0.75f};
+	// This is low enough to not saturate the motors.
+	constexpr float YAW_CONTROL_SP{0.02f};
+	control_sp(ControlAllocation::ControlAxis::ROLL) = 0.f;
+	control_sp(ControlAllocation::ControlAxis::PITCH) = 0.f;
+	control_sp(ControlAllocation::ControlAxis::YAW) = YAW_CONTROL_SP;
+	control_sp(ControlAllocation::ControlAxis::THRUST_X) = 0.f;
+	control_sp(ControlAllocation::ControlAxis::THRUST_Y) = 0.f;
+	control_sp(ControlAllocation::ControlAxis::THRUST_Z) = THRUST_Z_TOTAL;
+	allocator.setControlSetpoint(control_sp);
+
+	// Since MC_AIRMODE was not set explicitly, assume airmode is disabled.
+	allocator.allocate();
+
+	const auto &actuator_sp = allocator.getActuatorSetpoint();
+	// This value is based off of the effectiveness matrix. If the effectiveness matrix is changed,
+	// this will need to be changed.
+	constexpr float YAW_EFFECTIVENESS_FACTOR{5.f};
+	constexpr float YAW_DIFF_PER_MOTOR{YAW_CONTROL_SP * YAW_EFFECTIVENESS_FACTOR};
+	// At yaw condition, there will be 2 different actuator values.
+	constexpr int MOTOR_COUNT{4};
+	constexpr float HIGH_THRUST_Z_PER_MOTOR{-THRUST_Z_TOTAL / MOTOR_COUNT + YAW_DIFF_PER_MOTOR};
+	constexpr float LOW_THRUST_Z_PER_MOTOR{-THRUST_Z_TOTAL / MOTOR_COUNT - YAW_DIFF_PER_MOTOR};
+
+	for (int i{0}; i < MOTOR_COUNT / 2; ++i) {
+		EXPECT_NEAR(actuator_sp(i), HIGH_THRUST_Z_PER_MOTOR, EXPECT_NEAR_TOL);
+	}
+
+	for (int i{MOTOR_COUNT / 2}; i < MOTOR_COUNT; ++i) {
+		EXPECT_NEAR(actuator_sp(i), LOW_THRUST_Z_PER_MOTOR, EXPECT_NEAR_TOL);
+	}
+
+	for (int i{MOTOR_COUNT}; i < ActuatorEffectiveness::NUM_ACTUATORS; ++i) {
+		EXPECT_NEAR(actuator_sp(i), 0.f, EXPECT_NEAR_TOL);
+	}
+}
+
+// This tests that a control setpoint for z-thrust + yaw returns the desired actuator setpoint.
+// This test saturates the yaw response, but does not reduce total thrust.
+TEST(ControlAllocationSequentialDesaturationTest, AirmodeDisabledThrustAndSaturatedYaw)
+{
+	ControlAllocationSequentialDesaturation allocator;
+	setup_quad_allocator(allocator);
+	matrix::Vector<float, ActuatorEffectiveness::NUM_AXES> control_sp;
+	// Negative, because +z is "downward".
+	constexpr float THRUST_Z_TOTAL{-0.75f};
+	// This is arbitrarily high to trigger strongest possible (saturated) yaw response.
+	constexpr float YAW_CONTROL_SP{0.25f};
+	control_sp(ControlAllocation::ControlAxis::ROLL) = 0.f;
+	control_sp(ControlAllocation::ControlAxis::PITCH) = 0.f;
+	control_sp(ControlAllocation::ControlAxis::YAW) = YAW_CONTROL_SP;
+	control_sp(ControlAllocation::ControlAxis::THRUST_X) = 0.f;
+	control_sp(ControlAllocation::ControlAxis::THRUST_Y) = 0.f;
+	control_sp(ControlAllocation::ControlAxis::THRUST_Z) = THRUST_Z_TOTAL;
+	allocator.setControlSetpoint(control_sp);
+
+	// Since MC_AIRMODE was not set explicitly, assume airmode is disabled.
+	allocator.allocate();
+
+	const auto &actuator_sp = allocator.getActuatorSetpoint();
+	// At max yaw, only 2 motors will carry all of the thrust.
+	constexpr int YAW_MOTORS{2};
+	constexpr float THRUST_Z_PER_MOTOR{-THRUST_Z_TOTAL / YAW_MOTORS};
+
+	for (int i{0}; i < YAW_MOTORS; ++i) {
+		EXPECT_NEAR(actuator_sp(i), THRUST_Z_PER_MOTOR, EXPECT_NEAR_TOL);
+	}
+
+	for (int i{YAW_MOTORS}; i < ActuatorEffectiveness::NUM_ACTUATORS; ++i) {
+		EXPECT_NEAR(actuator_sp(i), 0.f, EXPECT_NEAR_TOL);
+	}
+}
+
+// This tests that a control setpoint for z-thrust + pitch returns the desired actuator setpoint.
+// This test does not saturate the pitch response.
+TEST(ControlAllocationSequentialDesaturationTest, AirmodeDisabledThrustAndPitch)
+{
+	ControlAllocationSequentialDesaturation allocator;
+	setup_quad_allocator(allocator);
+	matrix::Vector<float, ActuatorEffectiveness::NUM_AXES> control_sp;
+	// Negative, because +z is "downward".
+	constexpr float THRUST_Z_TOTAL{-0.75f};
+	// This is low enough to not saturate the motors.
+	constexpr float PITCH_CONTROL_SP{0.1f};
+	control_sp(ControlAllocation::ControlAxis::ROLL) = 0.f;
+	control_sp(ControlAllocation::ControlAxis::PITCH) = PITCH_CONTROL_SP;
+	control_sp(ControlAllocation::ControlAxis::YAW) = 0.f;
+	control_sp(ControlAllocation::ControlAxis::THRUST_X) = 0.f;
+	control_sp(ControlAllocation::ControlAxis::THRUST_Y) = 0.f;
+	control_sp(ControlAllocation::ControlAxis::THRUST_Z) = THRUST_Z_TOTAL;
+	allocator.setControlSetpoint(control_sp);
+
+	// Since MC_AIRMODE was not set explicitly, assume airmode is disabled.
+	allocator.allocate();
+
+	const auto &actuator_sp = allocator.getActuatorSetpoint();
+	// This value is based off of the effectiveness matrix. If the effectiveness matrix is changed,
+	// this will need to be changed.
+	constexpr int MOTOR_COUNT{4};
+	constexpr float PITCH_DIFF_PER_MOTOR{PITCH_CONTROL_SP / MOTOR_COUNT};
+	// At control set point, there will be 2 different actuator values.
+	constexpr float HIGH_THRUST_Z_PER_MOTOR{-THRUST_Z_TOTAL / MOTOR_COUNT + PITCH_DIFF_PER_MOTOR};
+	constexpr float LOW_THRUST_Z_PER_MOTOR{-THRUST_Z_TOTAL / MOTOR_COUNT - PITCH_DIFF_PER_MOTOR};
+	EXPECT_NEAR(actuator_sp(0), HIGH_THRUST_Z_PER_MOTOR, EXPECT_NEAR_TOL);
+	EXPECT_NEAR(actuator_sp(1), LOW_THRUST_Z_PER_MOTOR, EXPECT_NEAR_TOL);
+	EXPECT_NEAR(actuator_sp(2), HIGH_THRUST_Z_PER_MOTOR, EXPECT_NEAR_TOL);
+	EXPECT_NEAR(actuator_sp(3), LOW_THRUST_Z_PER_MOTOR, EXPECT_NEAR_TOL);
+
+	for (int i{MOTOR_COUNT}; i < ActuatorEffectiveness::NUM_ACTUATORS; ++i) {
+		EXPECT_NEAR(actuator_sp(i), 0.f, EXPECT_NEAR_TOL);
+	}
+}
+
+// This tests that a control setpoint for z-thrust + yaw returns the desired actuator setpoint.
+// This test saturates yaw and demonstrates reduction of thrust for yaw.
+TEST(ControlAllocationSequentialDesaturationTest, AirmodeDisabledReducedThrustAndYaw)
+{
+	ControlAllocationSequentialDesaturation allocator;
+	setup_quad_allocator(allocator);
+	matrix::Vector<float, ActuatorEffectiveness::NUM_AXES> control_sp;
+	// Negative, because +z is "downward".
+	constexpr float DESIRED_THRUST_Z_PER_MOTOR{0.8f};
+	constexpr int MOTOR_COUNT{4};
+	constexpr float THRUST_Z_TOTAL{-DESIRED_THRUST_Z_PER_MOTOR * MOTOR_COUNT};
+	// This is arbitrarily high to trigger strongest possible (saturated) yaw response.
+	constexpr float YAW_CONTROL_SP{1.f};
+	control_sp(ControlAllocation::ControlAxis::ROLL) = 0.f;
+	control_sp(ControlAllocation::ControlAxis::PITCH) = 0.f;
+	control_sp(ControlAllocation::ControlAxis::YAW) = YAW_CONTROL_SP;
+	control_sp(ControlAllocation::ControlAxis::THRUST_X) = 0.f;
+	control_sp(ControlAllocation::ControlAxis::THRUST_Y) = 0.f;
+	control_sp(ControlAllocation::ControlAxis::THRUST_Z) = THRUST_Z_TOTAL;
+	allocator.setControlSetpoint(control_sp);
+
+	// Since MC_AIRMODE was not set explicitly, assume airmode is disabled.
+	allocator.allocate();
+
+	const auto &actuator_sp = allocator.getActuatorSetpoint();
+	// In the case of yaw saturation, thrust per motor will be reduced by the hard-coded
+	// magic-number yaw margin of 0.15f.
+	constexpr float YAW_MARGIN{0.15f}; // get this from a centralized source when available.
+	constexpr float YAW_DIFF_PER_MOTOR{1.0f + YAW_MARGIN - DESIRED_THRUST_Z_PER_MOTOR};
+	// At control set point, there will be 2 different actuator values.
+	constexpr float HIGH_THRUST_Z_PER_MOTOR{DESIRED_THRUST_Z_PER_MOTOR + YAW_DIFF_PER_MOTOR - YAW_MARGIN};
+	constexpr float LOW_THRUST_Z_PER_MOTOR{DESIRED_THRUST_Z_PER_MOTOR - YAW_DIFF_PER_MOTOR - YAW_MARGIN};
+	EXPECT_NEAR(actuator_sp(0), HIGH_THRUST_Z_PER_MOTOR, EXPECT_NEAR_TOL);
+	EXPECT_NEAR(actuator_sp(1), HIGH_THRUST_Z_PER_MOTOR, EXPECT_NEAR_TOL);
+	EXPECT_NEAR(actuator_sp(2), LOW_THRUST_Z_PER_MOTOR, EXPECT_NEAR_TOL);
+	EXPECT_NEAR(actuator_sp(3), LOW_THRUST_Z_PER_MOTOR, EXPECT_NEAR_TOL);
+
+	for (int i{MOTOR_COUNT}; i < ActuatorEffectiveness::NUM_ACTUATORS; ++i) {
+		EXPECT_NEAR(actuator_sp(i), 0.f, EXPECT_NEAR_TOL);
+	}
+}
+
+// This tests that a control setpoint for z-thrust + pitch returns the desired actuator setpoint.
+// This test saturates the pitch response such that thrust is reduced to (partially) compensate.
+TEST(ControlAllocationSequentialDesaturationTest, AirmodeDisabledReducedThrustAndPitch)
+{
+	ControlAllocationSequentialDesaturation allocator;
+	setup_quad_allocator(allocator);
+	matrix::Vector<float, ActuatorEffectiveness::NUM_AXES> control_sp;
+	// Negative, because +z is "downward".
+	constexpr float THRUST_Z_TOTAL{-0.75f * 4.f};
+	// This is high enough to saturate the pitch control.
+	constexpr float PITCH_CONTROL_SP{2.f};
+	control_sp(ControlAllocation::ControlAxis::ROLL) = 0.f;
+	control_sp(ControlAllocation::ControlAxis::PITCH) = PITCH_CONTROL_SP;
+	control_sp(ControlAllocation::ControlAxis::YAW) = 0.f;
+	control_sp(ControlAllocation::ControlAxis::THRUST_X) = 0.f;
+	control_sp(ControlAllocation::ControlAxis::THRUST_Y) = 0.f;
+	control_sp(ControlAllocation::ControlAxis::THRUST_Z) = THRUST_Z_TOTAL;
+	allocator.setControlSetpoint(control_sp);
+
+	// Since MC_AIRMODE was not set explicitly, assume airmode is disabled.
+	allocator.allocate();
+
+	const auto &actuator_sp = allocator.getActuatorSetpoint();
+	constexpr int MOTOR_COUNT{4};
+	// The maximum actuator value is
+	// 	THRUST_Z_TOTAL / MOTOR_COUNT + PITCH_CONTROL_SP / MOTOR_COUNT.
+	// The amount over 1 is the amount that each motor is reduced by.
+	// At control set point, there will be 2 different actuator values.
+	constexpr float OVERAGE_PER_MOTOR{-THRUST_Z_TOTAL / MOTOR_COUNT + PITCH_CONTROL_SP / MOTOR_COUNT - 1};
+	EXPECT_TRUE(OVERAGE_PER_MOTOR > 0.f);
+	constexpr float HIGH_THRUST_Z_PER_MOTOR{-THRUST_Z_TOTAL / MOTOR_COUNT + PITCH_CONTROL_SP / MOTOR_COUNT - OVERAGE_PER_MOTOR};
+	constexpr float LOW_THRUST_Z_PER_MOTOR{-THRUST_Z_TOTAL / MOTOR_COUNT - PITCH_CONTROL_SP / MOTOR_COUNT - OVERAGE_PER_MOTOR};
+	EXPECT_NEAR(actuator_sp(0), HIGH_THRUST_Z_PER_MOTOR, EXPECT_NEAR_TOL);
+	EXPECT_NEAR(actuator_sp(1), LOW_THRUST_Z_PER_MOTOR, EXPECT_NEAR_TOL);
+	EXPECT_NEAR(actuator_sp(2), HIGH_THRUST_Z_PER_MOTOR, EXPECT_NEAR_TOL);
+	EXPECT_NEAR(actuator_sp(3), LOW_THRUST_Z_PER_MOTOR, EXPECT_NEAR_TOL);
+
+	for (int i{MOTOR_COUNT}; i < ActuatorEffectiveness::NUM_ACTUATORS; ++i) {
+		EXPECT_NEAR(actuator_sp(i), 0.f, EXPECT_NEAR_TOL);
+	}
+}

src/lib/control_allocation/actuator_effectiveness/ActuatorEffectiveness.hpp

@@ -51,12 +51,13 @@ enum class AllocationMethod {
 	PSEUDO_INVERSE = 0,
 	SEQUENTIAL_DESATURATION = 1,
 	AUTO = 2,
+	METRIC = 3,
 };
 
 enum class ActuatorType {
 	MOTORS = 0,
 	SERVOS,
-
+	THRUSTERS,
 	COUNT
 };
 

src/lib/control_allocation/actuator_effectiveness/CMakeLists.txt

@@ -34,6 +34,13 @@
 px4_add_library(ActuatorEffectiveness
 	ActuatorEffectiveness.cpp
 	ActuatorEffectiveness.hpp
+<<<<<<<< HEAD:src/lib/actuator_effectiveness/CMakeLists.txt
+	ActuatorEffectivenessRotors.cpp
+	ActuatorEffectivenessRotors.hpp
+	ActuatorEffectivenessThrusters.cpp
+	ActuatorEffectivenessThrusters.hpp
+========
+>>>>>>>> origin:src/lib/control_allocation/actuator_effectiveness/CMakeLists.txt
 )
 
 target_compile_options(ActuatorEffectiveness PRIVATE ${MAX_CUSTOM_OPT_LEVEL})
@@ -43,3 +50,9 @@ target_link_libraries(ActuatorEffectiveness
 		mathlib
 		PID
 )
+<<<<<<<< HEAD:src/lib/actuator_effectiveness/CMakeLists.txt
+
+px4_add_functional_gtest(SRC ActuatorEffectivenessRotorsTest.cpp LINKLIBS ActuatorEffectiveness)
+px4_add_functional_gtest(SRC ActuatorEffectivenessThrustersTest.cpp LINKLIBS ActuatorEffectiveness)
+========
+>>>>>>>> origin:src/lib/control_allocation/actuator_effectiveness/CMakeLists.txt

src/lib/control_allocation/control_allocation/CMakeLists.txt

@@ -34,6 +34,8 @@
 px4_add_library(ControlAllocation
 	ControlAllocation.cpp
 	ControlAllocation.hpp
+	ControlAllocationMetric.cpp
+	ControlAllocationMetric.hpp
 	ControlAllocationPseudoInverse.cpp
 	ControlAllocationPseudoInverse.hpp
 	ControlAllocationSequentialDesaturation.cpp
@@ -44,4 +46,8 @@ target_include_directories(ControlAllocation PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})
 target_link_libraries(ControlAllocation PRIVATE mathlib)
 
 px4_add_unit_gtest(SRC ControlAllocationPseudoInverseTest.cpp LINKLIBS ControlAllocation)
+<<<<<<<< HEAD:src/lib/control_allocation/CMakeLists.txt
+px4_add_unit_gtest(SRC ControlAllocationMetric.cpp LINKLIBS ControlAllocation)
+========
+>>>>>>>> origin:src/lib/control_allocation/control_allocation/CMakeLists.txt
 # px4_add_functional_gtest(SRC ControlAllocationSequentialDesaturationTest.cpp LINKLIBS ControlAllocation ActuatorEffectiveness)

src/lib/control_allocation/control_allocation/ControlAllocation.hpp

@@ -71,7 +71,11 @@
 
 #include <matrix/matrix/math.hpp>
 
+<<<<<<<< HEAD:src/lib/control_allocation/ControlAllocation.hpp
+#include "actuator_effectiveness/ActuatorEffectiveness.hpp"
+========
 #include "control_allocation/actuator_effectiveness/ActuatorEffectiveness.hpp"
+>>>>>>>> origin:src/lib/control_allocation/control_allocation/ControlAllocation.hpp
 
 class ControlAllocation
 {

src/lib/mixer_module/functions/FunctionThrusters.hpp

@@ -0,0 +1,124 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2021-2022 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 "FunctionProviderBase.hpp"
+
+/**
+ * Functions: Thruster1 ... ThrusterMax
+ */
+class FunctionThrusters : public FunctionProviderBase
+{
+public:
+	static_assert(actuator_motors_s::NUM_CONTROLS == (int)OutputFunction::ThrusterMax - (int)OutputFunction::Thruster1 + 1,
+		      "Unexpected num thrusters");
+
+	static_assert(actuator_motors_s::ACTUATOR_FUNCTION_THRUSTER1 == (int)OutputFunction::Thruster1,
+		      "Unexpected thruster idx");
+
+	FunctionThrusters(const Context &context) :
+		_topic(&context.work_item, ORB_ID(actuator_motors)),
+		_thrust_factor(context.thrust_factor)
+	{
+		for (int i = 0; i < actuator_motors_s::NUM_CONTROLS; ++i) {
+			_data.control[i] = NAN;
+		}
+	}
+
+	static FunctionProviderBase *allocate(const Context &context) { return new FunctionThrusters(context); }
+
+	void update() override
+	{
+		if (_topic.update(&_data)) {
+			updateValues(_data.reversible_flags, _thrust_factor, _data.control, actuator_motors_s::NUM_CONTROLS);
+		}
+	}
+
+	float value(OutputFunction func) override { return _data.control[(int)func - (int)OutputFunction::Thruster1]; }
+
+	bool allowPrearmControl() const override { return false; }
+
+	uORB::SubscriptionCallbackWorkItem *subscriptionCallback() override { return &_topic; }
+
+	bool getLatestSampleTimestamp(hrt_abstime &t) const override { t = _data.timestamp_sample; return t != 0; }
+
+	static inline void updateValues(uint32_t reversible, float thrust_factor, float *values, int num_values)
+	{
+		// TODO(@Pedro-Roque): for now bypass this
+		/*if (thrust_factor > 0.f && thrust_factor <= 1.f) {
+
+			// thrust factor
+			//  rel_thrust = factor * x^2 + (1-factor) * x,
+			const float a = thrust_factor;
+			const float b = (1.f - thrust_factor);
+
+			// don't recompute for all values (ax^2+bx+c=0)
+			const float tmp1 = b / (2.f * a);
+			const float tmp2 = b * b / (4.f * a * a);
+
+			for (int i = 0; i < num_values; ++i) {
+				float control = values[i];
+
+				if (control > 0.f) {
+					values[i] = -tmp1 + sqrtf(tmp2 + (control / a));
+
+				} else if (control < -0.f) {
+					values[i] =  tmp1 - sqrtf(tmp2 - (control / a));
+
+				} else {
+					values[i] = 0.f;
+				}
+			}
+		}
+
+		for (int i = 0; i < num_values; ++i) {
+			if ((reversible & (1u << i)) == 0) {
+				if (values[i] < -FLT_EPSILON) {
+					values[i] = NAN;
+
+				} else {
+					// remap from [0, 1] to [-1, 1]
+					values[i] = values[i] * 2.f - 1.f;
+				}
+			}
+		}*/
+	}
+
+	bool reversible(OutputFunction func) const override { return false; }
+
+private:
+	uORB::SubscriptionCallbackWorkItem _topic;
+	actuator_motors_s _data{};
+	const float &_thrust_factor;
+};

src/modules/metric_allocator/CMakeLists.txt

@@ -0,0 +1,54 @@
+############################################################################
+#
+#   Copyright (c) 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_directories(${CMAKE_CURRENT_SOURCE_DIR})
+
+px4_add_module(
+	MODULE modules__metric_allocator
+	MAIN metric_allocator
+	COMPILE_FLAGS
+		${MAX_CUSTOM_OPT_LEVEL}
+	STACK_MAIN
+		3000
+	SRCS
+		ControlAllocator.cpp
+		ControlAllocator.hpp
+	MODULE_CONFIG
+		module.yaml
+	DEPENDS
+		mathlib
+		ActuatorEffectiveness
+		ControlAllocation
+		px4_work_queue
+		SlewRate
+)

src/modules/metric_allocator/ControlAllocator.cpp

@@ -0,0 +1,879 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2013-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 ControlAllocator.cpp
+ *
+ * Control allocator.
+ *
+ * @author Julien Lecoeur <julien.lecoeur@gmail.com>
+ */
+
+#include "ControlAllocator.hpp"
+
+#include <drivers/drv_hrt.h>
+#include <circuit_breaker/circuit_breaker.h>
+#include <mathlib/math/Limits.hpp>
+#include <mathlib/math/Functions.hpp>
+
+using namespace matrix;
+using namespace time_literals;
+
+ControlAllocator::ControlAllocator() :
+	ModuleParams(nullptr),
+	ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::rate_ctrl),
+	_loop_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": cycle"))
+{
+	_control_allocator_status_pub[0].advertise();
+	_control_allocator_status_pub[1].advertise();
+
+	_actuator_motors_pub.advertise();
+	_actuator_servos_pub.advertise();
+	_actuator_servos_trim_pub.advertise();
+
+	for (int i = 0; i < MAX_NUM_MOTORS; ++i) {
+		char buffer[17];
+		snprintf(buffer, sizeof(buffer), "CA_R%u_SLEW", i);
+		_param_handles.slew_rate_motors[i] = param_find(buffer);
+	}
+
+	for (int i = 0; i < MAX_NUM_SERVOS; ++i) {
+		char buffer[17];
+		snprintf(buffer, sizeof(buffer), "CA_SV%u_SLEW", i);
+		_param_handles.slew_rate_servos[i] = param_find(buffer);
+	}
+
+	parameters_updated();
+}
+
+ControlAllocator::~ControlAllocator()
+{
+	for (int i = 0; i < ActuatorEffectiveness::MAX_NUM_MATRICES; ++i) {
+		delete _control_allocation[i];
+	}
+
+	delete _actuator_effectiveness;
+
+	perf_free(_loop_perf);
+}
+
+bool
+ControlAllocator::init()
+{
+	if (!_vehicle_torque_setpoint_sub.registerCallback()) {
+		PX4_ERR("callback registration failed");
+		return false;
+	}
+
+	if (!_vehicle_thrust_setpoint_sub.registerCallback()) {
+		PX4_ERR("callback registration failed");
+		return false;
+	}
+
+#ifndef ENABLE_LOCKSTEP_SCHEDULER // Backup schedule would interfere with lockstep
+	ScheduleDelayed(50_ms);
+#endif
+
+	return true;
+}
+
+void
+ControlAllocator::parameters_updated()
+{
+	_has_slew_rate = false;
+
+	for (int i = 0; i < MAX_NUM_MOTORS; ++i) {
+		param_get(_param_handles.slew_rate_motors[i], &_params.slew_rate_motors[i]);
+		_has_slew_rate |= _params.slew_rate_motors[i] > FLT_EPSILON;
+	}
+
+	for (int i = 0; i < MAX_NUM_SERVOS; ++i) {
+		param_get(_param_handles.slew_rate_servos[i], &_params.slew_rate_servos[i]);
+		_has_slew_rate |= _params.slew_rate_servos[i] > FLT_EPSILON;
+	}
+
+	// Allocation method & effectiveness source
+	// Do this first: in case a new method is loaded, it will be configured below
+	bool updated = update_effectiveness_source();
+	update_allocation_method(updated); // must be called after update_effectiveness_source()
+
+	if (_num_control_allocation == 0) {
+		return;
+	}
+
+	for (int i = 0; i < _num_control_allocation; ++i) {
+		_control_allocation[i]->updateParameters();
+	}
+
+	update_effectiveness_matrix_if_needed(EffectivenessUpdateReason::CONFIGURATION_UPDATE);
+}
+
+void
+ControlAllocator::update_allocation_method(bool force)
+{
+	AllocationMethod configured_method = (AllocationMethod)_param_ca_method.get();
+
+	if (!_actuator_effectiveness) {
+		PX4_ERR("_actuator_effectiveness null");
+		return;
+	}
+
+	if (_allocation_method_id != configured_method || force) {
+
+		matrix::Vector<float, NUM_ACTUATORS> actuator_sp[ActuatorEffectiveness::MAX_NUM_MATRICES];
+
+		// Cleanup first
+		for (int i = 0; i < ActuatorEffectiveness::MAX_NUM_MATRICES; ++i) {
+			// Save current state
+			if (_control_allocation[i] != nullptr) {
+				actuator_sp[i] = _control_allocation[i]->getActuatorSetpoint();
+			}
+
+			delete _control_allocation[i];
+			_control_allocation[i] = nullptr;
+		}
+
+		_num_control_allocation = _actuator_effectiveness->numMatrices();
+
+		AllocationMethod desired_methods[ActuatorEffectiveness::MAX_NUM_MATRICES];
+		_actuator_effectiveness->getDesiredAllocationMethod(desired_methods);
+
+		bool normalize_rpy[ActuatorEffectiveness::MAX_NUM_MATRICES];
+		_actuator_effectiveness->getNormalizeRPY(normalize_rpy);
+
+		for (int i = 0; i < _num_control_allocation; ++i) {
+			AllocationMethod method = configured_method;
+
+			if (configured_method == AllocationMethod::AUTO) {
+				method = desired_methods[i];
+			}
+
+			switch (method) {
+			case AllocationMethod::PSEUDO_INVERSE:
+				_control_allocation[i] = new ControlAllocationPseudoInverse();
+				break;
+
+			case AllocationMethod::SEQUENTIAL_DESATURATION:
+				_control_allocation[i] = new ControlAllocationSequentialDesaturation();
+				break;
+
+			default:
+				PX4_ERR("Unknown allocation method");
+				break;
+			}
+
+			if (_control_allocation[i] == nullptr) {
+				PX4_ERR("alloc failed");
+				_num_control_allocation = 0;
+
+			} else {
+				_control_allocation[i]->setNormalizeRPY(normalize_rpy[i]);
+				_control_allocation[i]->setActuatorSetpoint(actuator_sp[i]);
+			}
+		}
+
+		_allocation_method_id = configured_method;
+	}
+}
+
+bool
+ControlAllocator::update_effectiveness_source()
+{
+	const EffectivenessSource source = (EffectivenessSource)_param_ca_airframe.get();
+
+	if (_effectiveness_source_id != source) {
+
+		// try to instanciate new effectiveness source
+		ActuatorEffectiveness *tmp = nullptr;
+
+		switch (source) {
+		case EffectivenessSource::NONE:
+		case EffectivenessSource::MULTIROTOR:
+			tmp = new ActuatorEffectivenessMultirotor(this);
+			break;
+
+		case EffectivenessSource::STANDARD_VTOL:
+			tmp = new ActuatorEffectivenessStandardVTOL(this);
+			break;
+
+		case EffectivenessSource::TILTROTOR_VTOL:
+			tmp = new ActuatorEffectivenessTiltrotorVTOL(this);
+			break;
+
+		case EffectivenessSource::TAILSITTER_VTOL:
+			tmp = new ActuatorEffectivenessTailsitterVTOL(this);
+			break;
+
+		case EffectivenessSource::ROVER_ACKERMANN:
+			tmp = new ActuatorEffectivenessRoverAckermann();
+			break;
+
+		case EffectivenessSource::ROVER_DIFFERENTIAL:
+			// rover_differential_control does allocation and publishes directly to actuator_motors topic
+			break;
+
+		case EffectivenessSource::FIXED_WING:
+			tmp = new ActuatorEffectivenessFixedWing(this);
+			break;
+
+		case EffectivenessSource::MOTORS_6DOF: // just a different UI from MULTIROTOR
+			tmp = new ActuatorEffectivenessUUV(this);
+			break;
+
+		case EffectivenessSource::MULTIROTOR_WITH_TILT:
+			tmp = new ActuatorEffectivenessMCTilt(this);
+			break;
+
+		case EffectivenessSource::CUSTOM:
+			tmp = new ActuatorEffectivenessCustom(this);
+			break;
+
+		case EffectivenessSource::HELICOPTER_TAIL_ESC:
+			tmp = new ActuatorEffectivenessHelicopter(this, ActuatorType::MOTORS);
+			break;
+
+		case EffectivenessSource::HELICOPTER_TAIL_SERVO:
+			tmp = new ActuatorEffectivenessHelicopter(this, ActuatorType::SERVOS);
+			break;
+
+		case EffectivenessSource::HELICOPTER_COAXIAL:
+			tmp = new ActuatorEffectivenessHelicopterCoaxial(this);
+			break;
+
+		default:
+			PX4_ERR("Unknown airframe");
+			break;
+		}
+
+		// Replace previous source with new one
+		if (tmp == nullptr) {
+			// It did not work, forget about it
+			PX4_ERR("Actuator effectiveness init failed");
+			_param_ca_airframe.set((int)_effectiveness_source_id);
+
+		} else {
+			// Swap effectiveness sources
+			delete _actuator_effectiveness;
+			_actuator_effectiveness = tmp;
+
+			// Save source id
+			_effectiveness_source_id = source;
+		}
+
+		return true;
+	}
+
+	return false;
+}
+
+void
+ControlAllocator::Run()
+{
+	if (should_exit()) {
+		_vehicle_torque_setpoint_sub.unregisterCallback();
+		_vehicle_thrust_setpoint_sub.unregisterCallback();
+		exit_and_cleanup();
+		return;
+	}
+
+	perf_begin(_loop_perf);
+
+#ifndef ENABLE_LOCKSTEP_SCHEDULER // Backup schedule would interfere with lockstep
+	// Push backup schedule
+	ScheduleDelayed(50_ms);
+#endif
+
+	// Check if parameters have changed
+	if (_parameter_update_sub.updated() && !_armed) {
+		// clear update
+		parameter_update_s param_update;
+		_parameter_update_sub.copy(&param_update);
+
+		if (_handled_motor_failure_bitmask == 0) {
+			// We don't update the geometry after an actuator failure, as it could lead to unexpected results
+			// (e.g. a user could add/remove motors, such that the bitmask isn't correct anymore)
+			updateParams();
+			parameters_updated();
+		}
+	}
+
+	if (_num_control_allocation == 0 || _actuator_effectiveness == nullptr) {
+		return;
+	}
+
+	{
+		vehicle_status_s vehicle_status;
+
+		if (_vehicle_status_sub.update(&vehicle_status)) {
+
+			_armed = vehicle_status.arming_state == vehicle_status_s::ARMING_STATE_ARMED;
+
+			ActuatorEffectiveness::FlightPhase flight_phase{ActuatorEffectiveness::FlightPhase::HOVER_FLIGHT};
+
+			// Check if the current flight phase is HOVER or FIXED_WING
+			if (vehicle_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) {
+				flight_phase = ActuatorEffectiveness::FlightPhase::HOVER_FLIGHT;
+
+			} else {
+				flight_phase = ActuatorEffectiveness::FlightPhase::FORWARD_FLIGHT;
+			}
+
+			// Special cases for VTOL in transition
+			if (vehicle_status.is_vtol && vehicle_status.in_transition_mode) {
+				if (vehicle_status.in_transition_to_fw) {
+					flight_phase = ActuatorEffectiveness::FlightPhase::TRANSITION_HF_TO_FF;
+
+				} else {
+					flight_phase = ActuatorEffectiveness::FlightPhase::TRANSITION_FF_TO_HF;
+				}
+			}
+
+			// Forward to effectiveness source
+			_actuator_effectiveness->setFlightPhase(flight_phase);
+		}
+	}
+
+	{
+		vehicle_control_mode_s vehicle_control_mode;
+
+		if (_vehicle_control_mode_sub.update(&vehicle_control_mode)) {
+			_publish_controls = vehicle_control_mode.flag_control_allocation_enabled;
+		}
+	}
+
+	// Guard against too small (< 0.2ms) and too large (> 20ms) dt's.
+	const hrt_abstime now = hrt_absolute_time();
+	const float dt = math::constrain(((now - _last_run) / 1e6f), 0.0002f, 0.02f);
+
+	bool do_update = false;
+	vehicle_torque_setpoint_s vehicle_torque_setpoint;
+	vehicle_thrust_setpoint_s vehicle_thrust_setpoint;
+
+	// Run allocator on torque changes
+	if (_vehicle_torque_setpoint_sub.update(&vehicle_torque_setpoint)) {
+		_torque_sp = matrix::Vector3f(vehicle_torque_setpoint.xyz);
+
+		do_update = true;
+		_timestamp_sample = vehicle_torque_setpoint.timestamp_sample;
+
+	}
+
+	// Also run allocator on thrust setpoint changes if the torque setpoint
+	// has not been updated for more than 5ms
+	if (_vehicle_thrust_setpoint_sub.update(&vehicle_thrust_setpoint)) {
+		_thrust_sp = matrix::Vector3f(vehicle_thrust_setpoint.xyz);
+
+		if (dt > 0.005f) {
+			do_update = true;
+			_timestamp_sample = vehicle_thrust_setpoint.timestamp_sample;
+		}
+	}
+
+	if (do_update) {
+		_last_run = now;
+
+		check_for_motor_failures();
+
+		update_effectiveness_matrix_if_needed(EffectivenessUpdateReason::NO_EXTERNAL_UPDATE);
+
+		// Set control setpoint vector(s)
+		matrix::Vector<float, NUM_AXES> c[ActuatorEffectiveness::MAX_NUM_MATRICES];
+		c[0](0) = _torque_sp(0);
+		c[0](1) = _torque_sp(1);
+		c[0](2) = _torque_sp(2);
+		c[0](3) = _thrust_sp(0);
+		c[0](4) = _thrust_sp(1);
+		c[0](5) = _thrust_sp(2);
+
+		if (_num_control_allocation > 1) {
+			if (_vehicle_torque_setpoint1_sub.copy(&vehicle_torque_setpoint)) {
+				c[1](0) = vehicle_torque_setpoint.xyz[0];
+				c[1](1) = vehicle_torque_setpoint.xyz[1];
+				c[1](2) = vehicle_torque_setpoint.xyz[2];
+			}
+
+			if (_vehicle_thrust_setpoint1_sub.copy(&vehicle_thrust_setpoint)) {
+				c[1](3) = vehicle_thrust_setpoint.xyz[0];
+				c[1](4) = vehicle_thrust_setpoint.xyz[1];
+				c[1](5) = vehicle_thrust_setpoint.xyz[2];
+			}
+		}
+
+		for (int i = 0; i < _num_control_allocation; ++i) {
+
+			_control_allocation[i]->setControlSetpoint(c[i]);
+
+			// Do allocation
+			_control_allocation[i]->allocate();
+			_actuator_effectiveness->allocateAuxilaryControls(dt, i, _control_allocation[i]->_actuator_sp); //flaps and spoilers
+			_actuator_effectiveness->updateSetpoint(c[i], i, _control_allocation[i]->_actuator_sp,
+								_control_allocation[i]->getActuatorMin(), _control_allocation[i]->getActuatorMax());
+
+			if (_has_slew_rate) {
+				_control_allocation[i]->applySlewRateLimit(dt);
+			}
+
+			_control_allocation[i]->clipActuatorSetpoint();
+		}
+	}
+
+	// Publish actuator setpoint and allocator status
+	publish_actuator_controls();
+
+	// Publish status at limited rate, as it's somewhat expensive and we use it for slower dynamics
+	// (i.e. anti-integrator windup)
+	if (now - _last_status_pub >= 5_ms) {
+		publish_control_allocator_status(0);
+
+		if (_num_control_allocation > 1) {
+			publish_control_allocator_status(1);
+		}
+
+		_last_status_pub = now;
+	}
+
+	perf_end(_loop_perf);
+}
+
+void
+ControlAllocator::update_effectiveness_matrix_if_needed(EffectivenessUpdateReason reason)
+{
+	ActuatorEffectiveness::Configuration config{};
+
+	if (reason == EffectivenessUpdateReason::NO_EXTERNAL_UPDATE
+	    && hrt_elapsed_time(&_last_effectiveness_update) < 100_ms) { // rate-limit updates
+		return;
+	}
+
+	if (_actuator_effectiveness->getEffectivenessMatrix(config, reason)) {
+		_last_effectiveness_update = hrt_absolute_time();
+
+		memcpy(_control_allocation_selection_indexes, config.matrix_selection_indexes,
+		       sizeof(_control_allocation_selection_indexes));
+
+		// Get the minimum and maximum depending on type and configuration
+		ActuatorEffectiveness::ActuatorVector minimum[ActuatorEffectiveness::MAX_NUM_MATRICES];
+		ActuatorEffectiveness::ActuatorVector maximum[ActuatorEffectiveness::MAX_NUM_MATRICES];
+		ActuatorEffectiveness::ActuatorVector slew_rate[ActuatorEffectiveness::MAX_NUM_MATRICES];
+		int actuator_idx = 0;
+		int actuator_idx_matrix[ActuatorEffectiveness::MAX_NUM_MATRICES] {};
+
+		actuator_servos_trim_s trims{};
+		static_assert(actuator_servos_trim_s::NUM_CONTROLS == actuator_servos_s::NUM_CONTROLS, "size mismatch");
+
+		for (int actuator_type = 0; actuator_type < (int)ActuatorType::COUNT; ++actuator_type) {
+			_num_actuators[actuator_type] = config.num_actuators[actuator_type];
+
+			for (int actuator_type_idx = 0; actuator_type_idx < config.num_actuators[actuator_type]; ++actuator_type_idx) {
+				if (actuator_idx >= NUM_ACTUATORS) {
+					_num_actuators[actuator_type] = 0;
+					PX4_ERR("Too many actuators");
+					break;
+				}
+
+				int selected_matrix = _control_allocation_selection_indexes[actuator_idx];
+
+				if ((ActuatorType)actuator_type == ActuatorType::MOTORS) {
+					if (actuator_type_idx >= MAX_NUM_MOTORS) {
+						PX4_ERR("Too many motors");
+						_num_actuators[actuator_type] = 0;
+						break;
+					}
+
+					if (_param_r_rev.get() & (1u << actuator_type_idx)) {
+						minimum[selected_matrix](actuator_idx_matrix[selected_matrix]) = -1.f;
+
+					} else {
+						minimum[selected_matrix](actuator_idx_matrix[selected_matrix]) = 0.f;
+					}
+
+					slew_rate[selected_matrix](actuator_idx_matrix[selected_matrix]) = _params.slew_rate_motors[actuator_type_idx];
+
+				} else if ((ActuatorType)actuator_type == ActuatorType::SERVOS) {
+					if (actuator_type_idx >= MAX_NUM_SERVOS) {
+						PX4_ERR("Too many servos");
+						_num_actuators[actuator_type] = 0;
+						break;
+					}
+
+					minimum[selected_matrix](actuator_idx_matrix[selected_matrix]) = -1.f;
+					slew_rate[selected_matrix](actuator_idx_matrix[selected_matrix]) = _params.slew_rate_servos[actuator_type_idx];
+					trims.trim[actuator_type_idx] = config.trim[selected_matrix](actuator_idx_matrix[selected_matrix]);
+
+				} else {
+					minimum[selected_matrix](actuator_idx_matrix[selected_matrix]) = -1.f;
+				}
+
+				maximum[selected_matrix](actuator_idx_matrix[selected_matrix]) = 1.f;
+
+				++actuator_idx_matrix[selected_matrix];
+				++actuator_idx;
+			}
+		}
+
+		// Handle failed actuators
+		if (_handled_motor_failure_bitmask) {
+			actuator_idx = 0;
+			memset(&actuator_idx_matrix, 0, sizeof(actuator_idx_matrix));
+
+			for (int motors_idx = 0; motors_idx < _num_actuators[0] && motors_idx < actuator_motors_s::NUM_CONTROLS; motors_idx++) {
+				int selected_matrix = _control_allocation_selection_indexes[actuator_idx];
+
+				if (_handled_motor_failure_bitmask & (1 << motors_idx)) {
+					ActuatorEffectiveness::EffectivenessMatrix &matrix = config.effectiveness_matrices[selected_matrix];
+
+					for (int i = 0; i < NUM_AXES; i++) {
+						matrix(i, actuator_idx_matrix[selected_matrix]) = 0.0f;
+					}
+				}
+
+				++actuator_idx_matrix[selected_matrix];
+				++actuator_idx;
+			}
+		}
+
+		for (int i = 0; i < _num_control_allocation; ++i) {
+			_control_allocation[i]->setActuatorMin(minimum[i]);
+			_control_allocation[i]->setActuatorMax(maximum[i]);
+			_control_allocation[i]->setSlewRateLimit(slew_rate[i]);
+
+			// Set all the elements of a row to 0 if that row has weak authority.
+			// That ensures that the algorithm doesn't try to control axes with only marginal control authority,
+			// which in turn would degrade the control of the main axes that actually should and can be controlled.
+
+			ActuatorEffectiveness::EffectivenessMatrix &matrix = config.effectiveness_matrices[i];
+
+			for (int n = 0; n < NUM_AXES; n++) {
+				bool all_entries_small = true;
+
+				for (int m = 0; m < config.num_actuators_matrix[i]; m++) {
+					if (fabsf(matrix(n, m)) > 0.05f) {
+						all_entries_small = false;
+					}
+				}
+
+				if (all_entries_small) {
+					matrix.row(n) = 0.f;
+				}
+			}
+
+			// Assign control effectiveness matrix
+			int total_num_actuators = config.num_actuators_matrix[i];
+			_control_allocation[i]->setEffectivenessMatrix(config.effectiveness_matrices[i], config.trim[i],
+					config.linearization_point[i], total_num_actuators, reason == EffectivenessUpdateReason::CONFIGURATION_UPDATE);
+		}
+
+		trims.timestamp = hrt_absolute_time();
+		_actuator_servos_trim_pub.publish(trims);
+	}
+}
+
+void
+ControlAllocator::publish_control_allocator_status(int matrix_index)
+{
+	control_allocator_status_s control_allocator_status{};
+	control_allocator_status.timestamp = hrt_absolute_time();
+
+	// TODO: disabled motors (?)
+
+	// Allocated control
+	const matrix::Vector<float, NUM_AXES> &allocated_control = _control_allocation[matrix_index]->getAllocatedControl();
+
+	// Unallocated control
+	const matrix::Vector<float, NUM_AXES> unallocated_control = _control_allocation[matrix_index]->getControlSetpoint() -
+			allocated_control;
+	control_allocator_status.unallocated_torque[0] = unallocated_control(0);
+	control_allocator_status.unallocated_torque[1] = unallocated_control(1);
+	control_allocator_status.unallocated_torque[2] = unallocated_control(2);
+	control_allocator_status.unallocated_thrust[0] = unallocated_control(3);
+	control_allocator_status.unallocated_thrust[1] = unallocated_control(4);
+	control_allocator_status.unallocated_thrust[2] = unallocated_control(5);
+
+	// override control_allocator_status in customized saturation logic for certain effectiveness types
+	_actuator_effectiveness->getUnallocatedControl(matrix_index, control_allocator_status);
+
+	// Allocation success flags
+	control_allocator_status.torque_setpoint_achieved = (Vector3f(control_allocator_status.unallocated_torque[0],
+			control_allocator_status.unallocated_torque[1],
+			control_allocator_status.unallocated_torque[2]).norm_squared() < 1e-6f);
+	control_allocator_status.thrust_setpoint_achieved = (Vector3f(control_allocator_status.unallocated_thrust[0],
+			control_allocator_status.unallocated_thrust[1],
+			control_allocator_status.unallocated_thrust[2]).norm_squared() < 1e-6f);
+
+	// Actuator saturation
+	const matrix::Vector<float, NUM_ACTUATORS> &actuator_sp = _control_allocation[matrix_index]->getActuatorSetpoint();
+	const matrix::Vector<float, NUM_ACTUATORS> &actuator_min = _control_allocation[matrix_index]->getActuatorMin();
+	const matrix::Vector<float, NUM_ACTUATORS> &actuator_max = _control_allocation[matrix_index]->getActuatorMax();
+
+	for (int i = 0; i < NUM_ACTUATORS; i++) {
+		if (actuator_sp(i) > (actuator_max(i) - FLT_EPSILON)) {
+			control_allocator_status.actuator_saturation[i] = control_allocator_status_s::ACTUATOR_SATURATION_UPPER;
+
+		} else if (actuator_sp(i) < (actuator_min(i) + FLT_EPSILON)) {
+			control_allocator_status.actuator_saturation[i] = control_allocator_status_s::ACTUATOR_SATURATION_LOWER;
+		}
+	}
+
+	// Handled motor failures
+	control_allocator_status.handled_motor_failure_mask = _handled_motor_failure_bitmask;
+
+	_control_allocator_status_pub[matrix_index].publish(control_allocator_status);
+}
+
+void
+ControlAllocator::publish_actuator_controls()
+{
+	if (!_publish_controls) {
+		return;
+	}
+
+	actuator_motors_s actuator_motors;
+	actuator_motors.timestamp = hrt_absolute_time();
+	actuator_motors.timestamp_sample = _timestamp_sample;
+
+	actuator_servos_s actuator_servos;
+	actuator_servos.timestamp = actuator_motors.timestamp;
+	actuator_servos.timestamp_sample = _timestamp_sample;
+
+	actuator_motors.reversible_flags = _param_r_rev.get();
+
+	int actuator_idx = 0;
+	int actuator_idx_matrix[ActuatorEffectiveness::MAX_NUM_MATRICES] {};
+
+	uint32_t stopped_motors = _actuator_effectiveness->getStoppedMotors() | _handled_motor_failure_bitmask;
+
+	// motors
+	int motors_idx;
+
+	for (motors_idx = 0; motors_idx < _num_actuators[0] && motors_idx < actuator_motors_s::NUM_CONTROLS; motors_idx++) {
+		int selected_matrix = _control_allocation_selection_indexes[actuator_idx];
+		float actuator_sp = _control_allocation[selected_matrix]->getActuatorSetpoint()(actuator_idx_matrix[selected_matrix]);
+		actuator_motors.control[motors_idx] = PX4_ISFINITE(actuator_sp) ? actuator_sp : NAN;
+
+		if (stopped_motors & (1u << motors_idx)) {
+			actuator_motors.control[motors_idx] = NAN;
+		}
+
+		++actuator_idx_matrix[selected_matrix];
+		++actuator_idx;
+	}
+
+	for (int i = motors_idx; i < actuator_motors_s::NUM_CONTROLS; i++) {
+		actuator_motors.control[i] = NAN;
+	}
+
+	_actuator_motors_pub.publish(actuator_motors);
+
+	// servos
+	if (_num_actuators[1] > 0) {
+		int servos_idx;
+
+		for (servos_idx = 0; servos_idx < _num_actuators[1] && servos_idx < actuator_servos_s::NUM_CONTROLS; servos_idx++) {
+			int selected_matrix = _control_allocation_selection_indexes[actuator_idx];
+			float actuator_sp = _control_allocation[selected_matrix]->getActuatorSetpoint()(actuator_idx_matrix[selected_matrix]);
+			actuator_servos.control[servos_idx] = PX4_ISFINITE(actuator_sp) ? actuator_sp : NAN;
+			++actuator_idx_matrix[selected_matrix];
+			++actuator_idx;
+		}
+
+		for (int i = servos_idx; i < actuator_servos_s::NUM_CONTROLS; i++) {
+			actuator_servos.control[i] = NAN;
+		}
+
+		_actuator_servos_pub.publish(actuator_servos);
+	}
+}
+
+void
+ControlAllocator::check_for_motor_failures()
+{
+	failure_detector_status_s failure_detector_status;
+
+	if ((FailureMode)_param_ca_failure_mode.get() > FailureMode::IGNORE
+	    && _failure_detector_status_sub.update(&failure_detector_status)) {
+		if (failure_detector_status.fd_motor) {
+
+			if (_handled_motor_failure_bitmask != failure_detector_status.motor_failure_mask) {
+				// motor failure bitmask changed
+				switch ((FailureMode)_param_ca_failure_mode.get()) {
+				case FailureMode::REMOVE_FIRST_FAILING_MOTOR: {
+						// Count number of failed motors
+						const int num_motors_failed = math::countSetBits(failure_detector_status.motor_failure_mask);
+
+						// Only handle if it is the first failure
+						if (_handled_motor_failure_bitmask == 0 && num_motors_failed == 1) {
+							_handled_motor_failure_bitmask = failure_detector_status.motor_failure_mask;
+							PX4_WARN("Removing motor from allocation (0x%x)", _handled_motor_failure_bitmask);
+
+							for (int i = 0; i < _num_control_allocation; ++i) {
+								_control_allocation[i]->setHadActuatorFailure(true);
+							}
+
+							update_effectiveness_matrix_if_needed(EffectivenessUpdateReason::MOTOR_ACTIVATION_UPDATE);
+						}
+					}
+					break;
+
+				default:
+					break;
+				}
+
+			}
+
+		} else if (_handled_motor_failure_bitmask != 0) {
+			// Clear bitmask completely
+			PX4_INFO("Restoring all motors");
+			_handled_motor_failure_bitmask = 0;
+
+			for (int i = 0; i < _num_control_allocation; ++i) {
+				_control_allocation[i]->setHadActuatorFailure(false);
+			}
+
+			update_effectiveness_matrix_if_needed(EffectivenessUpdateReason::MOTOR_ACTIVATION_UPDATE);
+		}
+	}
+}
+
+int ControlAllocator::task_spawn(int argc, char *argv[])
+{
+	ControlAllocator *instance = new ControlAllocator();
+
+	if (instance) {
+		_object.store(instance);
+		_task_id = task_id_is_work_queue;
+
+		if (instance->init()) {
+			return PX4_OK;
+		}
+
+	} else {
+		PX4_ERR("alloc failed");
+	}
+
+	delete instance;
+	_object.store(nullptr);
+	_task_id = -1;
+
+	return PX4_ERROR;
+}
+
+int ControlAllocator::print_status()
+{
+	PX4_INFO("Running");
+
+	// Print current allocation method
+	switch (_allocation_method_id) {
+	case AllocationMethod::NONE:
+		PX4_INFO("Method: None");
+		break;
+
+	case AllocationMethod::PSEUDO_INVERSE:
+		PX4_INFO("Method: Pseudo-inverse");
+		break;
+
+	case AllocationMethod::SEQUENTIAL_DESATURATION:
+		PX4_INFO("Method: Sequential desaturation");
+		break;
+
+	case AllocationMethod::AUTO:
+		PX4_INFO("Method: Auto");
+		break;
+	}
+
+	// Print current airframe
+	if (_actuator_effectiveness != nullptr) {
+		PX4_INFO("Effectiveness Source: %s", _actuator_effectiveness->name());
+	}
+
+	// Print current effectiveness matrix
+	for (int i = 0; i < _num_control_allocation; ++i) {
+		const ActuatorEffectiveness::EffectivenessMatrix &effectiveness = _control_allocation[i]->getEffectivenessMatrix();
+
+		if (_num_control_allocation > 1) {
+			PX4_INFO("Instance: %i", i);
+		}
+
+		PX4_INFO("  Effectiveness.T =");
+		effectiveness.T().print();
+		PX4_INFO("  minimum =");
+		_control_allocation[i]->getActuatorMin().T().print();
+		PX4_INFO("  maximum =");
+		_control_allocation[i]->getActuatorMax().T().print();
+		PX4_INFO("  Configured actuators: %i", _control_allocation[i]->numConfiguredActuators());
+	}
+
+	if (_handled_motor_failure_bitmask) {
+		PX4_INFO("Failed motors: %i (0x%x)", math::countSetBits(_handled_motor_failure_bitmask),
+			 _handled_motor_failure_bitmask);
+	}
+
+	// Print perf
+	perf_print_counter(_loop_perf);
+
+	return 0;
+}
+
+int ControlAllocator::custom_command(int argc, char *argv[])
+{
+	return print_usage("unknown command");
+}
+
+int ControlAllocator::print_usage(const char *reason)
+{
+	if (reason) {
+		PX4_WARN("%s\n", reason);
+	}
+
+	PRINT_MODULE_DESCRIPTION(
+		R"DESCR_STR(
+### Description
+This implements control allocation. It takes torque and thrust setpoints
+as inputs and outputs actuator setpoint messages.
+)DESCR_STR");
+
+	PRINT_MODULE_USAGE_NAME("control_allocator", "controller");
+	PRINT_MODULE_USAGE_COMMAND("start");
+	PRINT_MODULE_USAGE_DEFAULT_COMMANDS();
+
+	return 0;
+}
+
+/**
+ * Control Allocator app start / stop handling function
+ */
+extern "C" __EXPORT int metric_allocator_main(int argc, char *argv[]);
+
+int metric_allocator_main(int argc, char *argv[])
+{
+	return ControlAllocator::main(argc, argv);
+}

src/modules/metric_allocator/ControlAllocator.hpp

@@ -0,0 +1,220 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2013-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 ControlAllocator.hpp
+ *
+ * Control allocator.
+ *
+ * @author Julien Lecoeur <julien.lecoeur@gmail.com>
+ */
+
+#pragma once
+
+#include <ActuatorEffectiveness.hpp>
+#include <ActuatorEffectivenessMultirotor.hpp>
+#include <ActuatorEffectivenessStandardVTOL.hpp>
+#include <ActuatorEffectivenessTiltrotorVTOL.hpp>
+#include <ActuatorEffectivenessTailsitterVTOL.hpp>
+#include <ActuatorEffectivenessRoverAckermann.hpp>
+#include <ActuatorEffectivenessFixedWing.hpp>
+#include <ActuatorEffectivenessMCTilt.hpp>
+#include <ActuatorEffectivenessCustom.hpp>
+#include <ActuatorEffectivenessUUV.hpp>
+#include <ActuatorEffectivenessHelicopter.hpp>
+#include <ActuatorEffectivenessHelicopterCoaxial.hpp>
+
+#include <ControlAllocation.hpp>
+#include <ControlAllocationPseudoInverse.hpp>
+#include <ControlAllocationSequentialDesaturation.hpp>
+
+#include <lib/matrix/matrix/math.hpp>
+#include <lib/perf/perf_counter.h>
+#include <px4_platform_common/px4_config.h>
+#include <px4_platform_common/module.h>
+#include <px4_platform_common/module_params.h>
+#include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp>
+#include <uORB/Publication.hpp>
+#include <uORB/PublicationMulti.hpp>
+#include <uORB/Subscription.hpp>
+#include <uORB/SubscriptionCallback.hpp>
+#include <uORB/topics/actuator_motors.h>
+#include <uORB/topics/actuator_servos.h>
+#include <uORB/topics/actuator_servos_trim.h>
+#include <uORB/topics/control_allocator_status.h>
+#include <uORB/topics/parameter_update.h>
+#include <uORB/topics/vehicle_control_mode.h>
+#include <uORB/topics/vehicle_torque_setpoint.h>
+#include <uORB/topics/vehicle_thrust_setpoint.h>
+#include <uORB/topics/vehicle_status.h>
+#include <uORB/topics/failure_detector_status.h>
+
+class ControlAllocator : public ModuleBase<ControlAllocator>, public ModuleParams, public px4::ScheduledWorkItem
+{
+public:
+	static constexpr int NUM_ACTUATORS = ControlAllocation::NUM_ACTUATORS;
+	static constexpr int NUM_AXES = ControlAllocation::NUM_AXES;
+
+	static constexpr int MAX_NUM_MOTORS = actuator_motors_s::NUM_CONTROLS;
+	static constexpr int MAX_NUM_SERVOS = actuator_servos_s::NUM_CONTROLS;
+
+	using ActuatorVector = ActuatorEffectiveness::ActuatorVector;
+
+	ControlAllocator();
+
+	virtual ~ControlAllocator();
+
+	/** @see ModuleBase */
+	static int task_spawn(int argc, char *argv[]);
+
+	/** @see ModuleBase */
+	static int custom_command(int argc, char *argv[]);
+
+	/** @see ModuleBase */
+	static int print_usage(const char *reason = nullptr);
+
+	/** @see ModuleBase::print_status() */
+	int print_status() override;
+
+	void Run() override;
+
+	bool init();
+
+private:
+
+	struct ParamHandles {
+		param_t slew_rate_motors[MAX_NUM_MOTORS];
+		param_t slew_rate_servos[MAX_NUM_SERVOS];
+	};
+
+	struct Params {
+		float slew_rate_motors[MAX_NUM_MOTORS];
+		float slew_rate_servos[MAX_NUM_SERVOS];
+	};
+
+	/**
+	 * initialize some vectors/matrices from parameters
+	 */
+	void parameters_updated();
+
+	void update_allocation_method(bool force);
+	bool update_effectiveness_source();
+
+	void update_effectiveness_matrix_if_needed(EffectivenessUpdateReason reason);
+
+	void check_for_motor_failures();
+
+	void publish_control_allocator_status(int matrix_index);
+
+	void publish_actuator_controls();
+
+	AllocationMethod _allocation_method_id{AllocationMethod::NONE};
+	ControlAllocation *_control_allocation[ActuatorEffectiveness::MAX_NUM_MATRICES] {}; 	///< class for control allocation calculations
+	int _num_control_allocation{0};
+	hrt_abstime _last_effectiveness_update{0};
+
+	enum class EffectivenessSource {
+		NONE = -1,
+		MULTIROTOR = 0,
+		FIXED_WING = 1,
+		STANDARD_VTOL = 2,
+		TILTROTOR_VTOL = 3,
+		TAILSITTER_VTOL = 4,
+		ROVER_ACKERMANN = 5,
+		ROVER_DIFFERENTIAL = 6,
+		MOTORS_6DOF = 7,
+		MULTIROTOR_WITH_TILT = 8,
+		CUSTOM = 9,
+		HELICOPTER_TAIL_ESC = 10,
+		HELICOPTER_TAIL_SERVO = 11,
+		HELICOPTER_COAXIAL = 12,
+	};
+
+	enum class FailureMode {
+		IGNORE = 0,
+		REMOVE_FIRST_FAILING_MOTOR = 1,
+	};
+
+	EffectivenessSource _effectiveness_source_id{EffectivenessSource::NONE};
+	ActuatorEffectiveness *_actuator_effectiveness{nullptr}; 	///< class providing actuator effectiveness
+
+	uint8_t _control_allocation_selection_indexes[NUM_ACTUATORS * ActuatorEffectiveness::MAX_NUM_MATRICES] {};
+	int _num_actuators[(int)ActuatorType::COUNT] {};
+
+	// Inputs
+	uORB::SubscriptionCallbackWorkItem _vehicle_torque_setpoint_sub{this, ORB_ID(vehicle_torque_setpoint)};  /**< vehicle torque setpoint subscription */
+	uORB::SubscriptionCallbackWorkItem _vehicle_thrust_setpoint_sub{this, ORB_ID(vehicle_thrust_setpoint)};	 /**< vehicle thrust setpoint subscription */
+
+	uORB::Subscription _vehicle_torque_setpoint1_sub{ORB_ID(vehicle_torque_setpoint), 1};  /**< vehicle torque setpoint subscription (2. instance) */
+	uORB::Subscription _vehicle_thrust_setpoint1_sub{ORB_ID(vehicle_thrust_setpoint), 1};	 /**< vehicle thrust setpoint subscription (2. instance) */
+
+	// Outputs
+	uORB::PublicationMulti<control_allocator_status_s> _control_allocator_status_pub[2] {ORB_ID(control_allocator_status), ORB_ID(control_allocator_status)};
+
+	uORB::Publication<actuator_motors_s>	_actuator_motors_pub{ORB_ID(actuator_motors)};
+	uORB::Publication<actuator_servos_s>	_actuator_servos_pub{ORB_ID(actuator_servos)};
+	uORB::Publication<actuator_servos_trim_s>	_actuator_servos_trim_pub{ORB_ID(actuator_servos_trim)};
+
+	uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s};
+
+	uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)};
+	uORB::Subscription _vehicle_control_mode_sub{ORB_ID(vehicle_control_mode)};
+	uORB::Subscription _failure_detector_status_sub{ORB_ID(failure_detector_status)};
+
+	matrix::Vector3f _torque_sp;
+	matrix::Vector3f _thrust_sp;
+	bool _publish_controls{true};
+
+	// Reflects motor failures that are currently handled, not motor failures that are reported.
+	// For example, the system might report two motor failures, but only the first one is handled by CA
+	uint16_t _handled_motor_failure_bitmask{0};
+
+	perf_counter_t	_loop_perf;			/**< loop duration performance counter */
+
+	bool _armed{false};
+	hrt_abstime _last_run{0};
+	hrt_abstime _timestamp_sample{0};
+	hrt_abstime _last_status_pub{0};
+
+	ParamHandles _param_handles{};
+	Params _params{};
+	bool _has_slew_rate{false};
+
+	DEFINE_PARAMETERS(
+		(ParamInt<px4::params::CA_AIRFRAME>) _param_ca_airframe,
+		(ParamInt<px4::params::CA_METHOD>) _param_ca_method,
+		(ParamInt<px4::params::CA_FAILURE_MODE>) _param_ca_failure_mode,
+		(ParamInt<px4::params::CA_R_REV>) _param_r_rev
+	)
+
+};

src/modules/metric_allocator/Kconfig

@@ -0,0 +1,12 @@
+menuconfig MODULES_METRIC_ALLOCATOR
+	bool "metric_allocator"
+	default n
+	---help---
+		Enable support for metric_allocator
+
+menuconfig USER_METRIC_ALLOCATOR
+	bool "metric_allocator running as userspace module"
+	default y
+	depends on BOARD_PROTECTED && MODULES_METRIC_ALLOCATOR
+	---help---
+		Put metric_allocator in userspace memory