The open source OpenXR runtime
d/wmr: Improve readability in the camera source
authored by
Jan Schmidt
and committed by
Jakob Bornecrantz
7b4fffed
e9c97724
+28
-15
+28
-15
src/xrt/drivers/wmr/wmr_camera.c
+28
-15
src/xrt/drivers/wmr/wmr_camera.c
···
61
61
#define DEFAULT_EXPOSURE 6000
62
62
#define DEFAULT_GAIN 127
63
63
64
+
#define WMR_FRAMETYPE_SLAM 0x0
65
+
#define WMR_FRAMETYPE_CONTROLLER 0x2
66
+
67
+
#define WMR_DEBUG_SINK_SLAM 0
68
+
#define WMR_DEBUG_SINK_CONTROLLER 1
69
+
64
70
struct wmr_camera_active_cmd
65
71
{
66
72
__le32 magic;
···
338
344
339
345
uint16_t unknown16 = read16(&src);
340
346
uint16_t unknown16_2 = read16(&src);
347
+
src += 4; // Skip "Dlo+" magic bytes
348
+
uint16_t frametype = read16(&src);
349
+
/* frametype 0 is SLAM, frametype 2 is controller tracking */
350
+
bool slam_tracking_frame = (frametype == WMR_FRAMETYPE_SLAM);
341
351
342
-
WMR_CAM_TRACE(
343
-
cam, "Frame start TS %" PRIu64 " (%" PRIi64 " since last) end %" PRIu64 " dt %" PRIi64 " unknown %u %u",
344
-
frame_start_ts, frame_start_ts - cam->last_frame_ts, frame_end_ts, delta, unknown16, unknown16_2);
352
+
WMR_CAM_TRACE(cam,
353
+
"Frame start TS %" PRIu64 " (%" PRIi64 " since last) end %" PRIu64 " dt %" PRIi64
354
+
" unknown %u %u frame type %u",
355
+
frame_start_ts, frame_start_ts - cam->last_frame_ts, frame_end_ts, delta, unknown16, unknown16_2,
356
+
frametype);
345
357
358
+
/* Read values from the pixel header */
346
359
uint16_t exposure = xf->data[6] << 8 | xf->data[7];
347
360
uint8_t seq = xf->data[89];
348
361
uint8_t seq_delta = seq - cam->last_seq;
···
360
373
cam->last_frame_ts = frame_start_ts;
361
374
cam->last_seq = seq;
362
375
363
-
/* Exposure of 0 is a dark frame for controller tracking (usually ~60fps) */
364
-
int sink_index = (exposure == 0) ? 1 : 0;
365
-
376
+
/* Push to the appropriate debug output based on frame type */
377
+
int sink_index = slam_tracking_frame ? WMR_DEBUG_SINK_SLAM : WMR_DEBUG_SINK_CONTROLLER;
366
378
if (u_sink_debug_is_active(&cam->debug_sinks[sink_index])) {
367
379
u_sink_debug_push_frame(&cam->debug_sinks[sink_index], xf);
368
380
}
369
381
370
382
// Push to sinks
371
-
bool tracking_frame = sink_index == 0;
372
-
if (tracking_frame) {
383
+
if (slam_tracking_frame) {
373
384
// Tracking frames usually come at ~30fps
374
385
struct xrt_frame *frames[WMR_MAX_CAMERAS] = {NULL};
375
386
for (int i = 0; i < cam->slam_cam_count; i++) {
···
472
483
ceg->aeg = u_autoexpgain_create(U_AEG_STRATEGY_TRACKING, enable_aeg, frame_delay);
473
484
}
474
485
475
-
u_sink_debug_init(&cam->debug_sinks[0]);
476
-
u_sink_debug_init(&cam->debug_sinks[1]);
486
+
u_sink_debug_init(&cam->debug_sinks[WMR_DEBUG_SINK_SLAM]);
487
+
u_sink_debug_init(&cam->debug_sinks[WMR_DEBUG_SINK_CONTROLLER]);
477
488
u_var_add_root(cam, "WMR Camera", true);
478
489
u_var_add_log_level(cam, &cam->log_level, "Log level");
479
490
480
491
u_var_add_gui_header_begin(cam, NULL, "Camera Streams");
481
-
u_var_add_sink_debug(cam, &cam->debug_sinks[0], "Tracking Streams");
482
-
u_var_add_sink_debug(cam, &cam->debug_sinks[1], "Controller Streams");
492
+
u_var_add_sink_debug(cam, &cam->debug_sinks[WMR_DEBUG_SINK_SLAM], "SLAM Tracking Streams");
493
+
u_var_add_sink_debug(cam, &cam->debug_sinks[WMR_DEBUG_SINK_CONTROLLER], "Controller Tracking Streams");
483
494
u_var_add_gui_header_end(cam, NULL, NULL);
484
495
485
496
u_var_add_gui_header_begin(cam, NULL, "Exposure and gain control");
···
553
564
554
565
// Tidy the variable tracking.
555
566
u_var_remove_root(cam);
556
-
u_sink_debug_destroy(&cam->debug_sinks[0]);
557
-
u_sink_debug_destroy(&cam->debug_sinks[1]);
567
+
u_sink_debug_destroy(&cam->debug_sinks[WMR_DEBUG_SINK_SLAM]);
568
+
u_sink_debug_destroy(&cam->debug_sinks[WMR_DEBUG_SINK_CONTROLLER]);
558
569
559
570
free(cam);
560
571
}
···
589
600
for (int i = 0; i < NUM_XFERS; i++) {
590
601
uint8_t *recv_buf = malloc(cam->xfer_size);
591
602
592
-
libusb_fill_bulk_transfer(cam->xfers[i], cam->dev, 0x85, recv_buf, cam->xfer_size, img_xfer_cb, cam, 0);
603
+
libusb_fill_bulk_transfer(cam->xfers[i], cam->dev, LIBUSB_ENDPOINT_IN | 5, recv_buf, cam->xfer_size,
604
+
img_xfer_cb, cam, 0);
593
605
cam->xfers[i]->flags |= LIBUSB_TRANSFER_FREE_BUFFER;
594
606
595
607
res = libusb_submit_transfer(cam->xfers[i]);
···
690
702
{
691
703
DRV_TRACE_MARKER();
692
704
705
+
WMR_CAM_TRACE(cam, "Setting camera %d exposure %u gain %u", camera_id, exposure, gain);
693
706
struct wmr_camera_gain_cmd cmd = {
694
707
.magic = __cpu_to_le32(WMR_MAGIC),
695
708
.len = __cpu_to_le32(sizeof(struct wmr_camera_gain_cmd)),