fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-03 13:48:54 +08:00
Code Issues Packages Projects Releases Wiki Activity

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

Browse Source 
This reverts commit f7dadd9b89.
This commit is contained in:
Matthias Grob
2025-07-28 14:10:50 +02:00
parent 930c0451b5
commit 4b4cc2164c
2 changed files with 6 additions and 56 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/lib/control_allocation/control_allocation/CMakeLists.txt
+1 -1
View File
@@ -44,4 +44,4 @@ target_include_directories(ControlAllocation PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})
target_link_libraries(ControlAllocation PRIVATE mathlib) target_link_libraries(ControlAllocation PRIVATE mathlib)
px4_add_unit_gtest(SRC ControlAllocationPseudoInverseTest.cpp LINKLIBS ControlAllocation) 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
+5 -55
View File
@@ -40,27 +40,17 @@
#include <gtest/gtest.h> #include <gtest/gtest.h>
#include <ControlAllocationSequentialDesaturation.hpp> #include <ControlAllocationSequentialDesaturation.hpp>
#include <actuator_effectiveness/ActuatorEffectivenessRotors.hpp>
using namespace matrix; using namespace matrix;
namespace 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. // 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(0) = 1.0f;
geometry.rotors[0].position(1) = 1.0f; geometry.rotors[0].position(1) = 1.0f;
geometry.rotors[0].position(2) = 0.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.rotors[3].moment_ratio = -0.05f;
geometry.num_rotors = 4; geometry.num_rotors = 4;
return geometry; return geometry;
} }
@@ -107,48 +98,7 @@ ActuatorEffectiveness::EffectivenessMatrix make_quad_x_effectiveness()
ActuatorEffectiveness::EffectivenessMatrix effectiveness; ActuatorEffectiveness::EffectivenessMatrix effectiveness;
effectiveness.setZero(); effectiveness.setZero();
const auto geometry = make_quad_x_geometry(); 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; return effectiveness;
} }