add ref_timestamp (time when origin was set) to lpe

src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp

@@ -123,6 +123,7 @@ BlockLocalPositionEstimator::BlockLocalPositionEstimator() :
 	// misc
 	_polls(),
 	_timeStamp(hrt_absolute_time()),
+	_time_origin(0),
 	_timeStampLastBaro(hrt_absolute_time()),
 	_time_last_hist(0),
 	_time_last_flow(0),
@@ -378,6 +379,9 @@ void BlockLocalPositionEstimator::update()
 				    _init_origin_lat.get(),
 				    _init_origin_lon.get());
 
+		// set timestamp when origin was set to current time
+		_time_origin = _timeStamp;
+
 		mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] global origin init (parameter) : lat %6.2f lon %6.2f alt %5.1f m",
 					     double(_init_origin_lat.get()), double(_init_origin_lon.get()), double(_altOrigin));
 
@@ -610,7 +614,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().ref_timestamp = _timeStamp;
+		_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;
 		_pub_lpos.get().ref_alt = _altOrigin;

src/modules/local_position_estimator/BlockLocalPositionEstimator.hpp

@@ -345,6 +345,7 @@ private:
 	// misc
 	px4_pollfd_struct_t _polls[3];
 	uint64_t _timeStamp;
+	uint64_t _time_origin;
 	uint64_t _timeStampLastBaro;
 	uint64_t _time_last_hist;
 	uint64_t _time_last_flow;

src/modules/local_position_estimator/sensors/gps.cpp

@@ -66,6 +66,8 @@ void BlockLocalPositionEstimator::gpsInit()
 				map_projection_reproject(&_map_ref, -_x(X_x), -_x(X_y), &gpsLatOrigin, &gpsLonOrigin);
 				// reinit origin
 				map_projection_init(&_map_ref, gpsLatOrigin, gpsLonOrigin);
+				// set timestamp when origin was set to current time
+				_time_origin = _timeStamp;
 
 				// always override alt origin on first GPS to fix
 				// possible baro offset in global altitude at init

src/modules/local_position_estimator/sensors/vision.cpp

@@ -42,6 +42,8 @@ void BlockLocalPositionEstimator::visionInit()
 			mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] global origin init (vision) : lat %6.2f lon %6.2f alt %5.1f m",
 						     double(_sub_vision_pos.get().ref_lat), double(_sub_vision_pos.get().ref_lon), double(_sub_vision_pos.get().ref_alt));
 			map_projection_init(&_map_ref, _sub_vision_pos.get().ref_lat, _sub_vision_pos.get().ref_lon);
+			// set timestamp when origin was set to current time
+			_time_origin = _timeStamp;
 		}
 
 		if (!_altOriginInitialized) {