From 02c4e0361c49fbe09e27d3d0cf6820c4c1ae8381 Mon Sep 17 00:00:00 2001 From: bresch Date: Mon, 25 Jul 2022 16:19:07 +0200 Subject: [PATCH] MCPosControl: fix horizontal anti-reset windup algirithm Since the horizontal and vertical velocity controllers are now decoupled, it can be that the horizontal acceleration produced by the controller is actually greater than the desired one (by design). This condition would actually make the ARW run "backwards", degrading the controller performance. --- .../PositionControl/PositionControl.cpp | 11 +++++++++-- 1 file changed, 9 insertions(+), 2 deletions(-) diff --git a/src/modules/mc_pos_control/PositionControl/PositionControl.cpp b/src/modules/mc_pos_control/PositionControl/PositionControl.cpp index 8dd2c521e7..b770ab5a68 100644 --- a/src/modules/mc_pos_control/PositionControl/PositionControl.cpp +++ b/src/modules/mc_pos_control/PositionControl/PositionControl.cpp @@ -181,9 +181,16 @@ 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 Vector2f acc_sp_xy_produced = Vector2f(_thr_sp) * (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)); + + // The produced acceleration can be greater or smaller than the desired acceleration due to the saturations and the actual vertical thrust (computed independently). + // The ARW loop needs to run if the signal is saturated only. + const Vector2f acc_sp_xy = _acc_sp.xy(); + const Vector2f acc_limited_xy = (acc_sp_xy.norm_squared() > acc_sp_xy_produced.norm_squared()) + ? acc_sp_xy_produced + : acc_sp_xy; + vel_error.xy() = Vector2f(vel_error) - arw_gain * (acc_sp_xy - acc_limited_xy); // Make sure integral doesn't get NAN ControlMath::setZeroIfNanVector3f(vel_error);