diff --git a/ArduSub/ArduSub.cpp b/ArduSub/ArduSub.cpp
index e65ca17dcb..bc9858efb8 100644
--- a/ArduSub/ArduSub.cpp
+++ b/ArduSub/ArduSub.cpp
@@ -339,4 +339,16 @@ void Sub::update_altitude()
     }
 }
 
+bool Sub::control_check_barometer()
+{
+    if (!ap.depth_sensor_present) { // can't hold depth without a depth sensor
+        gcs().send_text(MAV_SEVERITY_WARNING, "Depth sensor is not connected.");
+        return false;
+    } else if (failsafe.sensor_health) {
+        gcs().send_text(MAV_SEVERITY_WARNING, "Depth sensor error.");
+        return false;
+    }
+    return true;
+}
+
 AP_HAL_MAIN_CALLBACKS(&sub);
diff --git a/ArduSub/Sub.h b/ArduSub/Sub.h
index d39e582815..343a6a7883 100644
--- a/ArduSub/Sub.h
+++ b/ArduSub/Sub.h
@@ -708,6 +708,8 @@ private:
 
     void convert_old_parameters(void);
 
+    bool control_check_barometer();
+
     enum Failsafe_Action {
         Failsafe_Action_None    = 0,
         Failsafe_Action_Warn    = 1,
diff --git a/ArduSub/control_althold.cpp b/ArduSub/control_althold.cpp
index 02e5886562..dcaafc3da7 100644
--- a/ArduSub/control_althold.cpp
+++ b/ArduSub/control_althold.cpp
@@ -9,8 +9,7 @@
 bool Sub::althold_init()
 {
 #if CONFIG_HAL_BOARD != HAL_BOARD_SITL
-    if (!ap.depth_sensor_present || failsafe.sensor_health) { // can't hold depth without a depth sensor, exit immediately.
-        gcs().send_text(MAV_SEVERITY_WARNING, "BAD DEPTH");
+    if(!control_check_barometer()) {
         return false;
     }
 #endif
diff --git a/ArduSub/control_surface.cpp b/ArduSub/control_surface.cpp
index 4e501725b5..f156e31fca 100644
--- a/ArduSub/control_surface.cpp
+++ b/ArduSub/control_surface.cpp
@@ -4,8 +4,7 @@
 bool Sub::surface_init()
 {
 #if CONFIG_HAL_BOARD != HAL_BOARD_SITL
-    if (!ap.depth_sensor_present || failsafe.sensor_health) { // can't hold depth without a depth sensor, exit immediately.
-        gcs().send_text(MAV_SEVERITY_WARNING, "BAD DEPTH");
+    if(!control_check_barometer()) {
         return false;
     }
 #endif