matrixfurry.com
The open source OpenXR runtime
1// Copyright 2022-2023, Collabora, Ltd.
2// SPDX-License-Identifier: BSL-1.0
3/*!
4 * @file
5 * @brief Interface for vive data sources
6 * @author Mateo de Mayo <mateo.demayo@collabora.com>
7 * @author Jakob Bornecrantz <jakob@collabora.com>
8 * @ingroup drv_vive
9 */
10
11#include "xrt/xrt_frame.h"
12#include "xrt/xrt_tracking.h"
13
14#include "os/os_threading.h"
15
16#include "math/m_clock_tracking.h"
17
18#include "util/u_deque.h"
19#include "util/u_logging.h"
20#include "util/u_trace_marker.h"
21
22#include "vive.h"
23
24
25/*!
26 * Manages the data streaming state related to a vive headset.
27 *
28 * @implements xrt_frame_node
29 */
30struct vive_source
31{
32 struct xrt_frame_node node;
33 enum u_logging_level log_level;
34
35 // Sinks
36 struct xrt_frame_sink sbs_sink; //!< Intermediate sink for SBS frames
37 struct xrt_imu_sink imu_sink; //!< Intermediate sink for IMU samples
38 struct xrt_slam_sinks in_sinks; //!< Pointers to intermediate sinks
39 struct xrt_slam_sinks out_sinks; //!< Pointers to downstream sinks
40
41 // V4L2 frame streaming state
42 bool timestamps_have_been_zero_until_now; //!< First v4l2 frames are zeroed
43 bool waiting_for_first_nonempty_frame; //!< Whether the first good frame has been received
44
45 // Frame timestamps
46 struct u_deque_timepoint_ns frame_timestamps; //! Queue of yet unused frame hw timestamps
47 struct os_mutex frame_timestamps_lock; //! Lock for accessing frame_timestamps
48 uint32_t last_frame_ticks; //! Last frame timestamp in device ticks
49 timepoint_ns last_frame_ts_ns; //! Last frame timestamp in device nanoseconds
50
51 // Clock offsets
52 time_duration_ns hw2mono; //!< Estimated offset from IMU to monotonic clock
53 time_duration_ns hw2v4l2; //!< Estimated offset from IMU to V4L2 clock
54};
55
56/*
57 *
58 * Vive source methods
59 *
60 */
61
62//! Find the best corresponding hw timestamp from this v4l2 frame, return
63//! whether it was found.
64bool
65vive_source_try_convert_v4l2_timestamp(struct vive_source *vs, struct xrt_frame *xf)
66{
67 assert(xf->timestamp != 0 || vs->timestamps_have_been_zero_until_now);
68 if (xf->timestamp == 0) {
69 return false;
70 }
71 vs->timestamps_have_been_zero_until_now = false;
72
73 struct u_deque_timepoint_ns vive_timestamps = vs->frame_timestamps;
74 struct os_mutex *vive_timestamps_lock = &vs->frame_timestamps_lock;
75
76 timepoint_ns v4l2_ts = xf->timestamp;
77
78 size_t vive_ts_count = u_deque_timepoint_ns_size(vive_timestamps);
79 if (vive_ts_count == 0) { // This seems to happen in some runs
80 // This code assumes vive_timestamps will always be populated before v4l2
81 // receives a frame, thus if we reach this, this assumption has failed.
82 // As a fallback we'll use the v4l2 timestamp corrected to monotonic clock.
83 VIVE_TRACE(vs, "No vive timestamps available for this v4l2 frame, will use v4l2 timestamp");
84 timepoint_ns hw_ts = v4l2_ts - vs->hw2v4l2;
85 xf->timestamp = hw_ts + vs->hw2mono;
86 return true;
87 }
88
89 os_mutex_lock(vive_timestamps_lock);
90
91 // Find i in vive_timestamps that would be closer to xf->timestamp in v4l2 clock
92 int closer_i = -1;
93 timepoint_ns vive_ts = -1;
94 time_duration_ns min_distance = INT64_MAX;
95 for (size_t i = 0; i < vive_ts_count; i++) {
96 vive_ts = u_deque_timepoint_ns_at(vive_timestamps, i);
97 timepoint_ns v4l2_ts_est = vive_ts + vs->hw2v4l2;
98 time_duration_ns distance = llabs(v4l2_ts_est - v4l2_ts);
99 if (distance < min_distance) {
100 closer_i = i;
101 min_distance = distance;
102 }
103 }
104
105 // Discard missed frames and set vive_timestamp to use in this frame
106 timepoint_ns vive_timestamp = 0;
107 for (; closer_i >= 0; closer_i--) {
108 u_deque_timepoint_ns_pop_front(vive_timestamps, &vive_timestamp);
109 }
110
111 os_mutex_unlock(vive_timestamps_lock);
112
113 // Our estimate is within a reasonable time distance
114 assert(min_distance < U_TIME_1S_IN_NS / CAMERA_FREQUENCY || vs->waiting_for_first_nonempty_frame);
115 vs->waiting_for_first_nonempty_frame = false;
116
117 // Update estimate of hw2v4l2 clock offset, only used for matching timestamps
118 m_clock_offset_a2b(CAMERA_FREQUENCY, vive_timestamp, xf->timestamp, &vs->hw2v4l2);
119
120 // Use vive_timestamp and put it in monotonic clock
121 xf->timestamp = vive_timestamp + vs->hw2mono; // Notice that we don't use hw2v4l2
122
123 return true;
124}
125
126static void
127vive_source_receive_sbs_frame(struct xrt_frame_sink *sink, struct xrt_frame *xf)
128{
129 struct vive_source *vs = container_of(sink, struct vive_source, sbs_sink);
130 bool should_push = vive_source_try_convert_v4l2_timestamp(vs, xf);
131
132 if (!should_push) {
133 VIVE_TRACE(vs, "skipped sbs img t=%" PRId64 " source_t=%" PRId64, xf->timestamp, xf->source_timestamp);
134 return;
135 }
136
137 VIVE_TRACE(vs, "sbs img t=%" PRId64 " source_t=%" PRId64, xf->timestamp, xf->source_timestamp);
138
139 if (vs->out_sinks.cams[0]) { // The split into left right will happen downstream
140 xrt_sink_push_frame(vs->out_sinks.cams[0], xf);
141 }
142}
143
144static void
145vive_source_receive_imu_sample(struct xrt_imu_sink *sink, struct xrt_imu_sample *s)
146{
147 struct vive_source *vs = container_of(sink, struct vive_source, imu_sink);
148
149 timepoint_ns ts = s->timestamp_ns;
150 struct xrt_vec3_f64 a = s->accel_m_s2;
151 struct xrt_vec3_f64 w = s->gyro_rad_secs;
152 VIVE_TRACE(vs, "imu t=%" PRId64 " a=(%f %f %f) w=(%f %f %f)", ts, a.x, a.y, a.z, w.x, w.y, w.z);
153
154 if (vs->out_sinks.imu) {
155 xrt_sink_push_imu(vs->out_sinks.imu, s);
156 }
157}
158
159static void
160vive_source_node_break_apart(struct xrt_frame_node *node)
161{}
162
163static void
164vive_source_node_destroy(struct xrt_frame_node *node)
165{
166 struct vive_source *vs = container_of(node, struct vive_source, node);
167 os_mutex_destroy(&vs->frame_timestamps_lock);
168 u_deque_timepoint_ns_destroy(&vs->frame_timestamps);
169
170 free(vs);
171}
172
173
174/*!
175 *
176 * Exported functions
177 *
178 */
179
180struct vive_source *
181vive_source_create(struct xrt_frame_context *xfctx)
182{
183 struct vive_source *vs = U_TYPED_CALLOC(struct vive_source);
184 vs->log_level = debug_get_log_option_vive_log();
185
186 // Setup sinks
187 vs->sbs_sink.push_frame = vive_source_receive_sbs_frame;
188 vs->imu_sink.push_imu = vive_source_receive_imu_sample;
189 vs->in_sinks.cam_count = 1;
190 vs->in_sinks.cams[0] = &vs->sbs_sink;
191 vs->in_sinks.imu = &vs->imu_sink;
192
193 vs->timestamps_have_been_zero_until_now = true;
194 vs->waiting_for_first_nonempty_frame = true;
195
196 vs->frame_timestamps = u_deque_timepoint_ns_create();
197 os_mutex_init(&vs->frame_timestamps_lock);
198
199 // Setup node
200 struct xrt_frame_node *xfn = &vs->node;
201 xfn->break_apart = vive_source_node_break_apart;
202 xfn->destroy = vive_source_node_destroy;
203 xrt_frame_context_add(xfctx, &vs->node);
204
205 VIVE_DEBUG(vs, "Vive source created");
206
207 return vs;
208}
209
210void
211vive_source_push_imu_packet(struct vive_source *vs, uint32_t age, timepoint_ns t, struct xrt_vec3 a, struct xrt_vec3 g)
212{
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!
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
274}
275
276void
277vive_source_push_frame_ticks(struct vive_source *vs, timepoint_ns ticks)
278{
279 ticks_to_ns(ticks, &vs->last_frame_ticks, &vs->last_frame_ts_ns);
280 u_deque_timepoint_ns_push_back(vs->frame_timestamps, vs->last_frame_ts_ns);
281}
282
283void
284vive_source_hook_into_sinks(struct vive_source *vs, struct xrt_slam_sinks *sinks)
285{
286 vs->out_sinks = *sinks;
287 sinks->cam_count = 1;
288 sinks->cams[0] = vs->in_sinks.cams[0];
289}