The open source OpenXR runtime
a/math: Introduce minimum skew clock tracker.
Add m_clock_windowed_skew_tracker that uses a windowed
minimum skew tracker with exponential smoothing to
do more accurate remote to local clock offset estimation.
Part-of: <https://gitlab.freedesktop.org/monado/monado/-/merge_requests/2188>
authored by
Jan Schmidt
and committed by
Marge Bot
34377371
8a535d0a
+317
-60
+2
-1
src/xrt/auxiliary/math/CMakeLists.txt
+2
-1
src/xrt/auxiliary/math/CMakeLists.txt
-56
src/xrt/auxiliary/math/m_clock_offset.h
-56
src/xrt/auxiliary/math/m_clock_offset.h
···
1
-
// Copyright 2022, Collabora, Ltd.
2
-
// SPDX-License-Identifier: BSL-1.0
3
-
/*!
4
-
* @file
5
-
* @brief Helpers to estimate offsets between clocks
6
-
* @author Mateo de Mayo <mateo.demayo@collabora.com>
7
-
* @ingroup aux_math
8
-
*/
9
-
10
-
#pragma once
11
-
12
-
#include "util/u_time.h"
13
-
14
-
#ifdef __cplusplus
15
-
extern "C" {
16
-
#endif
17
-
18
-
/*!
19
-
* Helper to estimate the offset between two clocks using exponential smoothing.
20
-
*
21
-
* Given a sample from two timestamp domains A and B that should have been
22
-
* sampled as close as possible, together with an estimate of the offset between
23
-
* A clock and B clock (or zero), it applies a smoothing average on the
24
-
* estimated offset and returns @p a in B clock.
25
-
*
26
-
* @param freq About how many times per second this function is called. Helps setting a good decay value.
27
-
* @param a Timestamp in clock A of the event
28
-
* @param b Timestamp in clock B of the event
29
-
* @param[in,out] inout_a2b Pointer to the current offset estimate from A to B, or 0 if unknown.
30
-
* Value pointed-to will be updated.
31
-
* @return timepoint_ns @p a in B clock
32
-
*/
33
-
static inline timepoint_ns
34
-
m_clock_offset_a2b(float freq, timepoint_ns a, timepoint_ns b, time_duration_ns *inout_a2b)
35
-
{
36
-
// This formulation of exponential filtering uses a fixed-precision integer for the
37
-
// alpha value and operates on the delta between the old and new a2b to avoid
38
-
// precision / overflow problems.
39
-
40
-
// Totally arbitrary way of computing alpha, if you have a better one, replace it
41
-
const time_duration_ns alpha = 1000 * (1.0 - 12.5 / freq); // Weight to put on accumulated a2b
42
-
time_duration_ns old_a2b = *inout_a2b;
43
-
time_duration_ns got_a2b = b - a;
44
-
time_duration_ns new_a2b;
45
-
if (old_a2b == 0) { // a2b has not been set yet
46
-
new_a2b = got_a2b;
47
-
} else {
48
-
new_a2b = ((old_a2b - got_a2b) * alpha) / 1000 + got_a2b;
49
-
}
50
-
*inout_a2b = new_a2b;
51
-
return a + new_a2b;
52
-
}
53
-
54
-
#ifdef __cplusplus
55
-
}
56
-
#endif
+208
src/xrt/auxiliary/math/m_clock_tracking.c
+208
src/xrt/auxiliary/math/m_clock_tracking.c
···
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 */
15
+
const time_duration_ns CLOCK_RESET_THRESHOLD = 100 * U_TIME_1MS_IN_NS;
16
+
const time_duration_ns CLOCK_RESET_HOLDOFF = 30 * U_TIME_1MS_IN_NS;
17
+
18
+
struct 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
+
24
+
static struct m_clock_observation
25
+
m_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
+
34
+
struct 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
+
64
+
struct m_clock_windowed_skew_tracker *
65
+
m_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
+
83
+
void
84
+
m_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
+
91
+
void
92
+
m_clock_windowed_skew_tracker_destroy(struct m_clock_windowed_skew_tracker *t)
93
+
{
94
+
free(t->window);
95
+
free(t);
96
+
}
97
+
98
+
void
99
+
m_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
+
184
+
bool
185
+
m_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
+
197
+
bool
198
+
m_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
+
}
+103
src/xrt/auxiliary/math/m_clock_tracking.h
+103
src/xrt/auxiliary/math/m_clock_tracking.h
···
1
+
// Copyright 2022, Collabora, Ltd.
2
+
// Copyright 2024, Jan Schmidt
3
+
// SPDX-License-Identifier: BSL-1.0
4
+
/*!
5
+
* @file
6
+
* @brief Helpers to estimate offsets between clocks
7
+
* @author Mateo de Mayo <mateo.demayo@collabora.com>
8
+
* @ingroup aux_math
9
+
*/
10
+
11
+
#pragma once
12
+
13
+
#include "xrt/xrt_defines.h"
14
+
#include "util/u_time.h"
15
+
16
+
#ifdef __cplusplus
17
+
extern "C" {
18
+
#endif
19
+
20
+
/*!
21
+
* Helper to estimate the offset between two clocks using exponential smoothing.
22
+
*
23
+
* Given a sample from two timestamp domains A and B that should have been
24
+
* sampled as close as possible, together with an estimate of the offset between
25
+
* A clock and B clock (or zero), it applies a smoothing average on the
26
+
* estimated offset and returns @p a in B clock.
27
+
*
28
+
* This estimator can be used when clock observations are arriving with a low
29
+
* delay and small jitter, or when accuracy is less important (on the order of
30
+
* the jitter that is present). It is very computationally cheap.
31
+
*
32
+
* @param freq About how many times per second this function is called. Helps setting a good decay value.
33
+
* @param a Timestamp in clock A of the event
34
+
* @param b Timestamp in clock B of the event
35
+
* @param[in,out] inout_a2b Pointer to the current offset estimate from A to B, or 0 if unknown.
36
+
* Value pointed-to will be updated.
37
+
* @return timepoint_ns @p a in B clock
38
+
*/
39
+
static inline timepoint_ns
40
+
m_clock_offset_a2b(float freq, timepoint_ns a, timepoint_ns b, time_duration_ns *inout_a2b)
41
+
{
42
+
// This formulation of exponential filtering uses a fixed-precision integer for the
43
+
// alpha value and operates on the delta between the old and new a2b to avoid
44
+
// precision / overflow problems.
45
+
46
+
// Totally arbitrary way of computing alpha, if you have a better one, replace it
47
+
const time_duration_ns alpha = 1000 * (1.0 - 12.5 / freq); // Weight to put on accumulated a2b
48
+
time_duration_ns old_a2b = *inout_a2b;
49
+
time_duration_ns got_a2b = b - a;
50
+
time_duration_ns new_a2b;
51
+
if (old_a2b == 0) { // a2b has not been set yet
52
+
new_a2b = got_a2b;
53
+
} else {
54
+
new_a2b = ((old_a2b - got_a2b) * alpha) / 1000 + got_a2b;
55
+
}
56
+
*inout_a2b = new_a2b;
57
+
return a + new_a2b;
58
+
}
59
+
60
+
/*!
61
+
* Helper to estimate the offset between two clocks using a windowed
62
+
* minimum-skew estimation plus exponential smoothing. The algorithm
63
+
* tracks the smallest offset within the window, on the theory that
64
+
* minima represent samples with the lowest transmission delay and jitter.
65
+
*
66
+
* More computationally intensive than the simple m_clock_offset_a2b estimator,
67
+
* but can estimate a clock with accuracy in the microsecond range
68
+
* even in the presence of 10s of milliseconds of jitter.
69
+
*
70
+
* Based on the approach in Dominique Fober, Yann Orlarey, Stéphane Letz.
71
+
* Real Time Clock Skew Estimation over Network Delays. [Technical Report] GRAME. 2005.
72
+
* https://hal.science/hal-02158803/document
73
+
*/
74
+
struct m_clock_windowed_skew_tracker;
75
+
76
+
/*!
77
+
* Allocate a struct m_clock_windowed_skew_tracker with a
78
+
* window of @param window_samples samples.
79
+
*/
80
+
struct m_clock_windowed_skew_tracker *
81
+
m_clock_windowed_skew_tracker_alloc(const size_t window_samples);
82
+
void
83
+
m_clock_windowed_skew_tracker_reset(struct m_clock_windowed_skew_tracker *t);
84
+
void
85
+
m_clock_windowed_skew_tracker_destroy(struct m_clock_windowed_skew_tracker *t);
86
+
87
+
void
88
+
m_clock_windowed_skew_tracker_push(struct m_clock_windowed_skew_tracker *t,
89
+
const timepoint_ns local_ts,
90
+
const timepoint_ns remote_ts);
91
+
92
+
bool
93
+
m_clock_windowed_skew_tracker_to_local(struct m_clock_windowed_skew_tracker *t,
94
+
const timepoint_ns remote_ts,
95
+
timepoint_ns *local_ts);
96
+
bool
97
+
m_clock_windowed_skew_tracker_to_remote(struct m_clock_windowed_skew_tracker *t,
98
+
const timepoint_ns local_ts,
99
+
timepoint_ns *remote_ts);
100
+
101
+
#ifdef __cplusplus
102
+
}
103
+
#endif
+1
-1
src/xrt/drivers/rift_s/rift_s_tracker.c
+1
-1
src/xrt/drivers/rift_s/rift_s_tracker.c
+1
-1
src/xrt/drivers/vive/vive_source.c
+1
-1
src/xrt/drivers/vive/vive_source.c
+1
src/xrt/drivers/wmr/wmr_controller_base.c
+1
src/xrt/drivers/wmr/wmr_controller_base.c