From e340f6fd7a7eae577ddf6172672773c7aa39ba5b Mon Sep 17 00:00:00 2001
From: ToppingXu <toppingxu@outlook.com>
Date: Fri, 2 Mar 2018 16:14:55 +0800
Subject: [PATCH] FixedwingPositionControl: fix air_gnd_angle judgement
 condition.

air_gnd_angle is from acos(), and never return a value that bigger than
M_PI_F
---
 src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp b/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp
index 9c3f0b6540..7ee026e643 100644
--- a/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp
+++ b/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp
@@ -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 {