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

ControlMath constrainxy: fix one of the condition

Browse Source
This commit is contained in:
Dennis Mannhart
2018-03-09 11:57:52 +01:00
committed by Lorenz Meier
parent 386b70dc58
commit c60481614e
1 changed files with 2 additions and 4 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/mc_pos_control/Utility/ControlMath.cpp
+2 -4
View File
@@ -139,7 +139,7 @@ matrix::Vector2f constrainXY(const matrix::Vector2f &v0, const matrix::Vector2f
} else if (v0.length() < 0.001f) {
/* The first vector is 0. */
return v0 * 0.0f;
return v1.normalized() * max;
} else {
/*
@@ -181,12 +181,10 @@ matrix::Vector2f constrainXY(const matrix::Vector2f &v0, const matrix::Vector2f
* - s (=scaling factor) needs to be positive
* - (max - ||v||) always larger than zero, otherwise it never entered this if-statement
* */
float s = 0.0f;
matrix::Vector2f u1 = v1.normalized();
float m = u1.dot(v0);
float c = v0.length() * v0.length() - max * max;
s = -m + sqrtf(m * m - c);
float s = -m + sqrtf(m * m - c);
return v0 + u1 * s;
}
}