Copter: don't assume home is at the origin

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);

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);