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

Fix bluerobotics/ardusub#151 Signed-off-by: Patrick José Pereira <[email protected]>
huimengyoux · Apr 19, 2018 · 0878f50 · 0878f50
1 parent 8fa1700
commit 0878f50
Show file tree

Hide file tree

Showing 4 changed files with 16 additions and 4 deletions.
diff --git 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
@@ -708,6 +708,8 @@ class Sub : public AP_HAL::HAL::Callbacks {
 
     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
@@ -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
@@ -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