From 7de8bee29eddbf1bc64ae8dbed7788cf79f0e13d Mon Sep 17 00:00:00 2001 From: TSC21 Date: Mon, 9 Mar 2020 14:31:26 +0000 Subject: [PATCH] microRTPS: timesync: interpolate --- msg/templates/urtps/microRTPS_timesync.cpp.em | 12 +++++++++--- msg/templates/urtps/microRTPS_timesync.h.em | 2 +- 2 files changed, 10 insertions(+), 4 deletions(-) diff --git a/msg/templates/urtps/microRTPS_timesync.cpp.em b/msg/templates/urtps/microRTPS_timesync.cpp.em index a013b1ced3..f65c589db6 100644 --- a/msg/templates/urtps/microRTPS_timesync.cpp.em +++ b/msg/templates/urtps/microRTPS_timesync.cpp.em @@ -157,9 +157,15 @@ bool TimeSync::addMeasurement(int64_t local_t1_ns, int64_t remote_t2_ns, int64_t return false; } - double schedule = std::min((double)_num_samples / WINDOW_SIZE, 1.); - double alpha = ALPHA_INITIAL * (1. - schedule) + ALPHA_FINAL * schedule; - double beta = BETA_INTIIAL * (1. - schedule) + BETA_FINAL * schedule; + double alpha = ALPHA_FINAL; + double beta = BETA_FINAL; + + if (_num_samples < WINDOW_SIZE) { + double schedule = (double)_num_samples / WINDOW_SIZE; + double s = 1. - exp(.5 * (1. - 1. / (1. - schedule))); + alpha = (1. - s) * ALPHA_INITIAL + s * ALPHA_FINAL; + beta = (1. - s) * BETA_INITIAL + s * BETA_FINAL; + } int64_t offset_prev = _offset_ns.load(); updateOffset(static_cast((_skew_ns_per_sync + _offset_ns.load()) * (1. - alpha) + measurement_offset * alpha)); diff --git a/msg/templates/urtps/microRTPS_timesync.h.em b/msg/templates/urtps/microRTPS_timesync.h.em index e97041eb71..e4764395a6 100644 --- a/msg/templates/urtps/microRTPS_timesync.h.em +++ b/msg/templates/urtps/microRTPS_timesync.h.em @@ -78,7 +78,7 @@ except AttributeError: static constexpr double ALPHA_INITIAL = 0.05; static constexpr double ALPHA_FINAL = 0.003; -static constexpr double BETA_INTIIAL = 0.05; +static constexpr double BETA_INITIAL = 0.05; static constexpr double BETA_FINAL = 0.003; static constexpr int WINDOW_SIZE = 500; static constexpr int64_t UNKNOWN = 0;