Plane: offset guided start point when using Q_GUIDED_MODE

2021-06-04 18:35:23 +10:00 · 2021-06-04 18:35:23 +10:00 · fd19ff3bea
parent f15497c6a9
commit fd19ff3bea
1 changed files with 8 additions and 0 deletions
--- 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;
 }