control_allocator: major refactoring & additions

- allow effectiveness matrix to select control allocator method
  (desaturation algorithm)
- add actuator_servos publication
- add support for multiple matrices (for vtol)
- add updateSetpoint callback method to actuator effectiveness to allow it
  to manipulate the actuator setpoint after allocation
- handle motor stopping & reversal
- add control surfaces & tilt servos
- handle standard vtol + tiltrotor
- rename MC rotors params & class to be more generically usable
- fixes and enables ActuatorEffectivenessRotorsTest

src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectiveness.cpp

@@ -0,0 +1,86 @@
+/****************************************************************************
+ *
+ *   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 "../ControlAllocation/ControlAllocation.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](ControlAllocation::ControlAxis::ROLL, actuator_idx) = torque(0);
+	effectiveness_matrices[selected_matrix](ControlAllocation::ControlAxis::PITCH, actuator_idx) = torque(1);
+	effectiveness_matrices[selected_matrix](ControlAllocation::ControlAxis::YAW, actuator_idx) = torque(2);
+	effectiveness_matrices[selected_matrix](ControlAllocation::ControlAxis::THRUST_X, actuator_idx) = thrust(0);
+	effectiveness_matrices[selected_matrix](ControlAllocation::ControlAxis::THRUST_Y, actuator_idx) = thrust(1);
+	effectiveness_matrices[selected_matrix](ControlAllocation::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;
+}

src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectiveness.hpp

@@ -41,18 +41,36 @@
 
 #pragma once
 
-#include <ControlAllocation/ControlAllocation.hpp>
-
 #include <matrix/matrix/math.hpp>
 
+enum class AllocationMethod {
+	NONE = -1,
+	PSEUDO_INVERSE = 0,
+	SEQUENTIAL_DESATURATION = 1,
+	AUTO = 2,
+};
+
+enum class ActuatorType {
+	MOTORS = 0,
+	SERVOS,
+
+	COUNT
+};
+
+
 class ActuatorEffectiveness
 {
 public:
 	ActuatorEffectiveness() = default;
 	virtual ~ActuatorEffectiveness() = default;
 
-	static constexpr uint8_t NUM_ACTUATORS = ControlAllocation::NUM_ACTUATORS;
-	static constexpr uint8_t NUM_AXES = ControlAllocation::NUM_AXES;
+	static constexpr int NUM_ACTUATORS = 16;
+	static constexpr int NUM_AXES = 6;
+
+	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,
@@ -61,6 +79,31 @@ public:
 		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];
+
+		int selected_matrix;
+
+		uint8_t matrix_selection_indexes[NUM_ACTUATORS * MAX_NUM_MATRICES];
+		int num_actuators[(int)ActuatorType::COUNT];
+	};
+
 	/**
 	 * Set the current flight phase
 	 *
@@ -71,22 +114,28 @@ public:
 		_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;
+		}
+	}
+
 	/**
 	 * Get the control effectiveness matrix if updated
 	 *
 	 * @return true if updated and matrix is set
 	 */
-	virtual bool getEffectivenessMatrix(matrix::Matrix<float, NUM_AXES, NUM_ACTUATORS> &matrix, bool force) = 0;
-
-	/**
-	 * Get the actuator trims
-	 *
-	 * @return Actuator trims
-	 */
-	const matrix::Vector<float, NUM_ACTUATORS> &getActuatorTrim() const
-	{
-		return _trim;
-	}
+	virtual bool getEffectivenessMatrix(Configuration &configuration, bool force) = 0;
 
 	/**
 	 * Get the current flight phase
@@ -98,17 +147,26 @@ public:
 		return _flight_phase;
 	}
 
-	/**
-	 * Get the number of actuators
-	 */
-	virtual int numActuators() const = 0;
-
 	/**
 	 * Display name
 	 */
 	virtual const char *name() const = 0;
 
+
+	/**
+	 * 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) {}
+
+	/**
+	 * Get a bitmask of motors to be stopped
+	 */
+	virtual uint32_t getStoppedMotors() const { return 0; }
+
 protected:
-	matrix::Vector<float, NUM_ACTUATORS> _trim;			///< Actuator trim
 	FlightPhase _flight_phase{FlightPhase::HOVER_FLIGHT};		///< Current flight phase
 };

src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessControlSurfaces.cpp

@@ -0,0 +1,159 @@
+/****************************************************************************
+ *
+ *   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 <px4_platform_common/log.h>
+
+#include "ActuatorEffectivenessControlSurfaces.hpp"
+
+using namespace matrix;
+
+ActuatorEffectivenessControlSurfaces::ActuatorEffectivenessControlSurfaces(ModuleParams *parent)
+	: ModuleParams(parent)
+{
+	for (int i = 0; i < MAX_COUNT; ++i) {
+		char buffer[17];
+		snprintf(buffer, sizeof(buffer), "CA_SV_CS%u_TYPE", i);
+		_param_handles[i].type = param_find(buffer);
+		snprintf(buffer, sizeof(buffer), "CA_SV_CS%u_TRQ_R", i);
+		_param_handles[i].torque[0] = param_find(buffer);
+		snprintf(buffer, sizeof(buffer), "CA_SV_CS%u_TRQ_P", i);
+		_param_handles[i].torque[1] = param_find(buffer);
+		snprintf(buffer, sizeof(buffer), "CA_SV_CS%u_TRQ_Y", i);
+		_param_handles[i].torque[2] = param_find(buffer);
+	}
+
+	_count_handle = param_find("CA_SV_CS_COUNT");
+	updateParams();
+}
+
+void ActuatorEffectivenessControlSurfaces::updateParams()
+{
+	ModuleParams::updateParams();
+
+	int32_t count = 0;
+
+	if (param_get(_count_handle, &count) != 0) {
+		PX4_ERR("param_get failed");
+		return;
+	}
+
+	_count = count;
+
+	for (int i = 0; i < _count; i++) {
+		param_get(_param_handles[i].type, (int32_t *)&_params[i].type);
+
+		Vector3f &torque = _params[i].torque;
+
+		for (int n = 0; n < 3; ++n) {
+			param_get(_param_handles[i].torque[n], &torque(n));
+		}
+
+		// TODO: enforce limits?
+		switch (_params[i].type) {
+		case Type::Elevator:
+			break;
+
+		case Type::Rudder:
+			break;
+
+		case Type::LeftElevon:
+			break;
+
+		case Type::RightElevon:
+			break;
+
+		case Type::LeftVTail:
+			break;
+
+		case Type::RightVTail:
+			break;
+
+		case Type::LeftFlaps:
+		case Type::RightFlaps:
+			torque.setZero();
+			break;
+
+		case Type::LeftAileron:
+			break;
+
+		case Type::RightAileron:
+			break;
+
+		case Type::Airbrakes:
+			torque.setZero();
+			break;
+
+		case Type::Custom:
+			break;
+		}
+	}
+}
+
+bool ActuatorEffectivenessControlSurfaces::getEffectivenessMatrix(Configuration &configuration, bool force)
+{
+	if (!force) {
+		return false;
+	}
+
+	for (int i = 0; i < _count; i++) {
+		configuration.addActuator(ActuatorType::SERVOS, _params[i].torque, Vector3f{});
+	}
+
+	return true;
+}
+
+void ActuatorEffectivenessControlSurfaces::applyFlapsAndAirbrakes(float flaps_control, float airbrakes_control,
+		int first_actuator_idx,
+		ActuatorVector &actuator_sp) const
+{
+	for (int i = 0; i < _count; ++i) {
+		switch (_params[i].type) {
+		// TODO: check sign
+		case ActuatorEffectivenessControlSurfaces::Type::LeftFlaps:
+			actuator_sp(i + first_actuator_idx) += flaps_control;
+			break;
+
+		case ActuatorEffectivenessControlSurfaces::Type::RightFlaps:
+			actuator_sp(i + first_actuator_idx) -= flaps_control;
+			break;
+
+		case ActuatorEffectivenessControlSurfaces::Type::Airbrakes:
+			actuator_sp(i + first_actuator_idx) += airbrakes_control;
+			break;
+
+		default:
+			break;
+		}
+	}
+
+}

src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessControlSurfaces.hpp

@@ -0,0 +1,95 @@
+/****************************************************************************
+ *
+ *   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.
+ *
+ ****************************************************************************/
+
+#pragma once
+
+#include "ActuatorEffectiveness.hpp"
+
+#include <px4_platform_common/module_params.h>
+
+class ActuatorEffectivenessControlSurfaces : public ModuleParams, public ActuatorEffectiveness
+{
+public:
+
+	static constexpr int MAX_COUNT = 8;
+
+	enum class Type : int32_t {
+		// This matches with the parameter
+		Elevator = 1,
+		Rudder = 2,
+		LeftElevon = 3,
+		RightElevon = 4,
+		LeftVTail = 5,
+		RightVTail = 6,
+		LeftFlaps = 7,
+		RightFlaps = 8,
+		LeftAileron = 9,
+		RightAileron = 10,
+		Airbrakes = 11,
+		Custom = 12,
+	};
+
+	struct Params {
+		Type type;
+
+		matrix::Vector3f torque;
+	};
+
+	ActuatorEffectivenessControlSurfaces(ModuleParams *parent);
+	virtual ~ActuatorEffectivenessControlSurfaces() = default;
+
+	bool getEffectivenessMatrix(Configuration &configuration, bool force) override;
+
+	const char *name() const override { return "Control Surfaces"; }
+
+	int count() const { return _count; }
+
+	const Params &config(int idx) const { return _params[idx]; }
+
+	void applyFlapsAndAirbrakes(float flaps_control, float airbrakes_control, int first_actuator_idx,
+				    ActuatorVector &actuator_sp) const;
+
+private:
+	void updateParams() override;
+
+	struct ParamHandles {
+		param_t type;
+
+		param_t torque[3];
+	};
+	ParamHandles _param_handles[MAX_COUNT];
+	param_t _count_handle;
+
+	Params _params[MAX_COUNT] {};
+	int _count{0};
+};

src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessMultirotor.cpp

@@ -1,198 +0,0 @@
-/****************************************************************************
- *
- *   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 ActuatorEffectivenessMultirotor.hpp
- *
- * Actuator effectiveness computed from rotors position and orientation
- *
- * @author Julien Lecoeur <julien.lecoeur@gmail.com>
- */
-
-#include "ActuatorEffectivenessMultirotor.hpp"
-
-ActuatorEffectivenessMultirotor::ActuatorEffectivenessMultirotor(ModuleParams *parent):
-	ModuleParams(parent)
-{
-}
-
-bool
-ActuatorEffectivenessMultirotor::getEffectivenessMatrix(matrix::Matrix<float, NUM_AXES, NUM_ACTUATORS> &matrix,
-		bool force)
-{
-	if (_updated || force) {
-		_updated = false;
-
-		// Get multirotor geometry
-		MultirotorGeometry geometry = {};
-		geometry.rotors[0].position_x = _param_ca_mc_r0_px.get();
-		geometry.rotors[0].position_y = _param_ca_mc_r0_py.get();
-		geometry.rotors[0].position_z = _param_ca_mc_r0_pz.get();
-		geometry.rotors[0].axis_x = _param_ca_mc_r0_ax.get();
-		geometry.rotors[0].axis_y = _param_ca_mc_r0_ay.get();
-		geometry.rotors[0].axis_z = _param_ca_mc_r0_az.get();
-		geometry.rotors[0].thrust_coef = _param_ca_mc_r0_ct.get();
-		geometry.rotors[0].moment_ratio = _param_ca_mc_r0_km.get();
-
-		geometry.rotors[1].position_x = _param_ca_mc_r1_px.get();
-		geometry.rotors[1].position_y = _param_ca_mc_r1_py.get();
-		geometry.rotors[1].position_z = _param_ca_mc_r1_pz.get();
-		geometry.rotors[1].axis_x = _param_ca_mc_r1_ax.get();
-		geometry.rotors[1].axis_y = _param_ca_mc_r1_ay.get();
-		geometry.rotors[1].axis_z = _param_ca_mc_r1_az.get();
-		geometry.rotors[1].thrust_coef = _param_ca_mc_r1_ct.get();
-		geometry.rotors[1].moment_ratio = _param_ca_mc_r1_km.get();
-
-		geometry.rotors[2].position_x = _param_ca_mc_r2_px.get();
-		geometry.rotors[2].position_y = _param_ca_mc_r2_py.get();
-		geometry.rotors[2].position_z = _param_ca_mc_r2_pz.get();
-		geometry.rotors[2].axis_x = _param_ca_mc_r2_ax.get();
-		geometry.rotors[2].axis_y = _param_ca_mc_r2_ay.get();
-		geometry.rotors[2].axis_z = _param_ca_mc_r2_az.get();
-		geometry.rotors[2].thrust_coef = _param_ca_mc_r2_ct.get();
-		geometry.rotors[2].moment_ratio = _param_ca_mc_r2_km.get();
-
-		geometry.rotors[3].position_x = _param_ca_mc_r3_px.get();
-		geometry.rotors[3].position_y = _param_ca_mc_r3_py.get();
-		geometry.rotors[3].position_z = _param_ca_mc_r3_pz.get();
-		geometry.rotors[3].axis_x = _param_ca_mc_r3_ax.get();
-		geometry.rotors[3].axis_y = _param_ca_mc_r3_ay.get();
-		geometry.rotors[3].axis_z = _param_ca_mc_r3_az.get();
-		geometry.rotors[3].thrust_coef = _param_ca_mc_r3_ct.get();
-		geometry.rotors[3].moment_ratio = _param_ca_mc_r3_km.get();
-
-		geometry.rotors[4].position_x = _param_ca_mc_r4_px.get();
-		geometry.rotors[4].position_y = _param_ca_mc_r4_py.get();
-		geometry.rotors[4].position_z = _param_ca_mc_r4_pz.get();
-		geometry.rotors[4].axis_x = _param_ca_mc_r4_ax.get();
-		geometry.rotors[4].axis_y = _param_ca_mc_r4_ay.get();
-		geometry.rotors[4].axis_z = _param_ca_mc_r4_az.get();
-		geometry.rotors[4].thrust_coef = _param_ca_mc_r4_ct.get();
-		geometry.rotors[4].moment_ratio = _param_ca_mc_r4_km.get();
-
-		geometry.rotors[5].position_x = _param_ca_mc_r5_px.get();
-		geometry.rotors[5].position_y = _param_ca_mc_r5_py.get();
-		geometry.rotors[5].position_z = _param_ca_mc_r5_pz.get();
-		geometry.rotors[5].axis_x = _param_ca_mc_r5_ax.get();
-		geometry.rotors[5].axis_y = _param_ca_mc_r5_ay.get();
-		geometry.rotors[5].axis_z = _param_ca_mc_r5_az.get();
-		geometry.rotors[5].thrust_coef = _param_ca_mc_r5_ct.get();
-		geometry.rotors[5].moment_ratio = _param_ca_mc_r5_km.get();
-
-		geometry.rotors[6].position_x = _param_ca_mc_r6_px.get();
-		geometry.rotors[6].position_y = _param_ca_mc_r6_py.get();
-		geometry.rotors[6].position_z = _param_ca_mc_r6_pz.get();
-		geometry.rotors[6].axis_x = _param_ca_mc_r6_ax.get();
-		geometry.rotors[6].axis_y = _param_ca_mc_r6_ay.get();
-		geometry.rotors[6].axis_z = _param_ca_mc_r6_az.get();
-		geometry.rotors[6].thrust_coef = _param_ca_mc_r6_ct.get();
-		geometry.rotors[6].moment_ratio = _param_ca_mc_r6_km.get();
-
-		geometry.rotors[7].position_x = _param_ca_mc_r7_px.get();
-		geometry.rotors[7].position_y = _param_ca_mc_r7_py.get();
-		geometry.rotors[7].position_z = _param_ca_mc_r7_pz.get();
-		geometry.rotors[7].axis_x = _param_ca_mc_r7_ax.get();
-		geometry.rotors[7].axis_y = _param_ca_mc_r7_ay.get();
-		geometry.rotors[7].axis_z = _param_ca_mc_r7_az.get();
-		geometry.rotors[7].thrust_coef = _param_ca_mc_r7_ct.get();
-		geometry.rotors[7].moment_ratio = _param_ca_mc_r7_km.get();
-
-		geometry.num_rotors = _param_ca_mc_r_count.get();
-
-		_num_actuators = computeEffectivenessMatrix(geometry, matrix);
-		return true;
-	}
-
-	return false;
-}
-
-int
-ActuatorEffectivenessMultirotor::computeEffectivenessMatrix(const MultirotorGeometry &geometry,
-		matrix::Matrix<float, NUM_AXES, NUM_ACTUATORS> &effectiveness)
-{
-	int num_actuators = 0;
-
-	effectiveness.setZero();
-
-	for (int i = 0; i < math::min(NUM_ROTORS_MAX, geometry.num_rotors); i++) {
-
-		// Get rotor axis
-		matrix::Vector3f axis(
-			geometry.rotors[i].axis_x,
-			geometry.rotors[i].axis_y,
-			geometry.rotors[i].axis_z
-		);
-
-		// 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
-		matrix::Vector3f position(
-			geometry.rotors[i].position_x,
-			geometry.rotors[i].position_y,
-			geometry.rotors[i].position_z
-		);
-
-		// Get coefficients
-		float ct = geometry.rotors[i].thrust_coef;
-		float km = geometry.rotors[i].moment_ratio;
-
-		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) = moment(j);
-			effectiveness(j + 3, i) = thrust(j);
-		}
-	}
-
-	num_actuators = geometry.num_rotors;
-
-	return num_actuators;
-}

src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessMultirotor.hpp

@@ -1,163 +0,0 @@
-/****************************************************************************
- *
- *   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 ActuatorEffectivenessMultirotor.hpp
- *
- * Actuator effectiveness computed from rotors position and orientation
- *
- * @author Julien Lecoeur <julien.lecoeur@gmail.com>
- */
-
-#pragma once
-
-#include "ActuatorEffectiveness.hpp"
-
-#include <px4_platform_common/module_params.h>
-#include <uORB/Subscription.hpp>
-#include <uORB/SubscriptionInterval.hpp>
-
-using namespace time_literals;
-
-class ActuatorEffectivenessMultirotor: public ModuleParams, public ActuatorEffectiveness
-{
-public:
-	ActuatorEffectivenessMultirotor(ModuleParams *parent);
-	virtual ~ActuatorEffectivenessMultirotor() = default;
-
-	static constexpr int NUM_ROTORS_MAX = 8;
-
-	struct RotorGeometry {
-		float position_x;
-		float position_y;
-		float position_z;
-		float axis_x;
-		float axis_y;
-		float axis_z;
-		float thrust_coef;
-		float moment_ratio;
-	};
-
-	struct MultirotorGeometry {
-		RotorGeometry rotors[NUM_ROTORS_MAX];
-		int num_rotors{0};
-	};
-
-	static int computeEffectivenessMatrix(const MultirotorGeometry &geometry,
-					      matrix::Matrix<float, NUM_AXES, NUM_ACTUATORS> &effectiveness);
-
-	bool getEffectivenessMatrix(matrix::Matrix<float, NUM_AXES, NUM_ACTUATORS> &matrix, bool force) override;
-
-	int numActuators() const override { return _num_actuators; }
-
-	const char *name() const override { return "Multirotor"; }
-private:
-	bool _updated{true};
-	int _num_actuators{0};
-
-	DEFINE_PARAMETERS(
-		(ParamFloat<px4::params::CA_MC_R0_PX>) _param_ca_mc_r0_px,
-		(ParamFloat<px4::params::CA_MC_R0_PY>) _param_ca_mc_r0_py,
-		(ParamFloat<px4::params::CA_MC_R0_PZ>) _param_ca_mc_r0_pz,
-		(ParamFloat<px4::params::CA_MC_R0_AX>) _param_ca_mc_r0_ax,
-		(ParamFloat<px4::params::CA_MC_R0_AY>) _param_ca_mc_r0_ay,
-		(ParamFloat<px4::params::CA_MC_R0_AZ>) _param_ca_mc_r0_az,
-		(ParamFloat<px4::params::CA_MC_R0_CT>) _param_ca_mc_r0_ct,
-		(ParamFloat<px4::params::CA_MC_R0_KM>) _param_ca_mc_r0_km,
-
-		(ParamFloat<px4::params::CA_MC_R1_PX>) _param_ca_mc_r1_px,
-		(ParamFloat<px4::params::CA_MC_R1_PY>) _param_ca_mc_r1_py,
-		(ParamFloat<px4::params::CA_MC_R1_PZ>) _param_ca_mc_r1_pz,
-		(ParamFloat<px4::params::CA_MC_R1_AX>) _param_ca_mc_r1_ax,
-		(ParamFloat<px4::params::CA_MC_R1_AY>) _param_ca_mc_r1_ay,
-		(ParamFloat<px4::params::CA_MC_R1_AZ>) _param_ca_mc_r1_az,
-		(ParamFloat<px4::params::CA_MC_R1_CT>) _param_ca_mc_r1_ct,
-		(ParamFloat<px4::params::CA_MC_R1_KM>) _param_ca_mc_r1_km,
-
-		(ParamFloat<px4::params::CA_MC_R2_PX>) _param_ca_mc_r2_px,
-		(ParamFloat<px4::params::CA_MC_R2_PY>) _param_ca_mc_r2_py,
-		(ParamFloat<px4::params::CA_MC_R2_PZ>) _param_ca_mc_r2_pz,
-		(ParamFloat<px4::params::CA_MC_R2_AX>) _param_ca_mc_r2_ax,
-		(ParamFloat<px4::params::CA_MC_R2_AY>) _param_ca_mc_r2_ay,
-		(ParamFloat<px4::params::CA_MC_R2_AZ>) _param_ca_mc_r2_az,
-		(ParamFloat<px4::params::CA_MC_R2_CT>) _param_ca_mc_r2_ct,
-		(ParamFloat<px4::params::CA_MC_R2_KM>) _param_ca_mc_r2_km,
-
-		(ParamFloat<px4::params::CA_MC_R3_PX>) _param_ca_mc_r3_px,
-		(ParamFloat<px4::params::CA_MC_R3_PY>) _param_ca_mc_r3_py,
-		(ParamFloat<px4::params::CA_MC_R3_PZ>) _param_ca_mc_r3_pz,
-		(ParamFloat<px4::params::CA_MC_R3_AX>) _param_ca_mc_r3_ax,
-		(ParamFloat<px4::params::CA_MC_R3_AY>) _param_ca_mc_r3_ay,
-		(ParamFloat<px4::params::CA_MC_R3_AZ>) _param_ca_mc_r3_az,
-		(ParamFloat<px4::params::CA_MC_R3_CT>) _param_ca_mc_r3_ct,
-		(ParamFloat<px4::params::CA_MC_R3_KM>) _param_ca_mc_r3_km,
-
-		(ParamFloat<px4::params::CA_MC_R4_PX>) _param_ca_mc_r4_px,
-		(ParamFloat<px4::params::CA_MC_R4_PY>) _param_ca_mc_r4_py,
-		(ParamFloat<px4::params::CA_MC_R4_PZ>) _param_ca_mc_r4_pz,
-		(ParamFloat<px4::params::CA_MC_R4_AX>) _param_ca_mc_r4_ax,
-		(ParamFloat<px4::params::CA_MC_R4_AY>) _param_ca_mc_r4_ay,
-		(ParamFloat<px4::params::CA_MC_R4_AZ>) _param_ca_mc_r4_az,
-		(ParamFloat<px4::params::CA_MC_R4_CT>) _param_ca_mc_r4_ct,
-		(ParamFloat<px4::params::CA_MC_R4_KM>) _param_ca_mc_r4_km,
-
-		(ParamFloat<px4::params::CA_MC_R5_PX>) _param_ca_mc_r5_px,
-		(ParamFloat<px4::params::CA_MC_R5_PY>) _param_ca_mc_r5_py,
-		(ParamFloat<px4::params::CA_MC_R5_PZ>) _param_ca_mc_r5_pz,
-		(ParamFloat<px4::params::CA_MC_R5_AX>) _param_ca_mc_r5_ax,
-		(ParamFloat<px4::params::CA_MC_R5_AY>) _param_ca_mc_r5_ay,
-		(ParamFloat<px4::params::CA_MC_R5_AZ>) _param_ca_mc_r5_az,
-		(ParamFloat<px4::params::CA_MC_R5_CT>) _param_ca_mc_r5_ct,
-		(ParamFloat<px4::params::CA_MC_R5_KM>) _param_ca_mc_r5_km,
-
-		(ParamFloat<px4::params::CA_MC_R6_PX>) _param_ca_mc_r6_px,
-		(ParamFloat<px4::params::CA_MC_R6_PY>) _param_ca_mc_r6_py,
-		(ParamFloat<px4::params::CA_MC_R6_PZ>) _param_ca_mc_r6_pz,
-		(ParamFloat<px4::params::CA_MC_R6_AX>) _param_ca_mc_r6_ax,
-		(ParamFloat<px4::params::CA_MC_R6_AY>) _param_ca_mc_r6_ay,
-		(ParamFloat<px4::params::CA_MC_R6_AZ>) _param_ca_mc_r6_az,
-		(ParamFloat<px4::params::CA_MC_R6_CT>) _param_ca_mc_r6_ct,
-		(ParamFloat<px4::params::CA_MC_R6_KM>) _param_ca_mc_r6_km,
-
-		(ParamFloat<px4::params::CA_MC_R7_PX>) _param_ca_mc_r7_px,
-		(ParamFloat<px4::params::CA_MC_R7_PY>) _param_ca_mc_r7_py,
-		(ParamFloat<px4::params::CA_MC_R7_PZ>) _param_ca_mc_r7_pz,
-		(ParamFloat<px4::params::CA_MC_R7_AX>) _param_ca_mc_r7_ax,
-		(ParamFloat<px4::params::CA_MC_R7_AY>) _param_ca_mc_r7_ay,
-		(ParamFloat<px4::params::CA_MC_R7_AZ>) _param_ca_mc_r7_az,
-		(ParamFloat<px4::params::CA_MC_R7_CT>) _param_ca_mc_r7_ct,
-		(ParamFloat<px4::params::CA_MC_R7_KM>) _param_ca_mc_r7_km,
-
-		(ParamInt<px4::params::CA_MC_R_COUNT>) _param_ca_mc_r_count
-	)
-};

src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessRotors.cpp

@@ -0,0 +1,247 @@
+/****************************************************************************
+ *
+ *   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);
+		}
+	}
+
+	_count_handle = param_find("CA_ROTOR_COUNT");
+
+	updateParams();
+}
+
+void ActuatorEffectivenessRotors::updateParams()
+{
+	ModuleParams::updateParams();
+
+	int32_t count = 0;
+
+	if (param_get(_count_handle, &count) != 0) {
+		PX4_ERR("param_get failed");
+		return;
+	}
+
+	_geometry.num_rotors = count;
+
+	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::getEffectivenessMatrix(Configuration &configuration, bool force)
+{
+	if (_updated || force) {
+		_updated = false;
+
+		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;
+	}
+
+	return false;
+}
+
+int
+ActuatorEffectivenessRotors::computeEffectivenessMatrix(const Geometry &geometry,
+		EffectivenessMatrix &effectiveness, int actuator_start_index)
+{
+	int num_actuators = 0;
+
+	for (int i = 0; i < math::min(NUM_ROTORS_MAX, 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.yaw_disabled) {
+			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);
+		}
+	}
+
+	return num_actuators;
+}
+
+uint32_t ActuatorEffectivenessRotors::updateAxisFromTilts(const ActuatorEffectivenessTilts &tilts, float tilt_control)
+{
+	if (!PX4_ISFINITE(tilt_control)) {
+		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);
+		float tilt_angle = math::lerp(tilt.min_angle, tilt.max_angle, (tilt_control + 1.f) / 2.f);
+		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;
+}

src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessRotors.hpp

@@ -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.hpp
+ *
+ * Actuator effectiveness computed from rotors position and orientation
+ *
+ * @author Julien Lecoeur <julien.lecoeur@gmail.com>
+ */
+
+#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 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 = 8;
+
+	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 yaw_disabled{false};
+	};
+
+	ActuatorEffectivenessRotors(ModuleParams *parent, AxisConfiguration axis_config = AxisConfiguration::Configurable,
+				    bool tilt_support = false);
+	virtual ~ActuatorEffectivenessRotors() = default;
+
+	void getDesiredAllocationMethod(AllocationMethod allocation_method_out[MAX_NUM_MATRICES]) const override
+	{
+		allocation_method_out[0] = AllocationMethod::SEQUENTIAL_DESATURATION;
+	}
+
+	static int computeEffectivenessMatrix(const Geometry &geometry,
+					      EffectivenessMatrix &effectiveness, int actuator_start_index = 0);
+
+	bool getEffectivenessMatrix(Configuration &configuration, bool force) override;
+
+	const char *name() const override { return "Multirotor"; }
+
+	/**
+	 * 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 enableYawControl(bool enable) { _geometry.yaw_disabled = !enable; }
+
+private:
+	void updateParams() override;
+
+	bool _updated{true};
+	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];
+	param_t _count_handle;
+
+	Geometry _geometry{};
+};

src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessRotorsTest.cpp

@@ -32,7 +32,7 @@
  ****************************************************************************/
 
 /**
- * @file ActuatorEffectivenessMultirotorTest.cpp
+ * @file ActuatorEffectivenessRotors.cpp
  *
  * Tests for Control Allocation Algorithms
  *
@@ -40,52 +40,55 @@
  */
 
 #include <gtest/gtest.h>
-#include <ActuatorEffectivenessMultirotor.hpp>
+#include "ActuatorEffectivenessRotors.hpp"
 
 using namespace matrix;
 
-TEST(ActuatorEffectivenessMultirotorTest, AllZeroCase)
+TEST(ActuatorEffectivenessRotors, AllZeroCase)
 {
 	// Quad wide geometry
-	ActuatorEffectivenessMultirotor::MultirotorGeometry geometry = {};
-	geometry.rotors[0].position_x = 1.0f;
-	geometry.rotors[0].position_y = 1.0f;
-	geometry.rotors[0].position_z = 0.0f;
-	geometry.rotors[0].axis_x = 0.0f;
-	geometry.rotors[0].axis_y = 0.0f;
-	geometry.rotors[0].axis_z = -1.0f;
+	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_x = -1.0f;
-	geometry.rotors[1].position_y = -1.0f;
-	geometry.rotors[1].position_z = 0.0f;
-	geometry.rotors[1].axis_x = 0.0f;
-	geometry.rotors[1].axis_y = 0.0f;
-	geometry.rotors[1].axis_z = -1.0f;
+	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_x = 1.0f;
-	geometry.rotors[2].position_y = -1.0f;
-	geometry.rotors[2].position_z = 0.0f;
-	geometry.rotors[2].axis_x = 0.0f;
-	geometry.rotors[2].axis_y = 0.0f;
-	geometry.rotors[2].axis_z = -1.0f;
+	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_x = -1.0f;
-	geometry.rotors[3].position_y = 1.0f;
-	geometry.rotors[3].position_z = 0.0f;
-	geometry.rotors[3].axis_x = 0.0f;
-	geometry.rotors[3].axis_y = 0.0f;
-	geometry.rotors[3].axis_z = -1.0f;
+	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;
 
-	matrix::Matrix<float, ActuatorEffectiveness::NUM_AXES, ActuatorEffectiveness::NUM_ACTUATORS> effectiveness =
-		ActuatorEffectivenessMultirotor::computeEffectivenessMatrix(geometry);
+	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},
@@ -95,8 +98,38 @@ TEST(ActuatorEffectivenessMultirotorTest, AllZeroCase)
 		{ 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}
 	};
-	matrix::Matrix<float, ActuatorEffectiveness::NUM_AXES, ActuatorEffectiveness::NUM_ACTUATORS> effectiveness_expected(
-		expected);
+	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/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessStandardVTOL.cpp

@@ -31,79 +31,76 @@
  *
  ****************************************************************************/
 
-/**
- * @file ActuatorEffectivenessStandardVTOL.hpp
- *
- * Actuator effectiveness for standard VTOL
- *
- * @author Julien Lecoeur <julien.lecoeur@gmail.com>
- */
-
 #include "ActuatorEffectivenessStandardVTOL.hpp"
+#include <ControlAllocation/ControlAllocation.hpp>
 
-ActuatorEffectivenessStandardVTOL::ActuatorEffectivenessStandardVTOL()
+using namespace matrix;
+
+ActuatorEffectivenessStandardVTOL::ActuatorEffectivenessStandardVTOL(ModuleParams *parent)
+	: ModuleParams(parent), _mc_rotors(this), _control_surfaces(this)
 {
-	setFlightPhase(FlightPhase::HOVER_FLIGHT);
 }
 
 bool
-ActuatorEffectivenessStandardVTOL::getEffectivenessMatrix(matrix::Matrix<float, NUM_AXES, NUM_ACTUATORS> &matrix,
-		bool force)
+ActuatorEffectivenessStandardVTOL::getEffectivenessMatrix(Configuration &configuration, bool force)
 {
-	if (!(_updated || force)) {
+	if (!force) {
 		return false;
 	}
 
-	switch (_flight_phase) {
-	case FlightPhase::HOVER_FLIGHT:  {
-			const float standard_vtol[NUM_AXES][NUM_ACTUATORS] = {
-				{-0.5f,  0.5f,  0.5f, -0.5f, 0.f, 0.0f, 0.0f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f},
-				{ 0.5f, -0.5f,  0.5f, -0.5f, 0.f, 0.f, 0.f, 0.0f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f},
-				{ 0.25f,  0.25f, -0.25f, -0.25f, 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.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.25f, -0.25f, -0.25f, -0.25f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f}
-			};
-			matrix = matrix::Matrix<float, NUM_AXES, NUM_ACTUATORS>(standard_vtol);
-			break;
-		}
+	// MC motors
+	configuration.selected_matrix = 0;
+	_mc_rotors.getEffectivenessMatrix(configuration, true);
+	_mc_motors_mask = (1u << _mc_rotors.geometry().num_rotors) - 1;
 
-	case FlightPhase::FORWARD_FLIGHT: {
-			const float standard_vtol[NUM_AXES][NUM_ACTUATORS] = {
-				{ 0.f, 0.f, 0.f, 0.f, 0.f, -0.5f, 0.5f, 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.5f, 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.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}
-			};
-			matrix = matrix::Matrix<float, NUM_AXES, NUM_ACTUATORS>(standard_vtol);
-			break;
-		}
+	// Pusher/Puller
+	configuration.selected_matrix = 1;
 
-	case FlightPhase::TRANSITION_HF_TO_FF:
-	case FlightPhase::TRANSITION_FF_TO_HF: {
-			const float standard_vtol[NUM_AXES][NUM_ACTUATORS] = {
-				{ -0.5f,  0.5f,  0.5f, -0.5f, 0.f, -0.5f, 0.5f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f},
-				{  0.5f, -0.5f,  0.5f, -0.5f, 0.f, 0.f, 0.f, 0.5f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f},
-				{  0.25f,  0.25f, -0.25f, -0.25f, 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.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.25f, -0.25f, -0.25f, -0.25f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f}
-			};
-			matrix = matrix::Matrix<float, NUM_AXES, NUM_ACTUATORS>(standard_vtol);
-			break;
-		}
+	for (int i = 0; i < _param_ca_stdvtol_n_p.get(); ++i) {
+		configuration.addActuator(ActuatorType::MOTORS, Vector3f{}, Vector3f{1.f, 0.f, 0.f});
 	}
 
-	_updated = false;
+	// Control Surfaces
+	configuration.selected_matrix = 1;
+	_first_control_surface_idx = configuration.num_actuators_matrix[configuration.selected_matrix];
+	_control_surfaces.getEffectivenessMatrix(configuration, true);
+
 	return true;
 }
 
-void
-ActuatorEffectivenessStandardVTOL::setFlightPhase(const FlightPhase &flight_phase)
+void ActuatorEffectivenessStandardVTOL::updateSetpoint(const matrix::Vector<float, NUM_AXES> &control_sp,
+		int matrix_index, ActuatorVector &actuator_sp)
 {
-	ActuatorEffectiveness::setFlightPhase(flight_phase);
-	_updated = true;
+	// apply flaps
+	if (matrix_index == 1) {
+		actuator_controls_s actuator_controls_1;
 
+		if (_actuator_controls_1_sub.copy(&actuator_controls_1)) {
+			float control_flaps = actuator_controls_1.control[actuator_controls_s::INDEX_FLAPS];
+			float airbrakes_control = actuator_controls_1.control[actuator_controls_s::INDEX_AIRBRAKES];
+			_control_surfaces.applyFlapsAndAirbrakes(control_flaps, airbrakes_control, _first_control_surface_idx, actuator_sp);
+		}
+	}
+}
+
+void ActuatorEffectivenessStandardVTOL::setFlightPhase(const FlightPhase &flight_phase)
+{
+	if (_flight_phase == flight_phase) {
+		return;
+	}
+
+	ActuatorEffectiveness::setFlightPhase(flight_phase);
+
+	// update stopped motors
+	switch (flight_phase) {
+	case FlightPhase::FORWARD_FLIGHT:
+		_stopped_motors = _mc_motors_mask;
+		break;
+
+	case FlightPhase::HOVER_FLIGHT:
+	case FlightPhase::TRANSITION_FF_TO_HF:
+	case FlightPhase::TRANSITION_HF_TO_FF:
+		_stopped_motors = 0;
+		break;
+	}
 }

src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessStandardVTOL.hpp

@@ -42,25 +42,50 @@
 #pragma once
 
 #include "ActuatorEffectiveness.hpp"
+#include "ActuatorEffectivenessRotors.hpp"
+#include "ActuatorEffectivenessControlSurfaces.hpp"
 
-class ActuatorEffectivenessStandardVTOL: public ActuatorEffectiveness
+#include <uORB/topics/actuator_controls.h>
+
+class ActuatorEffectivenessStandardVTOL : public ModuleParams, public ActuatorEffectiveness
 {
 public:
-	ActuatorEffectivenessStandardVTOL();
+	ActuatorEffectivenessStandardVTOL(ModuleParams *parent);
 	virtual ~ActuatorEffectivenessStandardVTOL() = default;
 
-	bool getEffectivenessMatrix(matrix::Matrix<float, NUM_AXES, NUM_ACTUATORS> &matrix, bool force) override;
-
-	/**
-	 * Set the current flight phase
-	 *
-	 * @param Flight phase
-	 */
-	void setFlightPhase(const FlightPhase &flight_phase) override;
-
-	int numActuators() const override { return 7; }
+	bool getEffectivenessMatrix(Configuration &configuration, bool force) override;
 
 	const char *name() const override { return "Standard VTOL"; }
-protected:
-	bool _updated{true};
+
+	int numMatrices() const override { return 2; }
+
+	void getDesiredAllocationMethod(AllocationMethod allocation_method_out[MAX_NUM_MATRICES]) const override
+	{
+		static_assert(MAX_NUM_MATRICES >= 2, "expecting at least 2 matrices");
+		allocation_method_out[0] = AllocationMethod::SEQUENTIAL_DESATURATION;
+		allocation_method_out[1] = AllocationMethod::PSEUDO_INVERSE;
+	}
+
+	void updateSetpoint(const matrix::Vector<float, NUM_AXES> &control_sp, int matrix_index,
+			    ActuatorVector &actuator_sp) override;
+
+	void setFlightPhase(const FlightPhase &flight_phase) override;
+
+	uint32_t getStoppedMotors() const override { return _stopped_motors; }
+
+private:
+	ActuatorEffectivenessRotors _mc_rotors;
+	ActuatorEffectivenessControlSurfaces _control_surfaces;
+
+	uint32_t _mc_motors_mask{}; ///< mc motors (stopped during forward flight)
+	uint32_t _stopped_motors{}; ///< currently stopped motors
+
+	int _first_control_surface_idx{0}; ///< applies to matrix 1
+
+	uORB::Subscription _actuator_controls_1_sub{ORB_ID(actuator_controls_0)};
+
+	DEFINE_PARAMETERS(
+		(ParamInt<px4::params::CA_STDVTOL_N_P>) _param_ca_stdvtol_n_p ///< number of pushers
+	)
+
 };

src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTiltrotorVTOL.cpp

@@ -41,76 +41,98 @@
 
 #include "ActuatorEffectivenessTiltrotorVTOL.hpp"
 
-ActuatorEffectivenessTiltrotorVTOL::ActuatorEffectivenessTiltrotorVTOL()
+using namespace matrix;
+
+ActuatorEffectivenessTiltrotorVTOL::ActuatorEffectivenessTiltrotorVTOL(ModuleParams *parent)
+	: ModuleParams(parent),
+	  _mc_rotors(this, ActuatorEffectivenessRotors::AxisConfiguration::Configurable, true),
+	  _control_surfaces(this), _tilts(this)
 {
 	setFlightPhase(FlightPhase::HOVER_FLIGHT);
 }
 bool
-ActuatorEffectivenessTiltrotorVTOL::getEffectivenessMatrix(matrix::Matrix<float, NUM_AXES, NUM_ACTUATORS> &matrix,
-		bool force)
+ActuatorEffectivenessTiltrotorVTOL::getEffectivenessMatrix(Configuration &configuration, bool force)
 {
-	if (!(_updated || force)) {
+	if (!_updated && !force) {
 		return false;
 	}
 
-	// Trim
-	float tilt = 0.0f;
+	// MC motors
+	configuration.selected_matrix = 0;
+	_mc_rotors.enableYawControl(!_tilts.hasYawControl());
+	_nontilted_motors = _mc_rotors.updateAxisFromTilts(_tilts, _last_tilt_control)
+			    << configuration.num_actuators[(int)ActuatorType::MOTORS];
+	_mc_rotors.getEffectivenessMatrix(configuration, true);
 
-	switch (_flight_phase) {
-	case FlightPhase::HOVER_FLIGHT:  {
-			tilt = 0.0f;
-			break;
-		}
-
-	case FlightPhase::FORWARD_FLIGHT: {
-			tilt = 1.5f;
-			break;
-		}
-
-	case FlightPhase::TRANSITION_FF_TO_HF:
-	case FlightPhase::TRANSITION_HF_TO_FF: {
-			tilt = 1.0f;
-			break;
-		}
-	}
-
-	// Trim: half throttle, tilted motors
-	_trim(0) = 0.5f;
-	_trim(1) = 0.5f;
-	_trim(2) = 0.5f;
-	_trim(3) = 0.5f;
-	_trim(4) = tilt;
-	_trim(5) = tilt;
-	_trim(6) = tilt;
-	_trim(7) = tilt;
-
-	// Effectiveness
-	const float tiltrotor_vtol[NUM_AXES][NUM_ACTUATORS] = {
-		{-0.5f * cosf(_trim(4)),  0.5f * cosf(_trim(5)),  0.5f * cosf(_trim(6)), -0.5f * cosf(_trim(7)), 0.5f * _trim(0) *sinf(_trim(4)), -0.5f * _trim(1) *sinf(_trim(5)), -0.5f * _trim(2) *sinf(_trim(6)), 0.5f * _trim(3) *sinf(_trim(7)), -0.5f, 0.5f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f},
-		{ 0.5f * cosf(_trim(4)), -0.5f * cosf(_trim(5)),  0.5f * cosf(_trim(6)), -0.5f * cosf(_trim(7)), -0.5f * _trim(0) *sinf(_trim(4)),  0.5f * _trim(1) *sinf(_trim(5)), -0.5f * _trim(2) *sinf(_trim(6)), 0.5f * _trim(3) *sinf(_trim(7)), 0.f, 0.f, 0.5f, 0.f, 0.f, 0.f, 0.f, 0.f},
-		{-0.5f * sinf(_trim(4)),  0.5f * sinf(_trim(5)),  0.5f * sinf(_trim(6)), -0.5f * sinf(_trim(7)), -0.5f * _trim(0) *cosf(_trim(4)), 0.5f * _trim(1) *cosf(_trim(5)), 0.5f * _trim(2) *cosf(_trim(6)), -0.5f * _trim(3) *cosf(_trim(7)), 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f},
-		{ 0.25f * sinf(_trim(4)), 0.25f * sinf(_trim(5)), 0.25f * sinf(_trim(6)), 0.25f * sinf(_trim(7)), 0.25f * _trim(0) *cosf(_trim(4)), 0.25f * _trim(1) *cosf(_trim(5)), 0.25f * _trim(2) *cosf(_trim(6)), 0.25f * _trim(3) *cosf(_trim(7)), 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.25f * cosf(_trim(4)), -0.25f * cosf(_trim(5)), -0.25f * cosf(_trim(6)), -0.25f * cosf(_trim(7)), 0.25f * _trim(0) *sinf(_trim(4)), 0.25f * _trim(1) *sinf(_trim(5)), 0.25f * _trim(2) *sinf(_trim(6)), 0.25f * _trim(3) *sinf(_trim(7)), 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f}
-	};
-	matrix = matrix::Matrix<float, NUM_AXES, NUM_ACTUATORS>(tiltrotor_vtol);
-
-	// Temporarily disable a few controls (WIP)
-	for (size_t j = 4; j < 8; j++) {
-		matrix(3, j) = 0.0f;
-		matrix(4, j) = 0.0f;
-		matrix(5, j) = 0.0f;
-	}
+	// Control Surfaces
+	configuration.selected_matrix = 1;
+	_first_control_surface_idx = configuration.num_actuators_matrix[configuration.selected_matrix];
+	_control_surfaces.getEffectivenessMatrix(configuration, true);
 
+	// Tilts
+	configuration.selected_matrix = 0;
+	_first_tilt_idx = configuration.num_actuators_matrix[configuration.selected_matrix];
+	_tilts.updateYawSign(_mc_rotors.geometry());
+	_tilts.getEffectivenessMatrix(configuration, true);
 
 	_updated = false;
 	return true;
 }
 
-void
-ActuatorEffectivenessTiltrotorVTOL::setFlightPhase(const FlightPhase &flight_phase)
+void ActuatorEffectivenessTiltrotorVTOL::updateSetpoint(const matrix::Vector<float, NUM_AXES> &control_sp,
+		int matrix_index, ActuatorVector &actuator_sp)
 {
+	// apply flaps
+	if (matrix_index == 1) {
+		actuator_controls_s actuator_controls_1;
+
+		if (_actuator_controls_1_sub.copy(&actuator_controls_1)) {
+			float control_flaps = -1.f; // For tilt-rotors INDEX_FLAPS is set as combined tilt. TODO: fix this
+			float airbrakes_control = actuator_controls_1.control[actuator_controls_s::INDEX_AIRBRAKES];
+			_control_surfaces.applyFlapsAndAirbrakes(control_flaps, airbrakes_control, _first_control_surface_idx, actuator_sp);
+		}
+	}
+
+	// apply tilt
+	if (matrix_index == 0) {
+		actuator_controls_s actuator_controls_1;
+
+		if (_actuator_controls_1_sub.copy(&actuator_controls_1)) {
+			float control_tilt = actuator_controls_1.control[4] * 2.f - 1.f;
+
+			if (fabsf(control_tilt - _last_tilt_control) > 0.01f && PX4_ISFINITE(_last_tilt_control)) {
+				_updated = true;
+			}
+
+			for (int i = 0; i < _tilts.count(); ++i) {
+				if (_tilts.config(i).tilt_direction == ActuatorEffectivenessTilts::TiltDirection::TowardsFront) {
+					actuator_sp(i + _first_tilt_idx) += control_tilt;
+				}
+			}
+
+			_last_tilt_control = control_tilt;
+		}
+	}
+}
+
+void ActuatorEffectivenessTiltrotorVTOL::setFlightPhase(const FlightPhase &flight_phase)
+{
+	if (_flight_phase == flight_phase) {
+		return;
+	}
+
 	ActuatorEffectiveness::setFlightPhase(flight_phase);
 
-	_updated = true;
+	// update stopped motors
+	switch (flight_phase) {
+	case FlightPhase::FORWARD_FLIGHT:
+		_stopped_motors = _nontilted_motors;
+		break;
+
+	case FlightPhase::HOVER_FLIGHT:
+	case FlightPhase::TRANSITION_FF_TO_HF:
+	case FlightPhase::TRANSITION_HF_TO_FF:
+		_stopped_motors = 0;
+		break;
+	}
 }

src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTiltrotorVTOL.hpp

@@ -42,25 +42,51 @@
 #pragma once
 
 #include "ActuatorEffectiveness.hpp"
+#include "ActuatorEffectivenessRotors.hpp"
+#include "ActuatorEffectivenessControlSurfaces.hpp"
+#include "ActuatorEffectivenessTilts.hpp"
 
-class ActuatorEffectivenessTiltrotorVTOL: public ActuatorEffectiveness
+#include <uORB/topics/actuator_controls.h>
+#include <uORB/Subscription.hpp>
+
+class ActuatorEffectivenessTiltrotorVTOL : public ModuleParams, public ActuatorEffectiveness
 {
 public:
-	ActuatorEffectivenessTiltrotorVTOL();
+	ActuatorEffectivenessTiltrotorVTOL(ModuleParams *parent);
 	virtual ~ActuatorEffectivenessTiltrotorVTOL() = default;
 
-	bool getEffectivenessMatrix(matrix::Matrix<float, NUM_AXES, NUM_ACTUATORS> &matrix, bool force) override;
+	bool getEffectivenessMatrix(Configuration &configuration, bool force) override;
+
+	int numMatrices() const override { return 2; }
+
+	void getDesiredAllocationMethod(AllocationMethod allocation_method_out[MAX_NUM_MATRICES]) const override
+	{
+		static_assert(MAX_NUM_MATRICES >= 2, "expecting at least 2 matrices");
+		allocation_method_out[0] = AllocationMethod::SEQUENTIAL_DESATURATION;
+		allocation_method_out[1] = AllocationMethod::PSEUDO_INVERSE;
+	}
 
-	/**
-	 * Set the current flight phase
-	 *
-	 * @param Flight phase
-	 */
 	void setFlightPhase(const FlightPhase &flight_phase) override;
 
-	int numActuators() const override { return 10; }
+	void updateSetpoint(const matrix::Vector<float, NUM_AXES> &control_sp, int matrix_index,
+			    ActuatorVector &actuator_sp) override;
 
 	const char *name() const override { return "VTOL Tiltrotor"; }
+
+	uint32_t getStoppedMotors() const override { return _stopped_motors; }
 protected:
 	bool _updated{true};
+	ActuatorEffectivenessRotors _mc_rotors;
+	ActuatorEffectivenessControlSurfaces _control_surfaces;
+	ActuatorEffectivenessTilts _tilts;
+
+	uint32_t _nontilted_motors{}; ///< motors that are not tiltable
+	uint32_t _stopped_motors{}; ///< currently stopped motors
+
+	int _first_control_surface_idx{0}; ///< applies to matrix 1
+	int _first_tilt_idx{0}; ///< applies to matrix 0
+
+	float _last_tilt_control{NAN};
+
+	uORB::Subscription _actuator_controls_1_sub{ORB_ID(actuator_controls_1)};
 };

src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTilts.cpp

@@ -0,0 +1,142 @@
+/****************************************************************************
+ *
+ *   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 "ActuatorEffectivenessTilts.hpp"
+
+#include <px4_platform_common/log.h>
+#include <lib/mathlib/mathlib.h>
+
+using namespace matrix;
+
+ActuatorEffectivenessTilts::ActuatorEffectivenessTilts(ModuleParams *parent)
+	: ModuleParams(parent)
+{
+	for (int i = 0; i < MAX_COUNT; ++i) {
+		char buffer[17];
+		snprintf(buffer, sizeof(buffer), "CA_SV_TL%u_CT", i);
+		_param_handles[i].control = param_find(buffer);
+		snprintf(buffer, sizeof(buffer), "CA_SV_TL%u_MINA", i);
+		_param_handles[i].min_angle = param_find(buffer);
+		snprintf(buffer, sizeof(buffer), "CA_SV_TL%u_MAXA", i);
+		_param_handles[i].max_angle = param_find(buffer);
+		snprintf(buffer, sizeof(buffer), "CA_SV_TL%u_TD", i);
+		_param_handles[i].tilt_direction = param_find(buffer);
+	}
+
+	_count_handle = param_find("CA_SV_TL_COUNT");
+	updateParams();
+}
+
+void ActuatorEffectivenessTilts::updateParams()
+{
+	ModuleParams::updateParams();
+
+	int32_t count = 0;
+
+	if (param_get(_count_handle, &count) != 0) {
+		PX4_ERR("param_get failed");
+		return;
+	}
+
+	_count = count;
+
+	for (int i = 0; i < count; i++) {
+		param_get(_param_handles[i].control, (int32_t *)&_params[i].control);
+		param_get(_param_handles[i].tilt_direction, (int32_t *)&_params[i].tilt_direction);
+		param_get(_param_handles[i].min_angle, &_params[i].min_angle);
+		param_get(_param_handles[i].max_angle, &_params[i].max_angle);
+
+		_params[i].min_angle = math::radians(_params[i].min_angle);
+		_params[i].max_angle = math::radians(_params[i].max_angle);
+
+		_yaw_torque[i] = 0.f;
+	}
+}
+
+bool ActuatorEffectivenessTilts::getEffectivenessMatrix(Configuration &configuration, bool force)
+{
+	if (!force) {
+		return false;
+	}
+
+	for (int i = 0; i < _count; i++) {
+		Vector3f torque{};
+
+		if (_params[i].control == Control::Yaw) {
+			torque(2) = _yaw_torque[i];
+		}
+
+		configuration.addActuator(ActuatorType::SERVOS, torque, Vector3f{});
+	}
+
+	return true;
+}
+
+void ActuatorEffectivenessTilts::updateYawSign(const ActuatorEffectivenessRotors::Geometry &geometry)
+{
+	for (int i = 0; i < geometry.num_rotors; ++i) {
+		int tilt_index = geometry.rotors[i].tilt_index;
+
+		if (tilt_index == -1 || tilt_index >= _count) {
+			continue;
+		}
+
+		if (_params[tilt_index].control != Control::Yaw) {
+			continue;
+		}
+
+		// Find the yaw torque sign by checking the motor position and tilt direction.
+		// Rotate position by -tilt_direction around z, then check the sign of y pos
+		float tilt_direction = math::radians((float)_params[tilt_index].tilt_direction);
+		Vector3f rotated_pos = Dcmf{Eulerf{0.f, 0.f, -tilt_direction}} * geometry.rotors[i].position;
+
+		if (rotated_pos(1) < -0.01f) { // add minimal margin
+			_yaw_torque[tilt_index] = 1.f;
+
+		} else if (rotated_pos(1) > 0.01f) {
+			_yaw_torque[tilt_index] = -1.f;
+		}
+	}
+}
+
+bool ActuatorEffectivenessTilts::hasYawControl() const
+{
+	for (int i = 0; i < _count; i++) {
+		if (_params[i].control == Control::Yaw) {
+			return true;
+		}
+	}
+
+	return false;
+}

src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTilts.hpp

@@ -0,0 +1,97 @@
+/****************************************************************************
+ *
+ *   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.
+ *
+ ****************************************************************************/
+
+#pragma once
+
+#include "ActuatorEffectiveness.hpp"
+#include "ActuatorEffectivenessRotors.hpp"
+
+#include <px4_platform_common/module_params.h>
+
+class ActuatorEffectivenessTilts : public ModuleParams, public ActuatorEffectiveness
+{
+public:
+
+	static constexpr int MAX_COUNT = 4;
+
+	enum class Control : int32_t {
+		// This matches with the parameter
+		None = 0,
+		Yaw = 1,
+	};
+	enum class TiltDirection : int32_t {
+		// This matches with the parameter
+		TowardsFront = 0,
+		TowardsRight = 90,
+	};
+
+	struct Params {
+		Control control;
+		float min_angle;
+		float max_angle;
+		TiltDirection tilt_direction;
+	};
+
+	ActuatorEffectivenessTilts(ModuleParams *parent);
+	virtual ~ActuatorEffectivenessTilts() = default;
+
+	bool getEffectivenessMatrix(Configuration &configuration, bool force) override;
+
+	const char *name() const override { return "Tilts"; }
+
+	int count() const { return _count; }
+
+	const Params &config(int idx) const { return _params[idx]; }
+
+	void updateYawSign(const ActuatorEffectivenessRotors::Geometry &geometry);
+
+	bool hasYawControl() const;
+
+private:
+	void updateParams() override;
+
+	struct ParamHandles {
+		param_t control;
+		param_t min_angle;
+		param_t max_angle;
+		param_t tilt_direction;
+	};
+
+	ParamHandles _param_handles[MAX_COUNT];
+	param_t _count_handle;
+
+	Params _params[MAX_COUNT] {};
+	int _count{0};
+
+	float _yaw_torque[MAX_COUNT] {};
+};

src/modules/control_allocator/ActuatorEffectiveness/CMakeLists.txt

@@ -32,9 +32,14 @@
 ############################################################################
 
 px4_add_library(ActuatorEffectiveness
+	ActuatorEffectiveness.cpp
 	ActuatorEffectiveness.hpp
-	ActuatorEffectivenessMultirotor.cpp
-	ActuatorEffectivenessMultirotor.hpp
+	ActuatorEffectivenessControlSurfaces.cpp
+	ActuatorEffectivenessControlSurfaces.hpp
+	ActuatorEffectivenessTilts.cpp
+	ActuatorEffectivenessTilts.hpp
+	ActuatorEffectivenessRotors.cpp
+	ActuatorEffectivenessRotors.hpp
 	ActuatorEffectivenessStandardVTOL.cpp
 	ActuatorEffectivenessStandardVTOL.hpp
 	ActuatorEffectivenessTiltrotorVTOL.cpp
@@ -46,7 +51,6 @@ target_include_directories(ActuatorEffectiveness PUBLIC ${CMAKE_CURRENT_SOURCE_D
 target_link_libraries(ActuatorEffectiveness
 	PRIVATE
 		mathlib
-		ControlAllocation
 )
 
-# px4_add_unit_gtest(SRC ActuatorEffectivenessMultirotorTest.cpp LINKLIBS ActuatorEffectiveness)
+px4_add_functional_gtest(SRC ActuatorEffectivenessRotorsTest.cpp LINKLIBS ActuatorEffectiveness)