From 0f0461197099cc7e097e40d023c71b1aae485a68 Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <andrew@tridgell.net>
Date: Sun, 29 May 2022 10:03:39 +1000
Subject: [PATCH] AP_Baro: auto-set field elevation from origin

---
 libraries/AP_Baro/AP_Baro.cpp | 56 +++++++++++++++++++++++++----------
 libraries/AP_Baro/AP_Baro.h   |  3 +-
 2 files changed, 42 insertions(+), 17 deletions(-)

diff --git a/libraries/AP_Baro/AP_Baro.cpp b/libraries/AP_Baro/AP_Baro.cpp
index 3a16235cf8..1b4adcf740 100644
--- a/libraries/AP_Baro/AP_Baro.cpp
+++ b/libraries/AP_Baro/AP_Baro.cpp
@@ -226,7 +226,7 @@ const AP_Param::GroupInfo AP_Baro::var_info[] = {
 #ifndef HAL_BUILD_AP_PERIPH
     // @Param: _FIELD_ELV
     // @DisplayName: field elevation
-    // @Description: User provided field elevation in meters. This is used to improve the calculation of the altitude the vehicle is at. This parameter is not persistent and will be reset to 0 every time the vehicle is rebooted. A value of 0 means no correction for takeoff height above sea level is performed.
+    // @Description: User provided field elevation in meters. This is used to improve the calculation of the altitude the vehicle is at. This parameter is not persistent and will be reset to 0 every time the vehicle is rebooted. Changes to this parameter will only be used when disarmed. A value of 0 means the EKF origin height is used for takeoff height above sea level.
     // @Units: m
     // @Increment: 0.1
     // @Volatile: True
@@ -961,21 +961,7 @@ void AP_Baro::update(void)
         }
     }
 #ifndef HAL_BUILD_AP_PERIPH
-    const uint32_t now_ms = AP_HAL::millis();
-    if (now_ms - _field_elevation_last_ms >= 1000 && fabsf(_field_elevation_active-_field_elevation) > 1.0) {
-      if (!AP::arming().is_armed()) {
-        _field_elevation_last_ms = now_ms;
-        _field_elevation_active = _field_elevation;
-        AP::ahrs().resetHeightDatum();
-        update_calibration();
-        BARO_SEND_TEXT(MAV_SEVERITY_INFO, "Barometer Field Elevation Set: %.0fm",_field_elevation_active);
-      }
-      else {
-        _field_elevation.set(_field_elevation_active);
-        _field_elevation.notify();
-        BARO_SEND_TEXT(MAV_SEVERITY_ALERT, "Failed to Set Field Elevation: Armed");
-      }
-    }
+    update_field_elevation();
 #endif
 
     // logging
@@ -996,6 +982,44 @@ void AP_Baro::update(void)
 #endif
 }
 
+/*
+  update field elevation value
+ */
+void AP_Baro::update_field_elevation(void)
+{
+    const uint32_t now_ms = AP_HAL::millis();
+    bool new_field_elev = false;
+    const bool armed = hal.util->get_soft_armed();
+    if (now_ms - _field_elevation_last_ms >= 1000) {
+        if (is_zero(_field_elevation_active) &&
+            is_zero(_field_elevation)) {
+            // auto-set based on origin
+            Location origin;
+            if (!armed && AP::ahrs().get_origin(origin)) {
+                _field_elevation_active = origin.alt * 0.01;
+                new_field_elev = true;
+            }
+        } else if (fabsf(_field_elevation_active-_field_elevation) > 1.0 &&
+                   !is_zero(_field_elevation)) {
+            // user has set field elevation
+            if (!armed) {
+                _field_elevation_active = _field_elevation;
+                new_field_elev = true;
+            } else {
+                _field_elevation.set(_field_elevation_active);
+                _field_elevation.notify();
+                BARO_SEND_TEXT(MAV_SEVERITY_ALERT, "Failed to Set Field Elevation: Armed");
+            }
+        }
+    }
+    if (new_field_elev && !armed) {
+        _field_elevation_last_ms = now_ms;
+        AP::ahrs().resetHeightDatum();
+        update_calibration();
+        BARO_SEND_TEXT(MAV_SEVERITY_INFO, "Field Elevation Set: %.0fm", _field_elevation_active);
+    }
+}
+
 /*
   call accumulate on all drivers
  */
diff --git a/libraries/AP_Baro/AP_Baro.h b/libraries/AP_Baro/AP_Baro.h
index 72bd3d7a6e..3d423164cd 100644
--- a/libraries/AP_Baro/AP_Baro.h
+++ b/libraries/AP_Baro/AP_Baro.h
@@ -324,7 +324,8 @@ private:
     // Logging function
     void Write_Baro(void);
     void Write_Baro_instance(uint64_t time_us, uint8_t baro_instance);
-    
+
+    void update_field_elevation();
 };
 
 namespace AP {