estimator messages add explicit timestamp_sample

- timestamp is uORB publication metadata
 - this allows us to see what the system saw at publication time plus the latency in estimation

msg/estimator_innovations.msg

@@ -1,4 +1,5 @@
 uint64 timestamp		# time since system start (microseconds)
+uint64 timestamp_sample         # the timestamp of the raw data (microseconds)
 
 # GPS
 float32[2] gps_hvel	# horizontal GPS velocity innovation (m/sec) and innovation variance ((m/sec)**2)

msg/estimator_sensor_bias.msg

@@ -4,6 +4,7 @@
 #
 
 uint64 timestamp		# time since system start (microseconds)
+uint64 timestamp_sample         # the timestamp of the raw data (microseconds)
 
 # In-run bias estimates (subtract from uncorrected data)
 

msg/estimator_states.msg

@@ -1,4 +1,5 @@
 uint64 timestamp		# time since system start (microseconds)
+uint64 timestamp_sample         # the timestamp of the raw data (microseconds)
 
 float32[24] states		# Internal filter states
 uint8 n_states		# Number of states effectively used

msg/estimator_status.msg

@@ -1,4 +1,5 @@
 uint64 timestamp		# time since system start (microseconds)
+uint64 timestamp_sample         # the timestamp of the raw data (microseconds)
 
 float32[3] vibe			# IMU vibration metrics in the following array locations
 # 0 : Gyro delta angle coning metric = filtered length of (delta_angle x prev_delta_angle)

msg/vehicle_attitude.msg

@@ -2,6 +2,8 @@
 
 uint64 timestamp		# time since system start (microseconds)
 
+uint64 timestamp_sample         # the timestamp of the raw data (microseconds)
+
 float32[4] q			# Quaternion rotation from XYZ body frame to NED earth frame.
 float32[4] delta_q_reset 	# Amount by which quaternion has changed during last reset
 uint8 quat_reset_counter	# Quaternion reset counter

msg/vehicle_global_position.msg

@@ -6,6 +6,7 @@
 #
 
 uint64 timestamp		# time since system start (microseconds)
+uint64 timestamp_sample         # the timestamp of the raw data (microseconds)
 
 float64 lat			# Latitude, (degrees)
 float64 lon			# Longitude, (degrees)

msg/vehicle_local_position.msg

@@ -1,6 +1,7 @@
 # Fused local position in NED.
 
 uint64 timestamp			# time since system start (microseconds)
+uint64 timestamp_sample                 # the timestamp of the raw data (microseconds)
 
 bool xy_valid				# true if x and y are valid
 bool z_valid				# true if z is valid

msg/wind_estimate.msg

@@ -1,4 +1,6 @@
 uint64 timestamp		# time since system start (microseconds)
+uint64 timestamp_sample         # the timestamp of the raw data (microseconds)
+
 float32 windspeed_north		# Wind component in north / X direction (m/sec)
 float32 windspeed_east		# Wind component in east / Y direction (m/sec)
 

msg/yaw_estimator_status.msg

@@ -1,6 +1,9 @@
 uint64 timestamp		# time since system start (microseconds)
+uint64 timestamp_sample         # the timestamp of the raw data (microseconds)
+
 float32 yaw_composite	# composite yaw from GSF (rad)
 float32 yaw_variance	# composite yaw variance from GSF (rad^2)
+
 float32[5] yaw		# yaw estimate for each model in the filter bank (rad)
 float32[5] innov_vn	# North velocity innovation for each model in the filter bank (m/s)
 float32[5] innov_ve	# East velocity innovation for each model in the filter bank (m/s)

src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp

@@ -358,10 +358,11 @@ AttitudeEstimatorQ::Run()
 
 		if (update(dt)) {
 			vehicle_attitude_s att = {};
-			att.timestamp = sensors.timestamp;
+			att.timestamp_sample = sensors.timestamp;
 			_q.copyTo(att.q);
 
 			/* the instance count is not used here */
+			att.timestamp = hrt_absolute_time();
 			_att_pub.publish(att);
 
 		}

src/modules/ekf2/EKF2.cpp

@@ -743,14 +743,11 @@ void EKF2::Run()
 			if (control_status.flags.tilt_align) {
 				// generate vehicle local position data
 				vehicle_local_position_s &lpos = _vehicle_local_position_pub.get();
+				lpos.timestamp_sample = imu_sample_new.time_us;
 
 				// generate vehicle odometry data
 				vehicle_odometry_s odom{};
-
-				lpos.timestamp = now;
-
-				odom.timestamp = hrt_absolute_time();
-				odom.timestamp_sample = now;
+				odom.timestamp_sample = imu_sample_new.time_us;
 
 				odom.local_frame = vehicle_odometry_s::LOCAL_FRAME_NED;
 
@@ -922,9 +919,11 @@ void EKF2::Run()
 				odom.velocity_covariance[odom.COVARIANCE_MATRIX_VZ_VARIANCE] = covariances[6];
 
 				// publish vehicle local position data
+				lpos.timestamp = _replay_mode ? now : hrt_absolute_time();
 				_vehicle_local_position_pub.update();
 
 				// publish vehicle odometry data
+				odom.timestamp = _replay_mode ? now : hrt_absolute_time();
 				_vehicle_odometry_pub.publish(odom);
 
 				// publish external visual odometry after fixed frame alignment if new odometry is received
@@ -975,8 +974,7 @@ void EKF2::Run()
 				if (_ekf.global_position_is_valid() && !_preflt_checker.hasFailed()) {
 					// generate and publish global position data
 					vehicle_global_position_s &global_pos = _vehicle_global_position_pub.get();
-
-					global_pos.timestamp = now;
+					global_pos.timestamp_sample = imu_sample_new.time_us;
 
 					if (fabsf(lpos_x_prev - lpos.x) > FLT_EPSILON || fabsf(lpos_y_prev - lpos.y) > FLT_EPSILON) {
 						map_projection_reproject(&ekf_origin, lpos.x, lpos.y, &global_pos.lat, &global_pos.lon);
@@ -1002,13 +1000,14 @@ void EKF2::Run()
 					}
 
 					global_pos.dead_reckoning = _ekf.inertial_dead_reckoning(); // True if this position is estimated through dead-reckoning
-
+					global_pos.timestamp = _replay_mode ? now : hrt_absolute_time();
 					_vehicle_global_position_pub.update();
 				}
 			}
 
 			// publish estimator states
 			estimator_states_s states;
+			states.timestamp_sample = imu_sample_new.time_us;
 			states.n_states = 24;
 			_ekf.getStateAtFusionHorizonAsVector().copyTo(states.states);
 			_ekf.covariances_diagonal().copyTo(states.covariances);
@@ -1017,6 +1016,7 @@ void EKF2::Run()
 
 			// publish estimator status
 			estimator_status_s status{};
+			status.timestamp_sample = imu_sample_new.time_us;
 			_ekf.getOutputTrackingError().copyTo(status.output_tracking_error);
 			_ekf.get_gps_check_status(&status.gps_check_fail_flags);
 			// only report enabled GPS check failures (the param indexes are shifted by 1 bit, because they don't include
@@ -1039,12 +1039,12 @@ void EKF2::Run()
 			status.pre_flt_fail_innov_vel_vert = _preflt_checker.hasVertVelFailed();
 			status.pre_flt_fail_innov_height = _preflt_checker.hasHeightFailed();
 			status.pre_flt_fail_mag_field_disturbed = control_status.flags.mag_field_disturbed;
-
+			status.timestamp = _replay_mode ? now : hrt_absolute_time();
 			_estimator_status_pub.publish(status);
 
 			{
 				// publish all corrected sensor readings and bias estimates after mag calibration is updated above
-				bias.timestamp = now;
+				bias.timestamp_sample = imu_sample_new.time_us;
 
 				// take device ids from sensor_selection_s if not using specific vehicle_imu_s
 				if (_imu_sub_index < 0) {
@@ -1062,6 +1062,7 @@ void EKF2::Run()
 				bias.mag_bias[1] = _last_valid_mag_cal[1];
 				bias.mag_bias[2] = _last_valid_mag_cal[2];
 
+				bias.timestamp = _replay_mode ? now : hrt_absolute_time();
 				_estimator_sensor_bias_pub.publish(bias);
 			}
 
@@ -1071,11 +1072,11 @@ void EKF2::Run()
 
 			if (_ekf.get_gps_drift_metrics(gps_drift, &blocked)) {
 				ekf_gps_drift_s drift_data;
-				drift_data.timestamp = now;
 				drift_data.hpos_drift_rate = gps_drift[0];
 				drift_data.vpos_drift_rate = gps_drift[1];
 				drift_data.hspd = gps_drift[2];
 				drift_data.blocked = blocked;
+				drift_data.timestamp = _replay_mode ? now : hrt_absolute_time();
 
 				_ekf_gps_drift_pub.publish(drift_data);
 			}
@@ -1163,7 +1164,7 @@ void EKF2::Run()
 			{
 				// publish estimator innovation data
 				estimator_innovations_s innovations;
-				innovations.timestamp = now;
+				innovations.timestamp_sample = imu_sample_new.time_us;
 				_ekf.getGpsVelPosInnov(&innovations.gps_hvel[0], innovations.gps_vvel, &innovations.gps_hpos[0],
 						       innovations.gps_vpos);
 				_ekf.getEvVelPosInnov(&innovations.ev_hvel[0], innovations.ev_vvel, &innovations.ev_hpos[0], innovations.ev_vpos);
@@ -1184,7 +1185,7 @@ void EKF2::Run()
 
 				// publish estimator innovation variance data
 				estimator_innovations_s innovation_var;
-				innovation_var.timestamp = now;
+				innovation_var.timestamp_sample = imu_sample_new.time_us;
 				_ekf.getGpsVelPosInnovVar(&innovation_var.gps_hvel[0], innovation_var.gps_vvel, &innovation_var.gps_hpos[0],
 							  innovation_var.gps_vpos);
 				_ekf.getEvVelPosInnovVar(&innovation_var.ev_hvel[0], innovation_var.ev_vvel, &innovation_var.ev_hpos[0],
@@ -1207,7 +1208,7 @@ void EKF2::Run()
 
 				// publish estimator innovation test ratio data
 				estimator_innovations_s test_ratios;
-				test_ratios.timestamp = now;
+				test_ratios.timestamp_sample = imu_sample_new.time_us;
 				_ekf.getGpsVelPosInnovRatio(test_ratios.gps_hvel[0], test_ratios.gps_vvel, test_ratios.gps_hpos[0],
 							    test_ratios.gps_vpos);
 				_ekf.getEvVelPosInnovRatio(test_ratios.ev_hvel[0], test_ratios.ev_vvel, test_ratios.ev_hpos[0],
@@ -1236,10 +1237,14 @@ void EKF2::Run()
 					resetPreFlightChecks();
 				}
 
+				innovations.timestamp = _replay_mode ? now : hrt_absolute_time();
 				_estimator_innovations_pub.publish(innovations);
-				_estimator_innovation_variances_pub.publish(innovation_var);
-				_estimator_innovation_test_ratios_pub.publish(test_ratios);
 
+				innovation_var.timestamp = _replay_mode ? now : hrt_absolute_time();
+				_estimator_innovation_variances_pub.publish(innovation_var);
+
+				test_ratios.timestamp = _replay_mode ? now : hrt_absolute_time();
+				_estimator_innovation_test_ratios_pub.publish(test_ratios);
 			}
 		}
 
@@ -1318,13 +1323,12 @@ void EKF2::publish_attitude(const hrt_abstime &timestamp)
 	if (_ekf.attitude_valid()) {
 		// generate vehicle attitude quaternion data
 		vehicle_attitude_s att;
-		att.timestamp = timestamp;
-
+		att.timestamp_sample = timestamp;
 		const Quatf q{_ekf.calculate_quaternion()};
 		q.copyTo(att.q);
 
 		_ekf.get_quat_reset(&att.delta_q_reset[0], &att.quat_reset_counter);
-
+		att.timestamp = _replay_mode ? timestamp : hrt_absolute_time();
 		_att_pub.publish(att);
 
 	}  else if (_replay_mode) {
@@ -1347,7 +1351,8 @@ void EKF2::publish_yaw_estimator_status(const hrt_abstime &timestamp)
 			       &yaw_est_test_data.innov_vn[0], &yaw_est_test_data.innov_ve[0],
 			       &yaw_est_test_data.weight[0])) {
 
-		yaw_est_test_data.timestamp = timestamp;
+		yaw_est_test_data.timestamp_sample = timestamp;
+		yaw_est_test_data.timestamp = _replay_mode ? timestamp : hrt_absolute_time();
 
 		_yaw_est_pub.publish(yaw_est_test_data);
 	}
@@ -1358,18 +1363,21 @@ void EKF2::publish_wind_estimate(const hrt_abstime &timestamp)
 	if (_ekf.get_wind_status()) {
 		// Publish wind estimate only if ekf declares them valid
 		wind_estimate_s wind_estimate{};
+		wind_estimate.timestamp_sample = timestamp;
+
 		const Vector2f wind_vel = _ekf.getWindVelocity();
 		const Vector2f wind_vel_var = _ekf.getWindVelocityVariance();
 		_ekf.getAirspeedInnov(wind_estimate.tas_innov);
 		_ekf.getAirspeedInnovVar(wind_estimate.tas_innov_var);
 		_ekf.getBetaInnov(wind_estimate.beta_innov);
 		_ekf.getBetaInnovVar(wind_estimate.beta_innov_var);
-		wind_estimate.timestamp = timestamp;
+
 		wind_estimate.windspeed_north = wind_vel(0);
 		wind_estimate.windspeed_east = wind_vel(1);
 		wind_estimate.variance_north = wind_vel_var(0);
 		wind_estimate.variance_east = wind_vel_var(1);
 		wind_estimate.tas_scale = 0.0f; //leave at 0 as scale is not estimated in ekf
+		wind_estimate.timestamp = _replay_mode ? timestamp : hrt_absolute_time();
 
 		_wind_pub.publish(wind_estimate);
 	}

src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp

@@ -500,9 +500,13 @@ void BlockLocalPositionEstimator::Run()
 		publishLocalPos();
 		publishOdom();
 		publishEstimatorStatus();
-		_pub_innov.get().timestamp = _timeStamp;
+
+		_pub_innov.get().timestamp_sample = _timeStamp;
+		_pub_innov.get().timestamp = hrt_absolute_time();
 		_pub_innov.update();
-		_pub_innov_var.get().timestamp = _timeStamp;
+
+		_pub_innov_var.get().timestamp_sample = _timeStamp;
+		_pub_innov_var.get().timestamp = hrt_absolute_time();
 		_pub_innov_var.update();
 
 		if ((_estimatorInitialized & EST_XY) && (_map_ref.init_done || _param_lpe_fake_origin.get())) {
@@ -578,7 +582,7 @@ void BlockLocalPositionEstimator::publishLocalPos()
 	if (PX4_ISFINITE(_x(X_x)) && PX4_ISFINITE(_x(X_y)) && PX4_ISFINITE(_x(X_z)) &&
 	    PX4_ISFINITE(_x(X_vx)) && PX4_ISFINITE(_x(X_vy))
 	    && PX4_ISFINITE(_x(X_vz))) {
-		_pub_lpos.get().timestamp = _timeStamp;
+		_pub_lpos.get().timestamp_sample = _timeStamp;
 
 		_pub_lpos.get().xy_valid = _estimatorInitialized & EST_XY;
 		_pub_lpos.get().z_valid = _estimatorInitialized & EST_Z;
@@ -628,6 +632,7 @@ void BlockLocalPositionEstimator::publishLocalPos()
 		_pub_lpos.get().vz_max = INFINITY;
 		_pub_lpos.get().hagl_min = INFINITY;
 		_pub_lpos.get().hagl_max = INFINITY;
+		_pub_lpos.get().timestamp = hrt_absolute_time();;
 		_pub_lpos.update();
 	}
 }
@@ -640,7 +645,7 @@ void BlockLocalPositionEstimator::publishOdom()
 	if (PX4_ISFINITE(_x(X_x)) && PX4_ISFINITE(_x(X_y)) && PX4_ISFINITE(_x(X_z)) &&
 	    PX4_ISFINITE(_x(X_vx)) && PX4_ISFINITE(_x(X_vy))
 	    && PX4_ISFINITE(_x(X_vz))) {
-		_pub_odom.get().timestamp = hrt_absolute_time();
+
 		_pub_odom.get().timestamp_sample = _timeStamp;
 		_pub_odom.get().local_frame = vehicle_odometry_s::LOCAL_FRAME_NED;
 
@@ -706,14 +711,14 @@ void BlockLocalPositionEstimator::publishOdom()
 		_pub_odom.get().velocity_covariance[_pub_odom.get().COVARIANCE_MATRIX_PITCHRATE_VARIANCE] = NAN;
 		_pub_odom.get().velocity_covariance[_pub_odom.get().COVARIANCE_MATRIX_YAWRATE_VARIANCE] = NAN;
 
+		_pub_odom.get().timestamp = hrt_absolute_time();
 		_pub_odom.update();
 	}
 }
 
 void BlockLocalPositionEstimator::publishEstimatorStatus()
 {
-	_pub_est_states.get().timestamp = _timeStamp;
-	_pub_est_status.get().timestamp = _timeStamp;
+	_pub_est_states.get().timestamp_sample = _timeStamp;
 
 	for (size_t i = 0; i < n_x; i++) {
 		_pub_est_states.get().states[i] = _x(i);
@@ -752,11 +757,17 @@ void BlockLocalPositionEstimator::publishEstimatorStatus()
 	_pub_est_states.get().covariances[23] = NAN;
 
 	_pub_est_states.get().n_states = n_x;
+	_pub_est_states.get().timestamp = hrt_absolute_time();
+	_pub_est_states.update();
+
+	// estimator_status
+	_pub_est_status.get().timestamp_sample = _timeStamp;
 	_pub_est_status.get().health_flags = _sensorFault;
 	_pub_est_status.get().timeout_flags = _sensorTimeout;
 	_pub_est_status.get().pos_horiz_accuracy = _pub_gpos.get().eph;
 	_pub_est_status.get().pos_vert_accuracy = _pub_gpos.get().epv;
 
+	_pub_est_status.get().timestamp = hrt_absolute_time();
 	_pub_est_status.update();
 }
 
@@ -790,7 +801,7 @@ void BlockLocalPositionEstimator::publishGlobalPos()
 	if (PX4_ISFINITE(lat) && PX4_ISFINITE(lon) && PX4_ISFINITE(alt) &&
 	    PX4_ISFINITE(xLP(X_vx)) && PX4_ISFINITE(xLP(X_vy)) &&
 	    PX4_ISFINITE(xLP(X_vz))) {
-		_pub_gpos.get().timestamp = _timeStamp;
+		_pub_gpos.get().timestamp_sample = _timeStamp;
 		_pub_gpos.get().lat = lat;
 		_pub_gpos.get().lon = lon;
 		_pub_gpos.get().alt = alt;
@@ -799,6 +810,7 @@ void BlockLocalPositionEstimator::publishGlobalPos()
 		_pub_gpos.get().terrain_alt = _altOrigin - xLP(X_tz);
 		_pub_gpos.get().terrain_alt_valid = _estimatorInitialized & EST_TZ;
 		_pub_gpos.get().dead_reckoning = !(_estimatorInitialized & EST_XY);
+		_pub_gpos.get().timestamp = hrt_absolute_time();
 		_pub_gpos.update();
 	}
 }