From ab8ece3961f50348cf4bf464cb7d953b182a0a44 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Mon, 24 Feb 2014 12:42:48 +0100 Subject: l1_pos_control: prevent NaNs for roll setpoint --- src/lib/ecl/l1/ecl_l1_pos_controller.cpp | 13 +++++++++++-- 1 file changed, 11 insertions(+), 2 deletions(-) (limited to 'src/lib') 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 + #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 */ -- cgit v1.2.3