fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-14 23:43:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

l1_pos_control: prevent NaNs for roll setpoint

Browse Source
This commit is contained in:
Julian Oes
2014-02-24 12:42:48 +01:00
parent 9bed1677bd
commit ab8ece3961
1 changed files with 11 additions and 2 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/lib/ecl/l1/ecl_l1_pos_controller.cpp
+11 -2
View File
@@ -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 */