diff --git a/ArduSub/Sub.h b/ArduSub/Sub.h
index 197ec12da9..5a8493d154 100644
--- a/ArduSub/Sub.h
+++ b/ArduSub/Sub.h
@@ -509,6 +509,7 @@ private:
 
     bool stabilize_init(void);
     void stabilize_run();
+    void control_depth();
     bool manual_init(void);
     void manual_run();
     void failsafe_sensors_check(void);
diff --git a/ArduSub/control_althold.cpp b/ArduSub/control_althold.cpp
index e0eef9ede5..d6fcf7ccda 100644
--- a/ArduSub/control_althold.cpp
+++ b/ArduSub/control_althold.cpp
@@ -96,6 +96,13 @@ void Sub::althold_run()
         }
     }
 
+    control_depth();
+
+    motors.set_forward(channel_forward->norm_input());
+    motors.set_lateral(channel_lateral->norm_input());
+}
+
+void Sub::control_depth() {
     // Hold actual position until zero derivative is detected
     static bool engageStopZ = true;
     // Get last user velocity direction to check for zero derivative points
@@ -125,7 +132,4 @@ void Sub::althold_run()
 
         pos_control.update_z_controller();
     }
-
-    motors.set_forward(channel_forward->norm_input());
-    motors.set_lateral(channel_lateral->norm_input());
 }