From f30f12341fba31452d5b9323308304a622d49e52 Mon Sep 17 00:00:00 2001
From: Matthias Grob <maetugr@gmail.com>
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 a824fe9aa6..7de97f461d 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 b3cfa8afa1..655b0f9fda 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
--
GitLab