fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-07-12 23:10:35 +08:00
Code Issues Packages Projects Releases Wiki Activity

PositionControl: correct horizontal margin calculation

Browse Source 
It was using the already reduced vertical thrust to do
the horizontal limitation resulting in no margin.
This commit is contained in:
Matthias Grob
2021-09-10 15:56:22 +02:00
parent d1f1e02afb
commit e7a90bf367
2 changed files with 26 additions and 12 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/mc_pos_control/PositionControl/PositionControlTest.cpp
+23 -10
View File
@@ -78,8 +78,8 @@ public:
_position_control.setPositionGains(Vector3f(1.f, 1.f, 1.f));
_position_control.setVelocityGains(Vector3f(20.f, 20.f, 20.f), Vector3f(20.f, 20.f, 20.f), Vector3f(20.f, 20.f, 20.f));
_position_control.setVelocityLimits(1.f, 1.f, 1.f);
_position_control.setThrustLimits(0.1f, 0.9f);
_position_control.setHorizontalThrustMargin(0.3f);
_position_control.setThrustLimits(0.1f, MAXIMUM_THRUST);
_position_control.setHorizontalThrustMargin(HORIZONTAL_THRUST_MARGIN);
_position_control.setTiltLimit(1.f);
_position_control.setHoverThrust(.5f);
@@ -113,6 +113,9 @@ public:
vehicle_local_position_setpoint_s _input_setpoint{};
vehicle_local_position_setpoint_s _output_setpoint{};
vehicle_attitude_setpoint_s _attitude{};
static constexpr float MAXIMUM_THRUST = 0.9f;
static constexpr float HORIZONTAL_THRUST_MARGIN = 0.3f;
};
class PositionControlBasicDirectionTest : public PositionControlBasicTest
@@ -186,24 +189,34 @@ TEST_F(PositionControlBasicTest, VelocityLimit)
TEST_F(PositionControlBasicTest, PositionControlMaxThrustLimit)
{
// Given a setpoint that drives the controller into vertical and horizontal saturation
_input_setpoint.x = 10.f;
_input_setpoint.y = 10.f;
_input_setpoint.z = -10.f;
// When you run it for one iteration
runController();
Vector3f thrust(_output_setpoint.thrust);
EXPECT_FLOAT_EQ(thrust(0), 0.f);
EXPECT_FLOAT_EQ(thrust(1), 0.f);
// Expect the remaining vertical thrust after allocating the horizontal margin
// sqrt(0.9^2 - 0.3^2) = 0.8485
EXPECT_FLOAT_EQ(thrust(2), -0.848528137423857f);
// Then the thrust vector length is limited by the maximum
EXPECT_FLOAT_EQ(thrust.norm(), MAXIMUM_THRUST);
// Then the horizontal thrust is limited by its margin
EXPECT_FLOAT_EQ(thrust(0), HORIZONTAL_THRUST_MARGIN / sqrt(2.f));
EXPECT_FLOAT_EQ(thrust(1), HORIZONTAL_THRUST_MARGIN / sqrt(2.f));
EXPECT_FLOAT_EQ(thrust(2),
-sqrt(MAXIMUM_THRUST * MAXIMUM_THRUST - HORIZONTAL_THRUST_MARGIN * HORIZONTAL_THRUST_MARGIN));
thrust.print();
// Then the collective thrust is limited by the maximum
EXPECT_EQ(_attitude.thrust_body[0], 0.f);
EXPECT_EQ(_attitude.thrust_body[1], 0.f);
EXPECT_FLOAT_EQ(_attitude.thrust_body[2], -0.848528137423857f);
EXPECT_FLOAT_EQ(_attitude.thrust_body[2], -MAXIMUM_THRUST);
EXPECT_FLOAT_EQ(_attitude.roll_body, 0.f);
EXPECT_FLOAT_EQ(_attitude.pitch_body, 0.f);
// Then the horizontal margin results in a tilt with the ratio of: horizontal margin / maximum thrust
EXPECT_FLOAT_EQ(_attitude.roll_body, asin((HORIZONTAL_THRUST_MARGIN / sqrt(2.f)) / MAXIMUM_THRUST));
// TODO: add this line back once attitude setpoint generation strategy does not align body yaw with heading all the time anymore
// EXPECT_FLOAT_EQ(_attitude.pitch_body, -asin((HORIZONTAL_THRUST_MARGIN / sqrt(2.f)) / MAXIMUM_THRUST));
}
TEST_F(PositionControlBasicTest, PositionControlMinThrustLimit)