From 41f3e1f9b43f95a2840fd3b2ee8b0ea9450de6b3 Mon Sep 17 00:00:00 2001
From: Paul Riseborough <p_riseborough@live.com.au>
Date: Wed, 6 Dec 2017 09:54:06 +1100
Subject: [PATCH] commander: centralise home position publication

---
 src/modules/commander/commander.cpp | 88 ++++++++++++++---------------
 1 file changed, 41 insertions(+), 47 deletions(-)

diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp
index bb9f601f62..02a4b7de86 100644
--- a/src/modules/commander/commander.cpp
+++ b/src/modules/commander/commander.cpp
@@ -1237,49 +1237,54 @@ static void commander_set_home_position(orb_advert_t &homePub, home_position_s &
 					const vehicle_local_position_s &localPosition, const vehicle_global_position_s &globalPosition,
 					const vehicle_attitude_s &attitude)
 {
-	//Need global and local position fix to be able to set home
-	if (!status_flags.condition_global_position_valid || !status_flags.condition_local_position_valid) {
+	// Need global and local position fix to be able to set home
+	// Ensure that the GPS accuracy is good enough
+	if (status_flags.condition_global_position_valid
+			&& status_flags.condition_local_position_valid
+			&& (globalPosition.eph > eph_threshold || globalPosition.epv > epv_threshold)) {
+
+		//Set home position
+		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;
+		home.z = localPosition.z;
+
+		matrix::Eulerf euler = matrix::Quatf(attitude.q);
+		home.yaw = euler.psi();
+
+		PX4_INFO("home: %.7f, %.7f, %.2f", home.lat, home.lon, (double)home.alt);
+
+		//Play tune first time we initialize HOME
+		if (!status_flags.condition_home_position_valid) {
+			tune_home_set(true);
+		}
+
+		/* mark home position as set */
+		status_flags.condition_home_position_valid = true;
+
+	} else if (!home.valid_alt && localPosition.z_global) {
+		// Handle case where we have started global height estimation after takeoff and can only set the home altitude
+		home.timestamp = hrt_absolute_time();
+		home.alt = localPosition.ref_alt;
+		home.valid_alt = true;
+		PX4_INFO("home alt: %.2f", (double)home.alt);
+	} else {
 		return;
 	}
 
-	//Ensure that the GPS accuracy is good enough for intializing home
-	if (globalPosition.eph > eph_threshold || globalPosition.epv > epv_threshold) {
-		return;
-	}
-
-	//Set home position
-	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;
-	home.z = localPosition.z;
-
-	matrix::Eulerf euler = matrix::Quatf(attitude.q);
-	home.yaw = euler.psi();
-
-	PX4_INFO("home: %.7f, %.7f, %.2f", home.lat, home.lon, (double)home.alt);
-
 	/* announce new home position */
 	if (homePub != nullptr) {
 		orb_publish(ORB_ID(home_position), homePub, &home);
-
 	} else {
 		homePub = orb_advertise(ORB_ID(home_position), &home);
 	}
-
-	//Play tune first time we initialize HOME
-	if (!status_flags.condition_home_position_valid) {
-		tune_home_set(true);
-	}
-
-	/* mark home position as set */
-	status_flags.condition_home_position_valid = true;
 }
 
 int commander_thread_main(int argc, char *argv[])
@@ -3120,20 +3125,9 @@ 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;
+		else if (!_home.valid_alt && local_position.z_global) {
+			commander_set_home_position(home_pub, _home, local_position, global_position, attitude);
 
-			/* 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