feat: add effectiveness of thrsuters and rotors to lib

src/lib/actuator_effectiveness/ActuatorEffectiveness.hpp

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

src/lib/actuator_effectiveness/ActuatorEffectivenessRotors.cpp

@@ -0,0 +1,312 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2020 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file ActuatorEffectivenessRotors.hpp
+ *
+ * Actuator effectiveness computed from rotors position and orientation
+ *
+ * @author Julien Lecoeur <julien.lecoeur@gmail.com>
+ */
+
+#include "ActuatorEffectivenessRotors.hpp"
+
+// #include "ActuatorEffectivenessTilts.hpp"
+
+using namespace matrix;
+
+ActuatorEffectivenessRotors::ActuatorEffectivenessRotors(ModuleParams *parent, AxisConfiguration axis_config,
+		bool tilt_support)
+	: ModuleParams(parent), _axis_config(axis_config), _tilt_support(tilt_support)
+{
+	for (int i = 0; i < NUM_ROTORS_MAX; ++i) {
+		char buffer[17];
+		snprintf(buffer, sizeof(buffer), "CA_ROTOR%u_PX", i);
+		_param_handles[i].position_x = param_find(buffer);
+		snprintf(buffer, sizeof(buffer), "CA_ROTOR%u_PY", i);
+		_param_handles[i].position_y = param_find(buffer);
+		snprintf(buffer, sizeof(buffer), "CA_ROTOR%u_PZ", i);
+		_param_handles[i].position_z = param_find(buffer);
+
+		if (_axis_config == AxisConfiguration::Configurable) {
+			snprintf(buffer, sizeof(buffer), "CA_ROTOR%u_AX", i);
+			_param_handles[i].axis_x = param_find(buffer);
+			snprintf(buffer, sizeof(buffer), "CA_ROTOR%u_AY", i);
+			_param_handles[i].axis_y = param_find(buffer);
+			snprintf(buffer, sizeof(buffer), "CA_ROTOR%u_AZ", i);
+			_param_handles[i].axis_z = param_find(buffer);
+		}
+
+		snprintf(buffer, sizeof(buffer), "CA_ROTOR%u_CT", i);
+		_param_handles[i].thrust_coef = param_find(buffer);
+
+		snprintf(buffer, sizeof(buffer), "CA_ROTOR%u_KM", i);
+		_param_handles[i].moment_ratio = param_find(buffer);
+
+		if (_tilt_support) {
+			snprintf(buffer, sizeof(buffer), "CA_ROTOR%u_TILT", i);
+			_param_handles[i].tilt_index = param_find(buffer);
+		}
+	}
+
+	updateParams();
+}
+
+void ActuatorEffectivenessRotors::updateParams()
+{
+	ModuleParams::updateParams();
+
+	_geometry.num_rotors = math::min(NUM_ROTORS_MAX, static_cast<int>(_param_ca_rotor_count.get()));
+
+	for (int i = 0; i < _geometry.num_rotors; ++i) {
+		Vector3f &position = _geometry.rotors[i].position;
+		param_get(_param_handles[i].position_x, &position(0));
+		param_get(_param_handles[i].position_y, &position(1));
+		param_get(_param_handles[i].position_z, &position(2));
+
+		Vector3f &axis = _geometry.rotors[i].axis;
+
+		switch (_axis_config) {
+		case AxisConfiguration::Configurable:
+			param_get(_param_handles[i].axis_x, &axis(0));
+			param_get(_param_handles[i].axis_y, &axis(1));
+			param_get(_param_handles[i].axis_z, &axis(2));
+			break;
+
+		case AxisConfiguration::FixedForward:
+			axis = Vector3f(1.f, 0.f, 0.f);
+			break;
+
+		case AxisConfiguration::FixedUpwards:
+			axis = Vector3f(0.f, 0.f, -1.f);
+			break;
+		}
+
+		param_get(_param_handles[i].thrust_coef, &_geometry.rotors[i].thrust_coef);
+		param_get(_param_handles[i].moment_ratio, &_geometry.rotors[i].moment_ratio);
+
+		if (_tilt_support) {
+			int32_t tilt_param{0};
+			param_get(_param_handles[i].tilt_index, &tilt_param);
+			_geometry.rotors[i].tilt_index = tilt_param - 1;
+
+		} else {
+			_geometry.rotors[i].tilt_index = -1;
+		}
+	}
+}
+
+bool
+ActuatorEffectivenessRotors::addActuators(Configuration &configuration)
+{
+	if (configuration.num_actuators[(int)ActuatorType::SERVOS] > 0) {
+		PX4_ERR("Wrong actuator ordering: servos need to be after motors");
+		return false;
+	}
+
+	int num_actuators = computeEffectivenessMatrix(_geometry,
+			    configuration.effectiveness_matrices[configuration.selected_matrix],
+			    configuration.num_actuators_matrix[configuration.selected_matrix]);
+	configuration.actuatorsAdded(ActuatorType::MOTORS, num_actuators);
+	return true;
+}
+
+int
+ActuatorEffectivenessRotors::computeEffectivenessMatrix(const Geometry &geometry,
+		EffectivenessMatrix &effectiveness, int actuator_start_index)
+{
+	int num_actuators = 0;
+
+	for (int i = 0; i < geometry.num_rotors; i++) {
+
+		if (i + actuator_start_index >= NUM_ACTUATORS) {
+			break;
+		}
+
+		++num_actuators;
+
+		// Get rotor axis
+		Vector3f axis = geometry.rotors[i].axis;
+
+		// Normalize axis
+		float axis_norm = axis.norm();
+
+		if (axis_norm > FLT_EPSILON) {
+			axis /= axis_norm;
+
+		} else {
+			// Bad axis definition, ignore this rotor
+			continue;
+		}
+
+		// Get rotor position
+		const Vector3f &position = geometry.rotors[i].position;
+
+		// Get coefficients
+		float ct = geometry.rotors[i].thrust_coef;
+		float km = geometry.rotors[i].moment_ratio;
+
+		if (geometry.propeller_torque_disabled) {
+			km = 0.f;
+		}
+
+		if (geometry.propeller_torque_disabled_non_upwards) {
+			bool upwards = fabsf(axis(0)) < 0.1f && fabsf(axis(1)) < 0.1f && axis(2) < -0.5f;
+
+			if (!upwards) {
+				km = 0.f;
+			}
+		}
+
+		if (fabsf(ct) < FLT_EPSILON) {
+			continue;
+		}
+
+		// Compute thrust generated by this rotor
+		matrix::Vector3f thrust = ct * axis;
+
+		// Compute moment generated by this rotor
+		matrix::Vector3f moment = ct * position.cross(axis) - ct * km * axis;
+
+		// Fill corresponding items in effectiveness matrix
+		for (size_t j = 0; j < 3; j++) {
+			effectiveness(j, i + actuator_start_index) = moment(j);
+			effectiveness(j + 3, i + actuator_start_index) = thrust(j);
+		}
+
+		if (geometry.yaw_by_differential_thrust_disabled) {
+			// set yaw effectiveness to 0 if yaw is controlled by other means (e.g. tilts)
+			effectiveness(2, i + actuator_start_index) = 0.f;
+		}
+
+		if (geometry.three_dimensional_thrust_disabled) {
+			// Special case tiltrotor: instead of passing a 3D thrust vector (that would mostly have a x-component in FW, and z in MC),
+			// pass the vector magnitude as z-component, plus the collective tilt. Passing 3D thrust plus tilt is not feasible as they
+			// can't be allocated independently, and with the current controller it's not possible to have collective tilt calculated
+			// by the allocator directly.
+
+			effectiveness(0 + 3, i + actuator_start_index) = 0.f;
+			effectiveness(1 + 3, i + actuator_start_index) = 0.f;
+			effectiveness(2 + 3, i + actuator_start_index) = -ct;
+		}
+	}
+
+	return num_actuators;
+}
+
+// TODO(@Jaeyoung-Lim): This should not be in the Rotors effectiveness, so we need to move it elsewhere
+/*
+uint32_t ActuatorEffectivenessRotors::updateAxisFromTilts(const ActuatorEffectivenessTilts &tilts,
+		float collective_tilt_control)
+{
+	if (!PX4_ISFINITE(collective_tilt_control)) {
+		collective_tilt_control = -1.f;
+	}
+
+	uint32_t nontilted_motors = 0;
+
+	for (int i = 0; i < _geometry.num_rotors; ++i) {
+		int tilt_index = _geometry.rotors[i].tilt_index;
+
+		if (tilt_index == -1 || tilt_index >= tilts.count()) {
+			nontilted_motors |= 1u << i;
+			continue;
+		}
+
+		const ActuatorEffectivenessTilts::Params &tilt = tilts.config(tilt_index);
+		const float tilt_angle = math::lerp(tilt.min_angle, tilt.max_angle, (collective_tilt_control + 1.f) / 2.f);
+		const float tilt_direction = math::radians((float)tilt.tilt_direction);
+		_geometry.rotors[i].axis = tiltedAxis(tilt_angle, tilt_direction);
+	}
+
+	return nontilted_motors;
+}
+*/
+
+Vector3f ActuatorEffectivenessRotors::tiltedAxis(float tilt_angle, float tilt_direction)
+{
+	Vector3f axis{0.f, 0.f, -1.f};
+	return Dcmf{Eulerf{0.f, -tilt_angle, tilt_direction}} * axis;
+}
+
+uint32_t ActuatorEffectivenessRotors::getMotors() const
+{
+	uint32_t motors = 0;
+
+	for (int i = 0; i < _geometry.num_rotors; ++i) {
+		motors |= 1u << i;
+	}
+
+	return motors;
+}
+
+uint32_t ActuatorEffectivenessRotors::getUpwardsMotors() const
+{
+	uint32_t upwards_motors = 0;
+
+	for (int i = 0; i < _geometry.num_rotors; ++i) {
+		const Vector3f &axis = _geometry.rotors[i].axis;
+
+		if (fabsf(axis(0)) < 0.1f && fabsf(axis(1)) < 0.1f && axis(2) < -0.5f) {
+			upwards_motors |= 1u << i;
+		}
+	}
+
+	return upwards_motors;
+}
+
+uint32_t ActuatorEffectivenessRotors::getForwardsMotors() const
+{
+	uint32_t forward_motors = 0;
+
+	for (int i = 0; i < _geometry.num_rotors; ++i) {
+		const Vector3f &axis = _geometry.rotors[i].axis;
+
+		if (axis(0) > 0.5f && fabsf(axis(1)) < 0.1f && fabsf(axis(2)) < 0.1f) {
+			forward_motors |= 1u << i;
+		}
+	}
+
+	return forward_motors;
+}
+
+bool
+ActuatorEffectivenessRotors::getEffectivenessMatrix(Configuration &configuration,
+		EffectivenessUpdateReason external_update)
+{
+	if (external_update == EffectivenessUpdateReason::NO_EXTERNAL_UPDATE) {
+		return false;
+	}
+
+	return addActuators(configuration);
+}

src/lib/actuator_effectiveness/ActuatorEffectivenessRotors.hpp

@@ -0,0 +1,156 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2020 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file ActuatorEffectivenessRotors.hpp
+ *
+ * Actuator effectiveness computed from rotors position and orientation
+ *
+ * @author Julien Lecoeur <julien.lecoeur@gmail.com>
+ */
+
+#pragma once
+
+#include "ActuatorEffectiveness.hpp"
+
+#include <px4_platform_common/module_params.h>
+#include <uORB/Subscription.hpp>
+#include <uORB/SubscriptionInterval.hpp>
+
+class ActuatorEffectivenessTilts;
+
+using namespace time_literals;
+
+class ActuatorEffectivenessRotors : public ModuleParams, public ActuatorEffectiveness
+{
+public:
+	enum class AxisConfiguration {
+		Configurable, ///< axis can be configured
+		FixedForward, ///< axis is fixed, pointing forwards (positive X)
+		FixedUpwards, ///< axis is fixed, pointing upwards (negative Z)
+	};
+
+	static constexpr int NUM_ROTORS_MAX = 12;
+
+	struct RotorGeometry {
+		matrix::Vector3f position;
+		matrix::Vector3f axis;
+		float thrust_coef;
+		float moment_ratio;
+		int tilt_index;
+	};
+
+	struct Geometry {
+		RotorGeometry rotors[NUM_ROTORS_MAX];
+		int num_rotors{0};
+		bool propeller_torque_disabled{false};
+		bool yaw_by_differential_thrust_disabled{false};
+		bool propeller_torque_disabled_non_upwards{false}; ///< keeps propeller torque enabled for upward facing motors
+		bool three_dimensional_thrust_disabled{false}; ///< for handling of tiltrotor VTOL, as they pass in 1D thrust and collective tilt
+	};
+
+	ActuatorEffectivenessRotors(ModuleParams *parent, AxisConfiguration axis_config = AxisConfiguration::Configurable,
+				    bool tilt_support = false);
+	virtual ~ActuatorEffectivenessRotors() = default;
+
+	bool getEffectivenessMatrix(Configuration &configuration, EffectivenessUpdateReason external_update) override;
+
+	void getDesiredAllocationMethod(AllocationMethod allocation_method_out[MAX_NUM_MATRICES]) const override
+	{
+		allocation_method_out[0] = AllocationMethod::SEQUENTIAL_DESATURATION;
+	}
+
+	void getNormalizeRPY(bool normalize[MAX_NUM_MATRICES]) const override
+	{
+		normalize[0] = true;
+	}
+
+	static int computeEffectivenessMatrix(const Geometry &geometry,
+					      EffectivenessMatrix &effectiveness, int actuator_start_index = 0);
+
+	bool addActuators(Configuration &configuration);
+
+	const char *name() const override { return "Rotors"; }
+
+	/**
+	 * Sets the motor axis from tilt configurations and current tilt control.
+	 * @param tilts configured tilt servos
+	 * @param tilt_control current tilt control in [-1, 1] (can be NAN)
+	 * @return the motors as bitset which are not tiltable
+	 */
+	uint32_t updateAxisFromTilts(const ActuatorEffectivenessTilts &tilts, float tilt_control);
+
+	const Geometry &geometry() const { return _geometry; }
+
+	/**
+	 * Get the tilted axis {0, 0, -1} rotated by -tilt_angle around y, then
+	 * rotated by tilt_direction around z.
+	 */
+	static matrix::Vector3f tiltedAxis(float tilt_angle, float tilt_direction);
+
+	void enablePropellerTorque(bool enable) { _geometry.propeller_torque_disabled = !enable; }
+
+	void enableYawByDifferentialThrust(bool enable) { _geometry.yaw_by_differential_thrust_disabled = !enable; }
+
+	void enablePropellerTorqueNonUpwards(bool enable) { _geometry.propeller_torque_disabled_non_upwards = !enable; }
+
+	void enableThreeDimensionalThrust(bool enable) { _geometry.three_dimensional_thrust_disabled = !enable; }
+
+	uint32_t getMotors() const;
+	uint32_t getUpwardsMotors() const;
+	uint32_t getForwardsMotors() const;
+
+private:
+	void updateParams() override;
+	const AxisConfiguration _axis_config;
+	const bool _tilt_support; ///< if true, tilt servo assignment params are loaded
+
+	struct ParamHandles {
+		param_t position_x;
+		param_t position_y;
+		param_t position_z;
+		param_t axis_x;
+		param_t axis_y;
+		param_t axis_z;
+		param_t thrust_coef;
+		param_t moment_ratio;
+		param_t tilt_index;
+	};
+	ParamHandles _param_handles[NUM_ROTORS_MAX];
+
+	Geometry _geometry{};
+
+	DEFINE_PARAMETERS(
+		(ParamInt<px4::params::CA_ROTOR_COUNT>) _param_ca_rotor_count
+	)
+};

src/lib/actuator_effectiveness/ActuatorEffectivenessRotorsTest.cpp

@@ -0,0 +1,135 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2020 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file ActuatorEffectivenessRotors.cpp
+ *
+ * Tests for Control Allocation Algorithms
+ *
+ * @author Julien Lecoeur <julien.lecoeur@gmail.com>
+ */
+
+#include <gtest/gtest.h>
+#include "ActuatorEffectivenessRotors.hpp"
+
+using namespace matrix;
+
+TEST(ActuatorEffectivenessRotors, AllZeroCase)
+{
+	// Quad wide geometry
+	ActuatorEffectivenessRotors::Geometry geometry = {};
+	geometry.rotors[0].position(0) = 1.0f;
+	geometry.rotors[0].position(1) = 1.0f;
+	geometry.rotors[0].position(2) = 0.0f;
+	geometry.rotors[0].axis(0) = 0.0f;
+	geometry.rotors[0].axis(1) = 0.0f;
+	geometry.rotors[0].axis(2) = -1.0f;
+	geometry.rotors[0].thrust_coef = 1.0f;
+	geometry.rotors[0].moment_ratio = 0.05f;
+
+	geometry.rotors[1].position(0) = -1.0f;
+	geometry.rotors[1].position(1) = -1.0f;
+	geometry.rotors[1].position(2) = 0.0f;
+	geometry.rotors[1].axis(0) = 0.0f;
+	geometry.rotors[1].axis(1) = 0.0f;
+	geometry.rotors[1].axis(2) = -1.0f;
+	geometry.rotors[1].thrust_coef = 1.0f;
+	geometry.rotors[1].moment_ratio = 0.05f;
+
+	geometry.rotors[2].position(0) = 1.0f;
+	geometry.rotors[2].position(1) = -1.0f;
+	geometry.rotors[2].position(2) = 0.0f;
+	geometry.rotors[2].axis(0) = 0.0f;
+	geometry.rotors[2].axis(1) = 0.0f;
+	geometry.rotors[2].axis(2) = -1.0f;
+	geometry.rotors[2].thrust_coef = 1.0f;
+	geometry.rotors[2].moment_ratio = -0.05f;
+
+	geometry.rotors[3].position(0) = -1.0f;
+	geometry.rotors[3].position(1) = 1.0f;
+	geometry.rotors[3].position(2) = 0.0f;
+	geometry.rotors[3].axis(0) = 0.0f;
+	geometry.rotors[3].axis(1) = 0.0f;
+	geometry.rotors[3].axis(2) = -1.0f;
+	geometry.rotors[3].thrust_coef = 1.0f;
+	geometry.rotors[3].moment_ratio = -0.05f;
+
+	geometry.num_rotors = 4;
+
+	ActuatorEffectiveness::EffectivenessMatrix effectiveness;
+	effectiveness.setZero();
+	ActuatorEffectivenessRotors::computeEffectivenessMatrix(geometry, effectiveness);
+
+	const float expected[ActuatorEffectiveness::NUM_AXES][ActuatorEffectiveness::NUM_ACTUATORS] = {
+		{-1.0f,   1.0f,   1.0f,  -1.0f,  0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f},
+		{ 1.0f,  -1.0f,   1.0f,  -1.0f,  0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f},
+		{ 0.05f,  0.05f, -0.05f, -0.05f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f},
+		{ 0.f,    0.f,    0.f,    0.f,   0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f},
+		{ 0.f,    0.f,    0.f,    0.f,   0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f},
+		{-1.0f,  -1.0f,  -1.0f,  -1.0f,  0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f}
+	};
+	ActuatorEffectiveness::EffectivenessMatrix effectiveness_expected(expected);
+
+	EXPECT_EQ(effectiveness, effectiveness_expected);
+}
+
+TEST(ActuatorEffectivenessRotors, Tilt)
+{
+	Vector3f axis_expected{0.f, 0.f, -1.f};
+	Vector3f axis = ActuatorEffectivenessRotors::tiltedAxis(0.f, 0.f);
+	EXPECT_EQ(axis, axis_expected);
+
+	axis_expected = Vector3f{1.f, 0.f, 0.f};
+	axis = ActuatorEffectivenessRotors::tiltedAxis(M_PI_F / 2.f, 0.f);
+	EXPECT_EQ(axis, axis_expected);
+
+	axis_expected = Vector3f{1.f / sqrtf(2.f), 0.f, -1.f / sqrtf(2.f)};
+	axis = ActuatorEffectivenessRotors::tiltedAxis(M_PI_F / 2.f / 2.f, 0.f);
+	EXPECT_EQ(axis, axis_expected);
+
+	axis_expected = Vector3f{-1.f, 0.f, 0.f};
+	axis = ActuatorEffectivenessRotors::tiltedAxis(-M_PI_F / 2.f, 0.f);
+	EXPECT_EQ(axis, axis_expected);
+
+	axis_expected = Vector3f{0.f, 0.f, -1.f};
+	axis = ActuatorEffectivenessRotors::tiltedAxis(0.f, M_PI_F / 2.f);
+	EXPECT_EQ(axis, axis_expected);
+
+	axis_expected = Vector3f{0.f, 1.f, 0.f};
+	axis = ActuatorEffectivenessRotors::tiltedAxis(M_PI_F / 2.f, M_PI_F / 2.f);
+	EXPECT_EQ(axis, axis_expected);
+
+	axis_expected = Vector3f{0.f, -1.f, 0.f};
+	axis = ActuatorEffectivenessRotors::tiltedAxis(-M_PI_F / 2.f, M_PI_F / 2.f);
+	EXPECT_EQ(axis, axis_expected);
+}

src/lib/actuator_effectiveness/ActuatorEffectivenessThrusters.cpp

@@ -0,0 +1,205 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2020 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file ActuatorEffectivenessThrusters.hpp
+ *
+ * Actuator effectiveness computed from thrusters position and orientation
+ *
+ * @author Pedro Roque, <padr@kth.se>
+ */
+
+#include "ActuatorEffectivenessThrusters.hpp"
+
+using namespace matrix;
+
+ActuatorEffectivenessThrusters::ActuatorEffectivenessThrusters(ModuleParams *parent, AxisConfiguration axis_config,
+		bool tilt_support)
+	: ModuleParams(parent), _axis_config(axis_config), _tilt_support(tilt_support)
+{
+
+	for (int i = 0; i < NUM_THRUSTERS_MAX; ++i) {
+		char buffer[17];
+		snprintf(buffer, sizeof(buffer), "CA_THRUSTER%u_PX", i);
+		_param_handles[i].position_x = param_find(buffer);
+		snprintf(buffer, sizeof(buffer), "CA_THRUSTER%u_PY", i);
+		_param_handles[i].position_y = param_find(buffer);
+		snprintf(buffer, sizeof(buffer), "CA_THRUSTER%u_PZ", i);
+		_param_handles[i].position_z = param_find(buffer);
+
+		if (_axis_config == AxisConfiguration::Configurable) {
+			snprintf(buffer, sizeof(buffer), "CA_THRUSTER%u_AX", i);
+			_param_handles[i].axis_x = param_find(buffer);
+			snprintf(buffer, sizeof(buffer), "CA_THRUSTER%u_AY", i);
+			_param_handles[i].axis_y = param_find(buffer);
+			snprintf(buffer, sizeof(buffer), "CA_THRUSTER%u_AZ", i);
+			_param_handles[i].axis_z = param_find(buffer);
+		}
+
+		snprintf(buffer, sizeof(buffer), "CA_THRUSTER%u_CT", i);
+		_param_handles[i].thrust_coef = param_find(buffer);
+
+		if (_tilt_support) {
+			PX4_ERR("Tilt support not implemented");
+		}
+	}
+
+	_count_handle = param_find("CA_THRUSTER_CNT");
+
+	updateParams();
+}
+
+void ActuatorEffectivenessThrusters::updateParams()
+{
+	ModuleParams::updateParams();
+
+	int32_t count = 0;
+
+	if (param_get(_count_handle, &count) != 0) {
+		PX4_ERR("param_get failed");
+		return;
+	}
+
+	_geometry.num_thrusters = math::min(NUM_THRUSTERS_MAX, (int)count);
+
+	for (int i = 0; i < _geometry.num_thrusters; ++i) {
+		Vector3f &position = _geometry.thrusters[i].position;
+		param_get(_param_handles[i].position_x, &position(0));
+		param_get(_param_handles[i].position_y, &position(1));
+		param_get(_param_handles[i].position_z, &position(2));
+
+		Vector3f &axis = _geometry.thrusters[i].axis;
+
+		switch (_axis_config) {
+		case AxisConfiguration::Configurable:
+			param_get(_param_handles[i].axis_x, &axis(0));
+			param_get(_param_handles[i].axis_y, &axis(1));
+			param_get(_param_handles[i].axis_z, &axis(2));
+			break;
+		}
+
+		param_get(_param_handles[i].thrust_coef, &_geometry.thrusters[i].thrust_coef);
+	}
+}
+
+bool
+ActuatorEffectivenessThrusters::addActuators(Configuration &configuration)
+{
+	if (configuration.num_actuators[(int)ActuatorType::SERVOS] > 0) {
+		PX4_ERR("Wrong actuator ordering: servos need to be after motors");
+		return false;
+	}
+
+	int num_actuators = computeEffectivenessMatrix(_geometry,
+			    configuration.effectiveness_matrices[configuration.selected_matrix],
+			    configuration.num_actuators_matrix[configuration.selected_matrix]);
+	configuration.actuatorsAdded(ActuatorType::THRUSTERS, num_actuators);
+	return true;
+}
+
+int
+ActuatorEffectivenessThrusters::computeEffectivenessMatrix(const Geometry &geometry,
+		EffectivenessMatrix &effectiveness, int actuator_start_index)
+{
+	int num_actuators = 0;
+
+	for (int i = 0; i < geometry.num_thrusters; i++) {
+
+		if (i + actuator_start_index >= NUM_ACTUATORS) {
+			break;
+		}
+
+		++num_actuators;
+
+		// Get rotor axis
+		Vector3f axis = geometry.thrusters[i].axis;
+
+		// Normalize axis
+		float axis_norm = axis.norm();
+
+		if (axis_norm > FLT_EPSILON) {
+			axis /= axis_norm;
+
+		} else {
+			// Bad axis definition, ignore this rotor
+			continue;
+		}
+
+		// Get rotor position
+		const Vector3f &position = geometry.thrusters[i].position;
+
+		// Get coefficients
+		float ct = geometry.thrusters[i].thrust_coef;
+
+		if (fabsf(ct) < FLT_EPSILON) {
+			continue;
+		}
+
+		// Compute thrust generated by this rotor
+		matrix::Vector3f thrust = ct * axis;
+
+		// Compute moment generated by this rotor
+		matrix::Vector3f moment = ct * position.cross(axis);
+
+		// Fill corresponding items in effectiveness matrix
+		for (size_t j = 0; j < 3; j++) {
+			effectiveness(j, i + actuator_start_index) = moment(j);
+			effectiveness(j + 3, i + actuator_start_index) = thrust(j);
+		}
+	}
+
+	return num_actuators;
+}
+
+
+uint32_t ActuatorEffectivenessThrusters::getThrusters() const
+{
+	uint32_t thrusters = 0;
+
+	for (int i = 0; i < _geometry.num_thrusters; ++i) {
+		thrusters |= 1u << i;
+	}
+
+	return thrusters;
+}
+
+bool
+ActuatorEffectivenessThrusters::getEffectivenessMatrix(Configuration &configuration,
+		EffectivenessUpdateReason external_update)
+{
+	if (external_update == EffectivenessUpdateReason::NO_EXTERNAL_UPDATE) {
+		return false;
+	}
+
+	return addActuators(configuration);
+}

src/lib/actuator_effectiveness/ActuatorEffectivenessThrusters.hpp

@@ -0,0 +1,121 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2020 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file ActuatorEffectivenessThrusters.hpp
+ *
+ * Actuator effectiveness computed from thrusters position and orientation
+ *
+ * @author Pedro Roque, <padr@kth.se>
+ */
+
+#pragma once
+
+#include "ActuatorEffectiveness.hpp"
+
+#include <px4_platform_common/module_params.h>
+#include <uORB/Subscription.hpp>
+#include <uORB/SubscriptionInterval.hpp>
+
+class ActuatorEffectivenessTilts;
+
+using namespace time_literals;
+
+class ActuatorEffectivenessThrusters : public ModuleParams, public ActuatorEffectiveness
+{
+public:
+	enum class AxisConfiguration {
+		Configurable, ///< axis can be configured
+	};
+
+	static constexpr int NUM_THRUSTERS_MAX = 12;
+
+	struct ThrusterGeometry {
+		matrix::Vector3f position;
+		matrix::Vector3f axis;
+		float thrust_coef;
+	};
+
+	struct Geometry {
+		ThrusterGeometry thrusters[NUM_THRUSTERS_MAX];
+		int num_thrusters{0};
+	};
+
+	ActuatorEffectivenessThrusters(ModuleParams *parent, AxisConfiguration axis_config = AxisConfiguration::Configurable,
+				       bool tilt_support = false);
+	virtual ~ActuatorEffectivenessThrusters() = default;
+
+	bool getEffectivenessMatrix(Configuration &configuration, EffectivenessUpdateReason external_update) override;
+
+	void getDesiredAllocationMethod(AllocationMethod allocation_method_out[MAX_NUM_MATRICES]) const override
+	{
+		allocation_method_out[0] =
+			AllocationMethod::SEQUENTIAL_DESATURATION;  // TODO(@Jaeyoung-Lim): needs to be updated based on metric mixer
+	}
+
+	void getNormalizeRPY(bool normalize[MAX_NUM_MATRICES]) const override
+	{
+		// TODO(@Jaeyoung-Lim): needs to be updated based on metric mixer
+		normalize[0] = true;
+	}
+
+	static int computeEffectivenessMatrix(const Geometry &geometry,
+					      EffectivenessMatrix &effectiveness, int actuator_start_index = 0);
+
+	bool addActuators(Configuration &configuration);
+
+	const char *name() const override { return "Thrusters"; }
+
+	const Geometry &geometry() const { return _geometry; }
+
+	uint32_t getThrusters() const;
+
+private:
+	void updateParams() override;
+	const AxisConfiguration _axis_config;
+	const bool _tilt_support; ///< if true, tilt servo assignment params are loaded
+
+	struct ParamHandles {
+		param_t position_x;
+		param_t position_y;
+		param_t position_z;
+		param_t axis_x;
+		param_t axis_y;
+		param_t axis_z;
+		param_t thrust_coef;
+	};
+	ParamHandles _param_handles[NUM_THRUSTERS_MAX];
+	param_t _count_handle;
+
+	Geometry _geometry{};
+};

src/lib/actuator_effectiveness/ActuatorEffectivenessThrustersTest.cpp

@@ -0,0 +1,135 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2020 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file ActuatorEffectivenessRotors.cpp
+ *
+ * Tests for Control Allocation Algorithms
+ *
+ * @author Julien Lecoeur <julien.lecoeur@gmail.com>
+ */
+
+#include <gtest/gtest.h>
+#include "ActuatorEffectivenessRotors.hpp"
+
+using namespace matrix;
+
+TEST(ActuatorEffectivenessRotors, AllZeroCase)
+{
+	// Quad wide geometry
+	ActuatorEffectivenessRotors::Geometry geometry = {};
+	geometry.rotors[0].position(0) = 1.0f;
+	geometry.rotors[0].position(1) = 1.0f;
+	geometry.rotors[0].position(2) = 0.0f;
+	geometry.rotors[0].axis(0) = 0.0f;
+	geometry.rotors[0].axis(1) = 0.0f;
+	geometry.rotors[0].axis(2) = -1.0f;
+	geometry.rotors[0].thrust_coef = 1.0f;
+	geometry.rotors[0].moment_ratio = 0.05f;
+
+	geometry.rotors[1].position(0) = -1.0f;
+	geometry.rotors[1].position(1) = -1.0f;
+	geometry.rotors[1].position(2) = 0.0f;
+	geometry.rotors[1].axis(0) = 0.0f;
+	geometry.rotors[1].axis(1) = 0.0f;
+	geometry.rotors[1].axis(2) = -1.0f;
+	geometry.rotors[1].thrust_coef = 1.0f;
+	geometry.rotors[1].moment_ratio = 0.05f;
+
+	geometry.rotors[2].position(0) = 1.0f;
+	geometry.rotors[2].position(1) = -1.0f;
+	geometry.rotors[2].position(2) = 0.0f;
+	geometry.rotors[2].axis(0) = 0.0f;
+	geometry.rotors[2].axis(1) = 0.0f;
+	geometry.rotors[2].axis(2) = -1.0f;
+	geometry.rotors[2].thrust_coef = 1.0f;
+	geometry.rotors[2].moment_ratio = -0.05f;
+
+	geometry.rotors[3].position(0) = -1.0f;
+	geometry.rotors[3].position(1) = 1.0f;
+	geometry.rotors[3].position(2) = 0.0f;
+	geometry.rotors[3].axis(0) = 0.0f;
+	geometry.rotors[3].axis(1) = 0.0f;
+	geometry.rotors[3].axis(2) = -1.0f;
+	geometry.rotors[3].thrust_coef = 1.0f;
+	geometry.rotors[3].moment_ratio = -0.05f;
+
+	geometry.num_rotors = 4;
+
+	ActuatorEffectiveness::EffectivenessMatrix effectiveness;
+	effectiveness.setZero();
+	ActuatorEffectivenessRotors::computeEffectivenessMatrix(geometry, effectiveness);
+
+	const float expected[ActuatorEffectiveness::NUM_AXES][ActuatorEffectiveness::NUM_ACTUATORS] = {
+		{-1.0f,   1.0f,   1.0f,  -1.0f,  0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f},
+		{ 1.0f,  -1.0f,   1.0f,  -1.0f,  0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f},
+		{ 0.05f,  0.05f, -0.05f, -0.05f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f},
+		{ 0.f,    0.f,    0.f,    0.f,   0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f},
+		{ 0.f,    0.f,    0.f,    0.f,   0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f},
+		{-1.0f,  -1.0f,  -1.0f,  -1.0f,  0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f}
+	};
+	ActuatorEffectiveness::EffectivenessMatrix effectiveness_expected(expected);
+
+	EXPECT_EQ(effectiveness, effectiveness_expected);
+}
+
+TEST(ActuatorEffectivenessRotors, Tilt)
+{
+	Vector3f axis_expected{0.f, 0.f, -1.f};
+	Vector3f axis = ActuatorEffectivenessRotors::tiltedAxis(0.f, 0.f);
+	EXPECT_EQ(axis, axis_expected);
+
+	axis_expected = Vector3f{1.f, 0.f, 0.f};
+	axis = ActuatorEffectivenessRotors::tiltedAxis(M_PI_F / 2.f, 0.f);
+	EXPECT_EQ(axis, axis_expected);
+
+	axis_expected = Vector3f{1.f / sqrtf(2.f), 0.f, -1.f / sqrtf(2.f)};
+	axis = ActuatorEffectivenessRotors::tiltedAxis(M_PI_F / 2.f / 2.f, 0.f);
+	EXPECT_EQ(axis, axis_expected);
+
+	axis_expected = Vector3f{-1.f, 0.f, 0.f};
+	axis = ActuatorEffectivenessRotors::tiltedAxis(-M_PI_F / 2.f, 0.f);
+	EXPECT_EQ(axis, axis_expected);
+
+	axis_expected = Vector3f{0.f, 0.f, -1.f};
+	axis = ActuatorEffectivenessRotors::tiltedAxis(0.f, M_PI_F / 2.f);
+	EXPECT_EQ(axis, axis_expected);
+
+	axis_expected = Vector3f{0.f, 1.f, 0.f};
+	axis = ActuatorEffectivenessRotors::tiltedAxis(M_PI_F / 2.f, M_PI_F / 2.f);
+	EXPECT_EQ(axis, axis_expected);
+
+	axis_expected = Vector3f{0.f, -1.f, 0.f};
+	axis = ActuatorEffectivenessRotors::tiltedAxis(-M_PI_F / 2.f, M_PI_F / 2.f);
+	EXPECT_EQ(axis, axis_expected);
+}

src/lib/actuator_effectiveness/CMakeLists.txt

@@ -34,6 +34,10 @@
 px4_add_library(ActuatorEffectiveness
 	ActuatorEffectiveness.cpp
 	ActuatorEffectiveness.hpp
+	ActuatorEffectivenessRotors.cpp
+	ActuatorEffectivenessRotors.hpp
+	ActuatorEffectivenessThrusters.cpp
+	ActuatorEffectivenessThrusters.hpp
 )
 
 target_compile_options(ActuatorEffectiveness PRIVATE ${MAX_CUSTOM_OPT_LEVEL})
@@ -42,3 +46,6 @@ target_link_libraries(ActuatorEffectiveness
 	PRIVATE
 		mathlib
 )
+
+px4_add_functional_gtest(SRC ActuatorEffectivenessRotorsTest.cpp LINKLIBS ActuatorEffectiveness)
+px4_add_functional_gtest(SRC ActuatorEffectivenessThrustersTest.cpp LINKLIBS ActuatorEffectiveness)

src/lib/mixer_module/functions/FunctionThrusters.hpp

@@ -0,0 +1,124 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2021-2022 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+#pragma once
+
+#include "FunctionProviderBase.hpp"
+
+/**
+ * Functions: Thruster1 ... ThrusterMax
+ */
+class FunctionThrusters : public FunctionProviderBase
+{
+public:
+	static_assert(actuator_motors_s::NUM_CONTROLS == (int)OutputFunction::ThrusterMax - (int)OutputFunction::Thruster1 + 1,
+		      "Unexpected num thrusters");
+
+	static_assert(actuator_motors_s::ACTUATOR_FUNCTION_THRUSTER1 == (int)OutputFunction::Thruster1,
+		      "Unexpected thruster idx");
+
+	FunctionThrusters(const Context &context) :
+		_topic(&context.work_item, ORB_ID(actuator_motors)),
+		_thrust_factor(context.thrust_factor)
+	{
+		for (int i = 0; i < actuator_motors_s::NUM_CONTROLS; ++i) {
+			_data.control[i] = NAN;
+		}
+	}
+
+	static FunctionProviderBase *allocate(const Context &context) { return new FunctionThrusters(context); }
+
+	void update() override
+	{
+		if (_topic.update(&_data)) {
+			updateValues(_data.reversible_flags, _thrust_factor, _data.control, actuator_motors_s::NUM_CONTROLS);
+		}
+	}
+
+	float value(OutputFunction func) override { return _data.control[(int)func - (int)OutputFunction::Thruster1]; }
+
+	bool allowPrearmControl() const override { return false; }
+
+	uORB::SubscriptionCallbackWorkItem *subscriptionCallback() override { return &_topic; }
+
+	bool getLatestSampleTimestamp(hrt_abstime &t) const override { t = _data.timestamp_sample; return t != 0; }
+
+	static inline void updateValues(uint32_t reversible, float thrust_factor, float *values, int num_values)
+	{
+		// TODO(@Pedro-Roque): for now bypass this
+		/*if (thrust_factor > 0.f && thrust_factor <= 1.f) {
+
+			// thrust factor
+			//  rel_thrust = factor * x^2 + (1-factor) * x,
+			const float a = thrust_factor;
+			const float b = (1.f - thrust_factor);
+
+			// don't recompute for all values (ax^2+bx+c=0)
+			const float tmp1 = b / (2.f * a);
+			const float tmp2 = b * b / (4.f * a * a);
+
+			for (int i = 0; i < num_values; ++i) {
+				float control = values[i];
+
+				if (control > 0.f) {
+					values[i] = -tmp1 + sqrtf(tmp2 + (control / a));
+
+				} else if (control < -0.f) {
+					values[i] =  tmp1 - sqrtf(tmp2 - (control / a));
+
+				} else {
+					values[i] = 0.f;
+				}
+			}
+		}
+
+		for (int i = 0; i < num_values; ++i) {
+			if ((reversible & (1u << i)) == 0) {
+				if (values[i] < -FLT_EPSILON) {
+					values[i] = NAN;
+
+				} else {
+					// remap from [0, 1] to [-1, 1]
+					values[i] = values[i] * 2.f - 1.f;
+				}
+			}
+		}*/
+	}
+
+	bool reversible(OutputFunction func) const override { return false; }
+
+private:
+	uORB::SubscriptionCallbackWorkItem _topic;
+	actuator_motors_s _data{};
+	const float &_thrust_factor;
+};