diff --git a/ArduSub/Attitude.cpp b/ArduSub/Attitude.cpp
index 0829e35566..f3735eafd1 100644
--- a/ArduSub/Attitude.cpp
+++ b/ArduSub/Attitude.cpp
@@ -71,7 +71,7 @@ float Sub::get_roi_yaw()
     roi_yaw_counter++;
     if (roi_yaw_counter >= 4) {
         roi_yaw_counter = 0;
-        yaw_look_at_WP_bearing = pv_get_bearing_cd(inertial_nav.get_position(), roi_WP);
+        yaw_look_at_WP_bearing = get_bearing_cd(inertial_nav.get_position(), roi_WP);
     }
 
     return yaw_look_at_WP_bearing;
diff --git a/ArduSub/Sub.h b/ArduSub/Sub.h
index 21ef14fa72..5fdce87df7 100644
--- a/ArduSub/Sub.h
+++ b/ArduSub/Sub.h
@@ -628,8 +628,6 @@ private:
     void motors_output();
     Vector3f pv_location_to_vector(const Location& loc);
     float pv_alt_above_origin(float alt_above_home_cm);
-    float pv_get_bearing_cd(const Vector3f &origin, const Vector3f &destination);
-    float pv_get_horizontal_distance_cm(const Vector3f &origin, const Vector3f &destination);
     void init_rc_in();
     void init_rc_out();
     void enable_motor_output();
diff --git a/ArduSub/control_auto.cpp b/ArduSub/control_auto.cpp
index 6f061f480f..dd7f09faf8 100644
--- a/ArduSub/control_auto.cpp
+++ b/ArduSub/control_auto.cpp
@@ -599,7 +599,7 @@ float Sub::get_auto_heading(void)
     case AUTO_YAW_CORRECT_XTRACK: {
         // TODO return current yaw if not in appropriate mode
         // Bearing of current track (centidegrees)
-        float track_bearing = wp_nav.get_bearing_cd(wp_nav.get_wp_origin(), wp_nav.get_wp_destination());
+        float track_bearing = get_bearing_cd(wp_nav.get_wp_origin(), wp_nav.get_wp_destination());
 
         // Bearing from current position towards intermediate position target (centidegrees)
         float desired_angle = wp_nav.get_loiter_bearing_to_target();
diff --git a/ArduSub/control_guided.cpp b/ArduSub/control_guided.cpp
index f9763a48a9..922bc8a783 100644
--- a/ArduSub/control_guided.cpp
+++ b/ArduSub/control_guided.cpp
@@ -557,7 +557,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 = pv_get_horizontal_distance_cm(guided_limit.start_pos, curr_pos);
+        float horiz_move = get_horizontal_distance_cm(guided_limit.start_pos, curr_pos);
         if (horiz_move > guided_limit.horiz_max_cm) {
             return true;
         }
diff --git a/ArduSub/fence.cpp b/ArduSub/fence.cpp
index a101fe8e64..5f6781d4b4 100644
--- a/ArduSub/fence.cpp
+++ b/ArduSub/fence.cpp
@@ -13,7 +13,7 @@ void Sub::fence_check()
 
     Vector3f home = pv_location_to_vector(ahrs.get_home());
     Vector3f curr = inertial_nav.get_position();
-    int32_t home_distance = pv_get_horizontal_distance_cm(curr, home);
+    int32_t home_distance = get_horizontal_distance_cm(curr, home);
 
     // give fence library our current distance from home in meters
     fence.set_home_distance(home_distance*0.01f);
diff --git a/ArduSub/position_vector.cpp b/ArduSub/position_vector.cpp
index 35a838c0ff..6d4a3bb2c4 100644
--- a/ArduSub/position_vector.cpp
+++ b/ArduSub/position_vector.cpp
@@ -22,18 +22,3 @@ float Sub::pv_alt_above_origin(float alt_above_home_cm)
     return alt_above_home_cm + (ahrs.get_home().alt - origin.alt);
 }
 
-// pv_get_bearing_cd - return bearing in centi-degrees between two positions
-float Sub::pv_get_bearing_cd(const Vector3f &origin, const Vector3f &destination)
-{
-    float bearing = atan2f(destination.y-origin.y, destination.x-origin.x) * DEGX100;
-    if (bearing < 0) {
-        bearing += 36000;
-    }
-    return bearing;
-}
-
-// pv_get_horizontal_distance_cm - return distance between two positions in cm
-float Sub::pv_get_horizontal_distance_cm(const Vector3f &origin, const Vector3f &destination)
-{
-    return norm(destination.x-origin.x,destination.y-origin.y);
-}