diff options
Diffstat (limited to 'src/lib/ecl')
-rw-r--r-- | src/lib/ecl/ecl.h | 3 | ||||
-rw-r--r-- | src/lib/ecl/l1/ecl_l1_pos_controller.cpp | 13 |
2 files changed, 12 insertions, 4 deletions
diff --git a/src/lib/ecl/ecl.h b/src/lib/ecl/ecl.h index e0f207696..aa3c5000a 100644 --- a/src/lib/ecl/ecl.h +++ b/src/lib/ecl/ecl.h @@ -38,7 +38,6 @@ */ #include <drivers/drv_hrt.h> -#include <geo/geo.h> #define ecl_absolute_time hrt_absolute_time -#define ecl_elapsed_time hrt_elapsed_time
\ No newline at end of file +#define ecl_elapsed_time hrt_elapsed_time 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 */ |