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