Make control allocation and actuator effectiveness a non-module-specific library (#24196)

* Remove more circular dependencies with ActuatorEffectiveness

* Separate vehicle specific actuator effectiveness

Keep actuator effectivenss in control allocator

* Remove test dependency for now

* Group library directories
Fix

* Change directory names

* Rebase fix

src/lib/CMakeLists.txt

@@ -43,6 +43,7 @@ add_subdirectory(cdrstream EXCLUDE_FROM_ALL)
 add_subdirectory(circuit_breaker EXCLUDE_FROM_ALL)
 add_subdirectory(collision_prevention EXCLUDE_FROM_ALL)
 add_subdirectory(component_information EXCLUDE_FROM_ALL)
+add_subdirectory(control_allocation EXCLUDE_FROM_ALL)
 add_subdirectory(controllib EXCLUDE_FROM_ALL)
 add_subdirectory(conversion EXCLUDE_FROM_ALL)
 add_subdirectory(crc EXCLUDE_FROM_ALL)

src/lib/control_allocation/CMakeLists.txt

@@ -0,0 +1,35 @@
+############################################################################
+#
+#   Copyright (c) 2025 PX4 Development Team. All rights reserved.
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions
+# are met:
+#
+# 1. Redistributions of source code must retain the above copyright
+#    notice, this list of conditions and the following disclaimer.
+# 2. Redistributions in binary form must reproduce the above copyright
+#    notice, this list of conditions and the following disclaimer in
+#    the documentation and/or other materials provided with the
+#    distribution.
+# 3. Neither the name PX4 nor the names of its contributors may be
+#    used to endorse or promote products derived from this software
+#    without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+#
+############################################################################
+
+add_subdirectory(control_allocation)
+add_subdirectory(actuator_effectiveness)

src/lib/control_allocation/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/control_allocation/actuator_effectiveness/ActuatorEffectiveness.hpp

@@ -0,0 +1,225 @@
+/****************************************************************************
+ *
+ *   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,
+};
+
+enum class ActuatorType {
+	MOTORS = 0,
+	SERVOS,
+
+	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/control_allocation/actuator_effectiveness/CMakeLists.txt

@@ -0,0 +1,45 @@
+############################################################################
+#
+#   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
+)
+
+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
+)

src/lib/control_allocation/control_allocation/CMakeLists.txt

@@ -0,0 +1,47 @@
+############################################################################
+#
+#   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(ControlAllocation
+	ControlAllocation.cpp
+	ControlAllocation.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_functional_gtest(SRC ControlAllocationSequentialDesaturationTest.cpp LINKLIBS ControlAllocation ActuatorEffectiveness)

src/lib/control_allocation/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/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 "control_allocation/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/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/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/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/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/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/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);
+	}
+}