From da84d1367ba37721f5f7b2e9c264d466a1d60fa0 Mon Sep 17 00:00:00 2001 From: Jonathan Challinger Date: Mon, 2 Feb 2015 18:12:31 -0800 Subject: [PATCH] Copter: don't assume home is at the origin --- ArduCopter/control_rtl.pde | 3 ++- ArduCopter/navigation.pde | 3 ++- 2 files changed, 4 insertions(+), 2 deletions(-) diff --git a/ArduCopter/control_rtl.pde b/ArduCopter/control_rtl.pde index 15966b333e..e596c1964c 100644 --- a/ArduCopter/control_rtl.pde +++ b/ArduCopter/control_rtl.pde @@ -117,7 +117,8 @@ static void rtl_return_start() rally_point.alt = max(rally_point.alt, current_loc.alt); // ensure we do not descend before reaching home Vector3f destination = pv_location_to_vector(rally_point); #else - Vector3f destination = Vector3f(0,0,get_RTL_alt()); + Vector3f destination = pv_location_to_vector(ahrs.get_home()); + destination.z = get_RTL_alt(); #endif wp_nav.set_wp_destination(destination); diff --git a/ArduCopter/navigation.pde b/ArduCopter/navigation.pde index 504e4f1896..65e64e9162 100644 --- a/ArduCopter/navigation.pde +++ b/ArduCopter/navigation.pde @@ -66,7 +66,8 @@ static void calc_home_distance_and_bearing() // calculate home distance and bearing if (position_ok()) { home_distance = pythagorous2(curr.x, curr.y); - home_bearing = pv_get_bearing_cd(curr,Vector3f(0,0,0)); + Vector3f home = pv_location_to_vector(ahrs.get_home()); + home_bearing = pv_get_bearing_cd(curr,home); // update super simple bearing (if required) because it relies on home_bearing update_super_simple_bearing(false);