From 1cad36efb9c2ec8d44ff759d722cab802839992f Mon Sep 17 00:00:00 2001
From: Dennis Mannhart <dennis.mannhart@gmail.com>
Date: Thu, 28 Dec 2017 09:57:29 +0100
Subject: [PATCH] ControlMath: use sign for direction

---
 src/modules/mc_pos_control/Utility/ControlMath.cpp | 5 +++--
 1 file changed, 3 insertions(+), 2 deletions(-)

diff --git a/src/modules/mc_pos_control/Utility/ControlMath.cpp b/src/modules/mc_pos_control/Utility/ControlMath.cpp
index b39e0d1c1b..9870d7f22e 100644
--- a/src/modules/mc_pos_control/Utility/ControlMath.cpp
+++ b/src/modules/mc_pos_control/Utility/ControlMath.cpp
@@ -43,6 +43,7 @@
 #include "ControlMath.hpp"
 #include <platforms/px4_defines.h>
 #include <float.h>
+#include <mathlib/mathlib.h>
 
 namespace ControlMath
 {
@@ -124,7 +125,7 @@ void constrainPIDu(matrix::Vector3f &u, bool stop_I[2], const float Ulimits[2],
 		stop_I[0] = true;
 		u(0) = 0.0f;
 		u(1) = 0.0f;
-		u(2) = -Ulimits[0]; //N-E-D frame
+		u(2) = math::sign(u(2)) * Ulimits[0];
 
 	} else if (u.length() >= Ulimits[0]) {
 
@@ -151,7 +152,7 @@ void constrainPIDu(matrix::Vector3f &u, bool stop_I[2], const float Ulimits[2],
 		 * since we do not know better. (no direction given)
 		 */
 		if (u.length() < 0.0001f) {
-			u = matrix::Vector3f(0.0f, 0.0f, -Ulimits[1]);
+			u = matrix::Vector3f(0.0f, 0.0f, math::sign(u(2)) * Ulimits[1]);
 
 		} else {
 			u = u.normalized() * Ulimits[1];