From fd19ff3bea06ddb688c9af73bd47f8633f68b71c Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Fri, 4 Jun 2021 18:35:23 +1000 Subject: [PATCH] Plane: offset guided start point when using Q_GUIDED_MODE --- ArduPlane/mode_guided.cpp | 8 ++++++++ 1 file changed, 8 insertions(+) diff --git a/ArduPlane/mode_guided.cpp b/ArduPlane/mode_guided.cpp index 06b0d3d002..72277fc56c 100644 --- a/ArduPlane/mode_guided.cpp +++ b/ArduPlane/mode_guided.cpp @@ -9,6 +9,14 @@ bool ModeGuided::_enter() location. This matches the behaviour of the copter code */ plane.guided_WP_loc = plane.current_loc; + + if (plane.quadplane.guided_mode_enabled()) { + /* + if using Q_GUIDED_MODE then project forward by the stopping distance + */ + plane.guided_WP_loc.offset_bearing(degrees(plane.ahrs.groundspeed_vector().angle()), + plane.quadplane.stopping_distance()); + } plane.set_guided_WP(); return true; }