From 6ccf55b6fd67d8cc93c7cf0b5cffcba724b15fef Mon Sep 17 00:00:00 2001 From: bresch Date: Sun, 15 Mar 2020 16:15:22 +0100 Subject: [PATCH] MPC: add updateHoverThrust function This function updates the vertical velocity integrator with the change in hover thrust to avoid propagating discontinuities through the controller when changing the hover thrust. This is also important when using the hover thrust estimator as its estimate has unconstrained dynamics and can cause drops or kicks when the estimate updates faster than the velocity integrator. --- .../PositionControl/PositionControl.cpp | 6 ++++ .../PositionControl/PositionControl.hpp | 18 ++++++++++-- .../PositionControl/PositionControlTest.cpp | 29 +++++++++++++++++++ .../mc_pos_control/mc_pos_control_main.cpp | 4 +-- 4 files changed, 52 insertions(+), 5 deletions(-) diff --git a/src/modules/mc_pos_control/PositionControl/PositionControl.cpp b/src/modules/mc_pos_control/PositionControl/PositionControl.cpp index b68088f82b..f220b4fe22 100644 --- a/src/modules/mc_pos_control/PositionControl/PositionControl.cpp +++ b/src/modules/mc_pos_control/PositionControl/PositionControl.cpp @@ -63,6 +63,12 @@ void PositionControl::setThrustLimits(const float min, const float max) _lim_thr_max = max; } +void PositionControl::updateHoverThrust(const float hover_thrust_new) +{ + _vel_int(2) += hover_thrust_new - _hover_thrust; + setHoverThrust(hover_thrust_new); +} + void PositionControl::setState(const PositionControlStates &states) { _pos = states.position; diff --git a/src/modules/mc_pos_control/PositionControl/PositionControl.hpp b/src/modules/mc_pos_control/PositionControl/PositionControl.hpp index 4384dfd74c..607ba65094 100644 --- a/src/modules/mc_pos_control/PositionControl/PositionControl.hpp +++ b/src/modules/mc_pos_control/PositionControl/PositionControl.hpp @@ -114,10 +114,17 @@ public: void setTiltLimit(const float tilt) { _lim_tilt = tilt; } /** - * Set the maximum tilt angle in radians the output attitude is allowed to have - * @param thrust [0,1] with which the vehicle hovers not aacelerating down or up with level orientation + * Set the normalized hover thrust + * @param thrust [0,1] with which the vehicle hovers not acelerating down or up with level orientation */ - void setHoverThrust(const float thrust) { _hover_thrust = thrust; } + void setHoverThrust(const float hover_thrust) { _hover_thrust = hover_thrust; } + + /** + * Update the hover thrust without immediately affecting the output + * by adjusting the integrator. This prevents propagating the dynamics + * of the hover thrust signal directly to the output of the controller. + */ + void updateHoverThrust(const float hover_thrust_new); /** * Pass the current vehicle state to the controller @@ -156,6 +163,11 @@ public: */ void resetIntegral() { _vel_int.setZero(); } + /** + * @return the value of the velocity integrator + */ + matrix::Vector3f getIntegral() const { return _vel_int; } + /** * Get the controllers output local position setpoint * These setpoints are the ones which were executed on including PID output and feed-forward. diff --git a/src/modules/mc_pos_control/PositionControl/PositionControlTest.cpp b/src/modules/mc_pos_control/PositionControl/PositionControlTest.cpp index c994f024e3..4645241680 100644 --- a/src/modules/mc_pos_control/PositionControl/PositionControlTest.cpp +++ b/src/modules/mc_pos_control/PositionControl/PositionControlTest.cpp @@ -326,3 +326,32 @@ TEST_F(PositionControlBasicTest, InvalidState) _position_control.setState(states); EXPECT_FALSE(runController()); } + + +TEST_F(PositionControlBasicTest, UpdateHoverThrust) +{ + // GIVEN: some hover thrust and 0 velocity change + const float hover_thrust = 0.6f; + _position_control.setHoverThrust(hover_thrust); + + _input_setpoint.vx = 0.f; + _input_setpoint.vy = 0.f; + _input_setpoint.vz = -0.f; + + // WHEN: we run the controller + EXPECT_TRUE(runController()); + + // THEN: the output thrust equals the hover thrust + EXPECT_EQ(_output_setpoint.thrust[2], -hover_thrust); + + // HOWEVER WHEN: we set a new hover thrust through the update function + const float hover_thrust_new = 0.7f; + _position_control.updateHoverThrust(hover_thrust_new); + EXPECT_TRUE(runController()); + + // THEN: the integral is updated to avoid discontinuities and + // the output is still the same + EXPECT_EQ(_output_setpoint.thrust[2], -hover_thrust); + const Vector3f integrator_new(_position_control.getIntegral()); + EXPECT_EQ(integrator_new(2) - hover_thrust_new, -hover_thrust); +} diff --git a/src/modules/mc_pos_control/mc_pos_control_main.cpp b/src/modules/mc_pos_control/mc_pos_control_main.cpp index add3c572a6..ce658bcb30 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -373,7 +373,7 @@ MulticopterPositionControl::parameters_update(bool force) mavlink_log_critical(&_mavlink_log_pub, "Hover thrust has been constrained by min/max"); } - _control.setHoverThrust(_param_mpc_thr_hover.get()); + _control.updateHoverThrust(_param_mpc_thr_hover.get()); } _flight_tasks.handleParameterUpdate(); @@ -415,7 +415,7 @@ MulticopterPositionControl::poll_subscriptions() hover_thrust_estimate_s hte; if (_hover_thrust_estimate_sub.update(&hte)) { - _control.setHoverThrust(hte.hover_thrust); + _control.updateHoverThrust(hte.hover_thrust); } } }