dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

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.
This commit is contained in:
bresch 2022-07-25 16:19:07 +02:00 committed by Mathieu Bresciani
parent 87a5705960
commit 02c4e0361c
1 changed files with 9 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

11
src/modules/mc_pos_control/PositionControl/PositionControl.cpp
View File

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