From 75b6af77de63588c52dabf3befb0ea3b8d3697ff Mon Sep 17 00:00:00 2001 From: Dennis Mannhart Date: Wed, 20 Dec 2017 16:36:57 +0100 Subject: [PATCH] ControlMath: fix index and don't adjust sign with method but rather function caller needs to adjust for sign --- .../mc_pos_control/Utility/ControlMath.cpp | 15 ++++++--------- 1 file changed, 6 insertions(+), 9 deletions(-) diff --git a/src/modules/mc_pos_control/Utility/ControlMath.cpp b/src/modules/mc_pos_control/Utility/ControlMath.cpp index 6e45d8a02a..c8012c898c 100644 --- a/src/modules/mc_pos_control/Utility/ControlMath.cpp +++ b/src/modules/mc_pos_control/Utility/ControlMath.cpp @@ -109,16 +109,14 @@ matrix::Vector3f constrainTilt(const matrix::Vector3f &vec, const float &tilt_ma void constrainPIDu(matrix::Vector3f &u, bool sat[2], const float Ulimits[2], const float d[2]) { sat[0] = sat[1] = false; - float xy_max = sqrtf(Ulimits[2] * Ulimits[2] - u(2) * u(2)); + float xy_max = sqrtf(Ulimits[0] * Ulimits[0] - u(2) * u(2)); float xy_mag = matrix::Vector2f(u(0), u(0)).length(); - if (u(2) * u(2) >= Ulimits[0] * Ulimits[0]) { /* The desired u in D-direction exceeds maximum */ - /* Check if altitude saturated - * negative sign bc of N-E-D frame */ - if (-d[1] >= 0.0f) { + /* Check if altitude saturated */ + if (d[1] >= 0.0f) { sat[1] = true; } @@ -127,7 +125,7 @@ void constrainPIDu(matrix::Vector3f &u, bool sat[2], const float Ulimits[2], con u(1) = 0.0f; u(2) = Ulimits[0]; - } if (u.length() >= Ulimits[0]) { + } else if (u.length() >= Ulimits[0]) { /* The desired u_xy exceeds maximum */ @@ -145,9 +143,8 @@ void constrainPIDu(matrix::Vector3f &u, bool sat[2], const float Ulimits[2], con * if desired u_z is below minimum */ sat[0] = true; - /* Check if altitude is saturated - * negative sign bc of N-E-D frame */ - if (-d[1] <= 0.0f) { + /* Check if altitude is saturated */ + if (d[1] <= 0.0f) { sat[1] = true; }