matrixfurry.com
The open source OpenXR runtime
1/*
2 * Copyright 2021, Collabora, Ltd.
3 * Copyright 2022 Jan Schmidt
4 * SPDX-License-Identifier: BSL-1.0
5 *
6 */
7
8/*!
9 * @file
10 * @brief Oculus Rift S camera handling
11 *
12 * The Rift S camera module, handles reception and dispatch
13 * of camera frames.
14 *
15 * @author Jan Schmidt <jan@centricular.com>
16 * @ingroup drv_rift_s
17 */
18#include <string.h>
19
20#include "rift_s.h"
21#include "rift_s_camera.h"
22
23#include "os/os_threading.h"
24#include "xrt/xrt_byte_order.h"
25
26#include "xrt/xrt_defines.h"
27#include "xrt/xrt_frame.h"
28#include "xrt/xrt_frameserver.h"
29
30#include "util/u_autoexpgain.h"
31#include "util/u_debug.h"
32#include "util/u_var.h"
33#include "util/u_sink.h"
34#include "util/u_frame.h"
35#include "util/u_trace_marker.h"
36
37#define DEFAULT_EXPOSURE 6000
38#define DEFAULT_GAIN 127
39
40#define RIFT_S_MIN_EXPOSURE 38
41#define RIFT_S_MAX_EXPOSURE 14022
42
43#define RIFT_S_MIN_GAIN 16
44#define RIFT_S_MAX_GAIN 255
45
46//! Specifies whether the user wants to enable autoexposure from the start.
47DEBUG_GET_ONCE_BOOL_OPTION(rift_s_autoexposure, "RIFT_S_AUTOEXPOSURE", true)
48
49struct rift_s_camera
50{
51 struct os_mutex lock;
52
53 struct rift_s_tracker *tracker;
54
55 struct rift_s_camera_calibration_block *camera_calibration;
56
57 struct xrt_frame_sink in_sink; // Receive raw frames and split them
58
59 struct u_sink_debug debug_sinks[2];
60
61 rift_s_camera_report_t camera_report;
62
63 uint16_t last_slam_exposure, target_exposure;
64 uint8_t last_slam_gain, target_gain;
65
66 bool manual_control; //!< Whether to control exp/gain manually or with aeg
67 struct u_var_draggable_u16 exposure_ui; //! Widget to control `exposure` value
68 struct u_autoexpgain *aeg;
69};
70
71struct rift_s_camera_finder
72{
73 const char *hmd_serial_no;
74
75 struct xrt_fs *xfs;
76 struct xrt_frame_context *xfctx;
77};
78
79union rift_s_frame_data {
80 struct
81 {
82 uint8_t frame_type; // 0x06 or 0x86 (controller or SLAM exposure)
83 __le16 magic_abcd; // 0xabcd
84 __le16 frame_ctr; // Increments every exposure
85 __le32 const1; // QHWH
86 uint8_t pad1[7]; // all zeroes padding to 16 bytes
87 __le64 frame_ts; // microseconds
88 __le32 frame_ctr2; // Another frame counter, but only increments on alternate frames @ 30Hz
89 __le16 slam_exposure[5]; // One 16-bit per camera. Exposure duration?
90 uint8_t pad2[2]; // zero padding
91 uint8_t slam_gain[5]; // One byte per camera. 0x40 or 0xf0 depending on frame type
92 uint8_t pad3; // zero padding
93 __le16 unknown1; // changes every frame. No clear pattern
94 __le16 magic_face; // 0xface
95
96 } __attribute__((packed)) data;
97 uint8_t raw[50];
98};
99
100static void
101update_expgain(struct rift_s_camera *cam, struct xrt_frame *xf);
102
103static void
104receive_cam_frame(struct xrt_frame_sink *sink, struct xrt_frame *xf);
105
106static void
107on_video_device(struct xrt_prober *xp,
108 struct xrt_prober_device *pdev,
109 const char *product,
110 const char *manufacturer,
111 const char *serial,
112 void *ptr)
113{
114 struct rift_s_camera_finder *finder = (struct rift_s_camera_finder *)ptr;
115
116 /* Already found a device? */
117 if (finder->xfs != NULL)
118 return;
119
120 if (product == NULL || manufacturer == NULL || serial == NULL) {
121 return;
122 }
123
124 RIFT_S_TRACE("Inspecting video device %s - %s serial %s", manufacturer, product, serial);
125
126 if ((strcmp(product, "Rift S Sensor") == 0) && (strcmp(manufacturer, "Oculus VR") == 0)) {
127 // && (strcmp(serial, finder->hmd_serial_no) == 0)) {
128 // Serial no seems to be all zeros right now, so ignore it
129 xrt_prober_open_video_device(xp, pdev, finder->xfctx, &finder->xfs);
130 return;
131 }
132}
133
134struct rift_s_camera *
135rift_s_camera_create(struct xrt_prober *xp,
136 struct xrt_frame_context *xfctx,
137 const char *hmd_serial_no,
138 struct os_hid_device *hid,
139 struct rift_s_tracker *tracker,
140 struct rift_s_camera_calibration_block *camera_calibration)
141{
142 struct rift_s_camera_finder finder = {
143 0,
144 };
145
146 DRV_TRACE_MARKER();
147
148 /* Set up the finder with the HMD serial number and frame server context we want */
149 finder.xfctx = xfctx;
150 finder.hmd_serial_no = hmd_serial_no;
151
152 /* Re-probe devices. The v4l2 camera device should have appeared by now */
153 int retry_count = 5;
154 do {
155 xrt_result_t xret = xrt_prober_probe(xp);
156
157 if (xret != XRT_SUCCESS) {
158 return NULL;
159 }
160
161 xrt_prober_list_video_devices(xp, on_video_device, &finder);
162 if (finder.xfs != NULL) {
163 break;
164 }
165
166 /* Sleep 1 second before retry */
167 os_nanosleep((uint64_t)U_TIME_1S_IN_NS);
168 } while (retry_count-- > 0);
169
170 if (finder.xfs == NULL) {
171 RIFT_S_ERROR("Didn't find Rift S camera device");
172 return NULL;
173 }
174
175 struct rift_s_camera *cam = U_TYPED_CALLOC(struct rift_s_camera);
176
177 if (os_mutex_init(&cam->lock) != 0) {
178 RIFT_S_ERROR("Failed to init camera configuration mutex");
179 goto cleanup;
180 }
181
182 // Store the tracker
183 cam->tracker = tracker;
184 cam->camera_calibration = camera_calibration;
185
186 /* Configure default camera settings */
187 rift_s_protocol_camera_report_init(&cam->camera_report);
188 cam->camera_report.uvc_enable = 0x1;
189 cam->camera_report.radio_sync_flag = 0x1;
190
191 /* Store the defaults from the init() call into our current settings */
192 cam->last_slam_exposure = cam->camera_report.slam_frame_exposures[0];
193 cam->last_slam_gain = cam->camera_report.slam_frame_gains[0];
194
195 cam->target_exposure = DEFAULT_EXPOSURE;
196 cam->target_gain = DEFAULT_GAIN;
197
198 rift_s_camera_update(cam, hid);
199
200 cam->in_sink.push_frame = receive_cam_frame;
201
202 bool enable_aeg = debug_get_bool_option_rift_s_autoexposure();
203 int frame_delay = 2; // Exposure updates take effect on the 2nd frame after sending
204 cam->aeg = u_autoexpgain_create(U_AEG_STRATEGY_TRACKING, enable_aeg, frame_delay);
205
206 u_sink_debug_init(&cam->debug_sinks[0]);
207 u_sink_debug_init(&cam->debug_sinks[1]);
208
209 struct xrt_frame_sink *tmp = &cam->in_sink;
210
211 struct xrt_fs_mode *modes = NULL;
212 uint32_t count;
213
214 xrt_fs_enumerate_modes(finder.xfs, &modes, &count);
215
216 bool found_mode = false;
217 uint32_t selected_mode = 0;
218
219 for (; selected_mode < count; selected_mode++) {
220 if (modes[selected_mode].format == XRT_FORMAT_YUYV422) {
221 found_mode = true;
222 break;
223 }
224 if (modes[selected_mode].format == XRT_FORMAT_MJPEG) {
225 u_sink_create_format_converter(xfctx, XRT_FORMAT_L8, tmp, &tmp);
226 found_mode = true;
227 break;
228 }
229 }
230
231 if (!found_mode) {
232 selected_mode = 0;
233 RIFT_S_ERROR("Couldn't find compatible camera input format.");
234 goto cleanup;
235 }
236
237 free(modes);
238
239 u_var_add_root(cam, "Oculus Rift S Cameras", true);
240
241 u_var_add_bool(cam, &cam->manual_control, "Manual exposure and gain control");
242 cam->exposure_ui.val = &cam->target_exposure;
243 cam->exposure_ui.min = RIFT_S_MIN_EXPOSURE;
244 cam->exposure_ui.max = RIFT_S_MAX_EXPOSURE;
245 cam->exposure_ui.step = 25;
246
247 u_var_add_draggable_u16(cam, &cam->exposure_ui, "Exposure");
248 u_var_add_u8(cam, &cam->target_gain, "Gain");
249 u_var_add_gui_header(cam, NULL, "Auto exposure and gain control");
250 u_autoexpgain_add_vars(cam->aeg, cam, "");
251
252 u_var_add_gui_header(cam, NULL, "Camera Streams");
253 u_var_add_sink_debug(cam, &cam->debug_sinks[0], "Tracking Streams");
254 u_var_add_sink_debug(cam, &cam->debug_sinks[1], "Controller Streams");
255
256 /* Finally, start the video feed */
257 xrt_fs_stream_start(finder.xfs, tmp, XRT_FS_CAPTURE_TYPE_TRACKING, selected_mode);
258
259 return cam;
260
261cleanup:
262 rift_s_camera_destroy(cam);
263 return NULL;
264}
265
266void
267rift_s_camera_destroy(struct rift_s_camera *cam)
268{
269 u_var_remove_root(cam);
270 os_mutex_destroy(&cam->lock);
271 free(cam);
272}
273
274static bool
275parse_frame_data(const struct xrt_frame *xf, union rift_s_frame_data *row_data)
276{
277 /* Parse out the bits encoded as 8x8 blocks in the top rows */
278 unsigned int x, out_x;
279
280 if (xf->width != 50 * 8 * 8 || xf->height < 8)
281 return false;
282
283 uint8_t *pix = &xf->data[xf->width * 4];
284
285 int bit = 7;
286 for (x = 4, out_x = 0; x < xf->width; x += 8) {
287 uint8_t val = 0;
288 if (pix[x] > 128)
289 val = 1 << bit;
290
291 if (bit == 7) {
292 row_data->raw[out_x] = val;
293 } else {
294 row_data->raw[out_x] |= val;
295 }
296 if (bit > 0)
297 bit--;
298 else {
299 bit = 7;
300 out_x++;
301 }
302 }
303
304 /* Check magic numbers */
305 if (__le16_to_cpu(row_data->data.magic_abcd) != 0xabcd)
306 return false;
307 if (__le16_to_cpu(row_data->data.magic_face) != 0xface)
308 return false;
309
310 return true;
311}
312
313static int
314get_y_offset(struct rift_s_camera *cam, enum rift_s_camera_id cam_id, union rift_s_frame_data *row_data)
315{
316 /* There's a magic formula for computing the vertical offset of each camera view
317 * based on exposure, due to some internals of the headset. This formula extracted
318 * through trial and error */
319 int exposure = __le16_to_cpu(row_data->data.slam_exposure[cam_id]);
320 int y_offset = (exposure + 275) / 38;
321
322 if (y_offset > 375) {
323 y_offset = 375;
324 } else if (y_offset < 8) {
325 y_offset = 8;
326 }
327
328 return y_offset;
329}
330
331static struct xrt_frame *
332rift_s_camera_extract_frame(struct rift_s_camera *cam,
333 enum rift_s_camera_id cam_id,
334 struct xrt_frame *full_frame,
335 union rift_s_frame_data *row_data)
336{
337 struct rift_s_camera_calibration *calib = &cam->camera_calibration->cameras[cam_id];
338 struct xrt_rect roi = calib->roi;
339
340 roi.offset.h = get_y_offset(cam, cam_id, row_data);
341
342 struct xrt_frame *xf_crop = NULL;
343
344 u_frame_create_roi(full_frame, roi, &xf_crop);
345
346 return xf_crop;
347}
348
349static void
350receive_cam_frame(struct xrt_frame_sink *sink, struct xrt_frame *xf)
351{
352 struct rift_s_camera *cam = container_of(sink, struct rift_s_camera, in_sink);
353 bool release_xf = false;
354
355 RIFT_S_TRACE("cam img t=%" PRIu64 " source_t=%" PRIu64, xf->timestamp, xf->source_timestamp);
356
357 // If the format is YUYV422 we need to override it to L8 and double the width
358 // because the v4l2 device provides the wrong format description for the actual video
359 // data
360 if (xf->format == XRT_FORMAT_YUYV422) {
361 struct xrt_rect roi = {.offset = {0, 0}, .extent = {.w = xf->width, .h = xf->height}};
362 struct xrt_frame *xf_l8 = NULL;
363
364 u_frame_create_roi(xf, roi, &xf_l8);
365 xf_l8->width = 2 * xf->width;
366 xf_l8->format = XRT_FORMAT_L8;
367
368 xf = xf_l8;
369 release_xf = true;
370 }
371
372 // Dump mid-row of the 8 pix data line
373 union rift_s_frame_data row_data;
374
375 if (!parse_frame_data(xf, &row_data)) {
376 RIFT_S_TRACE("Invalid frame top-row data. Skipping");
377 return;
378 }
379
380 RIFT_S_DEBUG("frame ctr %u ts %" PRIu64
381 " µS pair ctr %u "
382 "exposure[0] %u gain[0] %u unk %u",
383 (uint16_t)__le16_to_cpu(row_data.data.frame_ctr), (uint64_t)__le64_to_cpu(row_data.data.frame_ts),
384 (uint32_t)__le32_to_cpu(row_data.data.frame_ctr2),
385 (uint16_t)__le16_to_cpu(row_data.data.slam_exposure[0]), row_data.data.slam_gain[0],
386 (uint16_t)__le16_to_cpu(row_data.data.unknown1));
387
388 // rift_s_hexdump_buffer("Row data", row_data.raw, sizeof(row_data.row));
389
390 // If the top left pixel is > 128, send as SLAM frame else controller
391 if (row_data.data.frame_type & 0x80) {
392 int y_offset = get_y_offset(cam, 0, &row_data);
393 struct xrt_rect roi = {.offset = {0, y_offset}, .extent = {.w = xf->width, .h = 480}};
394
395 struct xrt_frame *xf_crop = NULL;
396 u_frame_create_roi(xf, roi, &xf_crop);
397 u_sink_debug_push_frame(&cam->debug_sinks[0], xf_crop);
398 xrt_frame_reference(&xf_crop, NULL);
399
400 /* Extract camera frames and push to the tracker */
401 struct xrt_frame *frames[RIFT_S_CAMERA_COUNT] = {0};
402 for (int i = 0; i < RIFT_S_CAMERA_COUNT; i++) {
403 frames[i] = rift_s_camera_extract_frame(cam, CAM_IDX_TO_ID[i], xf, &row_data);
404 }
405
406 /* Update the exposure for all cameras based on the auto exposure for the left camera view */
407 //! @todo Update expgain independently for each camera like in WMR
408 update_expgain(cam, frames[0]);
409
410 uint64_t frame_ts_ns = (uint64_t)__le64_to_cpu(row_data.data.frame_ts) * OS_NS_PER_USEC;
411 rift_s_tracker_push_slam_frames(cam->tracker, frame_ts_ns, frames);
412
413 for (int i = 0; i < RIFT_S_CAMERA_COUNT; i++) {
414 xrt_frame_reference(&frames[i], NULL);
415 }
416 } else {
417 struct xrt_rect roi = {.offset = {0, 40}, .extent = {.w = xf->width, .h = 480}};
418 struct xrt_frame *xf_crop = NULL;
419
420 u_frame_create_roi(xf, roi, &xf_crop);
421 u_sink_debug_push_frame(&cam->debug_sinks[1], xf_crop);
422 xrt_frame_reference(&xf_crop, NULL);
423 }
424 if (release_xf)
425 xrt_frame_reference(&xf, NULL);
426}
427
428static void
429update_expgain(struct rift_s_camera *cam, struct xrt_frame *xf)
430{
431 if (!cam->manual_control && xf != NULL) {
432 u_autoexpgain_update(cam->aeg, xf);
433
434 uint16_t new_target_exposure;
435 uint8_t new_target_gain;
436
437 new_target_exposure =
438 CLAMP(u_autoexpgain_get_exposure(cam->aeg), RIFT_S_MIN_EXPOSURE, RIFT_S_MAX_EXPOSURE);
439 new_target_gain = CLAMP(u_autoexpgain_get_gain(cam->aeg), RIFT_S_MIN_GAIN, RIFT_S_MAX_GAIN);
440
441 if (cam->target_exposure != new_target_exposure || cam->target_gain != new_target_gain) {
442 RIFT_S_DEBUG("AEG exposure now %u (cur %u) gain %u (cur %u)", new_target_exposure,
443 cam->target_exposure, new_target_gain, cam->target_gain);
444
445 os_mutex_lock(&cam->lock);
446 cam->target_exposure = new_target_exposure;
447 cam->target_gain = new_target_gain;
448 os_mutex_unlock(&cam->lock);
449 }
450 }
451}
452
453/* Called from the Rift S system device USB loop, so we can check
454 * and send an exposure/gain change command if needed */
455void
456rift_s_camera_update(struct rift_s_camera *cam, struct os_hid_device *hid)
457{
458 bool need_update = false;
459 int i;
460
461 os_mutex_lock(&cam->lock);
462 if (cam->target_exposure != cam->last_slam_exposure) {
463 for (i = 0; i < 5; i++) {
464 cam->camera_report.slam_frame_exposures[i] = cam->target_exposure;
465 }
466 cam->last_slam_exposure = cam->target_exposure;
467 need_update = true;
468 }
469
470 if (cam->target_gain != cam->last_slam_gain) {
471 for (i = 0; i < 5; i++) {
472 cam->camera_report.slam_frame_gains[i] = cam->target_gain;
473 }
474 cam->last_slam_gain = cam->target_gain;
475 need_update = true;
476 }
477 os_mutex_unlock(&cam->lock);
478
479 if (need_update) {
480 RIFT_S_DEBUG("Updating AEG exposure to %u gain %u", cam->target_exposure, cam->target_gain);
481 if (rift_s_protocol_send_camera_report(hid, &cam->camera_report) < 0) {
482 RIFT_S_WARN("Failed to update camera settings");
483 }
484 }
485}