fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-05-02 02:24:06 +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 f7dadd9b89f3052735a12bd617755e78cf5305e2.
This commit is contained in:
Matthias Grob 2025-04-16 11:11:06 +02:00
parent 6a7e53e5de
commit f42f925aaf
2 changed files with 6 additions and 56 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
src/lib/control_allocation/control_allocation/CMakeLists.txt
View File

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

60
src/lib/control_allocation/control_allocation/ControlAllocationSequentialDesaturationTest.cpp
View File

@ -40,27 +40,17 @@
#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.
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;
}