diff --git a/ArduSub/Attitude.cpp b/ArduSub/Attitude.cpp
index a93c194bd9..c630c4f01d 100644
--- a/ArduSub/Attitude.cpp
+++ b/ArduSub/Attitude.cpp
@@ -64,7 +64,7 @@ float Sub::get_roi_yaw()
     roi_yaw_counter++;
     if (roi_yaw_counter >= 4) {
         roi_yaw_counter = 0;
-        yaw_look_at_WP_bearing = get_bearing_cd(inertial_nav.get_position(), roi_WP);
+        yaw_look_at_WP_bearing = get_bearing_cd(inertial_nav.get_position().xy(), roi_WP.xy());
     }
 
     return yaw_look_at_WP_bearing;
diff --git a/ArduSub/control_auto.cpp b/ArduSub/control_auto.cpp
index d54368939f..a7f01cb57b 100644
--- a/ArduSub/control_auto.cpp
+++ b/ArduSub/control_auto.cpp
@@ -516,7 +516,7 @@ float Sub::get_auto_heading()
     case AUTO_YAW_CORRECT_XTRACK: {
         // TODO return current yaw if not in appropriate mode
         // Bearing of current track (centidegrees)
-        float track_bearing = get_bearing_cd(wp_nav.get_wp_origin(), wp_nav.get_wp_destination());
+        float track_bearing = get_bearing_cd(wp_nav.get_wp_origin().xy(), wp_nav.get_wp_destination().xy());
 
         // Bearing from current position towards intermediate position target (centidegrees)
         const Vector2f target_vel_xy{pos_control.get_vel_target_cms().x, pos_control.get_vel_target_cms().y};
diff --git a/ArduSub/control_guided.cpp b/ArduSub/control_guided.cpp
index 9a9a2ce5c9..7d8c2ae7e3 100644
--- a/ArduSub/control_guided.cpp
+++ b/ArduSub/control_guided.cpp
@@ -551,7 +551,7 @@ bool Sub::guided_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;
         }