fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-25 06:43:35 +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 21:57:11 +01:00
committed by Lorenz Meier
parent d39a8b5a48
commit e340f6fd7a
1 changed files with 1 additions and 1 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp
+1 -1
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())); 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 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; nav_speed_2d = air_speed_2d;
} else { } else {