Remove inclusion of rotors in library to enable test (#24286)

src/lib/control_allocation/control_allocation/CMakeLists.txt

@@ -44,4 +44,4 @@ target_include_directories(ControlAllocation PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})
 target_link_libraries(ControlAllocation PRIVATE mathlib)
 
 px4_add_unit_gtest(SRC ControlAllocationPseudoInverseTest.cpp LINKLIBS ControlAllocation)
-# px4_add_functional_gtest(SRC ControlAllocationSequentialDesaturationTest.cpp LINKLIBS ControlAllocation ActuatorEffectiveness)
+px4_add_functional_gtest(SRC ControlAllocationSequentialDesaturationTest.cpp LINKLIBS ControlAllocation ActuatorEffectiveness)

src/lib/control_allocation/control_allocation/ControlAllocationSequentialDesaturationTest.cpp

@@ -40,17 +40,27 @@
 
 #include <gtest/gtest.h>
 #include <ControlAllocationSequentialDesaturation.hpp>
-#include <actuator_effectiveness/ActuatorEffectivenessRotors.hpp>
 
 using namespace matrix;
 
 namespace
 {
+struct RotorGeometryTest {
+	matrix::Vector3f position;
+	matrix::Vector3f axis;
+	float thrust_coef;
+	float moment_ratio;
+};
+
+struct GeometryTest {
+	RotorGeometryTest rotors[ActuatorEffectiveness::NUM_ACTUATORS];
+	int num_rotors{0};
+};
 
 // Makes and returns a Geometry object for a "standard" quad-x quadcopter.
-ActuatorEffectivenessRotors::Geometry make_quad_x_geometry()
+GeometryTest make_quad_x_geometry()
 {
-	ActuatorEffectivenessRotors::Geometry geometry = {};
+	GeometryTest geometry = {};
 	geometry.rotors[0].position(0) = 1.0f;
 	geometry.rotors[0].position(1) = 1.0f;
 	geometry.rotors[0].position(2) = 0.0f;
@@ -88,7 +98,6 @@ ActuatorEffectivenessRotors::Geometry make_quad_x_geometry()
 	geometry.rotors[3].moment_ratio = -0.05f;
 
 	geometry.num_rotors = 4;
-
 	return geometry;
 }
 
@@ -98,7 +107,48 @@ ActuatorEffectiveness::EffectivenessMatrix make_quad_x_effectiveness()
 	ActuatorEffectiveness::EffectivenessMatrix effectiveness;
 	effectiveness.setZero();
 	const auto geometry = make_quad_x_geometry();
-	ActuatorEffectivenessRotors::computeEffectivenessMatrix(geometry, effectiveness);
+
+	// Minimalistically copied from ActuatorEffectivenessRotors::computeEffectivenessMatrix
+	for (int i = 0; i < geometry.num_rotors; i++) {
+
+		// 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 (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 (int j = 0; j < 3; j++) {
+			effectiveness(j, i) = moment(j);
+			effectiveness(j + 3, i) = thrust(j);
+		}
+	}
+
 	return effectiveness;
 }