mirror of https://github.com/ArduPilot/ardupilot
90 lines
3.2 KiB
C++
90 lines
3.2 KiB
C++
|
/*
|
||
|
|
This program is free software: you can redistribute it and/or modify
|
||
|
|
it under the terms of the GNU General Public License as published by
|
||
|
|
the Free Software Foundation, either version 3 of the License, or
|
||
|
|
(at your option) any later version.
|
||
|
|
|
||
|
|
This program is distributed in the hope that it will be useful,
|
||
|
|
but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||
|
|
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
||
|
|
GNU General Public License for more details.
|
||
|
|
|
||
|
|
You should have received a copy of the GNU General Public License
|
||
|
|
along with this program. If not, see <http://www.gnu.org/licenses/>.
|
||
|
|
*/
|
||
|
|
/*
|
||
|
|
class to correct an offboard timestamp in microseconds into a local
|
||
|
|
timestamp, removing timing jitter caused by the transport.
|
||
|
|
|
||
|
|
It takes the offboard_usec timestamp which is a timestamp provided
|
||
|
|
in a protocol from a remote device, and the time of arrival of the
|
||
|
|
message in local microseconds. It returns an improved estimate of
|
||
|
|
the time that the message was generated on the remote system in the
|
||
|
|
local time domain
|
||
|
|
|
||
|
|
The algorithm assumes two things:
|
||
|
|
|
||
|
|
1) the data did not come from the future in our local time-domain
|
||
|
|
2) the data is not older than max_lag_ms in our local 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
|
||
|
|
*/
|
||
|
|
|
||
|
|
#include <AP_HAL/AP_HAL.h>
|
||
|
|
#include "JitterCorrection.h"
|
||
|
|
|
||
|
|
// constructor
|
||
|
|
JitterCorrection::JitterCorrection(uint16_t _max_lag_ms, uint16_t _convergence_loops) :
|
||
|
|
max_lag_ms(_max_lag_ms),
|
||
|
|
convergence_loops(_convergence_loops)
|
||
|
|
{}
|
||
|
|
|
||
|
|
/*
|
||
|
|
correct an offboard timestamp in microseconds into a local
|
||
|
|
timestamp, removing timing jitter caused by the transport.
|
||
|
|
|
||
|
|
Return a value in microseconds since boot in the local time domain
|
||
|
|
*/
|
||
|
|
uint64_t JitterCorrection::correct_offboard_timestamp_usec(uint64_t offboard_usec, uint64_t local_usec)
|
||
|
|
{
|
||
|
|
int64_t diff_us = int64_t(local_usec) - int64_t(offboard_usec);
|
||
|
|
|
||
|
|
if (!initialised ||
|
||
|
|
diff_us < 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
|
||
|
|
link_offset_usec = diff_us;
|
||
|
|
initialised = true;
|
||
|
|
}
|
||
|
|
|
||
|
|
int64_t estimate_us = offboard_usec + link_offset_usec;
|
||
|
|
|
||
|
|
if (estimate_us + (max_lag_ms*1000U) < int64_t(local_usec)) {
|
||
|
|
// 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_usec - (max_lag_ms*1000U);
|
||
|
|
link_offset_usec = estimate_us - offboard_usec;
|
||
|
|
}
|
||
|
|
|
||
|
|
if (min_sample_counter == 0) {
|
||
|
|
min_sample_us = diff_us;
|
||
|
|
}
|
||
|
|
min_sample_counter++;
|
||
|
|
if (diff_us < min_sample_us) {
|
||
|
|
min_sample_us = diff_us;
|
||
|
|
}
|
||
|
|
if (min_sample_counter == convergence_loops) {
|
||
|
|
// we have the requested number of samples of the transport
|
||
|
|
// lag for convergence. To account for long term clock drift
|
||
|
|
// we set the diff we will use in future to this value
|
||
|
|
link_offset_usec = min_sample_us;
|
||
|
|
min_sample_counter = 0;
|
||
|
|
}
|
||
|
|
|
||
|
|
return uint64_t(estimate_us);
|
||
|
|
}
|