Browse Source

Merge pull request #497 from thomasgubler/l1_sine_eta1

l1: fix constrain of sine_eta1
sbg
Lorenz Meier 11 years ago
parent
commit
0b89051be1
  1. 2
      src/lib/ecl/l1/ecl_l1_pos_controller.cpp

2
src/lib/ecl/l1/ecl_l1_pos_controller.cpp

@ -190,7 +190,7 @@ void ECL_L1_Pos_Controller::navigate_waypoints(const math::Vector2f &vector_A, c @@ -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 sine_eta1 = xtrackErr / math::max(_L1_distance , 0.1f);
/* 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);
eta = eta1 + eta2;
/* bearing from current position to L1 point */

Loading…
Cancel
Save