dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

positionControl: add check on sign before sqrtf

This commit is contained in:
Nicolas Martin 2020-10-22 15:58:13 +02:00 committed by Daniel Agar
parent 5b2f41de12
commit 4b0038bd43
1 changed files with 6 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

7
src/modules/mc_pos_control/PositionControl/PositionControl.cpp
View File

@ -164,7 +164,12 @@ void PositionControl::_velocityControl(const float dt)
// Get allowed horizontal thrust after prioritizing vertical control
const float thrust_max_squared = _lim_thr_max * _lim_thr_max;
const float thrust_z_squared = _thr_sp(2) * _thr_sp(2);
float thrust_max_xy = sqrtf(thrust_max_squared - thrust_z_squared);
const float thrust_max_xy_squared = thrust_max_squared - thrust_z_squared;
float thrust_max_xy = 0;
if (thrust_max_xy_squared > 0) {
thrust_max_xy = sqrtf(thrust_max_xy_squared);
}
// Saturate thrust in horizontal direction
const Vector2f thrust_sp_xy(_thr_sp);