From 0d2b70e607339c0514000b3950bf07c00d1e92c8 Mon Sep 17 00:00:00 2001 From: chobits Date: Tue, 12 Jun 2018 17:59:00 +0800 Subject: [PATCH] GCS_MAVLink: fix ATT_POS_MOCAP timestamp handle --- libraries/GCS_MAVLink/GCS_Common.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/libraries/GCS_MAVLink/GCS_Common.cpp b/libraries/GCS_MAVLink/GCS_Common.cpp index fdea7e5c45..8a17bede87 100644 --- a/libraries/GCS_MAVLink/GCS_Common.cpp +++ b/libraries/GCS_MAVLink/GCS_Common.cpp @@ -2171,7 +2171,8 @@ void GCS_MAVLINK::handle_att_pos_mocap(mavlink_message_t *msg) Quaternion attitude = Quaternion(m.q); const float posErr = 0; // parameter required? const float angErr = 0; // parameter required? - const uint32_t timestamp_ms = m.time_usec * 0.001; + // correct offboard timestamp to be in local ms since boot + uint32_t timestamp_ms = correct_offboard_timestamp_usec_to_ms(m.time_usec, PAYLOAD_SIZE(chan, ATT_POS_MOCAP)); const uint32_t reset_timestamp_ms = 0; // no data available AP::ahrs().writeExtNavData(sensor_offset,