From f30f12341fba31452d5b9323308304a622d49e52 Mon Sep 17 00:00:00 2001 From: Matthias Grob Date: Wed, 8 May 2019 08:08:36 +0200 Subject: [PATCH] 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. --- src/modules/mc_pos_control/PositionControl.cpp | 3 +++ src/modules/mc_pos_control/Utility/ControlMath.cpp | 3 +-- 2 files changed, 4 insertions(+), 2 deletions(-) diff --git a/src/modules/mc_pos_control/PositionControl.cpp b/src/modules/mc_pos_control/PositionControl.cpp index a824fe9aa617..7de97f461d24 100644 --- a/src/modules/mc_pos_control/PositionControl.cpp +++ b/src/modules/mc_pos_control/PositionControl.cpp @@ -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); diff --git a/src/modules/mc_pos_control/Utility/ControlMath.cpp b/src/modules/mc_pos_control/Utility/ControlMath.cpp index b3cfa8afa1d2..655b0f9fda0f 100644 --- a/src/modules/mc_pos_control/Utility/ControlMath.cpp +++ b/src/modules/mc_pos_control/Utility/ControlMath.cpp @@ -57,8 +57,7 @@ vehicle_attitude_setpoint_s thrustToAttitude(const Vector3f &thr_sp, const float } else { // no thrust, set Z axis to safe value - body_z.zero(); - body_z(2) = 1.0f; + body_z = Vector3f(0.f, 0.f, 1.f); } // vector of desired yaw direction in XY plane, rotated by PI/2