diff --git a/libraries/AC_WPNav/AC_Circle.cpp b/libraries/AC_WPNav/AC_Circle.cpp
index aea8447204..d583e9ae72 100644
--- a/libraries/AC_WPNav/AC_Circle.cpp
+++ b/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().xy(), _center.tofloat().xy());
+        _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;
diff --git a/libraries/AC_WPNav/AC_WPNav.cpp b/libraries/AC_WPNav/AC_WPNav.cpp
index d77740b03f..017b275a15 100644
--- a/libraries/AC_WPNav/AC_WPNav.cpp
+++ b/libraries/AC_WPNav/AC_WPNav.cpp
@@ -399,13 +399,11 @@ void AC_WPNav::shift_wp_origin_and_destination_to_current_pos_xy()
     _pos_control.init_xy_controller();
 
     // get current and target locations
-    const Vector3f& curr_pos = _inav.get_position();
+    const Vector2f& curr_pos = _inav.get_position_xy();
 
     // shift origin and destination horizontally
-    _origin.x = curr_pos.x;
-    _origin.y = curr_pos.y;
-    _destination.x = curr_pos.x;
-    _destination.y = curr_pos.y;
+    _origin.xy() = curr_pos;
+    _destination.xy() = curr_pos;
 }
 
 /// shifts the origin and destination horizontally to the achievable stopping point
@@ -561,13 +559,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
 {
-    return get_horizontal_distance_cm(_inav.get_position().xy(), _destination.xy());
+    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().xy(), _destination.xy());
+    return get_bearing_cd(_inav.get_position_xy(), _destination.xy());
 }
 
 /// update_wpnav - run the wp controller - should be called at 100hz or higher
diff --git a/libraries/AC_WPNav/AC_WPNav_OA.cpp b/libraries/AC_WPNav/AC_WPNav_OA.cpp
index d850b0d908..d577f228b1 100644
--- a/libraries/AC_WPNav/AC_WPNav_OA.cpp
+++ b/libraries/AC_WPNav/AC_WPNav_OA.cpp
@@ -44,7 +44,7 @@ float AC_WPNav_OA::get_wp_distance_to_destination() const
         return AC_WPNav::get_wp_distance_to_destination();
     }
 
-    return get_horizontal_distance_cm(_inav.get_position().xy(), _destination_oabak.xy());
+    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
@@ -55,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().xy(), _destination_oabak.xy());
+    return get_bearing_cd(_inav.get_position_xy(), _destination_oabak.xy());
 }
 
 /// true when we have come within RADIUS cm of the waypoint