fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-11-05 05:11:35 +08:00
Code Issues Packages Projects Releases Wiki Activity

PositionControl: fix horizontal integral anti-windup

Browse Source
This commit is contained in:
Matthias Grob
2020-03-30 09:58:39 +02:00
parent 91057fe024
commit 58fd3e2ddc
1 changed files with 3 additions and 2 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/mc_pos_control/PositionControl/PositionControl.cpp
+3 -2
View File
@@ -178,8 +178,9 @@ void PositionControl::_velocityControl(const float dt)
// Use tracking Anti-Windup for horizontal direction: during saturation, the integrator is used to unsaturate the output // 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 // see Anti-Reset Windup for PID controllers, L.Rundqwist, 1990
const float arw_gain = 2.f / _gain_vel_p(0); const Vector2f acc_sp_xy_limited = Vector2f(_thr_sp) * (CONSTANTS_ONE_G / _hover_thrust);
vel_error.xy() = Vector2f(vel_error) - (arw_gain * (thrust_sp_xy - Vector2f(_thr_sp))); const float arw_gain = 2.f / (_gain_vel_p(0) * (CONSTANTS_ONE_G / _hover_thrust));
vel_error.xy() = Vector2f(vel_error) - (arw_gain * (Vector2f(_acc_sp) - acc_sp_xy_limited));
// Make sure integral doesn't get NAN // Make sure integral doesn't get NAN
ControlMath::setZeroIfNanVector3f(vel_error); ControlMath::setZeroIfNanVector3f(vel_error);