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

FixedwingPositionControl: fix air_gnd_angle judgement condition.

Browse Source 
air_gnd_angle is from acos(), and never return a value that bigger than
M_PI_F
This commit is contained in:
ToppingXu 2018-03-02 16:14:55 +08:00 committed by Lorenz Meier
parent d39a8b5a48
commit e340f6fd7a
1 changed files with 1 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp
View File

@ -658,7 +658,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> &curr_pos, cons
float air_gnd_angle = acosf((air_speed_2d * ground_speed) / (air_speed_2d.length() * ground_speed.length()));
// if angle > 90 degrees or groundspeed is less than threshold, replace groundspeed with airspeed projection
if ((fabsf(air_gnd_angle) > M_PI_F) || (ground_speed.length() < 3.0f)) {
if ((fabsf(air_gnd_angle) > M_PI_2_F) || (ground_speed.length() < 3.0f)) {
nav_speed_2d = air_speed_2d;
} else {