fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-22 10:33:33 +08:00
Code Issues Packages Projects Releases Wiki Activity

vehicle_odometry: add timestamp_sample field for latency monitoring

Browse Source
This commit is contained in:
Mohammed Kabir
2020-04-28 13:58:43 -04:00
committed by Daniel Agar
parent ccaa103164
commit 5ffe88672e
9 changed files with 26 additions and 15 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/mavlink/mavlink_messages.cpp
+2 -2
View File
@@ -2558,7 +2558,7 @@ protected:
}
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 @@ protected:
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];
src/modules/mavlink/mavlink_receiver.cpp
+8 -3
View File
@@ -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;