From 934a7af579ccdec48f1b040d1e796cd89fb12873 Mon Sep 17 00:00:00 2001
From: Paul Riseborough <p_riseborough@live.com.au>
Date: Wed, 6 Dec 2017 07:34:30 +1100
Subject: [PATCH] commander: Set home alt to EKF origin if global navigation
 commences in-flight

The EKF origin height is calculated to be where the vehicle was at takeoff and is suitable as a surrogate home altitude.
---
 src/modules/commander/commander.cpp | 22 ++++++++++++++++++++++
 1 file changed, 22 insertions(+)

diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp
index 3630dd3cf1..bb9f601f62 100644
--- a/src/modules/commander/commander.cpp
+++ b/src/modules/commander/commander.cpp
@@ -1251,7 +1251,10 @@ static void commander_set_home_position(orb_advert_t &homePub, home_position_s &
 	home.timestamp = hrt_absolute_time();
 	home.lat = globalPosition.lat;
 	home.lon = globalPosition.lon;
+	home.valid_hpos = true;
+
 	home.alt = globalPosition.alt;
+	home.valid_alt = true;
 
 	home.x = localPosition.x;
 	home.y = localPosition.y;
@@ -3114,6 +3117,25 @@ int commander_thread_main(int argc, char *argv[])
 
 		}
 
+		/* Set home position altitude to EKF origin height if home is not set and the EKF has a global origin.
+		 * This allows home atitude to be used in the calculation of height above takeoff location when GPS
+		 * use has commenced after takeoff. */
+		if (!_home.valid_alt && local_position.z_global) {
+			_home.timestamp = hrt_absolute_time();
+			_home.alt = local_position.ref_alt;
+			_home.valid_alt = true;
+
+			/* publish new home altitude */
+			if (home_pub != nullptr) {
+				orb_publish(ORB_ID(home_position), home_pub, &_home);
+
+			} else {
+				home_pub = orb_advertise(ORB_ID(home_position), &_home);
+			}
+
+			PX4_INFO("home alt: %.2f", (double)_home.alt);
+		}
+
 		// check for arming state change
 		if (was_armed != armed.armed) {
 			status_changed = true;