From b5be990109a00814102005b6b809d64daadc40d8 Mon Sep 17 00:00:00 2001
From: Paul Riseborough <p_riseborough@live.com.au>
Date: Thu, 7 Dec 2017 10:40:42 +1100
Subject: [PATCH] lpe: fix incorrect setting of local_position.z_global

---
 .../local_position_estimator/BlockLocalPositionEstimator.cpp   | 3 ++-
 .../local_position_estimator/BlockLocalPositionEstimator.hpp   | 1 +
 src/modules/local_position_estimator/sensors/baro.cpp          | 1 +
 src/modules/local_position_estimator/sensors/gps.cpp           | 1 +
 src/modules/local_position_estimator/sensors/mocap.cpp         | 1 +
 src/modules/local_position_estimator/sensors/vision.cpp        | 1 +
 6 files changed, 7 insertions(+), 1 deletion(-)

diff --git a/src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp b/src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp
index 6c170a18b0..a32444ce1a 100644
--- a/src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp
+++ b/src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp
@@ -138,6 +138,7 @@ BlockLocalPositionEstimator::BlockLocalPositionEstimator() :
 	// reference altitudes
 	_altOrigin(0),
 	_altOriginInitialized(false),
+	_altOriginGlobal(false),
 	_baroAltOrigin(0),
 	_gpsAltOrigin(0),
 
@@ -614,7 +615,7 @@ void BlockLocalPositionEstimator::publishLocalPos()
 
 		_pub_lpos.get().yaw = _eul(2);
 		_pub_lpos.get().xy_global = _estimatorInitialized & EST_XY;
-		_pub_lpos.get().z_global = !(_sensorTimeout & SENSOR_BARO);
+		_pub_lpos.get().z_global = !(_sensorTimeout & SENSOR_BARO) && _altOriginGlobal;
 		_pub_lpos.get().ref_timestamp = _time_origin;
 		_pub_lpos.get().ref_lat = _map_ref.lat_rad * 180 / M_PI;
 		_pub_lpos.get().ref_lon = _map_ref.lon_rad * 180 / M_PI;
diff --git a/src/modules/local_position_estimator/BlockLocalPositionEstimator.hpp b/src/modules/local_position_estimator/BlockLocalPositionEstimator.hpp
index 4bc7ae1b77..44fc1d2124 100644
--- a/src/modules/local_position_estimator/BlockLocalPositionEstimator.hpp
+++ b/src/modules/local_position_estimator/BlockLocalPositionEstimator.hpp
@@ -365,6 +365,7 @@ private:
 	// reference altitudes
 	float _altOrigin;
 	bool _altOriginInitialized;
+	bool _altOriginGlobal; // true when the altitude of the origin is defined wrt a global reference frame
 	float _baroAltOrigin;
 	float _gpsAltOrigin;
 
diff --git a/src/modules/local_position_estimator/sensors/baro.cpp b/src/modules/local_position_estimator/sensors/baro.cpp
index 2948f673cb..12f8d1f5a6 100644
--- a/src/modules/local_position_estimator/sensors/baro.cpp
+++ b/src/modules/local_position_estimator/sensors/baro.cpp
@@ -31,6 +31,7 @@ void BlockLocalPositionEstimator::baroInit()
 
 		if (!_altOriginInitialized) {
 			_altOriginInitialized = true;
+			_altOriginGlobal = false;
 			_altOrigin = _baroAltOrigin;
 		}
 	}
diff --git a/src/modules/local_position_estimator/sensors/gps.cpp b/src/modules/local_position_estimator/sensors/gps.cpp
index 63cd63ac6d..d141a78658 100644
--- a/src/modules/local_position_estimator/sensors/gps.cpp
+++ b/src/modules/local_position_estimator/sensors/gps.cpp
@@ -73,6 +73,7 @@ void BlockLocalPositionEstimator::gpsInit()
 				// possible baro offset in global altitude at init
 				_altOrigin = _gpsAltOrigin;
 				_altOriginInitialized = true;
+				_altOriginGlobal = true;
 
 				mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] global origin init (gps) : lat %6.2f lon %6.2f alt %5.1f m",
 							     gpsLatOrigin, gpsLonOrigin, double(_gpsAltOrigin));
diff --git a/src/modules/local_position_estimator/sensors/mocap.cpp b/src/modules/local_position_estimator/sensors/mocap.cpp
index 248fad388f..af5fd9137c 100644
--- a/src/modules/local_position_estimator/sensors/mocap.cpp
+++ b/src/modules/local_position_estimator/sensors/mocap.cpp
@@ -34,6 +34,7 @@ void BlockLocalPositionEstimator::mocapInit()
 
 		if (!_altOriginInitialized) {
 			_altOriginInitialized = true;
+			_altOriginGlobal = false;
 			_altOrigin = 0;
 		}
 	}
diff --git a/src/modules/local_position_estimator/sensors/vision.cpp b/src/modules/local_position_estimator/sensors/vision.cpp
index 9f3d99ee2d..7299499f91 100644
--- a/src/modules/local_position_estimator/sensors/vision.cpp
+++ b/src/modules/local_position_estimator/sensors/vision.cpp
@@ -48,6 +48,7 @@ void BlockLocalPositionEstimator::visionInit()
 
 		if (!_altOriginInitialized) {
 			_altOriginInitialized = true;
+			_altOriginGlobal = true;
 			_altOrigin = _sub_vision_pos.get().z_global ? _sub_vision_pos.get().ref_alt : 0.0f;
 		}
 	}