From 131e1bdef58bd4784ec2f2df472bb0b2bcd33d48 Mon Sep 17 00:00:00 2001 From: Jacob Walser Date: Wed, 3 May 2017 18:13:06 -0400 Subject: [PATCH] Sub: Bugfix for external baro failsafe handling when no baro is connected at boot --- ArduSub/Sub.h | 1 + ArduSub/control_althold.cpp | 2 +- ArduSub/control_surface.cpp | 2 +- ArduSub/failsafe.cpp | 5 ++++- ArduSub/sensors.cpp | 4 +++- ArduSub/surface_bottom_detector.cpp | 2 +- ArduSub/system.cpp | 10 +++++----- 7 files changed, 16 insertions(+), 10 deletions(-) diff --git a/ArduSub/Sub.h b/ArduSub/Sub.h index c4450d1bb7..4b69e0e165 100644 --- a/ArduSub/Sub.h +++ b/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; diff --git a/ArduSub/control_althold.cpp b/ArduSub/control_althold.cpp index 227a52362f..aaf0572153 100644 --- a/ArduSub/control_althold.cpp +++ b/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; } diff --git a/ArduSub/control_surface.cpp b/ArduSub/control_surface.cpp index d6b8e44266..1fe03b1c70 100644 --- a/ArduSub/control_surface.cpp +++ b/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; } diff --git a/ArduSub/failsafe.cpp b/ArduSub/failsafe.cpp index 7b6cac0581..35f7717e71 100644 --- a/ArduSub/failsafe.cpp +++ b/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; diff --git a/ArduSub/sensors.cpp b/ArduSub/sensors.cpp index 57dc7adde8..19e96543af 100644 --- a/ArduSub/sensors.cpp +++ b/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) diff --git a/ArduSub/surface_bottom_detector.cpp b/ArduSub/surface_bottom_detector.cpp index 760f185077..6441eed010 100644 --- a/ArduSub/surface_bottom_detector.cpp +++ b/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 diff --git a/ArduSub/system.cpp b/ArduSub/system.cpp index a586373372..483457be10 100644 --- a/ArduSub/system.cpp +++ b/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); - break; + ap.depth_sensor_present = true; + 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);