matrixfurry.com
The open source OpenXR runtime
1// Copyright 2024, Jan Schmidt
2// SPDX-License-Identifier: BSL-1.0
3/*!
4 * @file
5 * @brief Helpers to estimate offsets between clocks
6 * @author Jan Schmidt <jan@centricular.com>
7 * @ingroup aux_math
8 */
9#include "util/u_misc.h"
10#include "m_clock_tracking.h"
11
12/* Fixed constants for discontinuity detection and
13 * subsequent hold-off. These could be made configurable
14 * if that turns out to be desirable */
15const time_duration_ns CLOCK_RESET_THRESHOLD = 100 * U_TIME_1MS_IN_NS;
16const time_duration_ns CLOCK_RESET_HOLDOFF = 30 * U_TIME_1MS_IN_NS;
17
18struct m_clock_observation
19{
20 timepoint_ns local_ts; /* Timestamp from local / reference clock */
21 time_duration_ns skew; /* skew = local_ts - remote_ts */
22};
23
24static struct m_clock_observation
25m_clock_observation_init(timepoint_ns local_ts, timepoint_ns remote_ts)
26{
27 struct m_clock_observation ret = {
28 .local_ts = local_ts,
29 .skew = local_ts - remote_ts,
30 };
31 return ret;
32}
33
34struct m_clock_windowed_skew_tracker
35{
36 /* Maximum size of the window in samples */
37 size_t max_window_samples;
38 /* Current size of the window in samples (smaller than maximum after init or reset) */
39 size_t current_window_samples;
40
41 /* Observations ringbuffer window */
42 struct m_clock_observation *window;
43 /* Position in the observations window */
44 size_t current_window_pos;
45
46 /* Track the position in the window of the smallest
47 * skew value */
48 time_duration_ns current_min_skew;
49 size_t current_min_skew_pos;
50
51 /* the most recently submitted observation */
52 bool have_last_observation;
53 struct m_clock_observation last_observation;
54
55 /* Last discontinuity timestamp, used for holdoff after a reset */
56 timepoint_ns clock_reset_ts;
57
58 /* Smoothing and output */
59 bool have_skew_estimate;
60 timepoint_ns current_local_anchor;
61 time_duration_ns current_skew; /* Offset between local time and the remote */
62};
63
64struct m_clock_windowed_skew_tracker *
65m_clock_windowed_skew_tracker_alloc(const size_t window_samples)
66{
67 struct m_clock_windowed_skew_tracker *t = U_TYPED_CALLOC(struct m_clock_windowed_skew_tracker);
68 if (t == NULL) {
69 return NULL;
70 }
71
72 t->window = U_TYPED_ARRAY_CALLOC(struct m_clock_observation, window_samples);
73 if (t->window == NULL) {
74 free(t);
75 return NULL;
76 }
77
78 t->max_window_samples = window_samples;
79
80 return t;
81}
82
83void
84m_clock_windowed_skew_tracker_reset(struct m_clock_windowed_skew_tracker *t)
85{
86 // Clear time tracking
87 t->have_last_observation = false;
88 t->current_window_samples = 0;
89}
90
91void
92m_clock_windowed_skew_tracker_destroy(struct m_clock_windowed_skew_tracker *t)
93{
94 free(t->window);
95 free(t);
96}
97
98void
99m_clock_windowed_skew_tracker_push(struct m_clock_windowed_skew_tracker *t,
100 const timepoint_ns local_ts,
101 const timepoint_ns remote_ts)
102{
103 struct m_clock_observation obs = m_clock_observation_init(local_ts, remote_ts);
104
105 if (t->have_last_observation) {
106 time_duration_ns skew_delta = t->last_observation.skew - obs.skew;
107 if (-skew_delta >= CLOCK_RESET_THRESHOLD || skew_delta >= CLOCK_RESET_THRESHOLD) {
108 // Too large a delta between observations. Reset the smoothing to adapt more quickly
109 t->clock_reset_ts = local_ts;
110 t->current_window_pos = 0;
111 t->current_window_samples = 0;
112
113 t->have_last_observation = true;
114 t->last_observation = obs;
115 return;
116 }
117
118 // After a reset, ignore all samples briefly in order to
119 // let the new timeline settle.
120 if (local_ts - t->clock_reset_ts < CLOCK_RESET_HOLDOFF) {
121 return;
122 }
123 t->clock_reset_ts = 0;
124 }
125 t->last_observation = obs;
126
127 if (t->current_window_samples < t->max_window_samples) {
128 /* Window is still being filled */
129
130 if (t->current_window_pos == 0) {
131 /* First sample. Take it as-is */
132 t->current_min_skew = t->current_skew = obs.skew;
133 t->current_local_anchor = local_ts;
134 t->current_min_skew_pos = 0;
135 } else if (obs.skew <= t->current_min_skew) {
136 /* We found a new minimum. Take it */
137 t->current_min_skew = obs.skew;
138 t->current_local_anchor = local_ts;
139 t->current_min_skew_pos = t->current_window_pos;
140 }
141
142 /* Grow the stored observation array */
143 t->window[t->current_window_samples++] = obs;
144
145 } else if (obs.skew <= t->current_min_skew) {
146 /* Found a new minimum skew. */
147 t->window[t->current_window_pos] = obs;
148
149 t->current_local_anchor = local_ts;
150 t->current_min_skew = obs.skew;
151 t->current_min_skew_pos = t->current_window_pos;
152 } else if (t->current_window_pos == t->current_min_skew_pos) {
153 /* Replacing the previous minimum skew. Find the new minimum */
154 t->window[t->current_window_pos] = obs;
155
156 struct m_clock_observation *new_min = &t->window[0];
157 size_t new_min_index = 0;
158
159 for (size_t i = 1; i < t->current_window_samples; i++) {
160 struct m_clock_observation *cur = &t->window[i];
161 if (cur->skew <= new_min->skew) {
162 new_min = cur;
163 new_min_index = i;
164 }
165 }
166
167 t->current_local_anchor = new_min->local_ts;
168 t->current_min_skew = new_min->skew;
169 t->current_min_skew_pos = new_min_index;
170 } else {
171 /* Just insert the observation */
172 t->window[t->current_window_pos] = obs;
173 }
174
175 /* Wrap around the window index */
176 t->current_window_pos = (t->current_window_pos + 1) % t->max_window_samples;
177
178 /* Update the moving average skew */
179 size_t w = t->current_window_samples;
180 t->current_skew = (t->current_min_skew + t->current_skew * (w - 1)) / w;
181 t->have_skew_estimate = true;
182}
183
184bool
185m_clock_windowed_skew_tracker_to_local(struct m_clock_windowed_skew_tracker *t,
186 const timepoint_ns remote_ts,
187 timepoint_ns *local_ts)
188{
189 if (!t->have_skew_estimate) {
190 return false;
191 }
192
193 *local_ts = remote_ts + t->current_skew;
194 return true;
195}
196
197bool
198m_clock_windowed_skew_tracker_to_remote(struct m_clock_windowed_skew_tracker *t,
199 const timepoint_ns local_ts,
200 timepoint_ns *remote_ts)
201{
202 if (!t->have_skew_estimate) {
203 return false;
204 }
205
206 *remote_ts = local_ts - t->current_skew;
207 return true;
208}