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

GCS_MAVLink: use JitterCorrection class

This commit is contained in:
Andrew Tridgell 2018-06-22 12:46:56 +10:00
parent 93abb41e77
commit 259723eb26
2 changed files with 6 additions and 68 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

11
libraries/GCS_MAVLink/GCS.h
View File

@ -22,6 +22,7 @@
#include <AP_AdvancedFailsafe/AP_AdvancedFailsafe.h>
#include <AP_VisualOdom/AP_VisualOdom.h>
#include <AP_Common/AP_FWVersion.h>
#include <AP_RTC/JitterCorrection.h>
// check if a message will fit in the payload space available
#define PAYLOAD_SIZE(chan, id) (GCS_MAVLINK::packet_overhead_chan(chan)+MAVLINK_MSG_ID_ ## id ## _LEN)
@ -571,14 +572,8 @@ private:
bool active;
} alternative;
// state associated with offboard transport lag correction
struct {
bool initialised;
int64_t link_offset_usec;
uint32_t min_sample_counter;
int64_t min_sample_us;
} lag_correction;
JitterCorrection lag_correction;
// we cache the current location and send it even if the AHRS has
// no idea where we are:
struct Location global_position_current_loc;

63
libraries/GCS_MAVLink/GCS_Common.cpp
View File

@ -3351,20 +3351,12 @@ void GCS_MAVLINK::data_stream_send(void)
/*
correct an offboard timestamp in microseconds into a local timestamp
since boot in milliseconds. This is a transport lag correction function, and works by assuming two key things:
1) the data did not come from the future in our time-domain
2) the data is not older than max_lag_ms in our time-domain
It works by estimating the transport lag by looking for the incoming
packet that had the least lag, and converging on the offset that is
associated with that lag
since boot in milliseconds. See the JitterCorrection code for details
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, uint16_t payload_size)
{
const uint32_t max_lag_us = 500*1000UL;
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
@ -3375,58 +3367,9 @@ uint32_t GCS_MAVLINK::correct_offboard_timestamp_usec_to_ms(uint64_t offboard_us
} else {
local_us = AP_HAL::micros64();
}
int64_t diff_us = int64_t(local_us) - int64_t(offboard_usec);
uint64_t corrected_us = lag_correction.correct_offboard_timestamp_usec(offboard_usec, local_us);
if (!lag_correction.initialised ||
diff_us < lag_correction.link_offset_usec) {
// this message arrived from the remote system with a
// timestamp that would imply the message was from the
// future. We know that isn't possible, so we adjust down the
// correction value
lag_correction.link_offset_usec = diff_us;
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
printf("link_offset_usec=%lld\n", (long long int)diff_us);
#endif
lag_correction.initialised = true;
}
int64_t estimate_us = offboard_usec + lag_correction.link_offset_usec;
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
if (estimate_us > (int64_t)local_us) {
// this should be impossible, just check it under SITL
printf("msg from future %lld\n", (long long int)(estimate_us - local_us));
}
#endif
if (estimate_us + max_lag_us < int64_t(local_us)) {
// this implies the message came from too far in the past. Clamp the lag estimate
// to assume the message had maximum lag
estimate_us = local_us - max_lag_us;
lag_correction.link_offset_usec = estimate_us - offboard_usec;
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
printf("offboard timestammp too old %lld\n", (long long int)(local_us - estimate_us));
#endif
}
if (lag_correction.min_sample_counter == 0) {
lag_correction.min_sample_us = diff_us;
}
lag_correction.min_sample_counter++;
if (diff_us < lag_correction.min_sample_us) {
lag_correction.min_sample_us = diff_us;
}
if (lag_correction.min_sample_counter == 200) {
// we have 200 samples of the transport lag. To
// account for long term clock drift we set the diff we will
// use in future to this value
lag_correction.link_offset_usec = lag_correction.min_sample_us;
lag_correction.min_sample_counter = 0;
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
printf("new link_offset_usec=%lld\n", (long long int)(lag_correction.min_sample_us));
#endif
}
return estimate_us / 1000U;
return corrected_us / 1000U;
}
GCS &gcs()