fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-11-03 05:01:36 +08:00
Code Issues Packages Projects Releases Wiki Activity

ControlMath: use sign for direction

Browse Source
This commit is contained in:
Dennis Mannhart
2018-04-05 07:30:12 +02:00
committed by Beat Küng
parent b0b72e63e7
commit 1cad36efb9
1 changed files with 3 additions and 2 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/mc_pos_control/Utility/ControlMath.cpp
+3 -2
View File
@@ -43,6 +43,7 @@
#include "ControlMath.hpp" #include "ControlMath.hpp"
#include <platforms/px4_defines.h> #include <platforms/px4_defines.h>
#include <float.h> #include <float.h>
#include <mathlib/mathlib.h>
namespace ControlMath namespace ControlMath
{ {
@@ -124,7 +125,7 @@ void constrainPIDu(matrix::Vector3f &u, bool stop_I[2], const float Ulimits[2],
stop_I[0] = true; stop_I[0] = true;
u(0) = 0.0f; u(0) = 0.0f;
u(1) = 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]) { } 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) * since we do not know better. (no direction given)
*/ */
if (u.length() < 0.0001f) { 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 { } else {
u = u.normalized() * Ulimits[1]; u = u.normalized() * Ulimits[1];