diff options
| author | Anton Babushkin <anton.babushkin@me.com> | 2014-02-24 23:49:36 +0400 |
|---|---|---|
| committer | Anton Babushkin <anton.babushkin@me.com> | 2014-02-24 23:49:36 +0400 |
| commit | 9ffc70de405fb3e85e6367c4e875747315472aae (patch) | |
| tree | 344065f98e2471b96938e57b073874b5cadc10fd /src/lib/ecl/l1/ecl_l1_pos_controller.cpp | |
| parent | d8fdade6aba5e0bd2b56c206d09da4a92fda5fa0 (diff) | |
| parent | 926c4701c71fb2689025decbc454d14c6df85e76 (diff) | |
| download | px4-firmware-9ffc70de405fb3e85e6367c4e875747315472aae.tar.gz px4-firmware-9ffc70de405fb3e85e6367c4e875747315472aae.tar.bz2 px4-firmware-9ffc70de405fb3e85e6367c4e875747315472aae.zip | |
Merge branch 'beta_mavlink' into beta_mavlink2
Diffstat (limited to 'src/lib/ecl/l1/ecl_l1_pos_controller.cpp')
| -rw-r--r-- | src/lib/ecl/l1/ecl_l1_pos_controller.cpp | 13 |
1 files changed, 11 insertions, 2 deletions
diff --git a/src/lib/ecl/l1/ecl_l1_pos_controller.cpp b/src/lib/ecl/l1/ecl_l1_pos_controller.cpp index 3b68a0a4e..d1c864d78 100644 --- a/src/lib/ecl/l1/ecl_l1_pos_controller.cpp +++ b/src/lib/ecl/l1/ecl_l1_pos_controller.cpp @@ -38,6 +38,8 @@ * */ +#include <float.h> + #include "ecl_l1_pos_controller.h" float ECL_L1_Pos_Controller::nav_roll() @@ -231,8 +233,15 @@ void ECL_L1_Pos_Controller::navigate_loiter(const math::Vector<2> &vector_A, con /* calculate the vector from waypoint A to current position */ math::Vector<2> vector_A_to_airplane = get_local_planar_vector(vector_A, vector_curr_position); - /* store the normalized vector from waypoint A to current position */ - math::Vector<2> vector_A_to_airplane_unit = (vector_A_to_airplane).normalized(); + math::Vector<2> vector_A_to_airplane_unit; + + /* prevent NaN when normalizing */ + if (vector_A_to_airplane.length() > FLT_EPSILON) { + /* store the normalized vector from waypoint A to current position */ + vector_A_to_airplane_unit = vector_A_to_airplane.normalized(); + } else { + vector_A_to_airplane_unit = vector_A_to_airplane; + } /* calculate eta angle towards the loiter center */ |