diff --git a/CMakeLists.txt b/CMakeLists.txt index c2af2cca3..4b8f94a59 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -198,10 +198,10 @@ cmake_dependent_option(XRT_BUILD_DRIVER_HANDTRACKING "Enable Camera Hand Trackin cmake_dependent_option(XRT_BUILD_DRIVER_DAYDREAM "Enable the Google Daydream View controller driver (BLE, via D-Bus)" ON "XRT_HAVE_DBUS" OFF) cmake_dependent_option(XRT_BUILD_DRIVER_ARDUINO "Enable Arduino input device with BLE via via D-Bus" ON "XRT_HAVE_DBUS" OFF) cmake_dependent_option(XRT_BUILD_DRIVER_ILLIXR "Enable ILLIXR driver" ON "ILLIXR_PATH" OFF) - option(XRT_BUILD_DRIVER_DUMMY "Enable dummy driver" ON) cmake_dependent_option(XRT_BUILD_DRIVER_ULV2 "Enable Ultraleap v2 driver" ON "Leap" OFF) cmake_dependent_option(XRT_BUILD_DRIVER_REMOTE "Enable remote debugging driver" ON "XRT_HAVE_LINUX OR ANDROID" OFF) +option(XRT_BUILD_DRIVER_WMR "Enable Windows Mixed Reality driver" ON) # These all use the Monado internal hid wrapper. cmake_dependent_option(XRT_BUILD_DRIVER_HDK "Enable HDK driver" ON "XRT_HAVE_INTERNAL_HID" OFF) @@ -239,6 +239,7 @@ list(APPEND AVAILABLE_DRIVERS "VF" "VIVE" "QWERTY" + "WMR" ) @@ -378,6 +379,7 @@ message(STATUS "# DRIVER_SURVIVE: ${XRT_BUILD_DRIVER_SURVIVE}") message(STATUS "# DRIVER_VF: ${XRT_BUILD_DRIVER_VF}") message(STATUS "# DRIVER_VIVE: ${XRT_BUILD_DRIVER_VIVE}") message(STATUS "# DRIVER_QWERTY: ${XRT_BUILD_DRIVER_QWERTY}") +message(STATUS "# DRIVER_WMR: ${XRT_BUILD_DRIVER_WMR}") message(STATUS "#####----- Config -----#####") if(XRT_FEATURE_SERVICE AND NOT XRT_FEATURE_OPENXR) diff --git a/doc/changes/big/mr.774.md b/doc/changes/big/mr.774.md new file mode 100644 index 000000000..3a2e186b9 --- /dev/null +++ b/doc/changes/big/mr.774.md @@ -0,0 +1,2 @@ +New WinMR driver, the initial commit only adds simple 3DoF support and not +distortion support. diff --git a/doc/changes/drivers/mr.774.md b/doc/changes/drivers/mr.774.md new file mode 100644 index 000000000..adcf95da0 --- /dev/null +++ b/doc/changes/drivers/mr.774.md @@ -0,0 +1 @@ +wmr: Initial commit of driver, 3DoF only. diff --git a/meson.build b/meson.build index 507e9c0a9..28983ffe9 100644 --- a/meson.build +++ b/meson.build @@ -161,7 +161,7 @@ if 'v4l2' in drivers endif if 'auto' in drivers - drivers += ['dummy', 'hdk', 'hydra', 'ns', 'psmv', 'remote'] + drivers += ['dummy', 'hdk', 'hydra', 'ns', 'psmv', 'remote', 'wmr'] endif openhmd = dependency('openhmd', required: openhmd_required) diff --git a/meson_options.txt b/meson_options.txt index 69ada8163..5953c21f1 100644 --- a/meson_options.txt +++ b/meson_options.txt @@ -3,7 +3,7 @@ option('drivers', type: 'array', - choices: ['auto', 'dummy', 'hdk', 'hydra', 'ns', 'ohmd', 'psmv', 'psvr', 'rs', 'v4l2', 'vf', 'vive', 'survive', 'daydream', 'arduino', 'remote', 'handtracking', 'qwerty', 'ulv2'], + choices: ['auto', 'dummy', 'hdk', 'hydra', 'ns', 'ohmd', 'psmv', 'psvr', 'rs', 'v4l2', 'vf', 'vive', 'wmr', 'survive', 'daydream', 'arduino', 'remote', 'handtracking', 'qwerty', 'ulv2'], value: ['auto'], description: 'Set of drivers to build') diff --git a/src/xrt/drivers/CMakeLists.txt b/src/xrt/drivers/CMakeLists.txt index 5c686bf03..c3b28a7ba 100644 --- a/src/xrt/drivers/CMakeLists.txt +++ b/src/xrt/drivers/CMakeLists.txt @@ -288,6 +288,22 @@ add_library(drv_multi STATIC ${MUlTI_SOURCE_FILES}) target_link_libraries(drv_multi PUBLIC xrt-interfaces aux_util) list(APPEND ENABLED_HEADSET_DRIVERS drv_multi) +if(XRT_BUILD_DRIVER_WMR) + set(WMR_SOURCE_FILES + wmr/wmr_common.h + wmr/wmr_hmd.c + wmr/wmr_hmd.h + wmr/wmr_interface.h + wmr/wmr_prober.c + wmr/wmr_protocol.c + wmr/wmr_protocol.h + ) + + add_library(drv_wmr STATIC ${WMR_SOURCE_FILES}) + target_link_libraries(drv_wmr PRIVATE xrt-interfaces aux_util) + list(APPEND ENABLED_HEADSET_DRIVERS wmr) +endif() + if(ENABLED_HEADSET_DRIVERS) set(ENABLED_DRIVERS ${ENABLED_HEADSET_DRIVERS} ${ENABLED_DRIVERS}) list(SORT ENABLED_DRIVERS) diff --git a/src/xrt/drivers/meson.build b/src/xrt/drivers/meson.build index fba34c8ef..6f13eb7c2 100644 --- a/src/xrt/drivers/meson.build +++ b/src/xrt/drivers/meson.build @@ -268,3 +268,19 @@ lib_drv_qwerty = static_library( ) drv_qwerty_include = include_directories('qwerty') + +lib_drv_wmr = static_library( + 'drv_wmr', + files( + 'wmr/wmr_common.h', + 'wmr/wmr_hmd.c', + 'wmr/wmr_hmd.h', + 'wmr/wmr_interface.h', + 'wmr/wmr_prober.c', + 'wmr/wmr_protocol.c', + 'wmr/wmr_protocol.h', + ), + include_directories: xrt_include, + dependencies: [aux], + build_by_default: 'wmr' in drivers, +) diff --git a/src/xrt/drivers/wmr/wmr_common.h b/src/xrt/drivers/wmr/wmr_common.h new file mode 100644 index 000000000..383445f8d --- /dev/null +++ b/src/xrt/drivers/wmr/wmr_common.h @@ -0,0 +1,44 @@ +// Copyright 2020-2021, N Madsen. +// Copyright 2020-2021, Collabora, Ltd. +// SPDX-License-Identifier: BSL-1.0 +/*! + * @file + * @brief Defines and constants related to WMR driver code. + * @author nima01 + * @author Jakob Bornecrantz + * @ingroup drv_wmr + */ + +#pragma once + + +#ifdef __cplusplus +extern "C" { +#endif + + +/*! + * Defines for the WMR driver. + * + * @ingroup drv_wmr + * @{ + */ + +#define MS_HOLOLENS_MANUFACTURER_STRING "Microsoft" +#define MS_HOLOLENS_PRODUCT_STRING "HoloLens Sensors" + +#define MICROSOFT_VID 0x045e +#define HOLOLENS_SENSORS_PID 0x0659 + +#define HP_VID 0x03f0 +#define REVERB_G1_PID 0x0c6a +#define REVERB_G2_PID 0x0580 + +/*! + * @} + */ + + +#ifdef __cplusplus +} +#endif diff --git a/src/xrt/drivers/wmr/wmr_hmd.c b/src/xrt/drivers/wmr/wmr_hmd.c new file mode 100644 index 000000000..e14a42629 --- /dev/null +++ b/src/xrt/drivers/wmr/wmr_hmd.c @@ -0,0 +1,537 @@ +// Copyright 2018, Philipp Zabel. +// Copyright 2020-2021, N Madsen. +// Copyright 2020-2021, Collabora, Ltd. +// SPDX-License-Identifier: BSL-1.0 +/*! + * @file + * @brief Driver code for a WMR HMD. + * @author Philipp Zabel + * @author nima01 + * @author Jakob Bornecrantz + * @ingroup drv_wmr + */ + +#include "xrt/xrt_config_os.h" +#include "xrt/xrt_device.h" + +#include "os/os_time.h" +#include "os/os_hid.h" + +#include "math/m_mathinclude.h" +#include "math/m_api.h" +#include "math/m_vec2.h" + +#include "util/u_var.h" +#include "util/u_misc.h" +#include "util/u_time.h" +#include "util/u_debug.h" +#include "util/u_device.h" +#include "util/u_distortion_mesh.h" + +#include "wmr_hmd.h" +#include "wmr_common.h" +#include "wmr_protocol.h" + +#include +#include +#include +#include +#ifndef XRT_OS_WINDOWS +#include // for sleep() +#endif + + +/* + * + * Hololens packets. + * + */ + +static void +hololens_unknown_17_decode_packet(struct wmr_hmd *wh, const unsigned char *buffer, int size) +{ + if (size >= 7) { + WMR_TRACE(wh, "Got packet 0x17 (%i)\n\t%02x %02x %02x %02x %02x %02x %02x ", size, buffer[0], buffer[1], + buffer[2], buffer[3], buffer[4], buffer[5], buffer[6]); + } else { + WMR_TRACE(wh, "Got packet 0x17 (%i)", size); + } +} + +static void +hololens_unknown_05_06_0E_decode_packet(struct wmr_hmd *wh, const unsigned char *buffer, int size) +{ + if (size >= 45) { + WMR_TRACE(wh, + "Got controller (%i)\n\t%02x %02x %02x %02x %02x %02x %02x %02x %02x %02x | %02x %02x %02x " + "%02x %02x %02x %02x %02x %02x %02x | %02x %02x %02x %02x %02x %02x %02x %02x %02x %02x", + size, buffer[0], buffer[1], buffer[2], buffer[3], buffer[4], buffer[5], buffer[6], buffer[7], + buffer[8], buffer[9], buffer[10], buffer[11], buffer[12], buffer[13], buffer[14], buffer[15], + buffer[16], buffer[17], buffer[18], buffer[19], buffer[20], buffer[21], buffer[22], + buffer[23], buffer[24], buffer[25], buffer[26], buffer[27], buffer[28], buffer[29]); + } else { + WMR_TRACE(wh, "Got controller packet (%i)\n\t%02x", size, buffer[0]); + } +} + +static void +hololens_sensors_decode_packet(struct wmr_hmd *wh, + struct hololens_sensors_packet *pkt, + const unsigned char *buffer, + int size) +{ + WMR_TRACE(wh, ""); + + if (size != 497 && size != 381) { + WMR_ERROR(wh, "invalid hololens sensor packet size (expected 381 or 497 but got %d)", size); + return; + } + + pkt->id = read8(&buffer); + for (int i = 0; i < 4; i++) { + pkt->temperature[i] = read16(&buffer); + } + + for (int i = 0; i < 4; i++) { + pkt->gyro_timestamp[i] = read64(&buffer); + } + + for (int i = 0; i < 3; i++) { + for (int j = 0; j < 32; j++) { + pkt->gyro[i][j] = read16(&buffer); + } + } + + for (int i = 0; i < 4; i++) { + pkt->accel_timestamp[i] = read64(&buffer); + } + + for (int i = 0; i < 3; i++) { + for (int j = 0; j < 4; j++) { + pkt->accel[i][j] = read32(&buffer); + } + } + + for (int i = 0; i < 4; i++) { + pkt->video_timestamp[i] = read64(&buffer); + } + + return; +} + +static bool +hololens_sensors_read_packets(struct wmr_hmd *wh) +{ + WMR_TRACE(wh, ""); + + unsigned char buffer[WMR_FEATURE_BUFFER_SIZE]; + + // Block for 100ms + int size = os_hid_read(wh->hid_hololens_senors_dev, buffer, sizeof(buffer), 100); + + if (size < 0) { + WMR_ERROR(wh, "Error reading from device"); + return false; + } else if (size == 0) { + WMR_TRACE(wh, "No more data to read"); + return true; // No more messages, return. + } else { + WMR_TRACE(wh, "Read %u bytes", size); + } + + switch (buffer[0]) { + case WMR_MS_HOLOLENS_MSG_SENSORS: + hololens_sensors_decode_packet(wh, &wh->packet, buffer, size); + + for (int i = 0; i < 4; i++) { + vec3_from_hololens_gyro(wh->packet.gyro, i, &wh->raw_gyro); + vec3_from_hololens_accel(wh->packet.accel, i, &wh->raw_accel); + + os_mutex_lock(&wh->fusion_mutex); + m_imu_3dof_update(&wh->fusion, wh->packet.gyro_timestamp[i] * WMR_MS_HOLOLENS_NS_PER_TICK, + &wh->raw_accel, &wh->raw_gyro); + os_mutex_unlock(&wh->fusion_mutex); + } + break; + case WMR_MS_HOLOLENS_MSG_UNKNOWN_05: + case WMR_MS_HOLOLENS_MSG_UNKNOWN_06: + case WMR_MS_HOLOLENS_MSG_UNKNOWN_0E: // + hololens_unknown_05_06_0E_decode_packet(wh, buffer, size); + break; + case WMR_MS_HOLOLENS_MSG_UNKNOWN_17: // + hololens_unknown_17_decode_packet(wh, buffer, size); + break; + case WMR_MS_HOLOLENS_MSG_CONTROL: + case WMR_MS_HOLOLENS_MSG_DEBUG: // + break; + default: // + WMR_DEBUG(wh, "Unknown message type: %02x, (%i)", buffer[0], size); + break; + } + + return true; +} + + +/* + * + * Control packets. + * + */ + +static void +control_ipd_value_decode(struct wmr_hmd *wh, const unsigned char *buffer, int size) +{ + if (size != 4) { + WMR_ERROR(wh, "Invalid control ipd distance packet size (expected 4 but got %i)", size); + return; + } + + uint8_t id = read8(&buffer); + uint8_t unknown = read8(&buffer); + uint16_t value = read16(&buffer); + + (void)id; + (void)unknown; + + wh->raw_ipd = value; + + WMR_DEBUG(wh, "Got IPD value: %04x", value); +} + +static bool +control_read_packets(struct wmr_hmd *wh) +{ + unsigned char buffer[WMR_FEATURE_BUFFER_SIZE]; + + // Do not block + int size = os_hid_read(wh->hid_control_dev, buffer, sizeof(buffer), 0); + + if (size < 0) { + WMR_ERROR(wh, "Error reading from device"); + return false; + } else if (size == 0) { + WMR_TRACE(wh, "No more data to read"); + return true; // No more messages, return. + } else { + WMR_TRACE(wh, "Read %u bytes", size); + } + + switch (buffer[0]) { + case WMR_CONTROL_MSG_IPD_VALUE: // + control_ipd_value_decode(wh, buffer, size); + break; + case WMR_CONTROL_MSG_UNKNOWN_05: // + break; + default: // + WMR_DEBUG(wh, "Unknown message type: %02x, (%i)", buffer[0], size); + break; + } + + return true; +} + + +/* + * + * Helpers and internal functions. + * + */ + +static void * +wmr_run_thread(void *ptr) +{ + struct wmr_hmd *wh = (struct wmr_hmd *)ptr; + + os_thread_helper_lock(&wh->oth); + while (os_thread_helper_is_running_locked(&wh->oth)) { + os_thread_helper_unlock(&wh->oth); + + // Does not block. + if (!control_read_packets(wh)) { + break; + } + + // Does block for a bit. + if (!hololens_sensors_read_packets(wh)) { + break; + } + } + + WMR_DEBUG(wh, "Exiting reading thread."); + + return NULL; +} + +static void +hololens_sensors_enable_imu(struct wmr_hmd *wh) +{ + int size = os_hid_write(wh->hid_hololens_senors_dev, hololens_sensors_imu_on, sizeof(hololens_sensors_imu_on)); + if (size <= 0) { + WMR_ERROR(wh, "Error writing to device"); + return; + } +} + +#define HID_SEND(HID, DATA, STR) \ + do { \ + int _ret = os_hid_set_feature(HID, DATA, sizeof(DATA)); \ + if (_ret < 0) { \ + WMR_ERROR(wh, "Send (%s): %i", STR, _ret); \ + } \ + } while (false); + +#define HID_GET(HID, DATA, STR) \ + do { \ + int _ret = os_hid_get_feature(HID, DATA[0], DATA, sizeof(DATA)); \ + if (_ret < 0) { \ + WMR_ERROR(wh, "Get (%s): %i", STR, _ret); \ + } \ + } while (false); + +static int +wmr_hmd_activate(struct wmr_hmd *wh) +{ + struct os_hid_device *hid = wh->hid_control_dev; + + WMR_TRACE(wh, "Activating HP Reverb G1/G2 HMD..."); + + + // Hack to power up the Reverb G1 display, thanks to OpenHMD contibutors. + // Sleep before we start seems to improve reliability. + // 300ms is what Windows seems to do, so cargo cult that. + os_nanosleep(U_TIME_1MS_IN_NS * 300); + + for (int i = 0; i < 4; i++) { + unsigned char cmd[64] = {0x50, 0x01}; + HID_SEND(hid, cmd, "loop"); + + unsigned char data[64] = {0x50}; + HID_GET(hid, data, "loop"); + + os_nanosleep(U_TIME_1MS_IN_NS * 10); // Sleep 10ms + } + + unsigned char data[64] = {0x09}; + HID_GET(hid, data, "data_1"); + + data[0] = 0x08; + HID_GET(hid, data, "data_2"); + + data[0] = 0x06; + HID_GET(hid, data, "data_3"); + + // Wake up the display. + unsigned char cmd[2] = {0x04, 0x01}; + HID_SEND(hid, cmd, "screen_on"); + + WMR_INFO(wh, "Sent activation report, sleeping for compositor."); + + /* + * Sleep so display completes power up and modes be enumerated. + * Two seconds seems to be needed, 1 was not enough. + */ + os_nanosleep(U_TIME_1MS_IN_NS * 2000); + + + return 0; +} + +static void +wmr_hmd_deactivate(struct wmr_hmd *wh) +{ + struct os_hid_device *hid = wh->hid_control_dev; + + /* Turn the screen off */ + unsigned char cmd[2] = {0x04, 0x00}; + HID_SEND(hid, cmd, "screen_off"); +} + +static void +wmr_hmd_update_inputs(struct xrt_device *xdev) +{ + struct wmr_hmd *wh = wmr_hmd(xdev); + (void)wh; +} + +static void +wmr_hmd_get_tracked_pose(struct xrt_device *xdev, + enum xrt_input_name name, + uint64_t at_timestamp_ns, + struct xrt_space_relation *out_relation) +{ + struct wmr_hmd *wh = wmr_hmd(xdev); + + if (name != XRT_INPUT_GENERIC_HEAD_POSE) { + WMR_ERROR(wh, "Unknown input name"); + return; + } + + // Clear relation. + U_ZERO(out_relation); + + os_mutex_lock(&wh->fusion_mutex); + out_relation->pose.orientation = wh->fusion.rot; + os_mutex_unlock(&wh->fusion_mutex); + + out_relation->relation_flags = (enum xrt_space_relation_flags)( // + XRT_SPACE_RELATION_ORIENTATION_VALID_BIT | // + XRT_SPACE_RELATION_ORIENTATION_TRACKED_BIT); +} + +static void +wmr_hmd_get_view_pose(struct xrt_device *xdev, + struct xrt_vec3 *eye_relation, + uint32_t view_index, + struct xrt_pose *out_pose) +{ + struct xrt_pose pose = {{0.0f, 0.0f, 0.0f, 1.0f}, {0.0f, 0.0f, 0.0f}}; + bool adjust = (view_index == 0); + + pose.position.x = eye_relation->x / 2.0f; + pose.position.y = eye_relation->y / 2.0f; + pose.position.z = eye_relation->z / 2.0f; + + // Adjust for left/right while also making sure there aren't any -0.f. + if (pose.position.x > 0.0f && adjust) { + pose.position.x = -pose.position.x; + } + if (pose.position.y > 0.0f && adjust) { + pose.position.y = -pose.position.y; + } + if (pose.position.z > 0.0f && adjust) { + pose.position.z = -pose.position.z; + } + + *out_pose = pose; +} + +static void +wmr_hmd_destroy(struct xrt_device *xdev) +{ + struct wmr_hmd *wh = wmr_hmd(xdev); + + // Destroy the thread object. + os_thread_helper_destroy(&wh->oth); + + if (wh->hid_hololens_senors_dev != NULL) { + os_hid_destroy(wh->hid_hololens_senors_dev); + wh->hid_hololens_senors_dev = NULL; + } + + if (wh->hid_control_dev != NULL) { + wmr_hmd_deactivate(wh); + os_hid_destroy(wh->hid_control_dev); + wh->hid_control_dev = NULL; + } + + // Destroy the fusion. + m_imu_3dof_close(&wh->fusion); + + os_mutex_destroy(&wh->fusion_mutex); + + free(wh); +} + +struct xrt_device * +wmr_hmd_create(struct os_hid_device *hid_holo, struct os_hid_device *hid_ctrl, enum u_logging_level ll) +{ + enum u_device_alloc_flags flags = + (enum u_device_alloc_flags)(U_DEVICE_ALLOC_HMD | U_DEVICE_ALLOC_TRACKING_NONE); + int ret = 0; + + struct wmr_hmd *wh = U_DEVICE_ALLOCATE(struct wmr_hmd, flags, 1, 0); + if (!wh) { + return NULL; + } + + // Populate the base members. + wh->base.update_inputs = wmr_hmd_update_inputs; + wh->base.get_tracked_pose = wmr_hmd_get_tracked_pose; + wh->base.get_view_pose = wmr_hmd_get_view_pose; + wh->base.destroy = wmr_hmd_destroy; + wh->base.name = XRT_DEVICE_GENERIC_HMD; + wh->base.device_type = XRT_DEVICE_TYPE_HMD; + wh->log_level = ll; + + wh->base.orientation_tracking_supported = true; + wh->base.position_tracking_supported = false; + wh->base.hand_tracking_supported = false; + wh->hid_hololens_senors_dev = hid_holo; + wh->hid_control_dev = hid_ctrl; + + snprintf(wh->base.str, XRT_DEVICE_NAME_LEN, "HP Reverb VR Headset"); + + // Mutex before thread. + ret = os_mutex_init(&wh->fusion_mutex); + if (ret != 0) { + WMR_ERROR(wh, "Failed to init mutex!"); + wmr_hmd_destroy(&wh->base); + wh = NULL; + return NULL; + } + + // Thread and other state. + ret = os_thread_helper_init(&wh->oth); + if (ret != 0) { + WMR_ERROR(wh, "Failed to init threading!"); + wmr_hmd_destroy(&wh->base); + wh = NULL; + return NULL; + } + + if (wmr_hmd_activate(wh) != 0) { + WMR_ERROR(wh, "Activation of HMD failed"); + wmr_hmd_destroy(&wh->base); + wh = NULL; + return NULL; + } + + // Switch on IMU on the HMD. + hololens_sensors_enable_imu(wh); + + + // Hand over hololens sensor device to reading thread. + ret = os_thread_helper_start(&wh->oth, wmr_run_thread, wh); + if (ret != 0) { + WMR_ERROR(wh, "Failed to start thread!"); + wmr_hmd_destroy(&wh->base); + wh = NULL; + return NULL; + } + + // Setup input. + wh->base.inputs[0].name = XRT_INPUT_GENERIC_HEAD_POSE; + + // TODO: Read config file from HMD, provide guestimate values for now. + struct u_device_simple_info info; + info.display.w_pixels = 4320; + info.display.h_pixels = 2160; + info.display.w_meters = 0.13f; + info.display.h_meters = 0.07f; + info.lens_horizontal_separation_meters = 0.13f / 2.0f; + info.lens_vertical_position_meters = 0.07f / 2.0f; + info.views[0].fov = 85.0f * (M_PI / 180.0f); + info.views[1].fov = 85.0f * (M_PI / 180.0f); + + if (!u_device_setup_split_side_by_side(&wh->base, &info)) { + WMR_ERROR(wh, "Failed to setup basic HMD device info"); + wmr_hmd_destroy(&wh->base); + wh = NULL; + return NULL; + } + + m_imu_3dof_init(&wh->fusion, M_IMU_3DOF_USE_GRAVITY_DUR_20MS); + + // Setup variable tracker. + u_var_add_root(wh, "WMR HMD", true); + u_var_add_gui_header(wh, &wh->gui.fusion, "3DoF Fusion"); + m_imu_3dof_add_vars(&wh->fusion, wh, ""); + u_var_add_gui_header(wh, &wh->gui.misc, "Misc"); + u_var_add_log_level(wh, &wh->log_level, "log_level"); + + // Distortion information, fills in xdev->compute_distortion(). + u_distortion_mesh_set_none(&wh->base); + + return &wh->base; +} diff --git a/src/xrt/drivers/wmr/wmr_hmd.h b/src/xrt/drivers/wmr/wmr_hmd.h new file mode 100644 index 000000000..6f0410140 --- /dev/null +++ b/src/xrt/drivers/wmr/wmr_hmd.h @@ -0,0 +1,104 @@ +// Copyright 2018, Philipp Zabel. +// Copyright 2020-2021, N Madsen. +// Copyright 2020-2021, Collabora, Ltd. +// SPDX-License-Identifier: BSL-1.0 +/*! + * @file + * @brief Interface to the WMR HMD driver code. + * @author Philipp Zabel + * @author nima01 + * @author Jakob Bornecrantz + * @ingroup drv_wmr + */ + +#pragma once + +#include "xrt/xrt_device.h" +#include "xrt/xrt_prober.h" +#include "os/os_threading.h" +#include "math/m_imu_3dof.h" +#include "util/u_logging.h" + +#include "wmr_protocol.h" + + +#ifdef __cplusplus +extern "C" { +#endif + + +enum rvb_g1_status_bits +{ + // clang-format off + REVERB_G1_STATUS_BIT_UNKNOWN_BIT_0 = (1 << 0), + REVERB_G1_STATUS_BIT_UNKNOWN_BIT_1 = (1 << 1), + REVERB_G1_STATUS_BIT_UNKNOWN_BIT_2 = (1 << 2), + REVERB_G1_STATUS_BIT_UNKNOWN_BIT_3 = (1 << 3), + REVERB_G1_STATUS_BIT_UNKNOWN_BIT_4 = (1 << 4), + REVERB_G1_STATUS_BIT_UNKNOWN_BIT_5 = (1 << 5), + REVERB_G1_STATUS_BIT_UNKNOWN_BIT_6 = (1 << 6), + REVERB_G1_STATUS_BIT_UNKNOWN_BIT_7 = (1 << 7), + // clang-format on +}; + +/*! + * @implements xrt_device + */ +struct wmr_hmd +{ + struct xrt_device base; + + //! Packet reading thread. + struct os_thread_helper oth; + + enum u_logging_level log_level; + + /*! + * This is the hololens sensor device, this is were we get all of the + * IMU data and read the config from. + * + * During start it is owned by the thread creating the device, after + * init it is owned by the reading thread, there is no mutex protecting + * this field as it's only used by the reading thread in @p oth. + */ + struct os_hid_device *hid_hololens_senors_dev; + struct os_hid_device *hid_control_dev; + + //! Latest raw IPD value from the device. + uint16_t raw_ipd; + + struct hololens_sensors_packet packet; + + struct xrt_vec3 raw_accel; + struct xrt_vec3 raw_gyro; + + struct os_mutex fusion_mutex; + //! Protected by @ref fusion_mutex. + struct m_imu_3dof fusion; + + struct + { + bool fusion; + bool misc; + } gui; +}; + +static inline struct wmr_hmd * +wmr_hmd(struct xrt_device *p) +{ + return (struct wmr_hmd *)p; +} + +struct xrt_device * +wmr_hmd_create(struct os_hid_device *hid_holo, struct os_hid_device *hid_ctrl, enum u_logging_level ll); + +#define WMR_TRACE(d, ...) U_LOG_XDEV_IFL_T(&d->base, d->log_level, __VA_ARGS__) +#define WMR_DEBUG(d, ...) U_LOG_XDEV_IFL_D(&d->base, d->log_level, __VA_ARGS__) +#define WMR_INFO(d, ...) U_LOG_XDEV_IFL_I(&d->base, d->log_level, __VA_ARGS__) +#define WMR_WARN(d, ...) U_LOG_XDEV_IFL_W(&d->base, d->log_level, __VA_ARGS__) +#define WMR_ERROR(d, ...) U_LOG_XDEV_IFL_E(&d->base, d->log_level, __VA_ARGS__) + + +#ifdef __cplusplus +} +#endif diff --git a/src/xrt/drivers/wmr/wmr_interface.h b/src/xrt/drivers/wmr/wmr_interface.h new file mode 100644 index 000000000..2969b8268 --- /dev/null +++ b/src/xrt/drivers/wmr/wmr_interface.h @@ -0,0 +1,48 @@ +// Copyright 2020-2021, N Madsen. +// Copyright 2020-2021, Collabora, Ltd. +// SPDX-License-Identifier: BSL-1.0 +/*! + * @file + * @brief Interface to the WMR driver. + * @author nima01 + * @author Jakob Bornecrantz + * @ingroup drv_wmr + */ + +#pragma once + + +#ifdef __cplusplus +extern "C" { +#endif + +/*! + * @defgroup drv_wmr + * @ingroup drv + * + * @brief Windows Mixed Reality driver. + */ + +/*! + * Probing function for Windows Mixed Reality devices. + * + * @ingroup drv_wmr + */ +int +wmr_found(struct xrt_prober *xp, + struct xrt_prober_device **devices, + size_t num_devices, + size_t index, + cJSON *attached_data, + struct xrt_device **out_xdev); + +/*! + * @dir drivers/wmr + * + * @brief @ref drv_wmr files. + */ + + +#ifdef __cplusplus +} +#endif diff --git a/src/xrt/drivers/wmr/wmr_prober.c b/src/xrt/drivers/wmr/wmr_prober.c new file mode 100644 index 000000000..bb8796220 --- /dev/null +++ b/src/xrt/drivers/wmr/wmr_prober.c @@ -0,0 +1,160 @@ +// Copyright 2020-2021, N Madsen. +// Copyright 2020-2021, Collabora, Ltd. +// SPDX-License-Identifier: BSL-1.0 +/*! + * @file + * @brief WMR prober code. + * @author nima01 + * @author Jakob Bornecrantz + * @ingroup drv_wmr + */ + +#include "xrt/xrt_prober.h" + +#include "util/u_misc.h" +#include "util/u_debug.h" +#include "util/u_logging.h" + +#include "wmr_interface.h" +#include "wmr_hmd.h" +#include "wmr_common.h" + +#include +#include +#include + + +/* + * + * Defines & structs. + * + */ + +DEBUG_GET_ONCE_LOG_OPTION(wmr_log, "WMR_LOG", U_LOGGING_INFO) + + +/* + * + * Functions. + * + */ + +static bool +check_and_get_interface_hp(struct xrt_prober_device *device, int *out_interface) +{ + if (device->product_id != REVERB_G1_PID && device->product_id != REVERB_G2_PID) { + return false; + } + + *out_interface = 0; + + return true; +} + +static bool +find_control_device(struct xrt_prober *xp, + struct xrt_prober_device **devices, + size_t num_devices, + enum u_logging_level ll, + struct xrt_prober_device **out_device, + int *out_interface) +{ + struct xrt_prober_device *dev = NULL; + int interface = 0; + + for (size_t i = 0; i < num_devices; i++) { + bool match = false; + + if (devices[i]->bus != XRT_BUS_TYPE_USB) { + continue; + } + + switch (devices[i]->vendor_id) { + case HP_VID: match = check_and_get_interface_hp(devices[i], &interface); break; + default: break; + } + + if (!match) { + continue; + } + + if (dev != NULL) { + U_LOG_IFL_W(ll, "Found multiple control devices, using the last."); + } + dev = devices[i]; + } + + unsigned char m_str[256] = {0}; + unsigned char p_str[256] = {0}; + xrt_prober_get_string_descriptor(xp, dev, XRT_PROBER_STRING_MANUFACTURER, m_str, sizeof(m_str)); + xrt_prober_get_string_descriptor(xp, dev, XRT_PROBER_STRING_PRODUCT, p_str, sizeof(p_str)); + + U_LOG_IFL_D(ll, "Found control device '%s' '%s' (%04X:%04X)", p_str, m_str, dev->vendor_id, dev->product_id); + + *out_device = dev; + *out_interface = interface; + + return dev != NULL; +} + + +/* + * + * Exported functions. + * + */ + +int +wmr_found(struct xrt_prober *xp, + struct xrt_prober_device **devices, + size_t num_devices, + size_t index, + cJSON *attached_data, + struct xrt_device **out_xdev) +{ + enum u_logging_level ll = debug_get_log_option_wmr_log(); + + struct xrt_prober_device *dev_holo = devices[index]; + struct xrt_prober_device *dev_ctrl = NULL; + int interface_holo = 2; + int interface_ctrl = 0; + + unsigned char buf[256] = {0}; + int result = xrt_prober_get_string_descriptor(xp, dev_holo, XRT_PROBER_STRING_PRODUCT, buf, sizeof(buf)); + + if (!xrt_prober_match_string(xp, dev_holo, XRT_PROBER_STRING_MANUFACTURER, MS_HOLOLENS_MANUFACTURER_STRING) || + !xrt_prober_match_string(xp, dev_holo, XRT_PROBER_STRING_PRODUCT, MS_HOLOLENS_PRODUCT_STRING)) { + U_LOG_IFL_E(ll, "HoloLens Sensors manufacturer or product strings did not match."); + return -1; + } + + if (!find_control_device(xp, devices, num_devices, ll, &dev_ctrl, &interface_ctrl)) { + U_LOG_IFL_E(ll, + "Did not find companion control device." + "\n\tCurrently only Reverb G1 and G2 is supported"); + return -1; + } + + struct os_hid_device *hid_holo = NULL; + result = xrt_prober_open_hid_interface(xp, dev_holo, interface_holo, &hid_holo); + if (result != 0) { + U_LOG_IFL_E(ll, "Failed to open HoloLens Sensors HID interface"); + return -1; + } + + struct os_hid_device *hid_ctrl = NULL; + result = xrt_prober_open_hid_interface(xp, dev_ctrl, interface_ctrl, &hid_ctrl); + if (result != 0) { + U_LOG_IFL_E(ll, "Failed to open HoloLens Sensors HID interface"); + return -1; + } + + struct xrt_device *p = wmr_hmd_create(hid_holo, hid_ctrl, ll); + if (!p) { + U_LOG_IFL_E(ll, "Failed to create Windows Mixed Reality device"); + return -1; + } + + *out_xdev = p; + return 1; +} diff --git a/src/xrt/drivers/wmr/wmr_protocol.c b/src/xrt/drivers/wmr/wmr_protocol.c new file mode 100644 index 000000000..3334cd5bc --- /dev/null +++ b/src/xrt/drivers/wmr/wmr_protocol.c @@ -0,0 +1,60 @@ +// Copyright 2018, Philipp Zabel. +// Copyright 2020-2021, N Madsen. +// Copyright 2020-2021, Collabora, Ltd. +// SPDX-License-Identifier: BSL-1.0 +/*! + * @file + * @brief WMR and MS HoloLens protocol helpers implementation. + * @author Philipp Zabel + * @author nima01 + * @ingroup drv_wmr + */ + +#include "wmr_protocol.h" + + +/* + * + * WMR and MS HoloLens Sensors protocol helpers + * + */ + +void +vec3_from_hololens_accel(int32_t sample[3][4], int i, struct xrt_vec3 *out_vec) +{ + out_vec->x = (float)sample[0][i] * 0.001f * -1.0f; + out_vec->y = (float)sample[1][i] * 0.001f * -1.0f; + out_vec->z = (float)sample[2][i] * 0.001f * -1.0f; +} + +void +vec3_from_hololens_gyro(int16_t sample[3][32], int i, struct xrt_vec3 *out_vec) +{ + out_vec->x = (float)(sample[0][8 * i + 0] + // + sample[0][8 * i + 1] + // + sample[0][8 * i + 2] + // + sample[0][8 * i + 3] + // + sample[0][8 * i + 4] + // + sample[0][8 * i + 5] + // + sample[0][8 * i + 6] + // + sample[0][8 * i + 7]) * + 0.001f * 0.125f; + out_vec->y = (float)(sample[1][8 * i + 0] + // + sample[1][8 * i + 1] + // + sample[1][8 * i + 2] + // + sample[1][8 * i + 3] + // + sample[1][8 * i + 4] + // + sample[1][8 * i + 5] + // + sample[1][8 * i + 6] + // + sample[1][8 * i + 7]) * + 0.001f * -0.125f; + out_vec->z = (float)(sample[2][8 * i + 0] + // + sample[2][8 * i + 1] + // + sample[2][8 * i + 2] + // + sample[2][8 * i + 3] + // + sample[2][8 * i + 4] + // + sample[2][8 * i + 5] + // + sample[2][8 * i + 6] + // + sample[2][8 * i + 7]) * + 0.001f * -0.125f; +} diff --git a/src/xrt/drivers/wmr/wmr_protocol.h b/src/xrt/drivers/wmr/wmr_protocol.h new file mode 100644 index 000000000..608a67d4c --- /dev/null +++ b/src/xrt/drivers/wmr/wmr_protocol.h @@ -0,0 +1,126 @@ +// Copyright 2018, Philipp Zabel. +// Copyright 2020-2021, N Madsen. +// Copyright 2020-2021, Collabora, Ltd. +// SPDX-License-Identifier: BSL-1.0 +/*! + * @file + * @brief WMR and MS HoloLens protocol constants, structures and helpers header + * @author Philipp Zabel + * @author nima01 + * @ingroup drv_wmr + */ + +#pragma once + +#include "math/m_vec2.h" + + +#ifdef __cplusplus +extern "C" { +#endif + + +/*! + * WMR and MS HoloLens Sensors protocol constants and structures + * + * @ingroup drv_wmr + * @{ + */ + +#define WMR_FEATURE_BUFFER_SIZE 497 +#define WMR_MS_HOLOLENS_NS_PER_TICK 100 + +#define WMR_MS_HOLOLENS_MSG_SENSORS 0x01 +#define WMR_MS_HOLOLENS_MSG_CONTROL 0x02 +#define WMR_MS_HOLOLENS_MSG_DEBUG 0x03 +#define WMR_MS_HOLOLENS_MSG_UNKNOWN_05 0x05 +#define WMR_MS_HOLOLENS_MSG_UNKNOWN_06 0x06 +#define WMR_MS_HOLOLENS_MSG_UNKNOWN_0E 0x0E +#define WMR_MS_HOLOLENS_MSG_UNKNOWN_17 0x17 + +#define WMR_CONTROL_MSG_IPD_VALUE 0x01 +#define WMR_CONTROL_MSG_UNKNOWN_05 0x05 + +static const unsigned char hololens_sensors_imu_on[64] = {0x02, 0x07}; + +struct hololens_sensors_packet +{ + uint8_t id; + uint16_t temperature[4]; + uint64_t gyro_timestamp[4]; + int16_t gyro[3][32]; + uint64_t accel_timestamp[4]; + int32_t accel[3][4]; + uint64_t video_timestamp[4]; +}; + +/*! + * @} + */ + + +/*! + * WMR and MS HoloLens Sensors protocol helpers + * + * @ingroup drv_wmr + * @{ + */ + +void +vec3_from_hololens_accel(int32_t sample[3][4], int i, struct xrt_vec3 *out_vec); + +void +vec3_from_hololens_gyro(int16_t sample[3][32], int i, struct xrt_vec3 *out_vec); + + +static inline uint8_t +read8(const unsigned char **buffer) +{ + uint8_t ret = **buffer; + *buffer += 1; + return ret; +} + +static inline int16_t +read16(const unsigned char **buffer) +{ + int16_t ret = (*(*buffer + 0) << 0) | // + (*(*buffer + 1) << 8); + *buffer += 2; + return ret; +} + +static inline int32_t +read32(const unsigned char **buffer) +{ + int32_t ret = (*(*buffer + 0) << 0) | // + (*(*buffer + 1) << 8) | // + (*(*buffer + 2) << 16) | // + (*(*buffer + 3) << 24); + *buffer += 4; + return ret; +} + +static inline uint64_t +read64(const unsigned char **buffer) +{ + uint64_t ret = ((uint64_t) * (*buffer + 0) << 0) | // + ((uint64_t) * (*buffer + 1) << 8) | // + ((uint64_t) * (*buffer + 2) << 16) | // + ((uint64_t) * (*buffer + 3) << 24) | // + ((uint64_t) * (*buffer + 4) << 32) | // + ((uint64_t) * (*buffer + 5) << 40) | // + ((uint64_t) * (*buffer + 6) << 48) | // + ((uint64_t) * (*buffer + 7) << 56); + *buffer += 8; + return ret; +} + +/*! + * @} + */ + + +#ifdef __cplusplus +} +#endif diff --git a/src/xrt/targets/common/CMakeLists.txt b/src/xrt/targets/common/CMakeLists.txt index 9095da43e..0a31ff126 100644 --- a/src/xrt/targets/common/CMakeLists.txt +++ b/src/xrt/targets/common/CMakeLists.txt @@ -102,6 +102,11 @@ if(XRT_BUILD_DRIVER_QWERTY) target_link_libraries(target_lists PRIVATE drv_qwerty) endif() +if(XRT_BUILD_DRIVER_WMR) + target_link_libraries(target_lists PRIVATE drv_wmr) +endif() + + #### # Instance # diff --git a/src/xrt/targets/common/target_lists.c b/src/xrt/targets/common/target_lists.c index 4413fbe62..20e825c6b 100644 --- a/src/xrt/targets/common/target_lists.c +++ b/src/xrt/targets/common/target_lists.c @@ -78,6 +78,12 @@ #include "qwerty/qwerty_interface.h" #endif +#ifdef XRT_BUILD_DRIVER_WMR +#include "wmr/wmr_interface.h" +#include "wmr/wmr_common.h" +#endif + + /*! * Each entry should be a vendor ID (VID), product ID (PID), a "found" function, * and a string literal name. @@ -121,6 +127,10 @@ struct xrt_prober_entry target_entry_list[] = { {ULV2_VID, ULV2_PID, ulv2_found, "Leap Motion Controller", "ulv2"}, #endif +#ifdef XRT_BUILD_DRIVER_WMR + {MICROSOFT_VID, HOLOLENS_SENSORS_PID, wmr_found, "Microsoft HoloLens Sensors", "wmr"}, +#endif // XRT_BUILD_DRIVER_WMR + {0x0000, 0x0000, NULL, NULL, NULL}, // Terminate }; diff --git a/src/xrt/targets/meson.build b/src/xrt/targets/meson.build index 9ac834703..4bbbdcc5d 100644 --- a/src/xrt/targets/meson.build +++ b/src/xrt/targets/meson.build @@ -61,6 +61,10 @@ if 'vive' in drivers driver_libs += [lib_drv_vive] endif +if 'wmr' in drivers + driver_libs += [lib_drv_wmr] +endif + if 'survive' in drivers driver_libs += [lib_drv_survive] endif