yemai/bizhang_-obav
1
0
Fork 0
mirror of https://gitee.com/xiaohuolufeihua/bizhang_-obav.git synced 2026-05-22 01:12:31 +00:00
Code Issues Packages Projects Releases Wiki Activity

Add "Tracking Anti-Reset Windup" in velocity PID controller

Browse Source
This commit is contained in:
bresch
2018-10-03 16:46:55 +02:00
committed by Roman Bapst
parent 88f6271061
commit 0863e95a2c
1 changed files with 9 additions and 17 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -291,25 +291,17 @@ void PositionControl::_velocityController(const float &dt)
_thr_sp(1) = thrust_desired_NE(1) / mag * thrust_max_NE;
}
// Get the direction of (r-y) in NE-direction.
float direction_NE = Vector2f(vel_err) * Vector2f(_vel_sp);
// Use tracking Anti-Windup for NE-direction: during saturation, the integrator is used to unsaturate the output
// see Anti-Reset Windup for PID controllers, L.Rundqwist, 1990
float arw_gain = 2.f / MPC_XY_VEL_P.get();
// Apply Anti-Windup in NE-direction.
bool stop_integral_NE = (thrust_desired_NE * thrust_desired_NE >= thrust_max_NE * thrust_max_NE &&
direction_NE >= 0.0f);
Vector2f vel_err_lim;
vel_err_lim(0) = vel_err(0) - (thrust_desired_NE(0) - _thr_sp(0)) * arw_gain;
vel_err_lim(1) = vel_err(1) - (thrust_desired_NE(1) - _thr_sp(1)) * arw_gain;
if (!stop_integral_NE) {
_thr_int(0) += vel_err(0) * MPC_XY_VEL_I.get() * dt;
_thr_int(1) += vel_err(1) * MPC_XY_VEL_I.get() * dt;
// magnitude of thrust integral can never exceed maximum throttle in NE
float integral_mag_NE = Vector2f(_thr_int).length();
if (integral_mag_NE > 0.0f && integral_mag_NE > thrust_max_NE) {
_thr_int(0) = _thr_int(0) / integral_mag_NE * thrust_max_NE;
_thr_int(1) = _thr_int(1) / integral_mag_NE * thrust_max_NE;
}
}
// Update integral
_thr_int(0) += MPC_XY_VEL_I.get() * vel_err_lim(0) * dt;
_thr_int(1) += MPC_XY_VEL_I.get() * vel_err_lim(1) * dt;
}
}