From e4b4d0064508b51f9d3867d41555e1225b72b774 Mon Sep 17 00:00:00 2001 From: mattbooker Date: Tue, 6 Sep 2022 21:56:33 -0400 Subject: [PATCH] Plane: Fixed divide by zero error when transitioning to guided --- ArduPlane/quadplane.cpp | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/ArduPlane/quadplane.cpp b/ArduPlane/quadplane.cpp index e6b07a2ac2..0805c74b69 100644 --- a/ArduPlane/quadplane.cpp +++ b/ArduPlane/quadplane.cpp @@ -2501,7 +2501,11 @@ void QuadPlane::vtol_position_controller(void) const float distance = diff_wp.length(); const Vector2f rel_groundspeed_vector = landing_closing_velocity(); const float rel_groundspeed_sq = rel_groundspeed_vector.length_squared(); - const float closing_groundspeed = rel_groundspeed_vector * diff_wp.normalized(); + float closing_groundspeed = 0; + + if (distance > 0.1) { + closing_groundspeed = rel_groundspeed_vector * diff_wp.normalized(); + } // calculate speed we should be at to reach the position2 // target speed at the position2 distance threshold, assuming