From 5253b8a3ee157ab0ba6621283e8f84f6daafddf4 Mon Sep 17 00:00:00 2001 From: James O'Shannessy <12959316+joshanne@users.noreply.github.com> Date: Thu, 29 Apr 2021 19:24:16 +1000 Subject: [PATCH] ArduPlane: Fix spiralling loiters when no fence return point can be found --- ArduPlane/fence.cpp | 8 +++++--- 1 file changed, 5 insertions(+), 3 deletions(-) diff --git a/ArduPlane/fence.cpp b/ArduPlane/fence.cpp index 5deb728bde..fdbd87e4ed 100644 --- a/ArduPlane/fence.cpp +++ b/ArduPlane/fence.cpp @@ -92,9 +92,11 @@ void Plane::fence_check() guided_WP_loc.lat = return_point[0]; guided_WP_loc.lng = return_point[1]; } else { - // should. not. happen. - guided_WP_loc.lat = current_loc.lat; - guided_WP_loc.lng = current_loc.lng; + // When no fence return point is found (ie. no inclusion fence uploaded, but exclusion is) + // we fail to obtain a valid fence return point. In this case, home is considered a safe + // return point. + guided_WP_loc.lat = home.lat; + guided_WP_loc.lng = home.lng; } }