The open source OpenXR runtime
d/vive: Improve and refactor IMU time calculation
+71
-9
+4
-1
src/xrt/drivers/vive/vive_device.c
+4
-1
src/xrt/drivers/vive/vive_device.c
···
432
432
m_relation_history_push(d->fusion.relation_hist, &rel, now_ns);
433
433
os_mutex_unlock(&d->fusion.mutex);
434
434
435
-
vive_source_push_imu_packet(d->source, d->imu.last_sample_ts_ns, acceleration, angular_velocity);
435
+
assert(j > 0);
436
+
uint32_t age = j <= 0 ? 0 : (uint32_t)(j - 1);
437
+
438
+
vive_source_push_imu_packet(d->source, age, d->imu.last_sample_ts_ns, acceleration, angular_velocity);
436
439
}
437
440
}
438
441
+65
-6
src/xrt/drivers/vive/vive_source.c
+65
-6
src/xrt/drivers/vive/vive_source.c
···
1
-
// Copyright 2022, Collabora, Ltd.
1
+
// Copyright 2022-2023, Collabora, Ltd.
2
2
// SPDX-License-Identifier: BSL-1.0
3
3
/*!
4
4
* @file
5
5
* @brief Interface for vive data sources
6
6
* @author Mateo de Mayo <mateo.demayo@collabora.com>
7
+
* @author Jakob Bornecrantz <jakob@collabora.com>
7
8
* @ingroup drv_vive
8
9
*/
9
10
···
16
17
17
18
#include "util/u_deque.h"
18
19
#include "util/u_logging.h"
20
+
#include "util/u_trace_marker.h"
19
21
20
22
#include "vive.h"
21
23
···
143
145
vive_source_receive_imu_sample(struct xrt_imu_sink *sink, struct xrt_imu_sample *s)
144
146
{
145
147
struct vive_source *vs = container_of(sink, struct vive_source, imu_sink);
146
-
s->timestamp_ns = m_clock_offset_a2b(IMU_FREQUENCY, s->timestamp_ns, os_monotonic_get_ns(), &vs->hw2mono);
148
+
147
149
timepoint_ns ts = s->timestamp_ns;
148
150
struct xrt_vec3_f64 a = s->accel_m_s2;
149
151
struct xrt_vec3_f64 w = s->gyro_rad_secs;
···
206
208
}
207
209
208
210
void
209
-
vive_source_push_imu_packet(struct vive_source *vs, timepoint_ns t, struct xrt_vec3 a, struct xrt_vec3 g)
211
+
vive_source_push_imu_packet(struct vive_source *vs, uint32_t age, timepoint_ns t, struct xrt_vec3 a, struct xrt_vec3 g)
210
212
{
211
-
struct xrt_vec3_f64 a64 = {a.x, a.y, a.z};
212
-
struct xrt_vec3_f64 g64 = {g.x, g.y, g.z};
213
-
struct xrt_imu_sample sample = {.timestamp_ns = t, .accel_m_s2 = a64, .gyro_rad_secs = g64};
213
+
/*
214
+
* We want the samples to be on sometime in the past, not future. This
215
+
* is due to USB latency, which we don't know, so we are guessing here.
216
+
* We also don't know if the timestamp given for the start of the sample
217
+
* or the end.
218
+
*
219
+
* We picked 2 here because that's about what the best gaming mice can
220
+
* do, it also seems to feel good with what seems to be reasonable
221
+
* present to display offset in the compositor.
222
+
*
223
+
* We also adjust for the "age" of a sample, the vive sends out 3
224
+
* samples per packet, most often only one is a new sample. But
225
+
* sometimes we get up to 3 new samples in one packet. So if age is
226
+
* greater then 0, adjust with that many MS (1000Hz sampler rate).
227
+
*/
228
+
229
+
// 2 ms in value.
230
+
const timepoint_ns t2ms_ns = U_TIME_1MS_IN_NS * 2;
231
+
232
+
// Extra in the past for age.
233
+
timepoint_ns age_diff_ns = age * U_TIME_1MS_IN_NS;
234
+
235
+
// Now.
236
+
timepoint_ns now_ns = (timepoint_ns)os_monotonic_get_ns();
237
+
238
+
// Calculated sample point.
239
+
timepoint_ns sample_point = now_ns - t2ms_ns - age_diff_ns;
240
+
241
+
// Time adjustment.
242
+
t = m_clock_offset_a2b(IMU_FREQUENCY, t, sample_point, &vs->hw2mono);
243
+
244
+
// Finished sample.
245
+
struct xrt_imu_sample sample = {
246
+
.timestamp_ns = t,
247
+
.accel_m_s2 = (struct xrt_vec3_f64){a.x, a.y, a.z},
248
+
.gyro_rad_secs = (struct xrt_vec3_f64){g.x, g.y, g.z},
249
+
};
250
+
251
+
// Push it out!
214
252
xrt_sink_push_imu(&vs->imu_sink, &sample);
253
+
254
+
// Only do this if we are really debugging stuff.
255
+
#ifdef XRT_FEATURE_TRACING
256
+
timepoint_ns diff_ns = t - (now_ns - age_diff_ns);
257
+
static timepoint_ns last_ns = 0;
258
+
if (last_ns == 0) {
259
+
last_ns = t;
260
+
}
261
+
262
+
double now_diff_ms = time_ns_to_ms_f(diff_ns);
263
+
double last_diff_ms = time_ns_to_ms_f(t - last_ns);
264
+
last_ns = t;
265
+
266
+
#ifdef U_TRACE_TRACY
267
+
TracyCPlot("Vive IMU to now(ms)", now_diff_ms);
268
+
TracyCPlot("Vive IMU to last(ms)", last_diff_ms);
269
+
TracyCPlot("Vive IMU age", age);
270
+
#endif
271
+
272
+
VIVE_TRACE(vs, "Sample diffs, now: %+.4fms, last: %+.4f, age: %u", now_diff_ms, last_diff_ms, age);
273
+
#endif
215
274
}
216
275
217
276
void
+2
-2
src/xrt/drivers/vive/vive_source.h
+2
-2
src/xrt/drivers/vive/vive_source.h
···
1
-
// Copyright 2022, Collabora, Ltd.
1
+
// Copyright 2022-2023, Collabora, Ltd.
2
2
// SPDX-License-Identifier: BSL-1.0
3
3
/*!
4
4
* @file
···
28
28
vive_source_create(struct xrt_frame_context *xfctx);
29
29
30
30
void
31
-
vive_source_push_imu_packet(struct vive_source *vs, timepoint_ns t, struct xrt_vec3 a, struct xrt_vec3 g);
31
+
vive_source_push_imu_packet(struct vive_source *vs, uint32_t age, timepoint_ns t, struct xrt_vec3 a, struct xrt_vec3 g);
32
32
33
33
void
34
34
vive_source_push_frame_ticks(struct vive_source *vs, timepoint_ns ticks);