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

l1: fix constrain of sine_eta1

This commit is contained in:
Thomas Gubler 2013-10-27 14:48:12 +01:00
parent 59c04adada
commit 4b63c54885
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/lib/ecl/l1/ecl_l1_pos_controller.cpp
View File

@ -190,7 +190,7 @@ void ECL_L1_Pos_Controller::navigate_waypoints(const math::Vector2f &vector_A, c
float xtrackErr = vector_A_to_airplane % vector_AB; float xtrackErr = vector_A_to_airplane % vector_AB;
float sine_eta1 = xtrackErr / math::max(_L1_distance , 0.1f); float sine_eta1 = xtrackErr / math::max(_L1_distance , 0.1f);
/* limit output to 45 degrees */ /* limit output to 45 degrees */
sine_eta1 = math::constrain(sine_eta1, -M_PI_F / 4.0f, +M_PI_F / 4.0f); sine_eta1 = math::constrain(sine_eta1, -0.7071f, 0.7071f); //sin(pi/4) = 0.7071
float eta1 = asinf(sine_eta1); float eta1 = asinf(sine_eta1);
eta = eta1 + eta2; eta = eta1 + eta2;
/* bearing from current position to L1 point */ /* bearing from current position to L1 point */