diff --git a/msg/vehicle_odometry.msg b/msg/vehicle_odometry.msg index 9db03b72ad2d..7f2f023eed51 100644 --- 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 index 12a337b12fa7..af4366bb218f 100644 --- 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 index 4040201de045..37356f158d13 100644 --- 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 index a1576e5a66dd..ea71c41a055a 100644 --- 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 index 734634e6ce69..07e29be85499 100644 --- 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 &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 index a69e4c91ae93..4aea91c4f263 100644 --- 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 &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 index 711332f4f32b..0e56b6896d54 100644 --- 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 index 580b4fe777d4..403c72ccb119 100644 --- 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 index 7b336b97964f..a0756467ab34 100644 --- 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]);