diff --git a/src/xrt/drivers/CMakeLists.txt b/src/xrt/drivers/CMakeLists.txt index 3601a98f9..ee3497dec 100644 --- a/src/xrt/drivers/CMakeLists.txt +++ b/src/xrt/drivers/CMakeLists.txt @@ -146,6 +146,8 @@ if(XRT_BUILD_DRIVER_VIVE) vive/vive_controller.c vive/vive_config.h vive/vive_config.c + vive/vive_lighthouse.h + vive/vive_lighthouse.c ) add_library(drv_vive STATIC ${VIVE_SOURCE_FILES}) diff --git a/src/xrt/drivers/meson.build b/src/xrt/drivers/meson.build index 5fd643bf4..644777888 100644 --- a/src/xrt/drivers/meson.build +++ b/src/xrt/drivers/meson.build @@ -129,7 +129,9 @@ lib_drv_vive = static_library( 'vive/vive_controller.c', 'vive/vive_controller.h', 'vive/vive_config.c', - 'vive/vive_config.h' + 'vive/vive_config.h', + 'vive/vive_lighthouse.c', + 'vive/vive_lighthouse.h', ), include_directories: [ xrt_include, diff --git a/src/xrt/drivers/vive/vive_device.c b/src/xrt/drivers/vive/vive_device.c index 3fbee3333..3d05c76d7 100644 --- a/src/xrt/drivers/vive/vive_device.c +++ b/src/xrt/drivers/vive/vive_device.c @@ -537,6 +537,9 @@ _decode_pulse_report(struct vive_device *d, const void *buffer) duration = __le16_to_cpu(pulse->duration); _print_v1_pulse(d, sensor_id, timestamp, duration); + + lighthouse_watchman_handle_pulse(&d->watchman, sensor_id, + duration, timestamp); } } @@ -809,6 +812,7 @@ vive_device_create(struct os_hid_device *mainboard_dev, if (ret < 0) { VIVE_ERROR(d, "Could not enable watchman receiver."); } else { + lighthouse_watchman_init(&d->watchman, "headset"); VIVE_DEBUG(d, "Successfully enabled watchman receiver."); } diff --git a/src/xrt/drivers/vive/vive_device.h b/src/xrt/drivers/vive/vive_device.h index ce0a64d17..cee95b170 100644 --- a/src/xrt/drivers/vive/vive_device.h +++ b/src/xrt/drivers/vive/vive_device.h @@ -14,6 +14,8 @@ #include "os/os_threading.h" #include "util/u_logging.h" +#include "vive_lighthouse.h" + #ifdef __cplusplus extern "C" { #endif @@ -59,6 +61,8 @@ struct vive_device struct os_hid_device *sensors_dev; struct os_hid_device *watchman_dev; + struct lighthouse_watchman watchman; + enum VIVE_VARIANT variant; struct os_thread_helper sensors_thread; diff --git a/src/xrt/drivers/vive/vive_lighthouse.c b/src/xrt/drivers/vive/vive_lighthouse.c new file mode 100644 index 000000000..0a00fdd19 --- /dev/null +++ b/src/xrt/drivers/vive/vive_lighthouse.c @@ -0,0 +1,577 @@ +// Copyright 2016-2019, Philipp Zabel +// Copyright 2020, Collabora, Ltd. +// SPDX-License-Identifier: BSL-1.0 +/*! + * @file + * @brief Vive Lighthouse Watchman implementation + * @author Lubosz Sarnecki + * @ingroup drv_vive + */ + +#include +#include +#include +#include +#include + +#include "math/m_api.h" +#include "util/u_debug.h" +#include "util/u_logging.h" + +#include "vive_lighthouse.h" + +static enum u_logging_level ll; + +#define LH_TRACE(...) U_LOG_IFL_T(ll, __VA_ARGS__) +#define LH_DEBUG(...) U_LOG_IFL_D(ll, __VA_ARGS__) +#define LH_INFO(...) U_LOG_IFL_I(ll, __VA_ARGS__) +#define LH_WARN(...) U_LOG_IFL_W(ll, __VA_ARGS__) +#define LH_ERROR(...) U_LOG_IFL_E(ll, __VA_ARGS__) + +DEBUG_GET_ONCE_LOG_OPTION(vive_log, "VIVE_LOG", U_LOGGING_WARN) + +struct lighthouse_ootx_report +{ + __le16 version; + __le32 serial; + __le16 phase[2]; + __le16 tilt[2]; + __u8 reset_count; + __u8 model_id; + __le16 curve[2]; + __s8 gravity[3]; + __le16 gibphase[2]; + __le16 gibmag[2]; +} __attribute__((packed)); + +static unsigned int watchman_id; + +float +_f16_to_float(uint16_t f16) +{ + unsigned int sign = f16 >> 15; + unsigned int exponent = (f16 >> 10) & 0x1f; + unsigned int mantissa = f16 & 0x3ff; + union { + float f32; + uint32_t u32; + } u; + + if (exponent == 0) { + if (!mantissa) { + /* zero */ + u.u32 = sign << 31; + } else { + /* subnormal */ + exponent = 127 - 14; + mantissa <<= 23 - 10; + /* + * convert to normal representation: + * shift up mantissa and drop MSB + */ + while (!(mantissa & (1 << 23))) { + mantissa <<= 1; + exponent--; + } + mantissa &= 0x7fffffu; + u.u32 = (sign << 31) | (exponent << 23) | mantissa; + } + } else if (exponent < 31) { + /* normal */ + exponent += 127 - 15; + mantissa <<= 23 - 10; + u.u32 = (sign << 31) | (exponent << 23) | mantissa; + } else if (mantissa == 0) { + /* infinite */ + u.u32 = (sign << 31) | (255 << 23); + } else { + /* NaN */ + u.u32 = 0x7fffffffu; + } + return u.f32; +} + +static inline float +__le16_to_float(__le16 le16) +{ + return _f16_to_float(__le16_to_cpu(le16)); +} + +static inline bool +pulse_in_this_sync_window(int32_t dt, uint16_t duration) +{ + return dt > -duration && (dt + duration) < (6500 + 250); +} + +static inline bool +pulse_in_next_sync_window(int32_t dt, uint16_t duration) +{ + int32_t dt_end = dt + duration; + + /* + * Allow 2000 pulses (40 µs) deviation from the expected interval + * between bases, and 1000 pulses (20 µs) for a single base. + */ + return (dt > (20000 - 2000) && (dt_end) < (20000 + 6500 + 2000)) || + (dt > (380000 - 2000) && (dt_end) < (380000 + 6500 + 2000)) || + (dt > (400000 - 1000) && (dt_end) < (400000 + 6500 + 1000)); +} + +static inline bool +pulse_in_sweep_window(int32_t dt, uint16_t duration) +{ + /* + * The J axis (horizontal) sweep starts 71111 ticks after the sync + * pulse start (32°) and ends at 346667 ticks (156°). + * The K axis (vertical) sweep starts at 55555 ticks (23°) and ends + * at 331111 ticks (149°). + */ + return dt > (55555 - 1000) && (dt + duration) < (346667 + 1000); +} + +static void +_handle_ootx_frame(struct lighthouse_base *base) +{ + struct lighthouse_ootx_report *report = (void *)(base->ootx + 2); + uint16_t len = (__le16)*base->ootx; + uint32_t crc = crc32(0L, Z_NULL, 0); + bool serial_changed = false; + uint32_t ootx_crc; + uint16_t version; + int ootx_version; + struct xrt_vec3 gravity; + int i; + + if (len != 33) { + LH_WARN( + "Lighthouse Base %X: unexpected OOTX payload length: %d", + base->serial, len); + return; + } + + ootx_crc = (__le32) * ((__le32 *)(base->ootx + 36)); /* (len+3)/4*4 */ + + crc = crc32(crc, base->ootx + 2, 33); + if (ootx_crc != crc) { + LH_ERROR("Lighthouse Base %X: CRC error: %08x != %08x", + base->serial, crc, ootx_crc); + return; + } + + version = __le16_to_cpu(report->version); + ootx_version = version & 0x3f; + if (ootx_version != 6) { + LH_ERROR( + "Lighthouse Base %X: unexpected OOTX frame version: %d", + base->serial, ootx_version); + return; + } + + base->firmware_version = version >> 6; + + if (base->serial != __le32_to_cpu(report->serial)) { + base->serial = __le32_to_cpu(report->serial); + serial_changed = true; + } + + for (i = 0; i < 2; i++) { + struct lighthouse_rotor_calibration *rotor; + + rotor = &base->calibration.rotor[i]; + rotor->tilt = __le16_to_float(report->tilt[i]); + rotor->phase = __le16_to_float(report->phase[i]); + rotor->curve = __le16_to_float(report->curve[i]); + rotor->gibphase = __le16_to_float(report->gibphase[i]); + rotor->gibmag = __le16_to_float(report->gibmag[i]); + } + + base->model_id = report->model_id; + + if (serial_changed) { + LH_INFO( + "Lighthouse Base %X: firmware version: %d, model id: %d, " + "channel: %c", + base->serial, base->firmware_version, base->model_id, + base->channel); + + for (i = 0; i < 2; i++) { + struct lighthouse_rotor_calibration *rotor; + + rotor = &base->calibration.rotor[i]; + + LH_INFO( + "Lighthouse Base %X: rotor %d: [ %12.9f %12.9f " + "%12.9f %12.9f %12.9f ]", + base->serial, i, rotor->tilt, rotor->phase, + rotor->curve, rotor->gibphase, rotor->gibmag); + } + } + + gravity.x = report->gravity[0]; + gravity.y = report->gravity[1]; + gravity.z = report->gravity[2]; + math_vec3_normalize(&gravity); + if (gravity.x != base->gravity.x || gravity.y != base->gravity.y || + gravity.z != base->gravity.z) { + base->gravity = gravity; + LH_INFO("Lighthouse Base %X: gravity: [ %9.6f %9.6f %9.6f ]", + base->serial, gravity.x, gravity.y, gravity.z); + } + + if (base->reset_count != report->reset_count) { + base->reset_count = report->reset_count; + LH_INFO("Lighthouse Base %X: reset count: %d", base->serial, + base->reset_count); + } +} + +static void +lighthouse_base_reset(struct lighthouse_base *base) +{ + base->data_sync = 0; + base->data_word = -1; + base->data_bit = 0; + memset(base->ootx, 0, sizeof(base->ootx)); +} + +static void +_handle_ootx_data_word(struct lighthouse_watchman *watchman, + struct lighthouse_base *base) +{ + uint16_t len = (__le16)*base->ootx; + + /* After 4 OOTX words we have received the base station serial number */ + if (base->data_word == 4) { + struct lighthouse_ootx_report *report = + (void *)(base->ootx + 2); + uint16_t ootx_version = __le16_to_cpu(report->version) & 0x3f; + uint32_t serial = __le32_to_cpu(report->serial); + + if (len != 33) { + LH_WARN("%s: unexpected OOTX frame length %d", + watchman->name, len); + return; + } + + if (ootx_version == 6 && serial != base->serial) { + LH_DEBUG("%s: spotted Lighthouse Base %X", + watchman->name, serial); + } + } + if (len == 33 && base->data_word == 20) { /* (len + 3)/4 * 2 + 2 */ + _handle_ootx_frame(base); + } +} + +static void +lighthouse_base_handle_ootx_data_bit(struct lighthouse_watchman *watchman, + struct lighthouse_base *base, + bool data) +{ + if (base->data_word >= (int)sizeof(base->ootx) / 2) { + base->data_word = -1; + } else if (base->data_word >= 0) { + if (base->data_bit == 16) { + /* Sync bit */ + base->data_bit = 0; + if (data) { + base->data_word++; + _handle_ootx_data_word(watchman, base); + } else { + LH_WARN("%s: Missed a sync bit, restarting", + watchman->name); + /* Missing sync bit, restart */ + base->data_word = -1; + } + } else if (base->data_bit < 16) { + /* + * Each 16-bit payload word contains two bytes, + * transmitted MSB-first. + */ + if (data) { + int idx = + 2 * base->data_word + (base->data_bit >> 3); + + base->ootx[idx] |= 0x80 >> (base->data_bit % 8); + } + base->data_bit++; + } + } + + /* Preamble detection */ + if (data) { + if (base->data_sync > 16) { + /* Preamble detected, restart bit capture */ + memset(base->ootx, 0, sizeof(base->ootx)); + base->data_word = 0; + base->data_bit = 0; + } + base->data_sync = 0; + } else { + base->data_sync++; + } +} + +static void +lighthouse_base_handle_frame(struct lighthouse_watchman *watchman, + struct lighthouse_base *base, + uint32_t sync_timestamp) +{ + struct lighthouse_frame *frame = &base->frame[base->active_rotor]; + + (void)watchman; + + if (!frame->sweep_ids) + return; + + frame->frame_duration = sync_timestamp - frame->sync_timestamp; + + /* + * If a single base station is running in 'B' mode, skipped frames + * will still contain old data. + */ + if (frame->frame_duration > 1000000) + return; + + // telemetry_send_lighthouse_frame(watchman->id, frame); +} + +/* + * The pulse length encodes three bits. The skip bit indicates whether the + * emitting base will enable the sweeping laser in the next sweep window. + * The data bit is collected to eventually assemble the OOTX frame. The rotor + * bit indicates whether the next sweep will be horizontal (0) or vertical (1): + * + * duration 3000 3500 4000 4500 5000 5500 6000 6500 (in 48 MHz ticks) + * skip 0 0 0 0 1 1 1 1 + * data 0 0 1 1 0 0 1 1 + * rotor 0 1 0 1 0 1 0 1 + */ +#define SKIP_BIT 4 +#define DATA_BIT 2 +#define ROTOR_BIT 1 + +static void +_handle_sync_pulse(struct lighthouse_watchman *watchman, + struct lighthouse_pulse *sync) +{ + struct lighthouse_base *base; + unsigned char channel; + int32_t dt; + unsigned int code; + + if (!sync->duration) + return; + + if (sync->duration < 2750 || sync->duration > 6750) { + LH_WARN("%s: Unknown pulse length: %d", watchman->name, + sync->duration); + return; + } + code = (sync->duration - 2750) / 500; + + dt = sync->timestamp - watchman->last_timestamp; + + /* 48 MHz / 120 Hz = 400000 cycles per sync pulse */ + if (dt > (400000 - 1000) && dt < (400000 + 1000)) { + /* Observing a single base station, channel A (or B, actually) + */ + channel = 'A'; + } else if (dt > (380000 - 1000) && dt < (380000 + 1000)) { + /* Observing two base stations, this is channel B */ + channel = 'B'; + } else if (dt > (20000 - 1000) && dt < (20000 + 1000)) { + /* Observing two base stations, this is channel C */ + channel = 'C'; + } else { + if (dt > -1000 && dt < 1000) { + /* + * Ignore, this means we prematurely finished + * assembling the last sync pulse. + */ + } else { + /* Irregular sync pulse */ + if (watchman->last_timestamp) + LH_WARN( + "%s: Irregular sync pulse: %08x -> %08x " + "(%+d)", + watchman->name, watchman->last_timestamp, + sync->timestamp, dt); + lighthouse_base_reset(&watchman->base[0]); + lighthouse_base_reset(&watchman->base[1]); + } + + watchman->last_timestamp = sync->timestamp; + return; + } + + base = &watchman->base[channel == 'C']; + base->channel = channel; + base->last_sync_timestamp = sync->timestamp; + lighthouse_base_handle_ootx_data_bit(watchman, base, (code & DATA_BIT)); + lighthouse_base_handle_frame(watchman, base, sync->timestamp); + + base->active_rotor = (code & ROTOR_BIT); + if (!(code & SKIP_BIT)) { + struct lighthouse_frame *frame = &base->frame[code & ROTOR_BIT]; + + watchman->active_base = base; + frame->sync_timestamp = sync->timestamp; + frame->sync_duration = sync->duration; + frame->sweep_ids = 0; + } + + watchman->last_timestamp = sync->timestamp; +} + +static void +_handle_sweep_pulse(struct lighthouse_watchman *watchman, + uint8_t id, + uint32_t timestamp, + uint16_t duration) +{ + struct lighthouse_base *base = watchman->active_base; + struct lighthouse_frame *frame; + int32_t offset; + + (void)id; + + if (!base) { + LH_WARN("%s: sweep without sync", watchman->name); + return; + } + + frame = &base->frame[base->active_rotor]; + + offset = timestamp - base->last_sync_timestamp; + + /* Ignore short sync pulses or sweeps without a corresponding sync */ + if (offset > 379000) + return; + + if (!pulse_in_sweep_window(offset, duration)) { + LH_WARN( + "%s: sweep offset out of range: rotor %u offset %u " + "duration %u", + watchman->name, base->active_rotor, offset, duration); + return; + } + + if (frame->sweep_ids & (1 << id)) { + LH_WARN( + "%s: sensor %u hit twice per frame, assuming reflection", + watchman->name, id); + return; + } + + frame->sweep_duration[id] = duration; + frame->sweep_offset[id] = offset; + frame->sweep_ids |= (1 << id); +} + +static void +accumulate_sync_pulse(struct lighthouse_watchman *watchman, + uint8_t id, + uint32_t timestamp, + uint16_t duration) +{ + int32_t dt = timestamp - watchman->last_sync.timestamp; + + if (dt > watchman->last_sync.duration || + watchman->last_sync.duration == 0) { + watchman->seen_by = 1 << id; + watchman->last_sync.timestamp = timestamp; + watchman->last_sync.duration = duration; + watchman->last_sync.id = id; + } else { + watchman->seen_by |= 1 << id; + if (timestamp < watchman->last_sync.timestamp) { + watchman->last_sync.duration += + watchman->last_sync.timestamp - timestamp; + watchman->last_sync.timestamp = timestamp; + } + if (duration > watchman->last_sync.duration) + watchman->last_sync.duration = duration; + watchman->last_sync.duration = duration; + } +} + +void +lighthouse_watchman_handle_pulse(struct lighthouse_watchman *watchman, + uint8_t id, + uint16_t duration, + uint32_t timestamp) +{ + int32_t dt; + + dt = timestamp - watchman->last_sync.timestamp; + + if (watchman->sync_lock) { + if (watchman->seen_by && dt > watchman->last_sync.duration) { + _handle_sync_pulse(watchman, &watchman->last_sync); + watchman->seen_by = 0; + } + + if (pulse_in_this_sync_window(dt, duration) || + pulse_in_next_sync_window(dt, duration)) { + accumulate_sync_pulse(watchman, id, timestamp, + duration); + } else if (pulse_in_sweep_window(dt, duration)) { + _handle_sweep_pulse(watchman, id, timestamp, duration); + } else { + /* + * Spurious pulse - this could be due to a reflection or + * misdetected sync. If dt > period, drop the sync lock. + * Maybe we should ignore a single missed sync. + */ + if (dt > 407500) { + watchman->sync_lock = false; + LH_WARN("%s: late pulse, lost sync", + watchman->name); + } else { + LH_WARN("%s: spurious pulse: %08x (%02x %d %u)", + watchman->name, timestamp, id, dt, + duration); + } + watchman->seen_by = 0; + } + } else { + /* + * If we've not locked onto the periodic sync signals, try to + * treat all pulses within the right duration range as potential + * sync pulses. + * This is still a bit naive. If the sensors are moved too + * close to the lighthouse base station, sweep pulse durations + * may fall into this range and sweeps may be misdetected as + * sync floods. + */ + if (duration >= 2750 && duration <= 6750) { + /* + * Decide we've locked on if the pulse falls into any + * of the expected time windows from the last + * accumulated sync pulse. + */ + if (pulse_in_next_sync_window(dt, duration)) { + LH_WARN("%s: sync locked", watchman->name); + watchman->sync_lock = true; + } + + accumulate_sync_pulse(watchman, id, timestamp, + duration); + } else { + /* Assume this is a sweep, ignore it until we lock */ + } + } +} + +void +lighthouse_watchman_init(struct lighthouse_watchman *watchman, const char *name) +{ + watchman->id = watchman_id++; + watchman->name = name; + watchman->seen_by = 0; + watchman->last_timestamp = 0; + watchman->last_sync.timestamp = 0; + watchman->last_sync.duration = 0; + ll = debug_get_log_option_vive_log(); +} diff --git a/src/xrt/drivers/vive/vive_lighthouse.h b/src/xrt/drivers/vive/vive_lighthouse.h new file mode 100644 index 000000000..224e13e8d --- /dev/null +++ b/src/xrt/drivers/vive/vive_lighthouse.h @@ -0,0 +1,110 @@ +// Copyright 2016-2019, Philipp Zabel +// Copyright 2020, Collabora, Ltd. +// SPDX-License-Identifier: BSL-1.0 +/*! + * @file + * @brief Vive Lighthouse Watchman implementation + * @author Lubosz Sarnecki + * @ingroup drv_vive + */ + +#pragma once + +#include +#include +#include +#include +#include +#include + +#include "xrt/xrt_defines.h" + +struct lighthouse_rotor_calibration +{ + float tilt; + float phase; + float curve; + float gibphase; + float gibmag; +}; + +struct lighthouse_base_calibration +{ + struct lighthouse_rotor_calibration rotor[2]; +}; + +struct lighthouse_frame +{ + uint32_t sync_timestamp; + uint32_t sync_duration; + uint32_t sync_ids; + uint32_t sweep_ids; + uint32_t sweep_offset[32]; + uint16_t sweep_duration[32]; + uint32_t frame_duration; +}; + +struct lighthouse_base +{ + int data_sync; + int data_word; + int data_bit; + uint8_t ootx[40]; + + int firmware_version; + uint32_t serial; + struct lighthouse_base_calibration calibration; + struct xrt_vec3 gravity; + char channel; + int model_id; + int reset_count; + + uint32_t last_sync_timestamp; + int active_rotor; + + struct lighthouse_frame frame[2]; +}; + +struct lighthouse_pulse +{ + uint32_t timestamp; + uint16_t duration; + uint8_t id; +}; + +struct lighthouse_sensor +{ + struct lighthouse_pulse sync; + struct lighthouse_pulse sweep; +}; + +struct tracking_model +{ + uint32_t num_points; + struct xrt_vec3 *points; + struct xrt_vec3 *normals; +}; + +struct lighthouse_watchman +{ + uint32_t id; + const char *name; + struct tracking_model model; + bool base_visible; + struct lighthouse_base base[2]; + struct lighthouse_base *active_base; + uint32_t seen_by; + uint32_t last_timestamp; + struct lighthouse_sensor sensor[32]; + struct lighthouse_pulse last_sync; + bool sync_lock; +}; + +void +lighthouse_watchman_handle_pulse(struct lighthouse_watchman *watchman, + uint8_t id, + uint16_t duration, + uint32_t timestamp); +void +lighthouse_watchman_init(struct lighthouse_watchman *watchman, + const char *name);