From 8640e117285b40e7fb6624924d41154bb334e65d Mon Sep 17 00:00:00 2001 From: Andrew Chapman Date: Wed, 7 May 2014 01:24:20 -0700 Subject: [PATCH] Copter: bug fix rally point landing target was drifting toward home by some distance as (0,0,0) was hard-coded in control_rtl.pde. Fixed, tested in SITL --- ArduCopter/control_rtl.pde | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/ArduCopter/control_rtl.pde b/ArduCopter/control_rtl.pde index 1ec6a0c954..c9bd1189e1 100644 --- a/ArduCopter/control_rtl.pde +++ b/ArduCopter/control_rtl.pde @@ -236,7 +236,7 @@ static void rtl_descent_start() rtl_state_complete = false; // Set wp navigation target to above home - wp_nav.set_loiter_target(Vector3f(0,0,0)); + wp_nav.set_loiter_target(wp_nav.get_wp_destination()); // initialise altitude target to stopping point pos_control.set_target_to_stopping_point_z(); @@ -295,7 +295,7 @@ static void rtl_land_start() rtl_state_complete = false; // Set wp navigation target to above home - wp_nav.set_loiter_target(Vector3f(0,0,0)); + wp_nav.set_loiter_target(wp_nav.get_wp_destination()); // initialise altitude target to stopping point pos_control.set_target_to_stopping_point_z();