Sub: Add failsafe mechanisms for depth sensor error

ArduSub/ArduSub.cpp

@@ -216,6 +216,8 @@ void Sub::fifty_hz_loop()
 
     failsafe_ekf_check();
 
+    failsafe_sensors_check();
+
     // Update servo output
     RC_Channels::set_pwm_all();
     SRV_Channels::limit_slew_rate(SRV_Channel::k_mount_tilt, g.cam_slew_limit, 0.02f);

ArduSub/GCS_Mavlink.cpp

@@ -156,9 +156,7 @@ NOINLINE void Sub::send_extended_status1(mavlink_channel_t chan)
                              MAV_SYS_STATUS_SENSOR_GPS |
                              MAV_SYS_STATUS_SENSOR_RC_RECEIVER);
 
-    if (ap.depth_sensor_present && barometer.all_healthy()) { // check all barometers
+    if (sensor_health.depth) { // check the internal barometer only
-        control_sensors_health |= MAV_SYS_STATUS_SENSOR_ABSOLUTE_PRESSURE;
-    } else if (barometer.healthy(0)) { // check the internal barometer only
         control_sensors_health |= MAV_SYS_STATUS_SENSOR_ABSOLUTE_PRESSURE;
     }
     if (g.compass_enabled && compass.healthy() && ahrs.use_compass()) {
@@ -1259,7 +1257,7 @@ void GCS_MAVLINK_Sub::handleMessage(mavlink_message_t* msg)
                     result = MAV_RESULT_FAILED;
                 }
             } else if (is_equal(packet.param3,1.0f)) {
-                if (!sub.ap.depth_sensor_present || sub.motors.armed() || sub.barometer.get_pressure() > 110000) {
+                if (!sub.sensor_health.depth || sub.motors.armed() || sub.barometer.get_pressure() > 110000) {
                     result = MAV_RESULT_FAILED;
                 } else {
                     sub.init_barometer(true);

ArduSub/Sub.h

@@ -237,7 +237,6 @@ 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 we have an external baro connected
         };
         uint32_t value;
     } ap;
@@ -268,9 +267,10 @@ private:
         uint8_t internal_pressure    : 1; // true if internal pressure is over threshold
         uint8_t internal_temperature : 1; // true if temperature is over threshold
         uint8_t crash                : 1; // true if we are crashed
+        uint8_t sensor_health        : 1; // true if at least one sensor has triggered a failsafe (currently only used for depth in depth enabled modes)
+
         uint32_t last_leak_warn_ms;      // last time a leak warning was sent to gcs
         uint32_t last_gcs_warn_ms;
-
         uint32_t last_heartbeat_ms;      // the time when the last HEARTBEAT message arrived from a GCS - used for triggering gcs failsafe
         uint32_t last_pilot_input_ms; // last time we received pilot input in the form of MANUAL_CONTROL or RC_CHANNELS_OVERRIDE messages
         uint32_t terrain_first_failure_ms;  // the first time terrain data access failed - used to calculate the duration of the failure
@@ -282,10 +282,14 @@ private:
 
     // sensor health for logging
     struct {
-        uint8_t baro        : 1;    // true if baro is healthy
+        uint8_t baro        : 1;    // true if any single baro is healthy
+        uint8_t depth       : 1;    // true if depth sensor is healthy
         uint8_t compass     : 1;    // true if compass is healthy
     } sensor_health;
 
+    // Baro sensor instance index of the external water pressure sensor
+    uint8_t depth_sensor_idx;
+
     AP_Motors6DOF motors;
 
     // GPS variables
@@ -590,6 +594,7 @@ private:
     void stabilize_run();
     bool manual_init(void);
     void manual_run();
+    void failsafe_sensors_check(void);
     void failsafe_crash_check();
     void failsafe_ekf_check(void);
     void failsafe_battery_check(void);

ArduSub/control_althold.cpp

@@ -9,8 +9,8 @@
 bool Sub::althold_init()
 {
 #if CONFIG_HAL_BOARD != HAL_BOARD_SITL
-    if (!ap.depth_sensor_present) { // can't hold depth without a depth sensor, exit immediately.
+    if (!sensor_health.depth) { // can't hold depth without a depth sensor, exit immediately.
-        gcs_send_text(MAV_SEVERITY_WARNING, "Depth hold requires external pressure sensor.");
+        gcs_send_text(MAV_SEVERITY_WARNING, "BAD DEPTH");
         return false;
     }
 #endif

ArduSub/control_surface.cpp

@@ -4,8 +4,8 @@
 bool Sub::surface_init()
 {
 #if CONFIG_HAL_BOARD != HAL_BOARD_SITL
-    if (!ap.depth_sensor_present) { // can't hold depth without a depth sensor, exit immediately.
+    if (!sensor_health.depth) { // can't hold depth without a depth sensor, exit immediately.
-        gcs_send_text(MAV_SEVERITY_WARNING, "Surface mode requires external pressure sensor.");
+        gcs_send_text(MAV_SEVERITY_WARNING, "BAD DEPTH");
         return false;
     }
 #endif

ArduSub/defines.h

@@ -56,7 +56,8 @@ enum mode_reason_t {
     MODE_REASON_FENCE_BREACH,
     MODE_REASON_TERRAIN_FAILSAFE,
     MODE_REASON_SURFACE_COMPLETE,
-    MODE_REASON_LEAK_FAILSAFE
+    MODE_REASON_LEAK_FAILSAFE,
+    MODE_REASON_BAD_DEPTH
 };
 
 // Acro Trainer types
@@ -193,6 +194,8 @@ enum RTLState {
 #define ERROR_SUBSYSTEM_NAVIGATION          22
 #define ERROR_SUBSYSTEM_FAILSAFE_TERRAIN    23
 #define ERROR_SUBSYSTEM_FAILSAFE_LEAK       24
+#define ERROR_SUBSYSTEM_FAILSAFE_SENSORS    25
+
 // general error codes
 #define ERROR_CODE_ERROR_RESOLVED           0
 #define ERROR_CODE_FAILED_TO_INITIALISE     1
@@ -220,8 +223,9 @@ enum RTLState {
 // EKF check definitions
 #define ERROR_CODE_EKFCHECK_BAD_VARIANCE       2
 #define ERROR_CODE_EKFCHECK_VARIANCE_CLEARED   0
+
 // Baro specific error codes
-#define ERROR_CODE_BARO_GLITCH              2
+#define ERROR_CODE_BAD_DEPTH              0
 
 //////////////////////////////////////////////////////////////////////////////
 // Battery monitoring

ArduSub/failsafe.cpp

@@ -64,6 +64,32 @@ void Sub::mainloop_failsafe_check()
     }
 }
 
+void Sub::failsafe_sensors_check(void)
+{
+    // We need a depth sensor to do any sort of auto z control
+    if (sensor_health.depth) {
+        failsafe.sensor_health = false;
+        return;
+    }
+
+    // only report once
+    if (failsafe.sensor_health) {
+        return;
+    }
+
+    failsafe.sensor_health = true;
+    gcs_send_text(MAV_SEVERITY_CRITICAL, "Depth sensor error!");
+    Log_Write_Error(ERROR_SUBSYSTEM_FAILSAFE_SENSORS, ERROR_CODE_BAD_DEPTH);
+
+    if (control_mode == ALT_HOLD || control_mode == SURFACE || mode_requires_GPS(control_mode)) {
+        // This should always succeed
+        if (!set_mode(MANUAL, MODE_REASON_BAD_DEPTH)) {
+            // We should never get here
+            init_disarm_motors();
+        }
+    }
+}
+
 void Sub::failsafe_ekf_check(void)
 {
     static uint32_t last_ekf_good_ms = 0;

ArduSub/sensors.cpp

@@ -18,6 +18,8 @@ void Sub::read_barometer(void)
     baro_climbrate = barometer.get_climb_rate() * 100.0f;
 
     motors.set_air_density_ratio(barometer.get_air_density_ratio());
+
+    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 (ap.depth_sensor_present) { // we can use the external pressure sensor for a very accurate and current measure of our z axis position
+    if (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

@@ -134,14 +134,15 @@ void Sub::init_ardupilot()
     barometer.update();
 
     for (uint8_t i = 0; i < barometer.num_instances(); i++) {
-        if (barometer.get_type(i) == AP_Baro::BARO_TYPE_WATER && barometer.healthy(i)) {
+        if (barometer.get_type(i) == AP_Baro::BARO_TYPE_WATER) {
             barometer.set_primary_baro(i);
-            ap.depth_sensor_present = true;
+            depth_sensor_idx = i;
+            sensor_health.depth = barometer.healthy(i);
             break;
         }
     }
 
-    if (!ap.depth_sensor_present) {
+    if (!sensor_health.depth) {
         // We only have onboard baro
         // No external underwater depth sensor detected
         barometer.set_primary_baro(0);