Maintenance: Fix sequential desaturation unit test dependency

src/lib/control_allocation/actuator_effectiveness/CMakeLists.txt

@@ -41,5 +41,4 @@ target_include_directories(ActuatorEffectiveness PUBLIC ${CMAKE_CURRENT_SOURCE_D
 target_link_libraries(ActuatorEffectiveness
 	PRIVATE
 		mathlib
-		PID
 )

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 VehicleActuatorEffectiveness)

src/lib/control_allocation/control_allocation/ControlAllocationSequentialDesaturationTest.cpp

@@ -40,27 +40,17 @@
 
 #include <gtest/gtest.h>
 #include <ControlAllocationSequentialDesaturation.hpp>
+#include <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.
-GeometryTest make_quad_x_geometry()
+ActuatorEffectivenessRotors::Geometry make_quad_x_geometry()
 {
-	GeometryTest 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;
@@ -98,6 +88,7 @@ GeometryTest make_quad_x_geometry()
 	geometry.rotors[3].moment_ratio = -0.05f;
 
 	geometry.num_rotors = 4;
+
 	return geometry;
 }
 
@@ -107,48 +98,7 @@ ActuatorEffectiveness::EffectivenessMatrix make_quad_x_effectiveness()
 	ActuatorEffectiveness::EffectivenessMatrix effectiveness;
 	effectiveness.setZero();
 	const auto geometry = make_quad_x_geometry();
-
-	// 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);
-		}
-	}
-
+	ActuatorEffectivenessRotors::computeEffectivenessMatrix(geometry, effectiveness);
 	return effectiveness;
 }
 

src/modules/control_allocator/VehicleActuatorEffectiveness/CMakeLists.txt

@@ -35,8 +35,9 @@ target_compile_options(VehicleActuatorEffectiveness PRIVATE ${MAX_CUSTOM_OPT_LEV
 target_include_directories(VehicleActuatorEffectiveness PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})
 target_link_libraries(VehicleActuatorEffectiveness
 	PRIVATE
-		mathlib
 		ActuatorEffectiveness
+		mathlib
+		PID
 )
 
 px4_add_functional_gtest(SRC ActuatorEffectivenessHelicopterTest.cpp LINKLIBS VehicleActuatorEffectiveness)