diff --git a/ArduSub/GCS_Mavlink.cpp b/ArduSub/GCS_Mavlink.cpp
index 66411bb577..0d0bad47eb 100644
--- a/ArduSub/GCS_Mavlink.cpp
+++ b/ArduSub/GCS_Mavlink.cpp
@@ -822,7 +822,7 @@ void GCS_MAVLINK_Sub::handle_message(const mavlink_message_t &msg)
          */
 
         if (!z_ignore && sub.control_mode == Mode::Number::ALT_HOLD) { // Control only target depth when in ALT_HOLD
-            sub.pos_control.set_pos_target_z_cm(packet.alt*100);
+            sub.pos_control.set_pos_desired_z_cm(packet.alt*100);
             break;
         }
 
diff --git a/ArduSub/mode_acro.cpp b/ArduSub/mode_acro.cpp
index 29b1e61891..534066938a 100644
--- a/ArduSub/mode_acro.cpp
+++ b/ArduSub/mode_acro.cpp
@@ -7,7 +7,7 @@
 
 bool ModeAcro::init(bool ignore_checks) {
     // set target altitude to zero for reporting
-    position_control->set_pos_target_z_cm(0);
+    position_control->set_pos_desired_z_cm(0);
 
     // attitude hold inputs become thrust inputs in acro mode
     // set to neutral to prevent chaotic behavior (esp. roll/pitch)
diff --git a/ArduSub/mode_althold.cpp b/ArduSub/mode_althold.cpp
index cf5b2a952f..4af9c04bf6 100644
--- a/ArduSub/mode_althold.cpp
+++ b/ArduSub/mode_althold.cpp
@@ -111,9 +111,9 @@ void ModeAlthold::control_depth() {
     //we allow full control to the pilot, but as soon as there's no input, we handle being at surface/bottom
     if (fabsf(target_climb_rate_cm_s) < 0.05f)  {
         if (sub.ap.at_surface) {
-            position_control->set_pos_target_z_cm(MIN(position_control->get_pos_target_z_cm(), g.surface_depth - 5.0f)); // set target to 5 cm below surface level
+            position_control->set_pos_desired_z_cm(MIN(position_control->get_pos_desired_z_cm(), g.surface_depth - 5.0f)); // set target to 5 cm below surface level
         } else if (sub.ap.at_bottom) {
-            position_control->set_pos_target_z_cm(MAX(inertial_nav.get_position_z_up_cm() + 10.0f, position_control->get_pos_target_z_cm())); // set target to 10 cm above bottom
+            position_control->set_pos_desired_z_cm(MAX(inertial_nav.get_position_z_up_cm() + 10.0f, position_control->get_pos_desired_z_cm())); // set target to 10 cm above bottom
         }
     }
 
diff --git a/ArduSub/mode_guided.cpp b/ArduSub/mode_guided.cpp
index 8816aaac20..64dbcb0f83 100644
--- a/ArduSub/mode_guided.cpp
+++ b/ArduSub/mode_guided.cpp
@@ -811,7 +811,7 @@ float ModeGuided::get_auto_heading()
         float track_bearing = get_bearing_cd(sub.wp_nav.get_wp_origin().xy(), sub.wp_nav.get_wp_destination().xy());
 
         // Bearing from current position towards intermediate position target (centidegrees)
-        const Vector2f target_vel_xy{position_control->get_vel_target_cms().x, position_control->get_vel_target_cms().y};
+        const Vector2f target_vel_xy = position_control->get_vel_target_cms().xy();
         float angle_error = 0.0f;
         if (target_vel_xy.length() >= position_control->get_max_speed_xy_cms() * 0.1f) {
             const float desired_angle_cd = degrees(target_vel_xy.angle()) * 100.0f;
diff --git a/ArduSub/mode_manual.cpp b/ArduSub/mode_manual.cpp
index 9d6e29892e..6bbb92eef6 100644
--- a/ArduSub/mode_manual.cpp
+++ b/ArduSub/mode_manual.cpp
@@ -3,7 +3,7 @@
 
 bool ModeManual::init(bool ignore_checks) {
     // set target altitude to zero for reporting
-    position_control->set_pos_target_z_cm(0);
+    position_control->set_pos_desired_z_cm(0);
 
     // attitude hold inputs become thrust inputs in manual mode
     // set to neutral to prevent chaotic behavior (esp. roll/pitch)
diff --git a/ArduSub/mode_stabilize.cpp b/ArduSub/mode_stabilize.cpp
index 52620b8d5d..51f9ef4732 100644
--- a/ArduSub/mode_stabilize.cpp
+++ b/ArduSub/mode_stabilize.cpp
@@ -3,7 +3,7 @@
 
 bool ModeStabilize::init(bool ignore_checks) {
     // set target altitude to zero for reporting
-    position_control->set_pos_target_z_cm(0);
+    position_control->set_pos_desired_z_cm(0);
     sub.last_pilot_heading = ahrs.yaw_sensor;
 
     return true;
diff --git a/ArduSub/mode_surftrak.cpp b/ArduSub/mode_surftrak.cpp
index 0bdc562f39..f119e02a89 100644
--- a/ArduSub/mode_surftrak.cpp
+++ b/ArduSub/mode_surftrak.cpp
@@ -81,8 +81,7 @@ bool ModeSurftrak::set_rangefinder_target_cm(float target_cm)
 
         // Initialize the terrain offset
         auto terrain_offset_cm = sub.inertial_nav.get_position_z_up_cm() - rangefinder_target_cm;
-        sub.pos_control.set_pos_offset_z_cm(terrain_offset_cm);
-        sub.pos_control.set_pos_offset_target_z_cm(terrain_offset_cm);
+        sub.pos_control.init_pos_terrain_cm(terrain_offset_cm);
 
     } else {
         reset();
@@ -97,8 +96,7 @@ void ModeSurftrak::reset()
     rangefinder_target_cm = INVALID_TARGET;
 
     // Reset the terrain offset
-    sub.pos_control.set_pos_offset_z_cm(0);
-    sub.pos_control.set_pos_offset_target_z_cm(0);
+    sub.pos_control.init_pos_terrain_cm(0);
 }
 
 /*
@@ -117,11 +115,11 @@ void ModeSurftrak::control_range() {
         }
         if (sub.ap.at_surface) {
             // Set target depth to 5 cm below SURFACE_DEPTH and reset
-            position_control->set_pos_target_z_cm(MIN(position_control->get_pos_target_z_cm(), g.surface_depth - 5.0f));
+            position_control->set_pos_desired_z_cm(MIN(position_control->get_pos_desired_z_cm(), g.surface_depth - 5.0f));
             reset();
         } else if (sub.ap.at_bottom) {
             // Set target depth to 10 cm above bottom and reset
-            position_control->set_pos_target_z_cm(MAX(inertial_nav.get_position_z_up_cm() + 10.0f, position_control->get_pos_target_z_cm()));
+            position_control->set_pos_desired_z_cm(MAX(inertial_nav.get_position_z_up_cm() + 10.0f, position_control->get_pos_desired_z_cm()));
             reset();
         } else {
             // Typical operation
@@ -164,7 +162,7 @@ void ModeSurftrak::update_surface_offset()
             }
 
             // Set the offset target, AC_PosControl will do the rest
-            sub.pos_control.set_pos_offset_target_z_cm(rangefinder_terrain_offset_cm);
+            sub.pos_control.set_pos_terrain_target_cm(rangefinder_terrain_offset_cm);
         }
     }
 #endif  // AP_RANGEFINDER_ENABLED