From c4db3dfccc4a1816db9b4bbaf91e5095fe998f76 Mon Sep 17 00:00:00 2001 From: Nima01 Date: Sat, 13 Nov 2021 03:35:13 +0100 Subject: [PATCH] d/wmr: Add basic Reverb (G1, Bluetooth) motion controller support. --- src/xrt/drivers/CMakeLists.txt | 2 + src/xrt/drivers/meson.build | 2 + src/xrt/drivers/wmr/wmr_bt_controller.c | 271 ++++++++++++++++++++++++ src/xrt/drivers/wmr/wmr_bt_controller.h | 81 +++++++ src/xrt/drivers/wmr/wmr_common.h | 6 +- src/xrt/drivers/wmr/wmr_config.h | 13 ++ src/xrt/drivers/wmr/wmr_hmd.c | 2 + src/xrt/drivers/wmr/wmr_hmd.h | 15 -- src/xrt/drivers/wmr/wmr_interface.h | 15 ++ src/xrt/drivers/wmr/wmr_prober.c | 75 ++++++- src/xrt/drivers/wmr/wmr_protocol.h | 11 + src/xrt/targets/common/target_lists.c | 1 + 12 files changed, 473 insertions(+), 21 deletions(-) create mode 100644 src/xrt/drivers/wmr/wmr_bt_controller.c create mode 100644 src/xrt/drivers/wmr/wmr_bt_controller.h diff --git a/src/xrt/drivers/CMakeLists.txt b/src/xrt/drivers/CMakeLists.txt index e05ba8ffe..035a7d671 100644 --- a/src/xrt/drivers/CMakeLists.txt +++ b/src/xrt/drivers/CMakeLists.txt @@ -300,6 +300,8 @@ if(XRT_BUILD_DRIVER_WMR) wmr/wmr_common.h wmr/wmr_config.c wmr/wmr_config.h + wmr/wmr_bt_controller.c + wmr/wmr_bt_controller.h wmr/wmr_hmd.c wmr/wmr_hmd.h wmr/wmr_interface.h diff --git a/src/xrt/drivers/meson.build b/src/xrt/drivers/meson.build index a21e5e788..1d05b2d3c 100644 --- a/src/xrt/drivers/meson.build +++ b/src/xrt/drivers/meson.build @@ -295,6 +295,8 @@ lib_drv_wmr = static_library( files( 'wmr/wmr_common.h', 'wmr/wmr_config.c', + 'wmr/wmr_bt_controller.c', + 'wmr/wmr_bt_controller.h', 'wmr/wmr_hmd.c', 'wmr/wmr_hmd.h', 'wmr/wmr_interface.h', diff --git a/src/xrt/drivers/wmr/wmr_bt_controller.c b/src/xrt/drivers/wmr/wmr_bt_controller.c new file mode 100644 index 000000000..953adacee --- /dev/null +++ b/src/xrt/drivers/wmr/wmr_bt_controller.c @@ -0,0 +1,271 @@ +// Copyright 2020-2021, N Madsen. +// Copyright 2020-2021, Collabora, Ltd. +// SPDX-License-Identifier: BSL-1.0 +/*! + * @file + * @brief Driver for Bluetooth based WMR Controller. + * @author Nis Madsen + * @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 "math/m_predict.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 "wmr_bt_controller.h" +#include "wmr_common.h" +#include "wmr_protocol.h" + +#include +#include +#include +#include +#ifndef XRT_OS_WINDOWS +#include // for sleep() +#endif + +#define WMR_TRACE(d, ...) U_LOG_XDEV_IFL_T(&d->base, d->ll, __VA_ARGS__) +#define WMR_DEBUG(d, ...) U_LOG_XDEV_IFL_D(&d->base, d->ll, __VA_ARGS__) +#define WMR_INFO(d, ...) U_LOG_XDEV_IFL_I(&d->base, d->ll, __VA_ARGS__) +#define WMR_WARN(d, ...) U_LOG_XDEV_IFL_W(&d->base, d->ll, __VA_ARGS__) +#define WMR_ERROR(d, ...) U_LOG_XDEV_IFL_E(&d->base, d->ll, __VA_ARGS__) + + +static inline struct wmr_bt_controller * +wmr_bt_controller(struct xrt_device *p) +{ + return (struct wmr_bt_controller *)p; +} + +static bool +control_read_packets(struct wmr_bt_controller *d) +{ + unsigned char buffer[WMR_FEATURE_BUFFER_SIZE]; + + // Do not block + int size = os_hid_read(d->controller_hid, buffer, sizeof(buffer), 0); + + if (size < 0) { + WMR_ERROR(d, "Error reading from controller device"); + return false; + } else if (size == 0) { + WMR_TRACE(d, "No more data to read from controller device"); + return true; // No more messages, return. + } else { + WMR_DEBUG(d, "Read %u bytes from controller device", size); + } + + switch (buffer[0]) { + case WMR_MS_HOLOLENS_MSG_SENSORS: // + if (size != 45) { + WMR_ERROR(d, "WMR Controller unexpected message size: %d", size); + return false; + } + WMR_DEBUG(d, + "%02x | " // msg type + "%02x %02x %02x %02x %02x %02x %02x %02x | " // buttons and inputs, battery + "%02x %02x %02x %02x %02x %02x %02x %02x %02x | " // accel + "%02x %02x | " // temp + "%02x %02x %02x %02x %02x %02x %02x %02x %02x | " // gyro + "%02x %02x %02x %02x %02x %02x %02x %02x %02x %02x | " // timestamp and more? + "%02x %02x %02x %02x %02x %02x", // device run state, status and more? + 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], + buffer[30], buffer[31], buffer[32], buffer[33], buffer[34], buffer[35], buffer[36], + buffer[37], buffer[38], buffer[39], buffer[40], buffer[41], buffer[42], buffer[43], + buffer[44]); + + const unsigned char *p = (unsigned char *)&buffer[1]; + + // HP Reverb G1 button mask: + // Stick_pressed: 0x01 + // Windows button: 0x02 + // Menu button: 0x04 + // Side button: 0x08 + // Touch-pad pressed: 0x10 + // BT pairing button: 0x20 + // Touch-pad touched: 0x40 + uint8_t buttons = read8(&p); + + // Todo: interpret analog stick data + uint8_t stick_1 = read8(&p); + uint8_t stick_2 = read8(&p); + uint8_t stick_3 = read8(&p); + + uint8_t trigger = read8(&p); // pressure: 0x00 - 0xFF + + // Touchpad coords range: 0x00 - 0x64. Both are 0xFF when untouched. + uint8_t pad_x = read8(&p); + uint8_t pad_y = read8(&p); + uint8_t battery = read8(&p); + int32_t accel_x = read24(&p); + int32_t accel_y = read24(&p); + int32_t accel_z = read24(&p); + int32_t temp = read16(&p); + int32_t gyro_x = read24(&p); + int32_t gyro_y = read24(&p); + int32_t gyro_z = read24(&p); + + uint64_t timestamp = read32(&p); // Maybe only part of timestamp. + read16(&p); // Unknown. Seems to depend on controller orientation. + read32(&p); // Unknown. + + read16(&p); // Unknown. Device state, etc. + read16(&p); + read16(&p); + + WMR_DEBUG(d, "timestamp %lu\ttemp %d\taccel x: %f\ty: %f\tz: %f\t\tgyro x: %f\tgyro y: %f\tgyro z: %f", + timestamp, temp, accel_x * 0.001f, accel_y * 0.001f, accel_z * 0.001f, gyro_x * 2e-6, + gyro_y * 2e-6, gyro_z * 2e-6); + + break; + default: // + WMR_DEBUG(d, "Unknown message type: %02x, size: %i from controller device", buffer[0], size); + break; + } + + return true; +} + +static void +wmr_bt_controller_set_output(struct xrt_device *xdev, enum xrt_output_name name, union xrt_output_value *value) +{ + // struct wmr_bt_controller *d = wmr_bt_controller(xdev); + // Todo: implement +} + + +static void +wmr_bt_controller_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_bt_controller *d = wmr_bt_controller(xdev); + // Todo: implement +} + +static void +wmr_bt_controller_update_inputs(struct xrt_device *xdev) +{ + // struct wmr_bt_controller *d = wmr_bt_controller(xdev); + // Todo: implement +} + +static void * +wmr_bt_controller_run_thread(void *ptr) +{ + struct wmr_bt_controller *d = wmr_bt_controller(ptr); + + + os_thread_helper_lock(&d->controller_thread); + while (os_thread_helper_is_running_locked(&d->controller_thread)) { + os_thread_helper_unlock(&d->controller_thread); + + // Does not block. + if (!control_read_packets(d)) { + break; + } + } + + WMR_DEBUG(d, "Exiting reading thread."); + + return NULL; +} + + +static void +wmr_bt_controller_destroy(struct xrt_device *xdev) +{ + struct wmr_bt_controller *d = wmr_bt_controller(xdev); + + // Destroy the thread object. + os_thread_helper_destroy(&d->controller_thread); + + + if (d->controller_hid != NULL) { + /* Do any deinit if we have a deinit function */ + // if (d->hmd_desc && d->hmd_desc->deinit_func) { + // d->hmd_desc->deinit_func(d); + // } + os_hid_destroy(d->controller_hid); + d->controller_hid = NULL; + } + + // Destroy the fusion. + m_imu_3dof_close(&d->fusion); + + free(d); +} + +struct xrt_device * +wmr_bt_controller_create(struct os_hid_device *controller_hid, + enum xrt_device_type controller_type, + enum u_logging_level ll) +{ + + enum u_device_alloc_flags flags = U_DEVICE_ALLOC_TRACKING_NONE; + struct wmr_bt_controller *d = U_DEVICE_ALLOCATE(struct wmr_bt_controller, flags, 1, 01); + + d->ll = ll; + d->controller_hid = controller_hid; + + d->base.destroy = wmr_bt_controller_destroy; + d->base.get_tracked_pose = wmr_bt_controller_get_tracked_pose; + d->base.set_output = wmr_bt_controller_set_output; + d->base.update_inputs = wmr_bt_controller_update_inputs; + + d->base.inputs[0].name = XRT_INPUT_GENERIC_HAND_TRACKING_LEFT; + d->base.inputs[1].name = XRT_INPUT_GENERIC_HAND_TRACKING_RIGHT; + + + d->base.name = XRT_DEVICE_WMR_CONTROLLER; + d->base.device_type = controller_type; + d->base.orientation_tracking_supported = true; + d->base.position_tracking_supported = false; + d->base.hand_tracking_supported = true; + + m_imu_3dof_init(&d->fusion, M_IMU_3DOF_USE_GRAVITY_DUR_20MS); + + + + int ret = 0; + + // Todo: Read config file from controller + + // Thread and other state. + ret = os_thread_helper_init(&d->controller_thread); + if (ret != 0) { + WMR_ERROR(d, "Failed to init WMR controller threading!"); + wmr_bt_controller_destroy(&d->base); + d = NULL; + return NULL; + } + + // Hand over controller device to reading thread. + ret = os_thread_helper_start(&d->controller_thread, wmr_bt_controller_run_thread, d); + if (ret != 0) { + WMR_ERROR(d, "Failed to start WMR controller thread!"); + wmr_bt_controller_destroy(&d->base); + d = NULL; + return NULL; + } + + + return &d->base; +} diff --git a/src/xrt/drivers/wmr/wmr_bt_controller.h b/src/xrt/drivers/wmr/wmr_bt_controller.h new file mode 100644 index 000000000..8915b4b0f --- /dev/null +++ b/src/xrt/drivers/wmr/wmr_bt_controller.h @@ -0,0 +1,81 @@ +// Copyright 2020-2021, N Madsen. +// Copyright 2020-2021, Collabora, Ltd. +// SPDX-License-Identifier: BSL-1.0 +/*! + * @file + * @brief Driver interface for Bluetooth based WMR motion controllers. + * Note: Only tested with HP Reverb (G1) controllers that are manually + * paired to a non hmd-integrated, generic BT usb adapter. + * @author Nis Madsen + * @ingroup drv_wmr + */ + + +#include "os/os_threading.h" +#include "xrt/xrt_prober.h" +#include "math/m_imu_3dof.h" +#include "util/u_logging.h" + +#include "xrt/xrt_device.h" +#include "xrt/xrt_prober.h" + +#include "wmr_protocol.h" +#include "wmr_config.h" + +#ifdef __cplusplus +extern "C" { +#endif + + +/*! + * A Bluetooth connected WMR Controller device, representing just a single controller. + * + * @ingroup drv_wmr + * @implements xrt_device + */ +struct wmr_bt_controller +{ + struct xrt_device base; + + struct os_hid_device *controller_hid; + struct os_thread_helper controller_thread; + struct os_mutex lock; + + struct + { + uint64_t time_ns; + uint32_t last_sample_time_raw; + timepoint_ns ts_received_ns; + } imu; + + struct m_imu_3dof fusion; + + struct + { + struct xrt_vec3 acc; + struct xrt_vec3 gyro; + } last; + + struct xrt_quat rot_filtered; + + enum u_logging_level ll; + + uint32_t last_ticks; + + // firmware configuration block, with device names etc + // struct wmr_config_header config_hdr; + + // Config data parsed from the firmware JSON + // wmr_bt_controller_config config; +}; + + +struct xrt_device * +wmr_bt_controller_create(struct os_hid_device *controller_hid, + enum xrt_device_type controller_type, + enum u_logging_level ll); + + +#ifdef __cplusplus +} +#endif diff --git a/src/xrt/drivers/wmr/wmr_common.h b/src/xrt/drivers/wmr/wmr_common.h index 1f87f91d8..dcc1b4409 100644 --- a/src/xrt/drivers/wmr/wmr_common.h +++ b/src/xrt/drivers/wmr/wmr_common.h @@ -4,7 +4,7 @@ /*! * @file * @brief Defines and constants related to WMR driver code. - * @author nima01 + * @author Nis Madsen * @author Jakob Bornecrantz * @ingroup drv_wmr */ @@ -29,11 +29,15 @@ extern "C" { #define MICROSOFT_VID 0x045e #define HOLOLENS_SENSORS_PID 0x0659 +#define WMR_CONTROLLER_PID 0x065b +#define WMR_CONTROLLER_LEFT_PRODUCT_STRING "Motion controller - Left" +#define WMR_CONTROLLER_RIGHT_PRODUCT_STRING "Motion controller - Right" #define HP_VID 0x03f0 #define REVERB_G1_PID 0x0c6a #define REVERB_G2_PID 0x0580 + #define LENOVO_VID 0x17ef #define EXPLORER_PID 0xb801 diff --git a/src/xrt/drivers/wmr/wmr_config.h b/src/xrt/drivers/wmr/wmr_config.h index f1279b39c..32d2ebd27 100644 --- a/src/xrt/drivers/wmr/wmr_config.h +++ b/src/xrt/drivers/wmr/wmr_config.h @@ -66,6 +66,19 @@ struct wmr_hmd_config bool wmr_config_parse(struct wmr_hmd_config *c, char *json_string, enum u_logging_level ll); + +struct wmr_bt_controller_config +{ + /* Todo: still work in progress */ + struct xrt_pose accel_pose; + struct xrt_pose gyro_pose; + struct xrt_pose mag_pose; +}; + +/* Todo: Extract and parse motion controller config. */ + + + #ifdef __cplusplus } #endif diff --git a/src/xrt/drivers/wmr/wmr_hmd.c b/src/xrt/drivers/wmr/wmr_hmd.c index ade6b8327..c9978b42f 100644 --- a/src/xrt/drivers/wmr/wmr_hmd.c +++ b/src/xrt/drivers/wmr/wmr_hmd.c @@ -198,6 +198,8 @@ hololens_sensors_read_packets(struct wmr_hmd *wh) hololens_unknown_17_decode_packet(wh, buffer, size); break; case WMR_MS_HOLOLENS_MSG_CONTROL: + WMR_DEBUG(wh, "WMR_MS_HOLOLENS_MSG_CONTROL: %02x, (%i)", buffer[0], size); + break; case WMR_MS_HOLOLENS_MSG_DEBUG: // break; default: // diff --git a/src/xrt/drivers/wmr/wmr_hmd.h b/src/xrt/drivers/wmr/wmr_hmd.h index b9a210957..177782d4b 100644 --- a/src/xrt/drivers/wmr/wmr_hmd.h +++ b/src/xrt/drivers/wmr/wmr_hmd.h @@ -27,21 +27,6 @@ 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 -}; - enum wmr_headset_type { WMR_HEADSET_GENERIC, diff --git a/src/xrt/drivers/wmr/wmr_interface.h b/src/xrt/drivers/wmr/wmr_interface.h index 96e7f1b1c..e140ecea5 100644 --- a/src/xrt/drivers/wmr/wmr_interface.h +++ b/src/xrt/drivers/wmr/wmr_interface.h @@ -37,6 +37,21 @@ wmr_found(struct xrt_prober *xp, cJSON *attached_data, struct xrt_device **out_xdev); + +/*! + * Probing function for Bluetooth WMR motion controllers. + * + * @ingroup drv_wmr + */ +int +wmr_bt_controller_found(struct xrt_prober *xp, + struct xrt_prober_device **devices, + size_t device_count, + size_t index, + cJSON *attached_data, + struct xrt_device **out_xdev); + + /*! * @dir drivers/wmr * diff --git a/src/xrt/drivers/wmr/wmr_prober.c b/src/xrt/drivers/wmr/wmr_prober.c index 6506ac3cb..cd4b2693c 100644 --- a/src/xrt/drivers/wmr/wmr_prober.c +++ b/src/xrt/drivers/wmr/wmr_prober.c @@ -4,7 +4,7 @@ /*! * @file * @brief WMR prober code. - * @author nima01 + * @author Nis Madsen * @author Jakob Bornecrantz * @ingroup drv_wmr */ @@ -17,6 +17,7 @@ #include "wmr_interface.h" #include "wmr_hmd.h" +#include "wmr_bt_controller.h" #include "wmr_common.h" #include @@ -171,12 +172,12 @@ wmr_found(struct xrt_prober *xp, return -1; } - U_LOG_IFL_D(ll, "Found HoloLens Sensors HMD device '%s' '%s' (vid %04X, pid %04X)", MS_HOLOLENS_MANUFACTURER_STRING, - MS_HOLOLENS_PRODUCT_STRING, dev_holo->vendor_id, dev_holo->product_id); + U_LOG_IFL_D(ll, "Found HoloLens Sensors HMD device '%s' '%s' (vid %04X, pid %04X)", + MS_HOLOLENS_MANUFACTURER_STRING, MS_HOLOLENS_PRODUCT_STRING, dev_holo->vendor_id, + dev_holo->product_id); if (!find_companion_device(xp, devices, device_count, ll, &hmd_type, &dev_companion, &interface_companion)) { - U_LOG_IFL_E(ll, - "Did not find HoloLens Sensors' companion device"); + U_LOG_IFL_E(ll, "Did not find HoloLens Sensors' companion device"); return -1; } @@ -203,3 +204,67 @@ wmr_found(struct xrt_prober *xp, *out_xdev = p; return 1; } + +int +wmr_bt_controller_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 os_hid_device *hid_controller = NULL; + + // Only handle Bluetooth connected controllers here. + if (devices[index]->bus != XRT_BUS_TYPE_BLUETOOTH) { + return 0; + } + + unsigned char product_name[XRT_DEVICE_PRODUCT_NAME_LEN] = {0}; + int ret = xrt_prober_get_string_descriptor(xp, devices[index], XRT_PROBER_STRING_PRODUCT, product_name, + sizeof(product_name)); + + enum xrt_device_type controller_type = XRT_DEVICE_TYPE_UNKNOWN; + int interface_controller = -1; + + switch (devices[index]->product_id) { + case WMR_CONTROLLER_PID: + if (strncmp((char *)product_name, WMR_CONTROLLER_LEFT_PRODUCT_STRING, sizeof(product_name)) == 0) { + controller_type = XRT_DEVICE_TYPE_LEFT_HAND_CONTROLLER; + interface_controller = 0; + break; + } else if (strncmp((char *)product_name, WMR_CONTROLLER_RIGHT_PRODUCT_STRING, sizeof(product_name)) == + 0) { + controller_type = XRT_DEVICE_TYPE_RIGHT_HAND_CONTROLLER; + interface_controller = 0; + break; + } + // else fall through + default: + U_LOG_IFL_D(ll, + "Unsupported controller device (Bluetooth): vid: 0x%04X, pid: 0x%04X, Product Name: '%s'", + devices[index]->vendor_id, devices[index]->product_id, product_name); + return -1; + } + + + + ret = xrt_prober_open_hid_interface(xp, devices[index], interface_controller, &hid_controller); + if (ret != 0) { + U_LOG_IFL_E(ll, "Failed to open WMR Bluetooth controller's HID interface"); + return -1; + } + + + struct xrt_device *p = wmr_bt_controller_create(hid_controller, controller_type, ll); + if (!p) { + U_LOG_IFL_E(ll, "Failed to create WMR controller (Bluetooth)"); + return -1; + } + + *out_xdev = p; + return 1; +} diff --git a/src/xrt/drivers/wmr/wmr_protocol.h b/src/xrt/drivers/wmr/wmr_protocol.h index b9635272a..13901f8a3 100644 --- a/src/xrt/drivers/wmr/wmr_protocol.h +++ b/src/xrt/drivers/wmr/wmr_protocol.h @@ -106,6 +106,17 @@ read16(const unsigned char **buffer) return ret; } +static inline int32_t +read24(const unsigned char **buffer) +{ + // Note: Preserve sign by shifting up to write MSB + int32_t ret = (*(*buffer + 0) << 8) | (*(*buffer + 1) << 16) | (*(*buffer + 2) << 24); + *buffer += 3; + + // restore 24 bit scale again + return ret >> 8; +} + static inline int32_t read32(const unsigned char **buffer) { diff --git a/src/xrt/targets/common/target_lists.c b/src/xrt/targets/common/target_lists.c index 396701610..5f9a9a258 100644 --- a/src/xrt/targets/common/target_lists.c +++ b/src/xrt/targets/common/target_lists.c @@ -129,6 +129,7 @@ struct xrt_prober_entry target_entry_list[] = { #ifdef XRT_BUILD_DRIVER_WMR {MICROSOFT_VID, HOLOLENS_SENSORS_PID, wmr_found, "Microsoft HoloLens Sensors", "wmr"}, + {MICROSOFT_VID, WMR_CONTROLLER_PID, wmr_bt_controller_found, "WMR Bluetooth controller", "wmr"}, #endif // XRT_BUILD_DRIVER_WMR {0x0000, 0x0000, NULL, NULL, NULL}, // Terminate