Browse Source

FixedwingPositionControl: fix air_gnd_angle judgement condition.

air_gnd_angle is from acos(), and never return a value that bigger than
M_PI_F
sbg
ToppingXu 7 years ago committed by Lorenz Meier
parent
commit
e340f6fd7a
  1. 2
      src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp

2
src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp

@ -658,7 +658,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> &curr_pos, cons @@ -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 {

Loading…
Cancel
Save