diff --git a/ArduCopter/autoyaw.cpp b/ArduCopter/autoyaw.cpp
index 215a1ce3df..96aa59da06 100644
--- a/ArduCopter/autoyaw.cpp
+++ b/ArduCopter/autoyaw.cpp
@@ -5,7 +5,7 @@ Mode::AutoYaw Mode::auto_yaw;
 // roi_yaw - returns heading towards location held in roi
 float Mode::AutoYaw::roi_yaw() const
 {
-    return get_bearing_cd(copter.inertial_nav.get_position(), roi);
+    return get_bearing_cd(copter.inertial_nav.get_position().xy(), roi.xy());
 }
 
 float Mode::AutoYaw::look_ahead_yaw()
diff --git a/ArduCopter/mode_follow.cpp b/ArduCopter/mode_follow.cpp
index a83719d09d..2cb0f61db1 100644
--- a/ArduCopter/mode_follow.cpp
+++ b/ArduCopter/mode_follow.cpp
@@ -108,9 +108,8 @@ void ModeFollow::run()
         // calculate vehicle heading
         switch (g2.follow.get_yaw_behave()) {
             case AP_Follow::YAW_BEHAVE_FACE_LEAD_VEHICLE: {
-                const Vector3f dist_vec_xy(dist_vec.x, dist_vec.y, 0.0f);
-                if (dist_vec_xy.length() > 1.0f) {
-                    yaw_cd = get_bearing_cd(Vector3f(), dist_vec_xy);
+                if (dist_vec.xy().length() > 1.0f) {
+                    yaw_cd = get_bearing_cd(Vector2f{}, dist_vec.xy());
                     use_yaw = true;
                 }
                 break;
@@ -126,9 +125,8 @@ void ModeFollow::run()
             }
 
             case AP_Follow::YAW_BEHAVE_DIR_OF_FLIGHT: {
-                const Vector3f vel_vec(desired_velocity_neu_cms.x, desired_velocity_neu_cms.y, 0.0f);
-                if (vel_vec.length() > 100.0f) {
-                    yaw_cd = get_bearing_cd(Vector3f(), vel_vec);
+                if (desired_velocity_neu_cms.xy().length() > 100.0f) {
+                    yaw_cd = get_bearing_cd(Vector2f{}, desired_velocity_neu_cms.xy());
                     use_yaw = true;
                 }
                 break;
diff --git a/ArduCopter/mode_guided.cpp b/ArduCopter/mode_guided.cpp
index 290d157f4c..e93a2b7962 100644
--- a/ArduCopter/mode_guided.cpp
+++ b/ArduCopter/mode_guided.cpp
@@ -1087,7 +1087,7 @@ bool ModeGuided::limit_check()
 
     // check if we have gone beyond horizontal limit
     if (guided_limit.horiz_max_cm > 0.0f) {
-        float horiz_move = get_horizontal_distance_cm(guided_limit.start_pos, curr_pos);
+        const float horiz_move = get_horizontal_distance_cm(guided_limit.start_pos.xy(), curr_pos.xy());
         if (horiz_move > guided_limit.horiz_max_cm) {
             return true;
         }
@@ -1118,7 +1118,7 @@ uint32_t ModeGuided::wp_distance() const
     case SubMode::WP:
         return wp_nav->get_wp_distance_to_destination();
     case SubMode::Pos:
-        return norm(guided_pos_target_cm.x - inertial_nav.get_position().x, guided_pos_target_cm.y - inertial_nav.get_position().y);
+        return get_horizontal_distance_cm(inertial_nav.get_position().xy(), guided_pos_target_cm.tofloat().xy());
     case SubMode::PosVelAccel:
         return pos_control->get_pos_error_xy_cm();
         break;
@@ -1133,7 +1133,7 @@ int32_t ModeGuided::wp_bearing() const
     case SubMode::WP:
         return wp_nav->get_wp_bearing_to_destination();
     case SubMode::Pos:
-        return get_bearing_cd(inertial_nav.get_position(), guided_pos_target_cm.tofloat());
+        return get_bearing_cd(inertial_nav.get_position().xy(), guided_pos_target_cm.tofloat().xy());
     case SubMode::PosVelAccel:
         return pos_control->get_bearing_to_target_cd();
         break;