Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-03-03 20:23:58 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_L1_Control: prevent another NaN in L1

Browse Source
This commit is contained in:
Andrew Tridgell 2014-08-14 14:09:09 +10:00
parent 85213dccbf
commit df3c565cd8
1 changed files with 3 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
libraries/AP_L1_Control/AP_L1_Control.cpp
View File

@ -169,6 +169,9 @@ void AP_L1_Control::update_waypoint(const struct Location &prev_WP, const struct
// if too small // if too small
if (AB.length() < 1.0e-6f) { if (AB.length() < 1.0e-6f) {
AB = location_diff(_current_loc, next_WP); AB = location_diff(_current_loc, next_WP);
if (AB.length() < 1.0e-6f) {
AB = Vector2f(cosf(_ahrs.yaw), sinf(_ahrs.yaw));
}
} }
AB.normalize(); AB.normalize();