From f42f925aafec9126099e64edd3796e2091c6e5ea Mon Sep 17 00:00:00 2001 From: Matthias Grob Date: Wed, 16 Apr 2025 11:11:06 +0200 Subject: [PATCH] Revert "Remove inclusion of rotors in library to enable test (#24286)" This reverts commit f7dadd9b89f3052735a12bd617755e78cf5305e2. --- .../control_allocation/CMakeLists.txt | 2 +- ...olAllocationSequentialDesaturationTest.cpp | 60 ++----------------- 2 files changed, 6 insertions(+), 56 deletions(-) diff --git a/src/lib/control_allocation/control_allocation/CMakeLists.txt b/src/lib/control_allocation/control_allocation/CMakeLists.txt index 4da638aac8..d65bd11fa0 100644 --- a/src/lib/control_allocation/control_allocation/CMakeLists.txt +++ b/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) diff --git a/src/lib/control_allocation/control_allocation/ControlAllocationSequentialDesaturationTest.cpp b/src/lib/control_allocation/control_allocation/ControlAllocationSequentialDesaturationTest.cpp index cd478a1b00..2e0af6bff4 100644 --- a/src/lib/control_allocation/control_allocation/ControlAllocationSequentialDesaturationTest.cpp +++ b/src/lib/control_allocation/control_allocation/ControlAllocationSequentialDesaturationTest.cpp @@ -40,27 +40,17 @@ #include #include +#include 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; }