diff --git a/ArduSub/Attitude.cpp b/ArduSub/Attitude.cpp
index c630c4f01d..071e9fc5b8 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().xy(), roi_WP.xy());
+        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/commands_logic.cpp b/ArduSub/commands_logic.cpp
index 7964ce224b..84fa971b09 100644
--- a/ArduSub/commands_logic.cpp
+++ b/ArduSub/commands_logic.cpp
@@ -509,18 +509,16 @@ bool Sub::verify_circle(const AP_Mission::Mission_Command& cmd)
     // check if we've reached the edge
     if (auto_mode == Auto_CircleMoveToEdge) {
         if (wp_nav.reached_wp_destination()) {
-            Vector3f curr_pos = inertial_nav.get_position();
             Vector3f circle_center = pv_location_to_vector(cmd.content.location);
 
             // set target altitude if not provided
             if (is_zero(circle_center.z)) {
-                circle_center.z = curr_pos.z;
+                circle_center.z = inertial_nav.get_altitude();
             }
 
             // set lat/lon position if not provided
             if (cmd.content.location.lat == 0 && cmd.content.location.lng == 0) {
-                circle_center.x = curr_pos.x;
-                circle_center.y = curr_pos.y;
+                circle_center.xy() = inertial_nav.get_position_xy();
             }
 
             // start circling
diff --git a/ArduSub/control_auto.cpp b/ArduSub/control_auto.cpp
index a7f01cb57b..422f7a974f 100644
--- a/ArduSub/control_auto.cpp
+++ b/ArduSub/control_auto.cpp
@@ -206,9 +206,7 @@ void Sub::auto_circle_movetoedge_start(const Location &circle_center, float radi
         }
 
         // if we are outside the circle, point at the edge, otherwise hold yaw
-        const Vector3p &circle_center_neu = circle_nav.get_center();
-        const Vector3f &curr_pos = inertial_nav.get_position();
-        float dist_to_center = norm(circle_center_neu.x - curr_pos.x, circle_center_neu.y - curr_pos.y);
+        float dist_to_center = get_horizontal_distance_cm(inertial_nav.get_position_xy().topostype(), circle_nav.get_center().xy());
         if (dist_to_center > circle_nav.get_radius() && dist_to_center > 500) {
             set_auto_yaw_mode(get_default_auto_yaw_mode(false));
         } else {