mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-03-03 04:03:59 -04:00
AP_L1_Control: prevent another NaN in L1
This commit is contained in:
parent
85213dccbf
commit
df3c565cd8
@ -169,6 +169,9 @@ void AP_L1_Control::update_waypoint(const struct Location &prev_WP, const struct
|
||||
// if too small
|
||||
if (AB.length() < 1.0e-6f) {
|
||||
AB = location_diff(_current_loc, next_WP);
|
||||
if (AB.length() < 1.0e-6f) {
|
||||
AB = Vector2f(cosf(_ahrs.yaw), sinf(_ahrs.yaw));
|
||||
}
|
||||
}
|
||||
AB.normalize();
|
||||
|
||||
|
Loading…
Reference in New Issue
Block a user