matrixfurry.com
The open source OpenXR runtime
1// Copyright 2016-2019, Philipp Zabel
2// Copyright 2020, Collabora, Ltd.
3// SPDX-License-Identifier: BSL-1.0
4/*!
5 * @file
6 * @brief Vive Lighthouse Watchman implementation
7 * @author Lubosz Sarnecki <lubosz.sarnecki@collabora.com>
8 * @ingroup drv_vive
9 */
10// IWYU pragma: no_include <asm/int-ll64.h>
11// IWYU pragma: no_include <linux/byteorder/little_endian.h>
12
13#include <stdint.h>
14#include <string.h>
15#include <zlib.h>
16#include <stdio.h>
17
18#include "math/m_api.h"
19
20#include "xrt/xrt_byte_order.h"
21
22#include "util/u_debug.h"
23#include "util/u_logging.h"
24
25#include "vive_lighthouse.h"
26
27static enum u_logging_level log_level;
28
29#define LH_TRACE(...) U_LOG_IFL_T(log_level, __VA_ARGS__)
30#define LH_DEBUG(...) U_LOG_IFL_D(log_level, __VA_ARGS__)
31#define LH_INFO(...) U_LOG_IFL_I(log_level, __VA_ARGS__)
32#define LH_WARN(...) U_LOG_IFL_W(log_level, __VA_ARGS__)
33#define LH_ERROR(...) U_LOG_IFL_E(log_level, __VA_ARGS__)
34
35DEBUG_GET_ONCE_LOG_OPTION(vive_log, "VIVE_LOG", U_LOGGING_WARN)
36
37struct lighthouse_ootx_report
38{
39 __le16 version;
40 __le32 serial;
41 __le16 phase[2];
42 __le16 tilt[2];
43 __u8 reset_count;
44 __u8 model_id;
45 __le16 curve[2];
46 __s8 gravity[3];
47 __le16 gibphase[2];
48 __le16 gibmag[2];
49} __attribute__((packed));
50
51static unsigned int watchman_id;
52
53float
54_f16_to_float(uint16_t f16)
55{
56 unsigned int sign = f16 >> 15;
57 unsigned int exponent = (f16 >> 10) & 0x1f;
58 unsigned int mantissa = f16 & 0x3ff;
59 union {
60 float f32;
61 uint32_t u32;
62 } u;
63
64 if (exponent == 0) {
65 if (!mantissa) {
66 /* zero */
67 u.u32 = sign << 31;
68 } else {
69 /* subnormal */
70 exponent = 127 - 14;
71 mantissa <<= 23 - 10;
72 /*
73 * convert to normal representation:
74 * shift up mantissa and drop MSB
75 */
76 while (!(mantissa & (1 << 23))) {
77 mantissa <<= 1;
78 exponent--;
79 }
80 mantissa &= 0x7fffffu;
81 u.u32 = (sign << 31) | (exponent << 23) | mantissa;
82 }
83 } else if (exponent < 31) {
84 /* normal */
85 exponent += 127 - 15;
86 mantissa <<= 23 - 10;
87 u.u32 = (sign << 31) | (exponent << 23) | mantissa;
88 } else if (mantissa == 0) {
89 /* infinite */
90 u.u32 = (sign << 31) | (255 << 23);
91 } else {
92 /* NaN */
93 u.u32 = 0x7fffffffu;
94 }
95 return u.f32;
96}
97
98static inline float
99__le16_to_float(__le16 le16)
100{
101 return _f16_to_float(__le16_to_cpu(le16));
102}
103
104static inline bool
105pulse_in_this_sync_window(int32_t dt, uint16_t duration)
106{
107 return dt > -duration && (dt + duration) < (6500 + 250);
108}
109
110static inline bool
111pulse_in_next_sync_window(int32_t dt, uint16_t duration)
112{
113 int32_t dt_end = dt + duration;
114
115 /*
116 * Allow 2000 pulses (40 µs) deviation from the expected interval
117 * between bases, and 1000 pulses (20 µs) for a single base.
118 */
119 return (dt > (20000 - 2000) && (dt_end) < (20000 + 6500 + 2000)) ||
120 (dt > (380000 - 2000) && (dt_end) < (380000 + 6500 + 2000)) ||
121 (dt > (400000 - 1000) && (dt_end) < (400000 + 6500 + 1000));
122}
123
124static inline bool
125pulse_in_sweep_window(int32_t dt, uint16_t duration)
126{
127 /*
128 * The J axis (horizontal) sweep starts 71111 ticks after the sync
129 * pulse start (32°) and ends at 346667 ticks (156°).
130 * The K axis (vertical) sweep starts at 55555 ticks (23°) and ends
131 * at 331111 ticks (149°).
132 */
133 return dt > (55555 - 1000) && (dt + duration) < (346667 + 1000);
134}
135
136static void
137_handle_ootx_frame(struct lighthouse_base *base)
138{
139 struct lighthouse_ootx_report *report = (void *)(base->ootx + 2);
140 uint16_t len = (__le16)*base->ootx;
141 uint32_t crc = crc32(0L, Z_NULL, 0);
142 bool serial_changed = false;
143 uint32_t ootx_crc;
144 uint16_t version;
145 int ootx_version;
146 struct xrt_vec3 gravity;
147 int i;
148
149 if (len != 33) {
150 LH_WARN("Lighthouse Base %X: unexpected OOTX payload length: %d", base->serial, len);
151 return;
152 }
153
154 ootx_crc = (__le32) * ((__le32 *)(base->ootx + 36)); /* (len+3)/4*4 */
155
156 crc = crc32(crc, base->ootx + 2, 33);
157 if (ootx_crc != crc) {
158 LH_ERROR("Lighthouse Base %X: CRC error: %08x != %08x", base->serial, crc, ootx_crc);
159 return;
160 }
161
162 version = __le16_to_cpu(report->version);
163 ootx_version = version & 0x3f;
164 if (ootx_version != 6) {
165 LH_ERROR("Lighthouse Base %X: unexpected OOTX frame version: %d", base->serial, ootx_version);
166 return;
167 }
168
169 base->firmware_version = version >> 6;
170
171 if (base->serial != __le32_to_cpu(report->serial)) {
172 base->serial = __le32_to_cpu(report->serial);
173 serial_changed = true;
174 }
175
176 for (i = 0; i < 2; i++) {
177 struct lighthouse_rotor_calibration *rotor;
178
179 rotor = &base->calibration.rotor[i];
180 rotor->tilt = __le16_to_float(report->tilt[i]);
181 rotor->phase = __le16_to_float(report->phase[i]);
182 rotor->curve = __le16_to_float(report->curve[i]);
183 rotor->gibphase = __le16_to_float(report->gibphase[i]);
184 rotor->gibmag = __le16_to_float(report->gibmag[i]);
185 }
186
187 base->model_id = report->model_id;
188
189 if (serial_changed) {
190 LH_INFO(
191 "Lighthouse Base %X: firmware version: %d, model id: %d, "
192 "channel: %c",
193 base->serial, base->firmware_version, base->model_id, base->channel);
194
195 for (i = 0; i < 2; i++) {
196 struct lighthouse_rotor_calibration *rotor;
197
198 rotor = &base->calibration.rotor[i];
199
200 LH_INFO(
201 "Lighthouse Base %X: rotor %d: [ %12.9f %12.9f "
202 "%12.9f %12.9f %12.9f ]",
203 base->serial, i, rotor->tilt, rotor->phase, rotor->curve, rotor->gibphase, rotor->gibmag);
204 }
205 }
206
207 gravity.x = report->gravity[0];
208 gravity.y = report->gravity[1];
209 gravity.z = report->gravity[2];
210 math_vec3_normalize(&gravity);
211 if (gravity.x != base->gravity.x || gravity.y != base->gravity.y || gravity.z != base->gravity.z) {
212 base->gravity = gravity;
213 LH_INFO("Lighthouse Base %X: gravity: [ %9.6f %9.6f %9.6f ]", base->serial, gravity.x, gravity.y,
214 gravity.z);
215 }
216
217 if (base->reset_count != report->reset_count) {
218 base->reset_count = report->reset_count;
219 LH_INFO("Lighthouse Base %X: reset count: %d", base->serial, base->reset_count);
220 }
221}
222
223static void
224lighthouse_base_reset(struct lighthouse_base *base)
225{
226 base->data_sync = 0;
227 base->data_word = -1;
228 base->data_bit = 0;
229 memset(base->ootx, 0, sizeof(base->ootx));
230}
231
232static void
233_handle_ootx_data_word(struct lighthouse_watchman *watchman, struct lighthouse_base *base)
234{
235 uint16_t len = (__le16)*base->ootx;
236
237 /* After 4 OOTX words we have received the base station serial number */
238 if (base->data_word == 4) {
239 struct lighthouse_ootx_report *report = (void *)(base->ootx + 2);
240 uint16_t ootx_version = __le16_to_cpu(report->version) & 0x3f;
241 uint32_t serial = __le32_to_cpu(report->serial);
242
243 if (len != 33) {
244 LH_WARN("%s: unexpected OOTX frame length %d", watchman->name, len);
245 return;
246 }
247
248 if (ootx_version == 6 && serial != base->serial) {
249 LH_DEBUG("%s: spotted Lighthouse Base %X", watchman->name, serial);
250 }
251 }
252 if (len == 33 && base->data_word == 20) { /* (len + 3)/4 * 2 + 2 */
253 _handle_ootx_frame(base);
254 }
255}
256
257static void
258lighthouse_base_handle_ootx_data_bit(struct lighthouse_watchman *watchman, struct lighthouse_base *base, bool data)
259{
260 if (base->data_word >= (int)sizeof(base->ootx) / 2) {
261 base->data_word = -1;
262 } else if (base->data_word >= 0) {
263 if (base->data_bit == 16) {
264 /* Sync bit */
265 base->data_bit = 0;
266 if (data) {
267 base->data_word++;
268 _handle_ootx_data_word(watchman, base);
269 } else {
270 LH_WARN("%s: Missed a sync bit, restarting", watchman->name);
271 /* Missing sync bit, restart */
272 base->data_word = -1;
273 }
274 } else if (base->data_bit < 16) {
275 /*
276 * Each 16-bit payload word contains two bytes,
277 * transmitted MSB-first.
278 */
279 if (data) {
280 int idx = 2 * base->data_word + (base->data_bit >> 3);
281
282 base->ootx[idx] |= 0x80 >> (base->data_bit % 8);
283 }
284 base->data_bit++;
285 }
286 }
287
288 /* Preamble detection */
289 if (data) {
290 if (base->data_sync > 16) {
291 /* Preamble detected, restart bit capture */
292 memset(base->ootx, 0, sizeof(base->ootx));
293 base->data_word = 0;
294 base->data_bit = 0;
295 }
296 base->data_sync = 0;
297 } else {
298 base->data_sync++;
299 }
300}
301
302static void
303lighthouse_base_handle_frame(struct lighthouse_watchman *watchman,
304 struct lighthouse_base *base,
305 uint32_t sync_timestamp)
306{
307 struct lighthouse_frame *frame = &base->frame[base->active_rotor];
308
309 (void)watchman;
310
311 if (!frame->sweep_ids)
312 return;
313
314 frame->frame_duration = sync_timestamp - frame->sync_timestamp;
315
316 /*
317 * If a single base station is running in 'B' mode, skipped frames
318 * will still contain old data.
319 */
320 if (frame->frame_duration > 1000000)
321 return;
322
323 // telemetry_send_lighthouse_frame(watchman->id, frame);
324}
325
326/*
327 * The pulse length encodes three bits. The skip bit indicates whether the
328 * emitting base will enable the sweeping laser in the next sweep window.
329 * The data bit is collected to eventually assemble the OOTX frame. The rotor
330 * bit indicates whether the next sweep will be horizontal (0) or vertical (1):
331 *
332 * duration 3000 3500 4000 4500 5000 5500 6000 6500 (in 48 MHz ticks)
333 * skip 0 0 0 0 1 1 1 1
334 * data 0 0 1 1 0 0 1 1
335 * rotor 0 1 0 1 0 1 0 1
336 */
337#define SKIP_BIT 4
338#define DATA_BIT 2
339#define ROTOR_BIT 1
340
341static void
342_handle_sync_pulse(struct lighthouse_watchman *watchman, struct lighthouse_pulse *sync)
343{
344 struct lighthouse_base *base;
345 unsigned char channel;
346 int32_t dt;
347 unsigned int code;
348
349 if (!sync->duration)
350 return;
351
352 if (sync->duration < 2750 || sync->duration > 6750) {
353 LH_WARN("%s: Unknown pulse length: %d", watchman->name, sync->duration);
354 return;
355 }
356 code = (sync->duration - 2750) / 500;
357
358 dt = sync->timestamp - watchman->last_timestamp;
359
360 /* 48 MHz / 120 Hz = 400000 cycles per sync pulse */
361 if (dt > (400000 - 1000) && dt < (400000 + 1000)) {
362 /* Observing a single base station, channel A (or B, actually)
363 */
364 channel = 'A';
365 } else if (dt > (380000 - 1000) && dt < (380000 + 1000)) {
366 /* Observing two base stations, this is channel B */
367 channel = 'B';
368 } else if (dt > (20000 - 1000) && dt < (20000 + 1000)) {
369 /* Observing two base stations, this is channel C */
370 channel = 'C';
371 } else {
372 if (dt > -1000 && dt < 1000) {
373 /*
374 * Ignore, this means we prematurely finished
375 * assembling the last sync pulse.
376 */
377 } else {
378 /* Irregular sync pulse */
379 if (watchman->last_timestamp)
380 LH_WARN(
381 "%s: Irregular sync pulse: %08x -> %08x "
382 "(%+d)",
383 watchman->name, watchman->last_timestamp, sync->timestamp, dt);
384 lighthouse_base_reset(&watchman->base[0]);
385 lighthouse_base_reset(&watchman->base[1]);
386 }
387
388 watchman->last_timestamp = sync->timestamp;
389 return;
390 }
391
392 base = &watchman->base[channel == 'C'];
393 base->channel = channel;
394 base->last_sync_timestamp = sync->timestamp;
395 lighthouse_base_handle_ootx_data_bit(watchman, base, (code & DATA_BIT));
396 lighthouse_base_handle_frame(watchman, base, sync->timestamp);
397
398 base->active_rotor = (code & ROTOR_BIT);
399 if (!(code & SKIP_BIT)) {
400 struct lighthouse_frame *frame = &base->frame[code & ROTOR_BIT];
401
402 watchman->active_base = base;
403 frame->sync_timestamp = sync->timestamp;
404 frame->sync_duration = sync->duration;
405 frame->sweep_ids = 0;
406 }
407
408 watchman->last_timestamp = sync->timestamp;
409}
410
411static void
412_handle_sweep_pulse(struct lighthouse_watchman *watchman, uint8_t id, uint32_t timestamp, uint16_t duration)
413{
414 struct lighthouse_base *base = watchman->active_base;
415 struct lighthouse_frame *frame;
416 int32_t offset;
417
418 (void)id;
419
420 if (!base) {
421 LH_WARN("%s: sweep without sync", watchman->name);
422 return;
423 }
424
425 frame = &base->frame[base->active_rotor];
426
427 offset = timestamp - base->last_sync_timestamp;
428
429 /* Ignore short sync pulses or sweeps without a corresponding sync */
430 if (offset > 379000)
431 return;
432
433 if (!pulse_in_sweep_window(offset, duration)) {
434 LH_WARN(
435 "%s: sweep offset out of range: rotor %u offset %u "
436 "duration %u",
437 watchman->name, base->active_rotor, offset, duration);
438 return;
439 }
440
441 if (frame->sweep_ids & (1 << id)) {
442 LH_WARN("%s: sensor %u triggered twice per frame, assuming reflection", watchman->name, id);
443 return;
444 }
445
446 frame->sweep_duration[id] = duration;
447 frame->sweep_offset[id] = offset;
448 frame->sweep_ids |= (1 << id);
449}
450
451static void
452accumulate_sync_pulse(struct lighthouse_watchman *watchman, uint8_t id, uint32_t timestamp, uint16_t duration)
453{
454 int32_t dt = timestamp - watchman->last_sync.timestamp;
455
456 if (dt > watchman->last_sync.duration || watchman->last_sync.duration == 0) {
457 watchman->seen_by = 1 << id;
458 watchman->last_sync.timestamp = timestamp;
459 watchman->last_sync.duration = duration;
460 watchman->last_sync.id = id;
461 } else {
462 watchman->seen_by |= 1 << id;
463 if (timestamp < watchman->last_sync.timestamp) {
464 watchman->last_sync.duration += watchman->last_sync.timestamp - timestamp;
465 watchman->last_sync.timestamp = timestamp;
466 }
467 if (duration > watchman->last_sync.duration)
468 watchman->last_sync.duration = duration;
469 watchman->last_sync.duration = duration;
470 }
471}
472
473void
474lighthouse_watchman_handle_pulse(struct lighthouse_watchman *watchman,
475 uint8_t id,
476 uint16_t duration,
477 uint32_t timestamp)
478{
479 int32_t dt;
480
481 dt = timestamp - watchman->last_sync.timestamp;
482
483 if (watchman->sync_lock) {
484 if (watchman->seen_by && dt > watchman->last_sync.duration) {
485 _handle_sync_pulse(watchman, &watchman->last_sync);
486 watchman->seen_by = 0;
487 }
488
489 if (pulse_in_this_sync_window(dt, duration) || pulse_in_next_sync_window(dt, duration)) {
490 accumulate_sync_pulse(watchman, id, timestamp, duration);
491 } else if (pulse_in_sweep_window(dt, duration)) {
492 _handle_sweep_pulse(watchman, id, timestamp, duration);
493 } else {
494 /*
495 * Spurious pulse - this could be due to a reflection or
496 * misdetected sync. If dt > period, drop the sync lock.
497 * Maybe we should ignore a single missed sync.
498 */
499 if (dt > 407500) {
500 watchman->sync_lock = false;
501 LH_WARN("%s: late pulse, lost sync", watchman->name);
502 } else {
503 LH_WARN("%s: spurious pulse: %08x (%02x %d %u)", watchman->name, timestamp, id, dt,
504 duration);
505 }
506 watchman->seen_by = 0;
507 }
508 } else {
509 /*
510 * If we've not locked onto the periodic sync signals, try to
511 * treat all pulses within the right duration range as potential
512 * sync pulses.
513 * This is still a bit naive. If the sensors are moved too
514 * close to the lighthouse base station, sweep pulse durations
515 * may fall into this range and sweeps may be misdetected as
516 * sync floods.
517 */
518 if (duration >= 2750 && duration <= 6750) {
519 /*
520 * Decide we've locked on if the pulse falls into any
521 * of the expected time windows from the last
522 * accumulated sync pulse.
523 */
524 if (pulse_in_next_sync_window(dt, duration)) {
525 LH_WARN("%s: sync locked", watchman->name);
526 watchman->sync_lock = true;
527 }
528
529 accumulate_sync_pulse(watchman, id, timestamp, duration);
530 } else {
531 /* Assume this is a sweep, ignore it until we lock */
532 }
533 }
534}
535
536void
537lighthouse_watchman_init(struct lighthouse_watchman *watchman, const char *name)
538{
539 watchman->id = watchman_id++;
540 watchman->name = name;
541 watchman->seen_by = 0;
542 watchman->last_timestamp = 0;
543 watchman->last_sync.timestamp = 0;
544 watchman->last_sync.duration = 0;
545 log_level = debug_get_log_option_vive_log();
546}