matrixfurry.com
The open source OpenXR runtime
1// Copyright 2021, Collabora, Ltd.
2// SPDX-License-Identifier: BSL-1.0
3/*!
4 * @file
5 * @brief WMR camera and IMU data source.
6 * @author Mateo de Mayo <mateo.demayo@collabora.com>
7 * @ingroup drv_wmr
8 */
9#include "wmr_source.h"
10#include "wmr_camera.h"
11#include "wmr_config.h"
12#include "wmr_protocol.h"
13
14#include "math/m_api.h"
15#include "math/m_clock_tracking.h"
16#include "math/m_filter_fifo.h"
17#include "util/u_debug.h"
18#include "util/u_sink.h"
19#include "util/u_var.h"
20#include "util/u_trace_marker.h"
21#include "xrt/xrt_tracking.h"
22#include "xrt/xrt_frameserver.h"
23
24#include <assert.h>
25#include <stdio.h>
26
27#define WMR_SOURCE_STR "WMR Source"
28
29#define WMR_TRACE(w, ...) U_LOG_IFL_T(w->log_level, __VA_ARGS__)
30#define WMR_DEBUG(w, ...) U_LOG_IFL_D(w->log_level, __VA_ARGS__)
31#define WMR_INFO(w, ...) U_LOG_IFL_I(w->log_level, __VA_ARGS__)
32#define WMR_WARN(w, ...) U_LOG_IFL_W(w->log_level, __VA_ARGS__)
33#define WMR_ERROR(w, ...) U_LOG_IFL_E(w->log_level, __VA_ARGS__)
34#define WMR_ASSERT(predicate, ...) \
35 do { \
36 bool p = predicate; \
37 if (!p) { \
38 U_LOG(U_LOGGING_ERROR, __VA_ARGS__); \
39 assert(false && "WMR_ASSERT failed: " #predicate); \
40 exit(EXIT_FAILURE); \
41 } \
42 } while (false);
43#define WMR_ASSERT_(predicate) WMR_ASSERT(predicate, "Assertion failed " #predicate)
44
45DEBUG_GET_ONCE_LOG_OPTION(wmr_log, "WMR_LOG", U_LOGGING_INFO)
46
47/*!
48 * Handles all the data sources from the WMR driver
49 *
50 * @todo Currently only properly handling tracking cameras, move IMU and other sources here
51 * @implements xrt_fs
52 * @implements xrt_frame_node
53 */
54struct wmr_source
55{
56 struct xrt_fs xfs;
57 struct xrt_frame_node node;
58 enum u_logging_level log_level; //!< Log level
59
60 struct wmr_hmd_config config;
61 struct wmr_camera *camera;
62
63 // Sinks (head tracking)
64 struct xrt_frame_sink cam_sinks[WMR_MAX_CAMERAS]; //!< Intermediate sinks for camera frames
65 struct xrt_imu_sink imu_sink; //!< Intermediate sink for IMU samples
66 struct xrt_slam_sinks in_sinks; //!< Pointers to intermediate sinks
67 struct xrt_slam_sinks out_sinks; //!< Pointers to downstream sinks
68
69 // UI Sinks (head tracking)
70 struct u_sink_debug ui_cam_sinks[WMR_MAX_CAMERAS]; //!< Sink to display camera frames in UI
71 struct m_ff_vec3_f32 *gyro_ff; //!< Queue of gyroscope data to display in UI
72 struct m_ff_vec3_f32 *accel_ff; //!< Queue of accelerometer data to display in UI
73
74 bool is_running; //!< Whether the device is streaming
75 bool first_imu_received; //!< Don't send frames until first IMU sample
76 timepoint_ns last_imu_ns; //!< Last timepoint received.
77 time_duration_ns hw2mono; //!< Estimated offset from IMU to monotonic clock
78 time_duration_ns cam_hw2mono; //!< Caches hw2mono for use in the full frame bundle
79};
80
81/*
82 *
83 * Sinks functionality
84 *
85 */
86
87#define DEFINE_RECEIVE_CAM(cam_id) \
88 static void receive_cam##cam_id(struct xrt_frame_sink *sink, struct xrt_frame *xf) \
89 { \
90 struct wmr_source *ws = container_of(sink, struct wmr_source, cam_sinks[cam_id]); \
91 if (cam_id == 0) { \
92 ws->cam_hw2mono = ws->hw2mono; \
93 } \
94 xf->timestamp += ws->cam_hw2mono; \
95 WMR_TRACE(ws, "cam" #cam_id " img t=%" PRId64 " source_t=%" PRId64, xf->timestamp, \
96 xf->source_timestamp); \
97 u_sink_debug_push_frame(&ws->ui_cam_sinks[cam_id], xf); \
98 if (ws->out_sinks.cams[cam_id] && ws->first_imu_received) { \
99 xrt_sink_push_frame(ws->out_sinks.cams[cam_id], xf); \
100 } \
101 }
102
103DEFINE_RECEIVE_CAM(0)
104DEFINE_RECEIVE_CAM(1)
105DEFINE_RECEIVE_CAM(2)
106DEFINE_RECEIVE_CAM(3)
107
108//! Define a function for each WMR_MAX_CAMERAS and reference it in this array
109void (*receive_cam[WMR_MAX_CAMERAS])(struct xrt_frame_sink *, struct xrt_frame *) = {
110 receive_cam0, //
111 receive_cam1, //
112 receive_cam2, //
113 receive_cam3, //
114};
115
116static void
117receive_imu_sample(struct xrt_imu_sink *sink, struct xrt_imu_sample *s)
118{
119 struct wmr_source *ws = container_of(sink, struct wmr_source, imu_sink);
120
121 // Convert hardware timestamp into monotonic clock. Update offset estimate hw2mono.
122 // Note this is only done with IMU samples as they have the smallest USB transmission time.
123 const float IMU_FREQ = 250.f; //!< @todo use 1000 if "average_imus" is false
124 timepoint_ns now_hw = s->timestamp_ns;
125 timepoint_ns now_mono = (timepoint_ns)os_monotonic_get_ns();
126 timepoint_ns ts = m_clock_offset_a2b(IMU_FREQ, now_hw, now_mono, &ws->hw2mono);
127
128 /*
129 * Check if the timepoint does time travel, we get one or two
130 * old samples when the device has not been cleanly shut down.
131 */
132 if (ws->last_imu_ns > ts) {
133 WMR_WARN(ws, "Received sample from the past, new: %" PRIu64 ", last: %" PRIu64 ", diff: %" PRIu64, ts,
134 s->timestamp_ns, ts - s->timestamp_ns);
135 return;
136 }
137
138 ws->first_imu_received = true;
139 ws->last_imu_ns = ts;
140 s->timestamp_ns = ts;
141
142 struct xrt_vec3_f64 a = s->accel_m_s2;
143 struct xrt_vec3_f64 w = s->gyro_rad_secs;
144 WMR_TRACE(ws, "imu t=%" PRId64 " a=(%f %f %f) w=(%f %f %f)", ts, a.x, a.y, a.z, w.x, w.y, w.z);
145
146 // Push to debug UI
147 struct xrt_vec3 gyro = {(float)w.x, (float)w.y, (float)w.z};
148 struct xrt_vec3 accel = {(float)a.x, (float)a.y, (float)a.z};
149 m_ff_vec3_f32_push(ws->gyro_ff, &gyro, ts);
150 m_ff_vec3_f32_push(ws->accel_ff, &accel, ts);
151
152 if (ws->out_sinks.imu) {
153 xrt_sink_push_imu(ws->out_sinks.imu, s);
154 }
155}
156
157
158/*
159 *
160 * Frameserver functionality
161 *
162 */
163
164static inline struct wmr_source *
165wmr_source_from_xfs(struct xrt_fs *xfs)
166{
167 struct wmr_source *ws = container_of(xfs, struct wmr_source, xfs);
168 return ws;
169}
170
171static bool
172wmr_source_enumerate_modes(struct xrt_fs *xfs, struct xrt_fs_mode **out_modes, uint32_t *out_count)
173{
174 WMR_ASSERT(false, "Not implemented");
175 return false;
176}
177
178static bool
179wmr_source_configure_capture(struct xrt_fs *xfs, struct xrt_fs_capture_parameters *cp)
180{
181 WMR_ASSERT(false, "Not implemented");
182 return false;
183}
184
185static bool
186wmr_source_stream_stop(struct xrt_fs *xfs)
187{
188 DRV_TRACE_MARKER();
189
190 struct wmr_source *ws = wmr_source_from_xfs(xfs);
191
192 bool stopped = wmr_camera_stop(ws->camera);
193 if (!stopped) {
194 WMR_ERROR(ws, "Unable to stop WMR cameras");
195 WMR_ASSERT_(false);
196 }
197
198 return stopped;
199}
200
201static bool
202wmr_source_is_running(struct xrt_fs *xfs)
203{
204 DRV_TRACE_MARKER();
205
206 struct wmr_source *ws = wmr_source_from_xfs(xfs);
207 return ws->is_running;
208}
209
210static bool
211wmr_source_stream_start(struct xrt_fs *xfs,
212 struct xrt_frame_sink *xs,
213 enum xrt_fs_capture_type capture_type,
214 uint32_t descriptor_index)
215{
216 DRV_TRACE_MARKER();
217
218 struct wmr_source *ws = wmr_source_from_xfs(xfs);
219
220 if (xs == NULL && capture_type == XRT_FS_CAPTURE_TYPE_TRACKING) {
221 WMR_INFO(ws, "Starting WMR stream in tracking mode");
222 } else if (xs != NULL && capture_type == XRT_FS_CAPTURE_TYPE_CALIBRATION) {
223 WMR_INFO(ws, "Starting WMR stream in calibration mode, will stream only cam0 frames");
224 ws->out_sinks.cam_count = 1;
225 ws->out_sinks.cams[0] = xs;
226 } else {
227 WMR_ASSERT(false, "Unsupported stream configuration xs=%p capture_type=%d", (void *)xs, capture_type);
228 return false;
229 }
230
231 bool started = wmr_camera_start(ws->camera);
232 if (!started) {
233 WMR_ERROR(ws, "Unable to start WMR cameras");
234 WMR_ASSERT_(false);
235 }
236
237 ws->is_running = started;
238 return ws->is_running;
239}
240
241static bool
242wmr_source_slam_stream_start(struct xrt_fs *xfs, struct xrt_slam_sinks *sinks)
243{
244 DRV_TRACE_MARKER();
245
246 struct wmr_source *ws = wmr_source_from_xfs(xfs);
247 if (sinks != NULL) {
248 ws->out_sinks = *sinks;
249 }
250 return wmr_source_stream_start(xfs, NULL, XRT_FS_CAPTURE_TYPE_TRACKING, 0);
251}
252
253
254/*
255 *
256 * Frame node functionality
257 *
258 */
259
260static void
261wmr_source_node_break_apart(struct xrt_frame_node *node)
262{
263 DRV_TRACE_MARKER();
264
265 struct wmr_source *ws = container_of(node, struct wmr_source, node);
266 wmr_source_stream_stop(&ws->xfs);
267}
268
269static void
270wmr_source_node_destroy(struct xrt_frame_node *node)
271{
272 DRV_TRACE_MARKER();
273
274 struct wmr_source *ws = container_of(node, struct wmr_source, node);
275 WMR_DEBUG(ws, "Destroying WMR source");
276 for (int i = 0; i < ws->config.tcam_count; i++) {
277 u_sink_debug_destroy(&ws->ui_cam_sinks[i]);
278 }
279 m_ff_vec3_f32_free(&ws->gyro_ff);
280 m_ff_vec3_f32_free(&ws->accel_ff);
281 u_var_remove_root(ws);
282 if (ws->camera != NULL) { // It could be null if XRT_HAVE_LIBUSB is not defined
283 wmr_camera_free(ws->camera);
284 }
285 free(ws);
286}
287
288
289/*
290 *
291 * Exported functions
292 *
293 */
294
295//! Create and open the frame server for IMU/camera streaming.
296struct xrt_fs *
297wmr_source_create(struct xrt_frame_context *xfctx, struct xrt_prober_device *dev_holo, struct wmr_hmd_config cfg)
298{
299 DRV_TRACE_MARKER();
300
301 struct wmr_source *ws = U_TYPED_CALLOC(struct wmr_source);
302 ws->log_level = debug_get_log_option_wmr_log();
303
304 // Setup xrt_fs
305 struct xrt_fs *xfs = &ws->xfs;
306 xfs->enumerate_modes = wmr_source_enumerate_modes;
307 xfs->configure_capture = wmr_source_configure_capture;
308 xfs->stream_start = wmr_source_stream_start;
309 xfs->slam_stream_start = wmr_source_slam_stream_start;
310 xfs->stream_stop = wmr_source_stream_stop;
311 xfs->is_running = wmr_source_is_running;
312 (void)snprintf(xfs->name, sizeof(xfs->name), WMR_SOURCE_STR);
313 (void)snprintf(xfs->product, sizeof(xfs->product), WMR_SOURCE_STR " Product");
314 (void)snprintf(xfs->manufacturer, sizeof(xfs->manufacturer), WMR_SOURCE_STR " Manufacturer");
315 (void)snprintf(xfs->serial, sizeof(xfs->serial), WMR_SOURCE_STR " Serial");
316 xfs->source_id = *((uint64_t *)"WMR_SRC\0");
317
318 // Setup sinks
319 for (int i = 0; i < WMR_MAX_CAMERAS; i++) {
320 ws->cam_sinks[i].push_frame = receive_cam[i];
321 }
322 ws->imu_sink.push_imu = receive_imu_sample;
323
324 ws->in_sinks.cam_count = cfg.tcam_count;
325 for (int i = 0; i < cfg.tcam_count; i++) {
326 ws->in_sinks.cams[i] = &ws->cam_sinks[i];
327 }
328 ws->in_sinks.imu = &ws->imu_sink;
329
330 struct wmr_camera_open_config options = {
331 .dev_holo = dev_holo,
332 .tcam_confs = cfg.tcams,
333 .tcam_sinks = ws->in_sinks.cams,
334 .tcam_count = cfg.tcam_count,
335 .slam_cam_count = cfg.slam_cam_count,
336 .log_level = ws->log_level,
337 };
338
339 ws->camera = wmr_camera_open(&options);
340 ws->config = cfg;
341
342 // Setup UI
343 for (int i = 0; i < cfg.tcam_count; i++) {
344 u_sink_debug_init(&ws->ui_cam_sinks[i]);
345 }
346 m_ff_vec3_f32_alloc(&ws->gyro_ff, 1000);
347 m_ff_vec3_f32_alloc(&ws->accel_ff, 1000);
348 u_var_add_root(ws, WMR_SOURCE_STR, false);
349 u_var_add_log_level(ws, &ws->log_level, "Log Level");
350 u_var_add_ro_ff_vec3_f32(ws, ws->gyro_ff, "Gyroscope");
351 u_var_add_ro_ff_vec3_f32(ws, ws->accel_ff, "Accelerometer");
352 for (int i = 0; i < cfg.tcam_count; i++) {
353 char label[] = "Camera NNNNNNNNNNN";
354 (void)snprintf(label, sizeof(label), "Camera %d", i);
355 u_var_add_sink_debug(ws, &ws->ui_cam_sinks[i], label);
356 }
357
358 // Setup node
359 struct xrt_frame_node *xfn = &ws->node;
360 xfn->break_apart = wmr_source_node_break_apart;
361 xfn->destroy = wmr_source_node_destroy;
362 xrt_frame_context_add(xfctx, &ws->node);
363
364 WMR_DEBUG(ws, "WMR Source created");
365
366 return xfs;
367}
368
369void
370wmr_source_push_imu_packet(struct xrt_fs *xfs, timepoint_ns t, struct xrt_vec3 accel, struct xrt_vec3 gyro)
371{
372 DRV_TRACE_MARKER();
373 struct wmr_source *ws = wmr_source_from_xfs(xfs);
374 struct xrt_vec3_f64 accel_f64 = {accel.x, accel.y, accel.z};
375 struct xrt_vec3_f64 gyro_f64 = {gyro.x, gyro.y, gyro.z};
376 struct xrt_imu_sample sample = {.timestamp_ns = t, .accel_m_s2 = accel_f64, .gyro_rad_secs = gyro_f64};
377 xrt_sink_push_imu(&ws->imu_sink, &sample);
378}