diff --git a/ArduSub/Sub.h b/ArduSub/Sub.h
index 8dafad4c51..36b1819bcd 100644
--- a/ArduSub/Sub.h
+++ b/ArduSub/Sub.h
@@ -516,8 +516,6 @@ private:
     void set_surfaced(bool at_surface);
     void set_bottomed(bool at_bottom);
     void motors_output();
-    Vector3f pv_location_to_vector(const Location& loc);
-    float pv_alt_above_origin(float alt_above_home_cm);
     void init_rc_in();
     void init_rc_out();
     void enable_motor_output();
diff --git a/ArduSub/commands_logic.cpp b/ArduSub/commands_logic.cpp
index 25ced66be1..70a006cdc0 100644
--- a/ArduSub/commands_logic.cpp
+++ b/ArduSub/commands_logic.cpp
@@ -509,7 +509,8 @@ 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 circle_center = pv_location_to_vector(cmd.content.location);
+            Vector3f circle_center;
+            UNUSED_RESULT(cmd.content.location.get_vector_from_origin_NEU(circle_center));
 
             // set target altitude if not provided
             if (is_zero(circle_center.z)) {
diff --git a/ArduSub/control_auto.cpp b/ArduSub/control_auto.cpp
index 2494b24eb1..8114a220fd 100644
--- a/ArduSub/control_auto.cpp
+++ b/ArduSub/control_auto.cpp
@@ -463,10 +463,11 @@ void Sub::set_auto_yaw_roi(const Location &roi_location)
 #endif  // HAL_MOUNT_ENABLED
     } else {
 #if HAL_MOUNT_ENABLED
-        // check if mount type requires us to rotate the quad
+        // check if mount type requires us to rotate the sub
         if (!camera_mount.has_pan_control()) {
-            roi_WP = pv_location_to_vector(roi_location);
-            set_auto_yaw_mode(AUTO_YAW_ROI);
+            if (roi_location.get_vector_from_origin_NEU(roi_WP)) {
+                set_auto_yaw_mode(AUTO_YAW_ROI);
+            }
         }
         // send the command to the camera mount
         camera_mount.set_roi_target(roi_location);
@@ -478,9 +479,10 @@ void Sub::set_auto_yaw_roi(const Location &roi_location)
         //      3: point at a location given by alt, lon, lat parameters
         //      4: point at a target given a target id (can't be implemented)
 #else
-        // if we have no camera mount aim the quad at the location
-        roi_WP = pv_location_to_vector(roi_location);
-        set_auto_yaw_mode(AUTO_YAW_ROI);
+        // if we have no camera mount aim the sub at the location
+        if (roi_location.get_vector_from_origin_NEU(roi_WP)) {
+            set_auto_yaw_mode(AUTO_YAW_ROI);
+        }
 #endif  // HAL_MOUNT_ENABLED
     }
 }
diff --git a/ArduSub/position_vector.cpp b/ArduSub/position_vector.cpp
deleted file mode 100644
index c71a8247d6..0000000000
--- a/ArduSub/position_vector.cpp
+++ /dev/null
@@ -1,33 +0,0 @@
-#include "Sub.h"
-
-// position_vector.pde related utility functions
-
-// position vectors are Vector3f
-//    .x = latitude from home in cm
-//    .y = longitude from home in cm
-//    .z = altitude above home in cm
-
-// pv_location_to_vector - convert lat/lon coordinates to a position vector
-Vector3f Sub::pv_location_to_vector(const Location& loc)
-{
-    Location origin;
-    if (!ahrs.get_origin(origin)) {
-        origin.zero();
-    }
-    float alt_above_origin = pv_alt_above_origin(loc.alt);  // convert alt-relative-to-home to alt-relative-to-origin
-    Vector3f vec = origin.get_distance_NED(loc);
-    vec.xy() *= 100;
-    vec.z = alt_above_origin;
-    return vec;
-}
-
-// pv_alt_above_origin - convert altitude above home to altitude above EKF origin
-float Sub::pv_alt_above_origin(float alt_above_home_cm)
-{
-    Location origin;
-    if (!ahrs.get_origin(origin)) {
-        origin.zero();
-    }
-    return alt_above_home_cm + (ahrs.get_home().alt - origin.alt);
-}
-