dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

PositionControl: correct horizontal margin calculation 

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
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

5
src/modules/mc_pos_control/PositionControl/PositionControl.cpp
View File

@ -145,16 +145,17 @@ void PositionControl::_velocityControl(const float dt)
// Prioritize vertical control while keeping a horizontal margin
const Vector2f thrust_sp_xy(_thr_sp);
const float thrust_sp_xy_norm = thrust_sp_xy.norm();
const float thrust_max_squared = math::sq(_lim_thr_max);
// Determine how much vertical thrust is left keeping horizontal margin
const float allocated_horizontal_thrust = math::min(thrust_sp_xy_norm, _lim_thr_xy_margin);
const float thrust_z_max_squared = math::sq(_lim_thr_max) - math::sq(allocated_horizontal_thrust);
const float thrust_z_max_squared = thrust_max_squared - math::sq(allocated_horizontal_thrust);
// Saturate maximal vertical thrust
_thr_sp(2) = math::max(_thr_sp(2), -sqrtf(thrust_z_max_squared));
// Determine how much horizontal thrust is left after prioritizing vertical control
const float thrust_max_xy_squared = thrust_z_max_squared - math::sq(_thr_sp(2));
const float thrust_max_xy_squared = thrust_max_squared - math::sq(_thr_sp(2));
float thrust_max_xy = 0;
if (thrust_max_xy_squared > 0) {

33
src/modules/mc_pos_control/PositionControl/PositionControlTest.cpp
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)