diff --git a/libraries/AP_RTC/JitterCorrection.cpp b/libraries/AP_RTC/JitterCorrection.cpp index 3aec4a23d5..a7589b50b8 100644 --- a/libraries/AP_RTC/JitterCorrection.cpp +++ b/libraries/AP_RTC/JitterCorrection.cpp @@ -87,3 +87,14 @@ uint64_t JitterCorrection::correct_offboard_timestamp_usec(uint64_t offboard_use return uint64_t(estimate_us); } + +/* + correct an offboard timestamp in microseconds into a local + timestamp, removing timing jitter caused by the transport. + + Return a value in milliseconds since boot in the local time domain + */ +uint32_t JitterCorrection::correct_offboard_timestamp_msec(uint32_t offboard_ms, uint32_t local_ms) +{ + return correct_offboard_timestamp_usec(offboard_ms*1000ULL, local_ms*1000ULL) / 1000ULL; +} diff --git a/libraries/AP_RTC/JitterCorrection.h b/libraries/AP_RTC/JitterCorrection.h index 70d93056ff..1798d583b2 100644 --- a/libraries/AP_RTC/JitterCorrection.h +++ b/libraries/AP_RTC/JitterCorrection.h @@ -13,6 +13,10 @@ public: // timestamp. See JitterCorrection.cpp for details uint64_t correct_offboard_timestamp_usec(uint64_t offboard_usec, uint64_t local_usec); + // correct an offboard timestamp to a jitter-free local + // timestamp. See JitterCorrection.cpp for details + uint32_t correct_offboard_timestamp_msec(uint32_t offboard_ms, uint32_t local_ms); + private: const uint16_t max_lag_ms; const uint16_t convergence_loops;