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); } } }