From 85ab3edc5f497bd6563a511b0421f2aff60f0770 Mon Sep 17 00:00:00 2001 From: Ferruccio Vicari Date: Tue, 19 Sep 2017 09:28:45 +0200 Subject: [PATCH] Plane: Smoother transition to QLOITER and QLAND Use estimated stopping position --- ArduPlane/quadplane.cpp | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/ArduPlane/quadplane.cpp b/ArduPlane/quadplane.cpp index c7ada253cd..26bd31ce0f 100644 --- a/ArduPlane/quadplane.cpp +++ b/ArduPlane/quadplane.cpp @@ -832,8 +832,9 @@ void QuadPlane::control_hover(void) void QuadPlane::init_loiter(void) { - // set target to current position - wp_nav->init_loiter_target(); + Vector3f stopping_point; + wp_nav->get_loiter_stopping_point_xy(stopping_point); + wp_nav->init_loiter_target(stopping_point); // initialize vertical speed and acceleration pos_control->set_speed_z(-pilot_velocity_z_max, pilot_velocity_z_max);