Sub: Send a clear message if depth sensor is not connected

Fix bluerobotics/ardusub#151

Signed-off-by: Patrick José Pereira <patrickelectric@gmail.com>

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);

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,

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.
+    if(!control_check_barometer()) {
-        gcs().send_text(MAV_SEVERITY_WARNING, "BAD DEPTH");
         return false;
     }
 #endif

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.
+    if(!control_check_barometer()) {
-        gcs().send_text(MAV_SEVERITY_WARNING, "BAD DEPTH");
         return false;
     }
 #endif