From adcf0d398d57cff88cd7de8b69d6286a2b04092b Mon Sep 17 00:00:00 2001
From: Willian Galvani <williangalvani@gmail.com>
Date: Thu, 5 Sep 2019 13:32:42 -0300
Subject: [PATCH] Sub: update calibration if reading is above water level

---
 ArduSub/sensors.cpp | 5 +++++
 1 file changed, 5 insertions(+)

diff --git a/ArduSub/sensors.cpp b/ArduSub/sensors.cpp
index 830b14b3be..a18e1db3a8 100644
--- a/ArduSub/sensors.cpp
+++ b/ArduSub/sensors.cpp
@@ -4,6 +4,11 @@
 void Sub::read_barometer()
 {
     barometer.update();
+    // If we are reading a positive altitude, the sensor needs calibration
+    // Even a few meters above the water we should have no significant depth reading
+    if(!motors.armed() && barometer.get_altitude() > 0) {
+        barometer.update_calibration();
+    }
 
     if (ap.depth_sensor_present) {
         sensor_health.depth = barometer.healthy(depth_sensor_idx);