diff --git a/src/xrt/auxiliary/math/CMakeLists.txt b/src/xrt/auxiliary/math/CMakeLists.txt index e1f34b9b1..af963fc90 100644 --- a/src/xrt/auxiliary/math/CMakeLists.txt +++ b/src/xrt/auxiliary/math/CMakeLists.txt @@ -7,7 +7,8 @@ add_library( aux_math STATIC m_api.h m_base.cpp - m_clock_offset.h + m_clock_tracking.c + m_clock_tracking.h m_documentation.hpp m_eigen_interop.hpp m_filter_fifo.c diff --git a/src/xrt/auxiliary/math/m_clock_offset.h b/src/xrt/auxiliary/math/m_clock_offset.h deleted file mode 100644 index 34fbcc704..000000000 --- a/src/xrt/auxiliary/math/m_clock_offset.h +++ /dev/null @@ -1,56 +0,0 @@ -// Copyright 2022, Collabora, Ltd. -// SPDX-License-Identifier: BSL-1.0 -/*! - * @file - * @brief Helpers to estimate offsets between clocks - * @author Mateo de Mayo - * @ingroup aux_math - */ - -#pragma once - -#include "util/u_time.h" - -#ifdef __cplusplus -extern "C" { -#endif - -/*! - * Helper to estimate the offset between two clocks using exponential smoothing. - * - * Given a sample from two timestamp domains A and B that should have been - * sampled as close as possible, together with an estimate of the offset between - * A clock and B clock (or zero), it applies a smoothing average on the - * estimated offset and returns @p a in B clock. - * - * @param freq About how many times per second this function is called. Helps setting a good decay value. - * @param a Timestamp in clock A of the event - * @param b Timestamp in clock B of the event - * @param[in,out] inout_a2b Pointer to the current offset estimate from A to B, or 0 if unknown. - * Value pointed-to will be updated. - * @return timepoint_ns @p a in B clock - */ -static inline timepoint_ns -m_clock_offset_a2b(float freq, timepoint_ns a, timepoint_ns b, time_duration_ns *inout_a2b) -{ - // This formulation of exponential filtering uses a fixed-precision integer for the - // alpha value and operates on the delta between the old and new a2b to avoid - // precision / overflow problems. - - // Totally arbitrary way of computing alpha, if you have a better one, replace it - const time_duration_ns alpha = 1000 * (1.0 - 12.5 / freq); // Weight to put on accumulated a2b - time_duration_ns old_a2b = *inout_a2b; - time_duration_ns got_a2b = b - a; - time_duration_ns new_a2b; - if (old_a2b == 0) { // a2b has not been set yet - new_a2b = got_a2b; - } else { - new_a2b = ((old_a2b - got_a2b) * alpha) / 1000 + got_a2b; - } - *inout_a2b = new_a2b; - return a + new_a2b; -} - -#ifdef __cplusplus -} -#endif diff --git a/src/xrt/auxiliary/math/m_clock_tracking.c b/src/xrt/auxiliary/math/m_clock_tracking.c new file mode 100644 index 000000000..73fe5a44a --- /dev/null +++ b/src/xrt/auxiliary/math/m_clock_tracking.c @@ -0,0 +1,208 @@ +// Copyright 2024, Jan Schmidt +// SPDX-License-Identifier: BSL-1.0 +/*! + * @file + * @brief Helpers to estimate offsets between clocks + * @author Jan Schmidt + * @ingroup aux_math + */ +#include "util/u_misc.h" +#include "m_clock_tracking.h" + +/* Fixed constants for discontinuity detection and + * subsequent hold-off. These could be made configurable + * if that turns out to be desirable */ +const time_duration_ns CLOCK_RESET_THRESHOLD = 100 * U_TIME_1MS_IN_NS; +const time_duration_ns CLOCK_RESET_HOLDOFF = 30 * U_TIME_1MS_IN_NS; + +struct m_clock_observation +{ + timepoint_ns local_ts; /* Timestamp from local / reference clock */ + time_duration_ns skew; /* skew = local_ts - remote_ts */ +}; + +static struct m_clock_observation +m_clock_observation_init(timepoint_ns local_ts, timepoint_ns remote_ts) +{ + struct m_clock_observation ret = { + .local_ts = local_ts, + .skew = local_ts - remote_ts, + }; + return ret; +} + +struct m_clock_windowed_skew_tracker +{ + /* Maximum size of the window in samples */ + size_t max_window_samples; + /* Current size of the window in samples (smaller than maximum after init or reset) */ + size_t current_window_samples; + + /* Observations ringbuffer window */ + struct m_clock_observation *window; + /* Position in the observations window */ + size_t current_window_pos; + + /* Track the position in the window of the smallest + * skew value */ + time_duration_ns current_min_skew; + size_t current_min_skew_pos; + + /* the most recently submitted observation */ + bool have_last_observation; + struct m_clock_observation last_observation; + + /* Last discontinuity timestamp, used for holdoff after a reset */ + timepoint_ns clock_reset_ts; + + /* Smoothing and output */ + bool have_skew_estimate; + timepoint_ns current_local_anchor; + time_duration_ns current_skew; /* Offset between local time and the remote */ +}; + +struct m_clock_windowed_skew_tracker * +m_clock_windowed_skew_tracker_alloc(const size_t window_samples) +{ + struct m_clock_windowed_skew_tracker *t = U_TYPED_CALLOC(struct m_clock_windowed_skew_tracker); + if (t == NULL) { + return NULL; + } + + t->window = U_TYPED_ARRAY_CALLOC(struct m_clock_observation, window_samples); + if (t->window == NULL) { + free(t); + return NULL; + } + + t->max_window_samples = window_samples; + + return t; +} + +void +m_clock_windowed_skew_tracker_reset(struct m_clock_windowed_skew_tracker *t) +{ + // Clear time tracking + t->have_last_observation = false; + t->current_window_samples = 0; +} + +void +m_clock_windowed_skew_tracker_destroy(struct m_clock_windowed_skew_tracker *t) +{ + free(t->window); + free(t); +} + +void +m_clock_windowed_skew_tracker_push(struct m_clock_windowed_skew_tracker *t, + const timepoint_ns local_ts, + const timepoint_ns remote_ts) +{ + struct m_clock_observation obs = m_clock_observation_init(local_ts, remote_ts); + + if (t->have_last_observation) { + time_duration_ns skew_delta = t->last_observation.skew - obs.skew; + if (-skew_delta >= CLOCK_RESET_THRESHOLD || skew_delta >= CLOCK_RESET_THRESHOLD) { + // Too large a delta between observations. Reset the smoothing to adapt more quickly + t->clock_reset_ts = local_ts; + t->current_window_pos = 0; + t->current_window_samples = 0; + + t->have_last_observation = true; + t->last_observation = obs; + return; + } + + // After a reset, ignore all samples briefly in order to + // let the new timeline settle. + if (local_ts - t->clock_reset_ts < CLOCK_RESET_HOLDOFF) { + return; + } + t->clock_reset_ts = 0; + } + t->last_observation = obs; + + if (t->current_window_samples < t->max_window_samples) { + /* Window is still being filled */ + + if (t->current_window_pos == 0) { + /* First sample. Take it as-is */ + t->current_min_skew = t->current_skew = obs.skew; + t->current_local_anchor = local_ts; + t->current_min_skew_pos = 0; + } else if (obs.skew <= t->current_min_skew) { + /* We found a new minimum. Take it */ + t->current_min_skew = obs.skew; + t->current_local_anchor = local_ts; + t->current_min_skew_pos = t->current_window_pos; + } + + /* Grow the stored observation array */ + t->window[t->current_window_samples++] = obs; + + } else if (obs.skew <= t->current_min_skew) { + /* Found a new minimum skew. */ + t->window[t->current_window_pos] = obs; + + t->current_local_anchor = local_ts; + t->current_min_skew = obs.skew; + t->current_min_skew_pos = t->current_window_pos; + } else if (t->current_window_pos == t->current_min_skew_pos) { + /* Replacing the previous minimum skew. Find the new minimum */ + t->window[t->current_window_pos] = obs; + + struct m_clock_observation *new_min = &t->window[0]; + size_t new_min_index = 0; + + for (size_t i = 1; i < t->current_window_samples; i++) { + struct m_clock_observation *cur = &t->window[i]; + if (cur->skew <= new_min->skew) { + new_min = cur; + new_min_index = i; + } + } + + t->current_local_anchor = new_min->local_ts; + t->current_min_skew = new_min->skew; + t->current_min_skew_pos = new_min_index; + } else { + /* Just insert the observation */ + t->window[t->current_window_pos] = obs; + } + + /* Wrap around the window index */ + t->current_window_pos = (t->current_window_pos + 1) % t->max_window_samples; + + /* Update the moving average skew */ + size_t w = t->current_window_samples; + t->current_skew = (t->current_min_skew + t->current_skew * (w - 1)) / w; + t->have_skew_estimate = true; +} + +bool +m_clock_windowed_skew_tracker_to_local(struct m_clock_windowed_skew_tracker *t, + const timepoint_ns remote_ts, + timepoint_ns *local_ts) +{ + if (!t->have_skew_estimate) { + return false; + } + + *local_ts = remote_ts + t->current_skew; + return true; +} + +bool +m_clock_windowed_skew_tracker_to_remote(struct m_clock_windowed_skew_tracker *t, + const timepoint_ns local_ts, + timepoint_ns *remote_ts) +{ + if (!t->have_skew_estimate) { + return false; + } + + *remote_ts = local_ts - t->current_skew; + return true; +} diff --git a/src/xrt/auxiliary/math/m_clock_tracking.h b/src/xrt/auxiliary/math/m_clock_tracking.h new file mode 100644 index 000000000..f71e153fe --- /dev/null +++ b/src/xrt/auxiliary/math/m_clock_tracking.h @@ -0,0 +1,103 @@ +// Copyright 2022, Collabora, Ltd. +// Copyright 2024, Jan Schmidt +// SPDX-License-Identifier: BSL-1.0 +/*! + * @file + * @brief Helpers to estimate offsets between clocks + * @author Mateo de Mayo + * @ingroup aux_math + */ + +#pragma once + +#include "xrt/xrt_defines.h" +#include "util/u_time.h" + +#ifdef __cplusplus +extern "C" { +#endif + +/*! + * Helper to estimate the offset between two clocks using exponential smoothing. + * + * Given a sample from two timestamp domains A and B that should have been + * sampled as close as possible, together with an estimate of the offset between + * A clock and B clock (or zero), it applies a smoothing average on the + * estimated offset and returns @p a in B clock. + * + * This estimator can be used when clock observations are arriving with a low + * delay and small jitter, or when accuracy is less important (on the order of + * the jitter that is present). It is very computationally cheap. + * + * @param freq About how many times per second this function is called. Helps setting a good decay value. + * @param a Timestamp in clock A of the event + * @param b Timestamp in clock B of the event + * @param[in,out] inout_a2b Pointer to the current offset estimate from A to B, or 0 if unknown. + * Value pointed-to will be updated. + * @return timepoint_ns @p a in B clock + */ +static inline timepoint_ns +m_clock_offset_a2b(float freq, timepoint_ns a, timepoint_ns b, time_duration_ns *inout_a2b) +{ + // This formulation of exponential filtering uses a fixed-precision integer for the + // alpha value and operates on the delta between the old and new a2b to avoid + // precision / overflow problems. + + // Totally arbitrary way of computing alpha, if you have a better one, replace it + const time_duration_ns alpha = 1000 * (1.0 - 12.5 / freq); // Weight to put on accumulated a2b + time_duration_ns old_a2b = *inout_a2b; + time_duration_ns got_a2b = b - a; + time_duration_ns new_a2b; + if (old_a2b == 0) { // a2b has not been set yet + new_a2b = got_a2b; + } else { + new_a2b = ((old_a2b - got_a2b) * alpha) / 1000 + got_a2b; + } + *inout_a2b = new_a2b; + return a + new_a2b; +} + +/*! + * Helper to estimate the offset between two clocks using a windowed + * minimum-skew estimation plus exponential smoothing. The algorithm + * tracks the smallest offset within the window, on the theory that + * minima represent samples with the lowest transmission delay and jitter. + * + * More computationally intensive than the simple m_clock_offset_a2b estimator, + * but can estimate a clock with accuracy in the microsecond range + * even in the presence of 10s of milliseconds of jitter. + * + * Based on the approach in Dominique Fober, Yann Orlarey, Stéphane Letz. + * Real Time Clock Skew Estimation over Network Delays. [Technical Report] GRAME. 2005. + * https://hal.science/hal-02158803/document + */ +struct m_clock_windowed_skew_tracker; + +/*! + * Allocate a struct m_clock_windowed_skew_tracker with a + * window of @param window_samples samples. + */ +struct m_clock_windowed_skew_tracker * +m_clock_windowed_skew_tracker_alloc(const size_t window_samples); +void +m_clock_windowed_skew_tracker_reset(struct m_clock_windowed_skew_tracker *t); +void +m_clock_windowed_skew_tracker_destroy(struct m_clock_windowed_skew_tracker *t); + +void +m_clock_windowed_skew_tracker_push(struct m_clock_windowed_skew_tracker *t, + const timepoint_ns local_ts, + const timepoint_ns remote_ts); + +bool +m_clock_windowed_skew_tracker_to_local(struct m_clock_windowed_skew_tracker *t, + const timepoint_ns remote_ts, + timepoint_ns *local_ts); +bool +m_clock_windowed_skew_tracker_to_remote(struct m_clock_windowed_skew_tracker *t, + const timepoint_ns local_ts, + timepoint_ns *remote_ts); + +#ifdef __cplusplus +} +#endif diff --git a/src/xrt/drivers/rift_s/rift_s_tracker.c b/src/xrt/drivers/rift_s/rift_s_tracker.c index f8239297c..b73b06818 100644 --- a/src/xrt/drivers/rift_s/rift_s_tracker.c +++ b/src/xrt/drivers/rift_s/rift_s_tracker.c @@ -24,7 +24,7 @@ #include #include "math/m_api.h" -#include "math/m_clock_offset.h" +#include "math/m_clock_tracking.h" #include "math/m_space.h" #include "math/m_vec3.h" diff --git a/src/xrt/drivers/vive/vive_source.c b/src/xrt/drivers/vive/vive_source.c index 2633a38ea..ee58f2359 100644 --- a/src/xrt/drivers/vive/vive_source.c +++ b/src/xrt/drivers/vive/vive_source.c @@ -13,7 +13,7 @@ #include "os/os_threading.h" -#include "math/m_clock_offset.h" +#include "math/m_clock_tracking.h" #include "util/u_deque.h" #include "util/u_logging.h" diff --git a/src/xrt/drivers/wmr/wmr_controller_base.c b/src/xrt/drivers/wmr/wmr_controller_base.c index e4ff582d3..1d46114fd 100644 --- a/src/xrt/drivers/wmr/wmr_controller_base.c +++ b/src/xrt/drivers/wmr/wmr_controller_base.c @@ -14,6 +14,7 @@ #include "math/m_mathinclude.h" #include "math/m_api.h" +#include "math/m_clock_tracking.h" #include "math/m_vec2.h" #include "math/m_predict.h" diff --git a/src/xrt/drivers/wmr/wmr_source.c b/src/xrt/drivers/wmr/wmr_source.c index 1d2f47b16..7c0546b3c 100644 --- a/src/xrt/drivers/wmr/wmr_source.c +++ b/src/xrt/drivers/wmr/wmr_source.c @@ -12,7 +12,7 @@ #include "wmr_protocol.h" #include "math/m_api.h" -#include "math/m_clock_offset.h" +#include "math/m_clock_tracking.h" #include "math/m_filter_fifo.h" #include "util/u_debug.h" #include "util/u_sink.h"