fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-05-16 13:37:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

ActuatorEffectivenessRotorsTest: add Hexarotor X

Browse Source
This commit is contained in:
Matthias Grob
2025-04-15 19:07:40 +02:00
parent 38922d10a0
commit 6a7e53e5de
1 changed files with 60 additions and 27 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessRotorsTest.cpp
+60 -27
View File
@@ -44,50 +44,32 @@
using namespace matrix;
TEST(ActuatorEffectivenessRotors, AllZeroCase)
TEST(ActuatorEffectivenessRotors, QuadrotorX)
{
// Quad wide 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;
geometry.rotors[0].axis(0) = 0.0f;
geometry.rotors[0].axis(1) = 0.0f;
geometry.rotors[0].axis(2) = -1.0f;
geometry.rotors[0].position = {1.f, 1.f, 0.f};
geometry.rotors[0].axis = {0.f, 0.f, -1.f};
geometry.rotors[0].thrust_coef = 1.0f;
geometry.rotors[0].moment_ratio = 0.05f;
geometry.rotors[1].position(0) = -1.0f;
geometry.rotors[1].position(1) = -1.0f;
geometry.rotors[1].position(2) = 0.0f;
geometry.rotors[1].axis(0) = 0.0f;
geometry.rotors[1].axis(1) = 0.0f;
geometry.rotors[1].axis(2) = -1.0f;
geometry.rotors[1].position = {-1.f, -1.f, 0.f};
geometry.rotors[1].axis = {0.f, 0.f, -1.f};
geometry.rotors[1].thrust_coef = 1.0f;
geometry.rotors[1].moment_ratio = 0.05f;
geometry.rotors[2].position(0) = 1.0f;
geometry.rotors[2].position(1) = -1.0f;
geometry.rotors[2].position(2) = 0.0f;
geometry.rotors[2].axis(0) = 0.0f;
geometry.rotors[2].axis(1) = 0.0f;
geometry.rotors[2].axis(2) = -1.0f;
geometry.rotors[2].position = {1.f, -1.f, 0.f};
geometry.rotors[2].axis = {0.f, 0.f, -1.f};
geometry.rotors[2].thrust_coef = 1.0f;
geometry.rotors[2].moment_ratio = -0.05f;
geometry.rotors[3].position(0) = -1.0f;
geometry.rotors[3].position(1) = 1.0f;
geometry.rotors[3].position(2) = 0.0f;
geometry.rotors[3].axis(0) = 0.0f;
geometry.rotors[3].axis(1) = 0.0f;
geometry.rotors[3].axis(2) = -1.0f;
geometry.rotors[3].position = {-1.f, 1.f, 0.f};
geometry.rotors[3].axis = {0.f, 0.f, -1.f};
geometry.rotors[3].thrust_coef = 1.0f;
geometry.rotors[3].moment_ratio = -0.05f;
geometry.num_rotors = 4;
ActuatorEffectiveness::EffectivenessMatrix effectiveness;
effectiveness.setZero();
ActuatorEffectivenessRotors::computeEffectivenessMatrix(geometry, effectiveness);
const float expected[ActuatorEffectiveness::NUM_AXES][ActuatorEffectiveness::NUM_ACTUATORS] = {
@@ -103,6 +85,57 @@ TEST(ActuatorEffectivenessRotors, AllZeroCase)
EXPECT_EQ(effectiveness, effectiveness_expected);
}
TEST(ActuatorEffectivenessRotors, HexarotorX)
{
ActuatorEffectivenessRotors::Geometry geometry = {};
geometry.rotors[0].position = {0.f, .5f, 0.f};
geometry.rotors[0].axis = {0.f, 0.f, -1.f};
geometry.rotors[0].thrust_coef = 1.0f;
geometry.rotors[0].moment_ratio = -0.05f;
geometry.rotors[1].position = {0.f, -.5f, 0.f};
geometry.rotors[1].axis = {0.f, 0.f, -1.f};
geometry.rotors[1].thrust_coef = 1.0f;
geometry.rotors[1].moment_ratio = 0.05f;
geometry.rotors[2].position = {.43f, -.25f, 0.f};
geometry.rotors[2].axis = {0.f, 0.f, -1.f};
geometry.rotors[2].thrust_coef = 1.0f;
geometry.rotors[2].moment_ratio = -0.05f;
geometry.rotors[3].position = {-.43f, .25f, 0.f};
geometry.rotors[3].axis = {0.f, 0.f, -1.f};
geometry.rotors[3].thrust_coef = 1.0f;
geometry.rotors[3].moment_ratio = 0.05f;
geometry.rotors[4].position = {.43f, .25f, 0.f};
geometry.rotors[4].axis = {0.f, 0.f, -1.f};
geometry.rotors[4].thrust_coef = 1.0f;
geometry.rotors[4].moment_ratio = 0.05f;
geometry.rotors[5].position = {-.43f, -.25f, 0.f};
geometry.rotors[5].axis = {0.f, 0.f, -1.f};
geometry.rotors[5].thrust_coef = 1.0f;
geometry.rotors[5].moment_ratio = -0.05f;
geometry.num_rotors = 6;
ActuatorEffectiveness::EffectivenessMatrix effectiveness;
ActuatorEffectivenessRotors::computeEffectivenessMatrix(geometry, effectiveness);
const float expected[ActuatorEffectiveness::NUM_AXES][ActuatorEffectiveness::NUM_ACTUATORS] = {
{-0.5f, 0.5f, 0.25f, -0.25f, -0.25f, 0.25f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f},
{ 0.f, 0.f, 0.43f, -0.43f, 0.43f, -0.43f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f},
{-0.05f, 0.05f, -0.05f, 0.05f, 0.05f, -0.05f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f},
{ 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f},
{ 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f},
{-1.f, -1.f, -1.f, -1.f, -1.f, -1.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f}
};
ActuatorEffectiveness::EffectivenessMatrix effectiveness_expected(expected);
EXPECT_EQ(effectiveness, effectiveness_expected);
}
TEST(ActuatorEffectivenessRotors, Tilt)
{
Vector3f axis_expected{0.f, 0.f, -1.f};