The open source OpenXR runtime
d/wmr: Shuffle and group things in camera file
+84
-62
+84
-62
src/xrt/drivers/wmr/wmr_camera.c
+84
-62
src/xrt/drivers/wmr/wmr_camera.c
···
22
22
#include "wmr_protocol.h"
23
23
#include "wmr_camera.h"
24
24
25
+
26
+
/*
27
+
*
28
+
* Defines and structs.
29
+
*
30
+
*/
31
+
25
32
#define WMR_CAM_TRACE(c, ...) U_LOG_IFL_T((c)->log_level, __VA_ARGS__)
26
33
#define WMR_CAM_DEBUG(c, ...) U_LOG_IFL_D((c)->log_level, __VA_ARGS__)
27
34
#define WMR_CAM_INFO(c, ...) U_LOG_IFL_I((c)->log_level, __VA_ARGS__)
···
79
86
80
87
enum u_logging_level log_level;
81
88
};
89
+
90
+
91
+
/*
92
+
*
93
+
* Helper functions.
94
+
*
95
+
*/
82
96
83
97
/* Some WMR headsets use 616538 byte transfers. HP G2 needs 1233018 (4 cameras)
84
98
* As a general formula, it seems we have:
···
214
228
return send_buffer_to_device(cam, (uint8_t *)&cmd, sizeof(cmd));
215
229
}
216
230
231
+
232
+
static void LIBUSB_CALL
233
+
img_xfer_cb(struct libusb_transfer *xfer)
234
+
{
235
+
struct wmr_camera *cam = xfer->user_data;
236
+
237
+
if (xfer->status != LIBUSB_TRANSFER_COMPLETED) {
238
+
WMR_CAM_TRACE(cam, "Camera transfer completed with status %u", xfer->status);
239
+
goto out;
240
+
}
241
+
242
+
if (xfer->actual_length < xfer->length) {
243
+
WMR_CAM_DEBUG(cam, "Camera transfer only delivered %d bytes", xfer->actual_length);
244
+
goto out;
245
+
}
246
+
247
+
WMR_CAM_TRACE(cam, "Camera transfer complete - %d bytes of %d", xfer->actual_length, xfer->length);
248
+
249
+
/* Convert the output into frames and send them off to debug / tracking */
250
+
struct xrt_frame *xf = NULL;
251
+
252
+
/* There's always one extra line of pixels with exposure info */
253
+
u_frame_create_one_off(XRT_FORMAT_L8, cam->frame_width, cam->frame_height + 1, &xf);
254
+
255
+
const uint8_t *src = xfer->buffer;
256
+
257
+
uint8_t *dst = xf->data;
258
+
size_t dst_remain = xf->size;
259
+
const size_t chunk_size = 0x6000 - 32;
260
+
261
+
while (dst_remain > 0) {
262
+
const size_t to_copy = dst_remain > chunk_size ? chunk_size : dst_remain;
263
+
264
+
/* TODO: See if there is useful info in the 32 byte packet headers.
265
+
* There seems to be a counter or timestamp there at least */
266
+
src += 0x20;
267
+
268
+
memcpy(dst, src, to_copy);
269
+
src += to_copy;
270
+
dst += to_copy;
271
+
dst_remain -= to_copy;
272
+
}
273
+
274
+
uint16_t exposure = xf->data[6] << 8 | xf->data[7];
275
+
uint8_t seq = xf->data[89];
276
+
277
+
WMR_CAM_TRACE(cam, "Camera frame seq %u (prev %u) - exposure %u", seq, cam->last_seq, exposure);
278
+
279
+
/* Exposure of 0 is a dark frame for controller tracking */
280
+
int sink_index = (exposure == 0) ? 1 : 0;
281
+
282
+
if (u_sink_debug_is_active(&cam->debug_sinks[sink_index])) {
283
+
u_sink_debug_push_frame(&cam->debug_sinks[sink_index], xf);
284
+
}
285
+
286
+
/* TODO: Push frame for tracking */
287
+
xrt_frame_reference(&xf, NULL);
288
+
289
+
cam->last_seq = seq;
290
+
out:
291
+
libusb_submit_transfer(xfer);
292
+
}
293
+
294
+
295
+
/*
296
+
*
297
+
* 'Exported' functions.
298
+
*
299
+
*/
300
+
217
301
struct wmr_camera *
218
302
wmr_camera_open(struct xrt_prober_device *dev_holo, enum u_logging_level ll)
219
303
{
···
302
386
}
303
387
304
388
free(cam);
305
-
}
306
-
307
-
static void LIBUSB_CALL
308
-
img_xfer_cb(struct libusb_transfer *xfer)
309
-
{
310
-
struct wmr_camera *cam = xfer->user_data;
311
-
312
-
if (xfer->status != LIBUSB_TRANSFER_COMPLETED) {
313
-
WMR_CAM_TRACE(cam, "Camera transfer completed with status %u", xfer->status);
314
-
goto out;
315
-
}
316
-
317
-
if (xfer->actual_length < xfer->length) {
318
-
WMR_CAM_DEBUG(cam, "Camera transfer only delivered %d bytes", xfer->actual_length);
319
-
goto out;
320
-
}
321
-
322
-
WMR_CAM_TRACE(cam, "Camera transfer complete - %d bytes of %d", xfer->actual_length, xfer->length);
323
-
324
-
/* Convert the output into frames and send them off to debug / tracking */
325
-
struct xrt_frame *xf = NULL;
326
-
327
-
/* There's always one extra line of pixels with exposure info */
328
-
u_frame_create_one_off(XRT_FORMAT_L8, cam->frame_width, cam->frame_height + 1, &xf);
329
-
330
-
const uint8_t *src = xfer->buffer;
331
-
332
-
uint8_t *dst = xf->data;
333
-
size_t dst_remain = xf->size;
334
-
const size_t chunk_size = 0x6000 - 32;
335
-
336
-
while (dst_remain > 0) {
337
-
const size_t to_copy = dst_remain > chunk_size ? chunk_size : dst_remain;
338
-
339
-
/* TODO: See if there is useful info in the 32 byte packet headers.
340
-
* There seems to be a counter or timestamp there at least */
341
-
src += 0x20;
342
-
343
-
memcpy(dst, src, to_copy);
344
-
src += to_copy;
345
-
dst += to_copy;
346
-
dst_remain -= to_copy;
347
-
}
348
-
349
-
uint16_t exposure = xf->data[6] << 8 | xf->data[7];
350
-
uint8_t seq = xf->data[89];
351
-
352
-
WMR_CAM_TRACE(cam, "Camera frame seq %u (prev %u) - exposure %u", seq, cam->last_seq, exposure);
353
-
354
-
/* Exposure of 0 is a dark frame for controller tracking */
355
-
int sink_index = (exposure == 0) ? 1 : 0;
356
-
357
-
if (u_sink_debug_is_active(&cam->debug_sinks[sink_index])) {
358
-
u_sink_debug_push_frame(&cam->debug_sinks[sink_index], xf);
359
-
}
360
-
361
-
/* TODO: Push frame for tracking */
362
-
xrt_frame_reference(&xf, NULL);
363
-
364
-
cam->last_seq = seq;
365
-
out:
366
-
libusb_submit_transfer(xfer);
367
389
}
368
390
369
391
bool