Sub: Bugfix for external baro failsafe handling when no baro is

connected at boot

ArduSub/Sub.h

@@ -233,6 +233,7 @@ private:
             enum HomeState home_state   : 2; // home status (unset, set, locked)
             uint8_t at_bottom           : 1; // true if we are at the bottom
             uint8_t at_surface          : 1; // true if we are at the surface
+            uint8_t depth_sensor_present: 1; // true if there is a depth sensor detected at boot
         };
         uint32_t value;
     } ap;

ArduSub/control_althold.cpp

@@ -9,7 +9,7 @@
 bool Sub::althold_init()
 {
 #if CONFIG_HAL_BOARD != HAL_BOARD_SITL
-    if (!sensor_health.depth) { // can't hold depth without a depth sensor, exit immediately.
+    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");
         return false;
     }

ArduSub/control_surface.cpp

@@ -4,7 +4,7 @@
 bool Sub::surface_init()
 {
 #if CONFIG_HAL_BOARD != HAL_BOARD_SITL
-    if (!sensor_health.depth) { // can't hold depth without a depth sensor, exit immediately.
+    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");
         return false;
     }

ArduSub/failsafe.cpp

@@ -66,6 +66,10 @@ void Sub::mainloop_failsafe_check()
 
 void Sub::failsafe_sensors_check(void)
 {
+    if (!ap.depth_sensor_present) {
+        return;
+    }
+
     // We need a depth sensor to do any sort of auto z control
     if (sensor_health.depth) {
         failsafe.sensor_health = false;
@@ -99,7 +103,6 @@ void Sub::failsafe_ekf_check(void)
         failsafe.ekf = false;
         AP_Notify::flags.ekf_bad = false;
         return;
-
     }
 
     float posVar, hgtVar, tasVar;

ArduSub/sensors.cpp

@@ -15,7 +15,9 @@ void Sub::read_barometer(void)
         Log_Write_Baro();
     }
 
-    sensor_health.depth = barometer.healthy(depth_sensor_idx);
+    if (ap.depth_sensor_present) {
+        sensor_health.depth = barometer.healthy(depth_sensor_idx);
+    }
 }
 
 void Sub::init_rangefinder(void)

ArduSub/surface_bottom_detector.cpp

@@ -24,7 +24,7 @@ void Sub::update_surface_and_bottom_detector()
     // check that we are not moving up or down
     bool vel_stationary = velocity.z > -0.05 && velocity.z < 0.05;
 
-    if (sensor_health.depth) { // we can use the external pressure sensor for a very accurate and current measure of our z axis position
+    if (ap.depth_sensor_present && sensor_health.depth) { // we can use the external pressure sensor for a very accurate and current measure of our z axis position
         current_depth = barometer.get_altitude(); // cm
 
 

ArduSub/system.cpp

@@ -123,8 +123,7 @@ void Sub::init_ardupilot()
     USERHOOK_INIT
 #endif
 
-    // read Baro pressure at ground
+    // Init baro and determine if we have external (depth) pressure sensor
-    //-----------------------------
     init_barometer(false);
     barometer.update();
 
@@ -132,12 +131,13 @@ void Sub::init_ardupilot()
         if (barometer.get_type(i) == AP_Baro::BARO_TYPE_WATER) {
             barometer.set_primary_baro(i);
             depth_sensor_idx = i;
-            sensor_health.depth = barometer.healthy(i);
+            ap.depth_sensor_present = true;
-            break;
+            sensor_health.depth = barometer.healthy(depth_sensor_idx); // initialize health flag
+            break; // Go with the first one we find
         }
     }
 
-    if (!sensor_health.depth) {
+    if (!ap.depth_sensor_present) {
         // We only have onboard baro
         // No external underwater depth sensor detected
         barometer.set_primary_baro(0);