From cfc37f2089285a24b65b5eae49bf516ba2fb916e Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Fri, 23 Aug 2019 15:31:43 +1000 Subject: [PATCH] GCS_MAVLink: log corrected timestamp for vision data this allows for debugging of link lag issues --- libraries/GCS_MAVLink/GCS.h | 1 + libraries/GCS_MAVLink/GCS_Common.cpp | 26 ++++++++++++++------------ 2 files changed, 15 insertions(+), 12 deletions(-) diff --git a/libraries/GCS_MAVLink/GCS.h b/libraries/GCS_MAVLink/GCS.h index 31e22e9c38..780f1f2eb2 100644 --- a/libraries/GCS_MAVLink/GCS.h +++ b/libraries/GCS_MAVLink/GCS.h @@ -626,6 +626,7 @@ private: const float yaw, const uint16_t payload_size); void log_vision_position_estimate_data(const uint64_t usec, + const uint32_t corrected_msec, const float x, const float y, const float z, diff --git a/libraries/GCS_MAVLink/GCS_Common.cpp b/libraries/GCS_MAVLink/GCS_Common.cpp index 4256abd8f8..a3bf001c67 100644 --- a/libraries/GCS_MAVLink/GCS_Common.cpp +++ b/libraries/GCS_MAVLink/GCS_Common.cpp @@ -2832,10 +2832,11 @@ void GCS_MAVLINK::handle_common_vision_position_estimate_data(const uint64_t use timestamp_ms, reset_timestamp_ms); - log_vision_position_estimate_data(usec, x, y, z, roll, pitch, yaw); + log_vision_position_estimate_data(usec, timestamp_ms, x, y, z, roll, pitch, yaw); } void GCS_MAVLINK::log_vision_position_estimate_data(const uint64_t usec, + const uint32_t corrected_msec, const float x, const float y, const float z, @@ -2843,16 +2844,17 @@ void GCS_MAVLINK::log_vision_position_estimate_data(const uint64_t usec, const float pitch, const float yaw) { - AP::logger().Write("VISP", "TimeUS,RemTimeUS,PX,PY,PZ,Roll,Pitch,Yaw", - "ssmmmddh", "FF000000", "QQffffff", - (uint64_t)AP_HAL::micros64(), - (uint64_t)usec, - (double)x, - (double)y, - (double)z, - (double)(roll * RAD_TO_DEG), - (double)(pitch * RAD_TO_DEG), - (double)(yaw * RAD_TO_DEG)); + AP::logger().Write("VISP", "TimeUS,RemTimeUS,CTimeMS,PX,PY,PZ,Roll,Pitch,Yaw", + "sssmmmddh", "FFC000000", "QQIffffff", + (uint64_t)AP_HAL::micros64(), + (uint64_t)usec, + corrected_msec, + (double)x, + (double)y, + (double)z, + (double)(roll * RAD_TO_DEG), + (double)(pitch * RAD_TO_DEG), + (double)(yaw * RAD_TO_DEG)); } void GCS_MAVLINK::handle_att_pos_mocap(const mavlink_message_t &msg) @@ -2888,7 +2890,7 @@ void GCS_MAVLINK::handle_att_pos_mocap(const mavlink_message_t &msg) float yaw; attitude.to_euler(roll, pitch, yaw); - log_vision_position_estimate_data(m.time_usec, m.x, m.y, m.z, roll, pitch, yaw); + log_vision_position_estimate_data(m.time_usec, timestamp_ms, m.x, m.y, m.z, roll, pitch, yaw); } void GCS_MAVLINK::handle_command_ack(const mavlink_message_t &msg)