vehicle_odometry: add timestamp_sample field for latency monitoring

PX4 · Apr 28, 2020 · 5ffe886 · 5ffe886
1 parent ccaa103
commit 5ffe886
Show file tree

Hide file tree

Showing 9 changed files with 26 additions and 15 deletions.
diff --git a/msg/vehicle_odometry.msg b/msg/vehicle_odometry.msg
@@ -1,6 +1,8 @@
 # Vehicle odometry data. Fits ROS REP 147 for aerial vehicles
 uint64 timestamp		# time since system start (microseconds)
 
+uint64 timestamp_sample
+
 # Covariance matrix index constants
 uint8 COVARIANCE_MATRIX_X_VARIANCE=0
 uint8 COVARIANCE_MATRIX_Y_VARIANCE=6

diff --git a/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp b/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp
@@ -274,7 +274,7 @@ AttitudeEstimatorQ::Run()
 					// vision external heading usage (ATT_EXT_HDG_M 1)
 					if (_param_att_ext_hdg_m.get() == 1) {
 						// Check for timeouts on data
-						_ext_hdg_good = vision.timestamp > 0 && (hrt_elapsed_time(&vision.timestamp) < 500000);
+						_ext_hdg_good = vision.timestamp_sample > 0 && (hrt_elapsed_time(&vision.timestamp_sample) < 500000);
 					}
 				}
 			}
@@ -303,7 +303,7 @@ AttitudeEstimatorQ::Run()
 					// Motion Capture external heading usage (ATT_EXT_HDG_M 2)
 					if (_param_att_ext_hdg_m.get() == 2) {
 						// Check for timeouts on data
-						_ext_hdg_good = mocap.timestamp > 0 && (hrt_elapsed_time(&mocap.timestamp) < 500000);
+						_ext_hdg_good = mocap.timestamp_sample > 0 && (hrt_elapsed_time(&mocap.timestamp_sample) < 500000);
 					}
 				}
 			}

diff --git a/src/modules/ekf2/ekf2_main.cpp b/src/modules/ekf2/ekf2_main.cpp
@@ -1169,7 +1169,7 @@ void Ekf2::Run()
 			}
 
 			// use timestamp from external computer, clocks are synchronized when using MAVROS
-			ev_data.time_us = _ev_odom.timestamp;
+			ev_data.time_us = _ev_odom.timestamp_sample;
 			_ekf.setExtVisionData(ev_data);
 
 			ekf2_timestamps.visual_odometry_timestamp_rel = (int16_t)((int64_t)_ev_odom.timestamp / 100 -
@@ -1230,7 +1230,9 @@ void Ekf2::Run()
 				vehicle_odometry_s odom{};
 
 				lpos.timestamp = now;
-				odom.timestamp = lpos.timestamp;
+
+				odom.timestamp = hrt_absolute_time();
+				odom.timestamp_sample = now;
 
 				odom.local_frame = odom.LOCAL_FRAME_NED;
 

diff --git a/src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp b/src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp
@@ -636,7 +636,8 @@ 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 = _timeStamp;
+		_pub_odom.get().timestamp = hrt_absolute_time();
+		_pub_odom.get().timestamp_sample = _timeStamp;
 		_pub_odom.get().local_frame = _pub_odom.get().LOCAL_FRAME_NED;
 
 		// position

diff --git a/src/modules/local_position_estimator/sensors/mocap.cpp b/src/modules/local_position_estimator/sensors/mocap.cpp
@@ -79,11 +79,11 @@ int BlockLocalPositionEstimator::mocapMeasure(Vector<float, n_y_mocap> &y)
 	}
 
 	if (!_mocap_xy_valid || !_mocap_z_valid) {
-		_time_last_mocap = _sub_mocap_odom.get().timestamp;
+		_time_last_mocap = _sub_mocap_odom.get().timestamp_sample;
 		return -1;
 
 	} else {
-		_time_last_mocap = _sub_mocap_odom.get().timestamp;
+		_time_last_mocap = _sub_mocap_odom.get().timestamp_sample;
 
 		if (PX4_ISFINITE(_sub_mocap_odom.get().x)) {
 			y.setZero();

diff --git a/src/modules/local_position_estimator/sensors/vision.cpp b/src/modules/local_position_estimator/sensors/vision.cpp
@@ -84,11 +84,11 @@ int BlockLocalPositionEstimator::visionMeasure(Vector<float, n_y_vision> &y)
 	}
 
 	if (!_vision_xy_valid || !_vision_z_valid) {
-		_time_last_vision_p = _sub_visual_odom.get().timestamp;
+		_time_last_vision_p = _sub_visual_odom.get().timestamp_sample;
 		return -1;
 
 	} else {
-		_time_last_vision_p = _sub_visual_odom.get().timestamp;
+		_time_last_vision_p = _sub_visual_odom.get().timestamp_sample;
 
 		if (PX4_ISFINITE(_sub_visual_odom.get().x)) {
 			y.setZero();
@@ -147,7 +147,7 @@ void BlockLocalPositionEstimator::visionCorrect()
 	// vision delayed x
 	uint8_t i_hist = 0;
 
-	float vision_delay = (_timeStamp - _sub_visual_odom.get().timestamp) * 1e-6f;	// measurement delay in seconds
+	float vision_delay = (_timeStamp - _sub_visual_odom.get().timestamp_sample) * 1e-6f;	// measurement delay in seconds
 
 	if (vision_delay < 0.0f) { vision_delay = 0.0f; }
 

diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp
@@ -2558,7 +2558,7 @@ class MavlinkStreamOdometry : public MavlinkStream
 		}
 
 		if (odom_updated) {
-			msg.time_usec = odom.timestamp;
+			msg.time_usec = odom.timestamp_sample;
 			msg.child_frame_id = MAV_FRAME_BODY_FRD;
 
 			// Current position
@@ -2972,7 +2972,7 @@ class MavlinkStreamAttPosMocap : public MavlinkStream
 		if (_mocap_sub.update(&mocap)) {
 			mavlink_att_pos_mocap_t msg{};
 
-			msg.time_usec = mocap.timestamp;
+			msg.time_usec = mocap.timestamp_sample;
 			msg.q[0] = mocap.q[0];
 			msg.q[1] = mocap.q[1];
 			msg.q[2] = mocap.q[2];

diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp
@@ -773,7 +773,9 @@ MavlinkReceiver::handle_message_att_pos_mocap(mavlink_message_t *msg)
 
 	vehicle_odometry_s mocap_odom{};
 
-	mocap_odom.timestamp = _mavlink_timesync.sync_stamp(mocap.time_usec);
+	mocap_odom.timestamp = hrt_absolute_time();
+	mocap_odom.timestamp_sample = _mavlink_timesync.sync_stamp(mocap.time_usec);
+
 	mocap_odom.x = mocap.x;
 	mocap_odom.y = mocap.y;
 	mocap_odom.z = mocap.z;
@@ -1251,7 +1253,9 @@ MavlinkReceiver::handle_message_vision_position_estimate(mavlink_message_t *msg)
 
 	vehicle_odometry_s visual_odom{};
 
-	visual_odom.timestamp = _mavlink_timesync.sync_stamp(ev.usec);
+	visual_odom.timestamp = hrt_absolute_time();
+	visual_odom.timestamp_sample = _mavlink_timesync.sync_stamp(ev.usec);
+
 	visual_odom.x = ev.x;
 	visual_odom.y = ev.y;
 	visual_odom.z = ev.z;
@@ -1291,7 +1295,8 @@ MavlinkReceiver::handle_message_odometry(mavlink_message_t *msg)
 
 	vehicle_odometry_s odometry{};
 
-	odometry.timestamp = _mavlink_timesync.sync_stamp(odom.time_usec);
+	odometry.timestamp = hrt_absolute_time();
+	odometry.timestamp_sample = _mavlink_timesync.sync_stamp(odom.time_usec);
 
 	/* The position is in a local FRD frame */
 	odometry.x = odom.x;

diff --git a/src/modules/simulator/simulator_mavlink.cpp b/src/modules/simulator/simulator_mavlink.cpp
@@ -1011,6 +1011,7 @@ int Simulator::publish_odometry_topic(const mavlink_message_t *odom_mavlink)
 	struct vehicle_odometry_s odom;
 
 	odom.timestamp = timestamp;
+	odom.timestamp_sample = timestamp;
 
 	const size_t POS_URT_SIZE = sizeof(odom.pose_covariance) / sizeof(odom.pose_covariance[0]);