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

@@ -281,6 +281,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

ArduPlane/ArduPlane.cpp

@@ -402,10 +402,7 @@ void Plane::update_GPS_50Hz(void)
 {
     gps.update();
 
-    // get position from AHRS
+    update_current_loc();
-    have_position = ahrs.get_location(current_loc);
-    ahrs.get_relative_position_D_home(relative_altitude);
-    relative_altitude *= -1.0f;
 }
 
 /*
@@ -833,4 +830,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);

ArduPlane/Plane.h

@@ -962,7 +962,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;
 

ArduPlane/commands.cpp

@@ -113,11 +113,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) &&
@@ -126,24 +128,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)) {
+            ret = AP::ahrs().set_home(loc);
-                // silently fail
-            }
         }
     }
+
+    // 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)