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

ControlMath: index fix and scaling fix

Browse Source
This commit is contained in:
Dennis Mannhart
2017-12-20 15:41:13 +01:00
committed by Beat Küng
parent c9cd920642
commit 19d50f9511
1 changed files with 3 additions and 3 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/mc_pos_control/Utility/ControlMath.cpp
+3 -3
View File
@@ -75,12 +75,12 @@ matrix::Vector3f constrainTilt(const matrix::Vector3f &vec, const float &tilt_ma
}
/* desired and maximum allowed horizontal magnitude */
float xy_mag = matrix::Vector2f(vec(0), vec(0)).length();
float xy_mag = matrix::Vector2f(vec(0), vec(1)).length();
float xy_mag_max = fabsf(vec(2)) * tanf(tilt_max);
if (xy_mag_max < xy_mag) {
float x0 = vec(0) * xy_mag / xy_mag_max;
float x1 = vec(1) * xy_mag / xy_mag_max;
float x0 = vec(0) * xy_mag_max / xy_mag;
float x1 = vec(1) * xy_mag_max / xy_mag;
return matrix::Vector3f(x0, x1, vec(2));
}