Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

GCS_MAVLink: take account of transport lag in vision timestamps

This commit is contained in:
Andrew Tridgell 2018-05-16 10:43:09 +10:00
parent 93cdb15aca
commit f4f62ececa
2 changed files with 25 additions and 10 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
libraries/GCS_MAVLink/GCS.h
View File

@ -24,7 +24,8 @@
#include <AP_Common/AP_FWVersion.h>
// 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;

27
libraries/GCS_MAVLink/GCS_Common.cpp
View File

@ -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 ||