From aa6c2fe864c7644efec52ff4a097c6d4799d3f81 Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <andrew@tridgell.net>
Date: Fri, 17 Feb 2023 09:03:22 +1100
Subject: [PATCH] Plane: ensure home is up to date on arming

remove any discrepancy which has crept in over the last few seconds

this also ensures that relative_altitude is updated, and copes with
the EKF refusing the resetHeightDatum call
---
 ArduPlane/AP_Arming.cpp | 12 ++++++++++++
 ArduPlane/ArduPlane.cpp | 17 +++++++++++++----
 ArduPlane/Plane.h       |  8 +++++++-
 ArduPlane/commands.cpp  | 24 ++++++++++++++++--------
 4 files changed, 48 insertions(+), 13 deletions(-)

diff --git a/ArduPlane/AP_Arming.cpp b/ArduPlane/AP_Arming.cpp
index 6c05442d35..d527469f59 100644
--- a/ArduPlane/AP_Arming.cpp
+++ b/ArduPlane/AP_Arming.cpp
@@ -306,6 +306,18 @@ bool AP_Arming_Plane::arm(const AP_Arming::Method method, const bool do_arming_c
         return false;
     }
 
+    if (plane.update_home()) {
+        // after update_home the home position could still be
+        // different from the current_loc if the EKF refused the
+        // resetHeightDatum call. If we are updating home then we want
+        // to force the home to be the current_loc so relative alt
+        // takeoffs work correctly
+        if (plane.ahrs.set_home(plane.current_loc)) {
+            // update current_loc
+            plane.update_current_loc();
+        }
+    }
+
     change_arm_state();
 
     // rising edge of delay_arming oneshot
diff --git a/ArduPlane/ArduPlane.cpp b/ArduPlane/ArduPlane.cpp
index 2f6c069b48..90e8d32bc7 100644
--- a/ArduPlane/ArduPlane.cpp
+++ b/ArduPlane/ArduPlane.cpp
@@ -406,10 +406,7 @@ void Plane::update_GPS_50Hz(void)
 {
     gps.update();
 
-    // get position from AHRS
-    have_position = ahrs.get_location(current_loc);
-    ahrs.get_relative_position_D_home(relative_altitude);
-    relative_altitude *= -1.0f;
+    update_current_loc();
 }
 
 /*
@@ -816,4 +813,16 @@ void Plane::get_osd_roll_pitch_rad(float &roll, float &pitch) const
     }
 }
 
+/*
+  update current_loc Location
+ */
+void Plane::update_current_loc(void)
+{
+    have_position = plane.ahrs.get_location(plane.current_loc);
+
+    // re-calculate relative altitude
+    ahrs.get_relative_position_D_home(plane.relative_altitude);
+    relative_altitude *= -1.0f;
+}
+
 AP_HAL_MAIN_CALLBACKS(&plane);
diff --git a/ArduPlane/Plane.h b/ArduPlane/Plane.h
index 637c1b169a..028e04d999 100644
--- a/ArduPlane/Plane.h
+++ b/ArduPlane/Plane.h
@@ -946,7 +946,13 @@ private:
 
     // commands.cpp
     void set_guided_WP(const Location &loc);
-    void update_home();
+
+    // update home position. Return true if update done
+    bool update_home();
+
+    // update current_loc
+    void update_current_loc(void);
+
     // set home location and store it persistently:
     bool set_home_persistently(const Location &loc) WARN_IF_UNUSED;
 
diff --git a/ArduPlane/commands.cpp b/ArduPlane/commands.cpp
index 3a0900acdb..027708e443 100644
--- a/ArduPlane/commands.cpp
+++ b/ArduPlane/commands.cpp
@@ -105,11 +105,13 @@ void Plane::set_guided_WP(const Location &loc)
   update home location from GPS
   this is called as long as we have 3D lock and the arming switch is
   not pushed
+
+  returns true if home is changed
 */
-void Plane::update_home()
+bool Plane::update_home()
 {
     if (hal.util->was_watchdog_armed()) {
-        return;
+        return false;
     }
     if ((g2.home_reset_threshold == -1) ||
         ((g2.home_reset_threshold > 0) &&
@@ -118,24 +120,30 @@ void Plane::update_home()
         // significantly. This allows us to cope with slow baro drift
         // but not re-do home and the baro if we have changed height
         // significantly
-        return;
+        return false;
     }
-    if (ahrs.home_is_set() && !ahrs.home_is_locked()) {
+    bool ret = false;
+    if (ahrs.home_is_set() && !ahrs.home_is_locked() && gps.status() >= AP_GPS::GPS_OK_FIX_3D) {
         Location loc;
-        if(ahrs.get_location(loc) && gps.status() >= AP_GPS::GPS_OK_FIX_3D) {
+        if (ahrs.get_location(loc)) {
             // we take the altitude directly from the GPS as we are
             // about to reset the baro calibration. We can't use AHRS
             // altitude or we can end up perpetuating a bias in
             // altitude, as AHRS alt depends on home alt, which means
             // we would have a circular dependency
             loc.alt = gps.location().alt;
-            if (!AP::ahrs().set_home(loc)) {
-                // silently fail
-            }
+            ret = AP::ahrs().set_home(loc);
         }
     }
+
+    // even if home is not updated we do a baro reset to stop baro
+    // drift errors while disarmed
     barometer.update_calibration();
     ahrs.resetHeightDatum();
+
+    update_current_loc();
+
+    return ret;
 }
 
 bool Plane::set_home_persistently(const Location &loc)