Browse Source

AP_RTC: added a millisecond jitter correction function

master
Andrew Tridgell 6 years ago committed by Randy Mackay
parent
commit
f1d32df783
  1. 11
      libraries/AP_RTC/JitterCorrection.cpp
  2. 4
      libraries/AP_RTC/JitterCorrection.h

11
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); 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;
}

4
libraries/AP_RTC/JitterCorrection.h

@ -13,6 +13,10 @@ public:
// timestamp. See JitterCorrection.cpp for details // timestamp. See JitterCorrection.cpp for details
uint64_t correct_offboard_timestamp_usec(uint64_t offboard_usec, uint64_t local_usec); 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: private:
const uint16_t max_lag_ms; const uint16_t max_lag_ms;
const uint16_t convergence_loops; const uint16_t convergence_loops;

Loading…
Cancel
Save