ArduPlane: Fix spiralling loiters when no fence return point can be found

This commit is contained in:
James O'Shannessy 2021-04-29 19:24:16 +10:00 committed by Peter Barker
parent 6d8d35f272
commit 5253b8a3ee

View File

@ -92,9 +92,11 @@ void Plane::fence_check()
guided_WP_loc.lat = return_point[0]; guided_WP_loc.lat = return_point[0];
guided_WP_loc.lng = return_point[1]; guided_WP_loc.lng = return_point[1];
} else { } else {
// should. not. happen. // When no fence return point is found (ie. no inclusion fence uploaded, but exclusion is)
guided_WP_loc.lat = current_loc.lat; // we fail to obtain a valid fence return point. In this case, home is considered a safe
guided_WP_loc.lng = current_loc.lng; // return point.
guided_WP_loc.lat = home.lat;
guided_WP_loc.lng = home.lng;
} }
} }