PositionControl: tiny minimal thrust length

To be able to still infer the direction of the thrust vector we
limit it to a minimal length even if MPC_THR_MIN is set to zero.

Note: This is a hotfix for certain specific applications.
The direction of the thrust vector in this corner case is very
likely to get into the tilt limit which is generally undesired.
This commit is contained in:
Matthias Grob
2019-05-08 08:08:36 +02:00
committed by Lorenz Meier
parent b35be4b0a6
commit f30f12341f
2 changed files with 4 additions and 2 deletions
@@ -265,6 +265,9 @@ void PositionControl::_velocityController(const float &dt)
float uMax = -_param_mpc_thr_min.get();
float uMin = -_param_mpc_thr_max.get();
// make sure there's always enough thrust vector length to infer the attitude
uMax = math::min(uMax, -10e-4f);
// Apply Anti-Windup in D-direction.
bool stop_integral_D = (thrust_desired_D >= uMax && vel_err(2) >= 0.0f) ||
(thrust_desired_D <= uMin && vel_err(2) <= 0.0f);