diff options
| author | Julian Oes <julian@oes.ch> | 2014-02-24 12:42:48 +0100 |
|---|---|---|
| committer | Julian Oes <julian@oes.ch> | 2014-02-24 12:42:48 +0100 |
| commit | ab8ece3961f50348cf4bf464cb7d953b182a0a44 (patch) | |
| tree | 146b770e82cc394b9d174a95b14146ad668bd0e9 /src | |
| parent | 9bed1677bd5bfd6b0f07d78e5bdcfacec34564a2 (diff) | |
| download | px4-firmware-ab8ece3961f50348cf4bf464cb7d953b182a0a44.tar.gz px4-firmware-ab8ece3961f50348cf4bf464cb7d953b182a0a44.tar.bz2 px4-firmware-ab8ece3961f50348cf4bf464cb7d953b182a0a44.zip | |
l1_pos_control: prevent NaNs for roll setpoint
Diffstat (limited to 'src')
| -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 */ |