lpe: fix incorrect setting of local_position.z_global

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;

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;
 

src/modules/local_position_estimator/sensors/baro.cpp

@@ -31,6 +31,7 @@ void BlockLocalPositionEstimator::baroInit()
 
 		if (!_altOriginInitialized) {
 			_altOriginInitialized = true;
+			_altOriginGlobal = false;
 			_altOrigin = _baroAltOrigin;
 		}
 	}

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));

src/modules/local_position_estimator/sensors/mocap.cpp

@@ -34,6 +34,7 @@ void BlockLocalPositionEstimator::mocapInit()
 
 		if (!_altOriginInitialized) {
 			_altOriginInitialized = true;
+			_altOriginGlobal = false;
 			_altOrigin = 0;
 		}
 	}

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;
 		}
 	}