From f4f62ececafdb10bec19b7543fbfb32b08f1ffde Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Wed, 16 May 2018 10:43:09 +1000 Subject: [PATCH] GCS_MAVLink: take account of transport lag in vision timestamps --- libraries/GCS_MAVLink/GCS.h | 8 +++++--- libraries/GCS_MAVLink/GCS_Common.cpp | 27 ++++++++++++++++++++------- 2 files changed, 25 insertions(+), 10 deletions(-) diff --git a/libraries/GCS_MAVLink/GCS.h b/libraries/GCS_MAVLink/GCS.h index 800aa472c0..743c1828fc 100644 --- a/libraries/GCS_MAVLink/GCS.h +++ b/libraries/GCS_MAVLink/GCS.h @@ -24,7 +24,8 @@ #include // check if a message will fit in the payload space available -#define HAVE_PAYLOAD_SPACE(chan, id) (comm_get_txspace(chan) >= GCS_MAVLINK::packet_overhead_chan(chan)+MAVLINK_MSG_ID_ ## id ## _LEN) +#define PAYLOAD_SIZE(chan, id) (GCS_MAVLINK::packet_overhead_chan(chan)+MAVLINK_MSG_ID_ ## id ## _LEN) +#define HAVE_PAYLOAD_SPACE(chan, id) (comm_get_txspace(chan) >= PAYLOAD_SIZE(chan, id)) #define CHECK_PAYLOAD_SIZE(id) if (comm_get_txspace(chan) < packet_overhead()+MAVLINK_MSG_ID_ ## id ## _LEN) return false #define CHECK_PAYLOAD_SIZE2(id) if (!HAVE_PAYLOAD_SPACE(chan, id)) return false @@ -473,7 +474,8 @@ private: const float z, const float roll, const float pitch, - const float yaw); + const float yaw, + const uint16_t payload_size); void log_vision_position_estimate_data(const uint64_t usec, const float x, const float y, @@ -489,7 +491,7 @@ private: correct an offboard timestamp in microseconds to a local time since boot in milliseconds */ - uint32_t correct_offboard_timestamp_usec_to_ms(uint64_t offboard_usec); + uint32_t correct_offboard_timestamp_usec_to_ms(uint64_t offboard_usec, uint16_t payload_size); mavlink_signing_t signing; static mavlink_signing_streams_t signing_streams; diff --git a/libraries/GCS_MAVLink/GCS_Common.cpp b/libraries/GCS_MAVLink/GCS_Common.cpp index 9da248be9d..9a39d229f4 100644 --- a/libraries/GCS_MAVLink/GCS_Common.cpp +++ b/libraries/GCS_MAVLink/GCS_Common.cpp @@ -1911,7 +1911,8 @@ void GCS_MAVLINK::handle_vision_position_estimate(mavlink_message_t *msg) mavlink_vision_position_estimate_t m; mavlink_msg_vision_position_estimate_decode(msg, &m); - handle_common_vision_position_estimate_data(m.usec, m.x, m.y, m.z, m.roll, m.pitch, m.yaw); + handle_common_vision_position_estimate_data(m.usec, m.x, m.y, m.z, m.roll, m.pitch, m.yaw, + PAYLOAD_SIZE(chan, VISION_POSITION_ESTIMATE)); } void GCS_MAVLINK::handle_global_vision_position_estimate(mavlink_message_t *msg) @@ -1919,7 +1920,8 @@ void GCS_MAVLINK::handle_global_vision_position_estimate(mavlink_message_t *msg) mavlink_global_vision_position_estimate_t m; mavlink_msg_global_vision_position_estimate_decode(msg, &m); - handle_common_vision_position_estimate_data(m.usec, m.x, m.y, m.z, m.roll, m.pitch, m.yaw); + handle_common_vision_position_estimate_data(m.usec, m.x, m.y, m.z, m.roll, m.pitch, m.yaw, + PAYLOAD_SIZE(chan, GLOBAL_VISION_POSITION_ESTIMATE)); } void GCS_MAVLINK::handle_vicon_position_estimate(mavlink_message_t *msg) @@ -1927,7 +1929,8 @@ void GCS_MAVLINK::handle_vicon_position_estimate(mavlink_message_t *msg) mavlink_vicon_position_estimate_t m; mavlink_msg_vicon_position_estimate_decode(msg, &m); - handle_common_vision_position_estimate_data(m.usec, m.x, m.y, m.z, m.roll, m.pitch, m.yaw); + handle_common_vision_position_estimate_data(m.usec, m.x, m.y, m.z, m.roll, m.pitch, m.yaw, + PAYLOAD_SIZE(chan, VICON_POSITION_ESTIMATE)); } // there are several messages which all have identical fields in them. @@ -1939,10 +1942,11 @@ void GCS_MAVLINK::handle_common_vision_position_estimate_data(const uint64_t use const float z, const float roll, const float pitch, - const float yaw) + const float yaw, + const uint16_t payload_size) { // correct offboard timestamp to be in local ms since boot - uint32_t timestamp_ms = correct_offboard_timestamp_usec_to_ms(usec); + uint32_t timestamp_ms = correct_offboard_timestamp_usec_to_ms(usec, payload_size); // sensor assumed to be at 0,0,0 body-frame; need parameters for this? // or a new message @@ -2794,10 +2798,19 @@ void GCS_MAVLINK::data_stream_send(void) Return a value in milliseconds since boot (for use by the EKF) */ -uint32_t GCS_MAVLINK::correct_offboard_timestamp_usec_to_ms(uint64_t offboard_usec) +uint32_t GCS_MAVLINK::correct_offboard_timestamp_usec_to_ms(uint64_t offboard_usec, uint16_t payload_size) { const uint32_t max_lag_us = 500*1000UL; - uint64_t local_us = AP_HAL::micros64(); + uint64_t local_us; + // if the HAL supports it then constrain the latest possible time + // the packet could have been sent by the uart receive time and + // the baudrate and packet size. + uint64_t uart_receive_time = _port->receive_time_constraint_us(payload_size); + if (uart_receive_time != 0) { + local_us = uart_receive_time; + } else { + local_us = AP_HAL::micros64(); + } int64_t diff_us = int64_t(local_us) - int64_t(offboard_usec); if (!lag_correction.initialised ||