From cdf37ca5571880fa13627bb4facda2f990281251 Mon Sep 17 00:00:00 2001 From: Matthias Grob Date: Fri, 20 Mar 2020 11:50:25 +0100 Subject: [PATCH] PositionControl: deconflict hover thrust estimator, acceleration control - Avoid constantly adjusting the velocity gains with the HTE - Make sure the hover thrust integral update doesn't break even though its unit is acceleration and not unit thrust anymore We need to convert the velocity gains to not contain/depend on the hover thrust. In horizontal direction it doesn't make sense to scale them with the hover thrust and in vertical direction the adjustments are already done in the acceleration to collective thrust conversion. --- .../mc_pos_control/PositionControl/PositionControl.cpp | 8 +++----- .../mc_pos_control/PositionControl/PositionControl.hpp | 7 +------ .../PositionControl/PositionControlTest.cpp | 4 +--- src/modules/mc_pos_control/mc_pos_control_main.cpp | 9 ++++++--- 4 files changed, 11 insertions(+), 17 deletions(-) diff --git a/src/modules/mc_pos_control/PositionControl/PositionControl.cpp b/src/modules/mc_pos_control/PositionControl/PositionControl.cpp index 6f00dd1e25..70d23c7885 100644 --- a/src/modules/mc_pos_control/PositionControl/PositionControl.cpp +++ b/src/modules/mc_pos_control/PositionControl/PositionControl.cpp @@ -67,7 +67,7 @@ void PositionControl::setThrustLimits(const float min, const float max) void PositionControl::updateHoverThrust(const float hover_thrust_new) { - _vel_int(2) += hover_thrust_new - _hover_thrust; + _vel_int(2) += (hover_thrust_new - _hover_thrust) * (CONSTANTS_ONE_G / hover_thrust_new); setHoverThrust(hover_thrust_new); } @@ -147,8 +147,6 @@ void PositionControl::_velocityControl(const float dt) Vector3f vel_error = _vel_sp - _vel; Vector3f acc_sp_velocity = vel_error.emult(_gain_vel_p) + _vel_int - _vel_dot.emult(_gain_vel_d); - // For backwards compatibility of the gains to non-acceleration based control, needs to be overcome with configuration conversion - acc_sp_velocity *= CONSTANTS_ONE_G / _hover_thrust; // No control input from setpoints or corresponding states which are NAN ControlMath::addIfNotNanVector3f(_acc_sp, acc_sp_velocity); @@ -179,7 +177,7 @@ void PositionControl::_velocityControl(const float dt) // Use tracking Anti-Windup for horizontal direction: during saturation, the integrator is used to unsaturate the output // see Anti-Reset Windup for PID controllers, L.Rundqwist, 1990 const Vector2f acc_sp_xy_limited = Vector2f(_thr_sp) * (CONSTANTS_ONE_G / _hover_thrust); - const float arw_gain = 2.f / (_gain_vel_p(0) * (CONSTANTS_ONE_G / _hover_thrust)); + const float arw_gain = 2.f / _gain_vel_p(0); vel_error.xy() = Vector2f(vel_error) - (arw_gain * (Vector2f(_acc_sp) - acc_sp_xy_limited)); // Make sure integral doesn't get NAN @@ -188,7 +186,7 @@ void PositionControl::_velocityControl(const float dt) _vel_int += vel_error.emult(_gain_vel_i) * dt; // limit thrust integral - _vel_int(2) = math::min(fabsf(_vel_int(2)), _lim_thr_max) * sign(_vel_int(2)); + _vel_int(2) = math::min(fabsf(_vel_int(2)), CONSTANTS_ONE_G) * sign(_vel_int(2)); } void PositionControl::_accelerationControl() diff --git a/src/modules/mc_pos_control/PositionControl/PositionControl.hpp b/src/modules/mc_pos_control/PositionControl/PositionControl.hpp index a581ec3529..4986c93a86 100644 --- a/src/modules/mc_pos_control/PositionControl/PositionControl.hpp +++ b/src/modules/mc_pos_control/PositionControl/PositionControl.hpp @@ -163,11 +163,6 @@ 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. @@ -205,7 +200,7 @@ private: float _lim_thr_max{}; ///< Maximum collective thrust allowed as output [-1,0] e.g. -0.1 float _lim_tilt{}; ///< Maximum tilt from level the output attitude is allowed to have - float _hover_thrust{}; ///< Thrust [0,1] with which the vehicle hovers not aacelerating down or up with level orientation + float _hover_thrust{}; ///< Thrust [0,1] with which the vehicle hovers not accelerating down or up with level orientation // States matrix::Vector3f _pos; /**< current position */ diff --git a/src/modules/mc_pos_control/PositionControl/PositionControlTest.cpp b/src/modules/mc_pos_control/PositionControl/PositionControlTest.cpp index 9a42f198e3..1facfc9234 100644 --- a/src/modules/mc_pos_control/PositionControl/PositionControlTest.cpp +++ b/src/modules/mc_pos_control/PositionControl/PositionControlTest.cpp @@ -76,7 +76,7 @@ public: PositionControlBasicTest() { _position_control.setPositionGains(Vector3f(1.f, 1.f, 1.f)); - _position_control.setVelocityGains(Vector3f(1.f, 1.f, 1.f), Vector3f(1.f, 1.f, 1.f), 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.setTiltLimit(1.f); @@ -387,6 +387,4 @@ TEST_F(PositionControlBasicTest, UpdateHoverThrust) // 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 2ad528ec54..520c8ad3dd 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -362,9 +362,12 @@ MulticopterPositionControl::parameters_update(bool force) } _control.setPositionGains(Vector3f(_param_mpc_xy_p.get(), _param_mpc_xy_p.get(), _param_mpc_z_p.get())); - _control.setVelocityGains(Vector3f(_param_mpc_xy_vel_p.get(), _param_mpc_xy_vel_p.get(), _param_mpc_z_vel_p.get()), - Vector3f(_param_mpc_xy_vel_i.get(), _param_mpc_xy_vel_i.get(), _param_mpc_z_vel_i.get()), - Vector3f(_param_mpc_xy_vel_d.get(), _param_mpc_xy_vel_d.get(), _param_mpc_z_vel_d.get())); + // backwards compatibility scale for velocity gains to non-acceleration based control, needs to be overcome with configuration conversion + const float hover_scale = 20.f; + _control.setVelocityGains(Vector3f(_param_mpc_xy_vel_p.get(), _param_mpc_xy_vel_p.get(), + _param_mpc_z_vel_p.get()) * hover_scale, + Vector3f(_param_mpc_xy_vel_i.get(), _param_mpc_xy_vel_i.get(), _param_mpc_z_vel_i.get()) * hover_scale, + Vector3f(_param_mpc_xy_vel_d.get(), _param_mpc_xy_vel_d.get(), _param_mpc_z_vel_d.get()) * hover_scale); _control.setVelocityLimits(_param_mpc_xy_vel_max.get(), _param_mpc_z_vel_max_up.get(), _param_mpc_z_vel_max_dn.get()); _control.setThrustLimits(_param_mpc_thr_min.get(), _param_mpc_thr_max.get()); _control.setTiltLimit(M_DEG_TO_RAD_F * _param_mpc_tiltmax_air.get()); // convert to radians!