fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-25 13:53:35 +08:00
Code Issues Packages Projects Releases Wiki Activity

ControlMath: fix index and don't adjust sign with method but rather function caller

Browse Source 
needs to adjust for sign
This commit is contained in:
Dennis Mannhart
2018-04-05 07:30:12 +02:00
committed by Beat Küng
parent 19d50f9511
commit 75b6af77de
1 changed files with 6 additions and 9 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/mc_pos_control/Utility/ControlMath.cpp
+6 -9
View File
@@ -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]) void constrainPIDu(matrix::Vector3f &u, bool sat[2], const float Ulimits[2], const float d[2])
{ {
sat[0] = sat[1] = false; 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(); float xy_mag = matrix::Vector2f(u(0), u(0)).length();
if (u(2) * u(2) >= Ulimits[0] * Ulimits[0]) { if (u(2) * u(2) >= Ulimits[0] * Ulimits[0]) {
/* The desired u in D-direction exceeds maximum */ /* The desired u in D-direction exceeds maximum */
/* Check if altitude saturated /* Check if altitude saturated */
* negative sign bc of N-E-D frame */ if (d[1] >= 0.0f) {
if (-d[1] >= 0.0f) {
sat[1] = true; 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(1) = 0.0f;
u(2) = Ulimits[0]; u(2) = Ulimits[0];
} if (u.length() >= Ulimits[0]) { } else if (u.length() >= Ulimits[0]) {
/* The desired u_xy exceeds maximum */ /* 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 */ * if desired u_z is below minimum */
sat[0] = true; sat[0] = true;
/* Check if altitude is saturated /* Check if altitude is saturated */
* negative sign bc of N-E-D frame */ if (d[1] <= 0.0f) {
if (-d[1] <= 0.0f) {
sat[1] = true; sat[1] = true;
} }