AC_WPNav: get_bearing & get_horizontal_distance use Vector2f

libraries/AC_WPNav/AC_Circle.cpp

@@ -200,7 +200,7 @@ bool AC_Circle::update(float climb_rate_cms)
         target.y += - _radius * sinf(-_angle);
 
         // heading is from vehicle to center of circle
-        _yaw = get_bearing_cd(_inav.get_position(), _center.tofloat());
+        _yaw = get_bearing_cd(_inav.get_position().xy(), _center.tofloat().xy());
 
         if ((_options.get() & CircleOptions::FACE_DIRECTION_OF_TRAVEL) != 0) {
             _yaw += is_positive(_rate)?-9000.0f:9000.0f;

libraries/AC_WPNav/AC_WPNav.cpp

@@ -561,15 +561,13 @@ void AC_WPNav::update_track_with_speed_accel_limits()
 /// get_wp_distance_to_destination - get horizontal distance to destination in cm
 float AC_WPNav::get_wp_distance_to_destination() const
 {
-    // get current location
-    const Vector3f &curr = _inav.get_position();
-    return norm(_destination.x-curr.x,_destination.y-curr.y);
+    return get_horizontal_distance_cm(_inav.get_position().xy(), _destination.xy());
 }
 
 /// get_wp_bearing_to_destination - get bearing to next waypoint in centi-degrees
 int32_t AC_WPNav::get_wp_bearing_to_destination() const
 {
-    return get_bearing_cd(_inav.get_position(), _destination);
+    return get_bearing_cd(_inav.get_position().xy(), _destination.xy());
 }
 
 /// update_wpnav - run the wp controller - should be called at 100hz or higher

libraries/AC_WPNav/AC_WPNav_OA.cpp

@@ -44,9 +44,7 @@ float AC_WPNav_OA::get_wp_distance_to_destination() const
         return AC_WPNav::get_wp_distance_to_destination();
     }
 
-    // get current location
-    const Vector3f &curr = _inav.get_position();
-    return norm(_destination_oabak.x-curr.x, _destination_oabak.y-curr.y);
+    return get_horizontal_distance_cm(_inav.get_position().xy(), _destination_oabak.xy());
 }
 
 /// get_wp_bearing_to_destination - get bearing to next waypoint in centi-degrees
@@ -57,7 +55,7 @@ int32_t AC_WPNav_OA::get_wp_bearing_to_destination() const
         return AC_WPNav::get_wp_bearing_to_destination();
     }
 
-    return get_bearing_cd(_inav.get_position(), _destination_oabak);
+    return get_bearing_cd(_inav.get_position().xy(), _destination_oabak.xy());
 }
 
 /// true when we have come within RADIUS cm of the waypoint