From e7a90bf367c93b4a9809ed1bc729e4b77818983d Mon Sep 17 00:00:00 2001 From: Matthias Grob Date: Fri, 10 Sep 2021 15:56:22 +0200 Subject: [PATCH] PositionControl: correct horizontal margin calculation It was using the already reduced vertical thrust to do the horizontal limitation resulting in no margin. --- .../PositionControl/PositionControl.cpp | 5 +-- .../PositionControl/PositionControlTest.cpp | 33 +++++++++++++------ 2 files changed, 26 insertions(+), 12 deletions(-) diff --git a/src/modules/mc_pos_control/PositionControl/PositionControl.cpp b/src/modules/mc_pos_control/PositionControl/PositionControl.cpp index c370668c4c..8008e2b338 100644 --- a/src/modules/mc_pos_control/PositionControl/PositionControl.cpp +++ b/src/modules/mc_pos_control/PositionControl/PositionControl.cpp @@ -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) { diff --git a/src/modules/mc_pos_control/PositionControl/PositionControlTest.cpp b/src/modules/mc_pos_control/PositionControl/PositionControlTest.cpp index 3c98d5da85..5afc32dde1 100644 --- a/src/modules/mc_pos_control/PositionControl/PositionControlTest.cpp +++ b/src/modules/mc_pos_control/PositionControl/PositionControlTest.cpp @@ -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)