Plane: prevent a discrepancy between EKF origin and home for altitude

ArduPlane/commands.cpp

@@ -131,7 +131,15 @@ void Plane::init_home()
 void Plane::update_home()
 {
     if (home_is_set == HOME_SET_NOT_LOCKED) {
-        ahrs.set_home(gps.location());
+        Location loc = gps.location();
+        Location origin;
+        // if an EKF origin is available then we leave home equal to
+        // the height of that origin. This ensures that our relative
+        // height calculations are using the same origin
+        if (ahrs.get_origin(origin)) {
+            loc.alt = origin.alt;
+        }
+        ahrs.set_home(loc);
         Log_Write_Home_And_Origin();
         GCS_MAVLINK::send_home_all(gps.location());
     }