The open source OpenXR runtime
d/wmr: Report frame source_sequence and source_timestamp
Extract timestamp in source 100ns units and convert to
nanoseconds, then place them in the emitted xrt_frames.
Unwrap the 8-bit frame sequence counter to 64-bits, and
add that too.
Add some comments and debug for other fields in the
incoming data.
+48
-4
+48
-4
src/xrt/drivers/wmr/wmr_camera.c
+48
-4
src/xrt/drivers/wmr/wmr_camera.c
···
13
13
#include <asm/byteorder.h>
14
14
#include <libusb.h>
15
15
#include <stdlib.h>
16
+
#include <assert.h>
17
+
#include <inttypes.h>
16
18
17
19
#include "os/os_threading.h"
18
20
#include "util/u_var.h"
···
79
81
size_t xfer_size;
80
82
uint32_t frame_width, frame_height;
81
83
uint8_t last_seq;
84
+
uint64_t last_frame_ts;
85
+
86
+
/* Unwrapped frame sequence number */
87
+
uint64_t frame_sequence;
82
88
83
89
struct libusb_transfer *xfers[NUM_XFERS];
84
90
···
267
273
while (dst_remain > 0) {
268
274
const size_t to_copy = dst_remain > chunk_size ? chunk_size : dst_remain;
269
275
270
-
/* TODO: See if there is useful info in the 32 byte packet headers.
271
-
* There seems to be a counter or timestamp there at least */
276
+
/* 32 byte header seems to contain:
277
+
* __be32 magic = "Dlo+"
278
+
* __le32 frame_ctr;
279
+
* __le32 slice_ctr;
280
+
* __u8 unknown[20]; - binary block where all bytes are different each slice,
281
+
* but repeat every 8 slices. They're different each boot
282
+
* of the headset. Might just be uninitialised memory?
283
+
*/
272
284
src += 0x20;
273
285
274
286
memcpy(dst, src, to_copy);
···
277
289
dst_remain -= to_copy;
278
290
}
279
291
292
+
/* There should be exactly a 26 byte footer left over */
293
+
assert(xfer->buffer + xfer->length - src == 26);
294
+
295
+
/* Footer contains:
296
+
* __le64 start_ts; - 100ns unit timestamp, from same clock as video_timestamps on the IMU feed
297
+
* __le64 end_ts; - 100ns unit timestamp, always about 111000 * 100ns later than start_ts ~= 90Hz
298
+
* __le16 ctr1; - Counter that increments by 88, but sometimes by 96, and wraps at 16384
299
+
* __le16 unknown0 - Unknown value, has only ever been 0
300
+
* __be32 magic - "Dlo+"
301
+
* __le16 frametype?- either 0x00 or 0x02. Every 3rd frame is 0x0, others are 0x2. Might be SLAM vs controllers?
302
+
*/
303
+
uint64_t frame_start_ts = read64(&src) * WMR_MS_HOLOLENS_NS_PER_TICK;
304
+
uint64_t frame_end_ts = read64(&src) * WMR_MS_HOLOLENS_NS_PER_TICK;
305
+
int64_t delta = frame_end_ts - frame_start_ts;
306
+
307
+
uint16_t unknown16 = read16(&src);
308
+
uint16_t unknown16_2 = read16(&src);
309
+
310
+
WMR_CAM_DEBUG(
311
+
cam, "Frame start TS %" PRIu64 " (%" PRIi64 " since last) end %" PRIu64 " dt %" PRIi64 " unknown %u %u",
312
+
frame_start_ts, frame_start_ts - cam->last_frame_ts, frame_end_ts, delta, unknown16, unknown16_2);
313
+
280
314
uint16_t exposure = xf->data[6] << 8 | xf->data[7];
281
315
uint8_t seq = xf->data[89];
316
+
uint8_t seq_delta = seq - cam->last_seq;
282
317
283
-
WMR_CAM_TRACE(cam, "Camera frame seq %u (prev %u) - exposure %u", seq, cam->last_seq, exposure);
318
+
/* Extend the sequence number to 64-bits */
319
+
cam->frame_sequence += seq_delta;
320
+
321
+
WMR_CAM_TRACE(cam, "Camera frame seq %u (prev %u) -> frame %" PRIu64 " - exposure %u", seq, cam->last_seq,
322
+
cam->frame_sequence, exposure);
323
+
324
+
xf->source_sequence = cam->frame_sequence;
325
+
xf->timestamp = xf->source_timestamp = frame_start_ts;
326
+
327
+
cam->last_frame_ts = frame_start_ts;
328
+
cam->last_seq = seq;
284
329
285
330
/* Exposure of 0 is a dark frame for controller tracking */
286
331
int sink_index = (exposure == 0) ? 1 : 0;
···
292
337
/* TODO: Push frame for tracking */
293
338
xrt_frame_reference(&xf, NULL);
294
339
295
-
cam->last_seq = seq;
296
340
if (cam->last_gain != cam->debug_gain) {
297
341
int i;
298
342