From 2ed3109c95f5adae738912dd3b5c764cc41422e9 Mon Sep 17 00:00:00 2001 From: Lubosz Sarnecki Date: Tue, 22 Oct 2019 18:13:39 +0200 Subject: [PATCH] d/vive: Add initial support for Vive Pro. --- src/xrt/drivers/vive/vive_device.c | 36 +++++++++++--- src/xrt/drivers/vive/vive_prober.c | 71 +++++++++++++++++++++++++++ src/xrt/targets/common/target_lists.c | 1 + 3 files changed, 102 insertions(+), 6 deletions(-) diff --git a/src/xrt/drivers/vive/vive_device.c b/src/xrt/drivers/vive/vive_device.c index eb5339453..8244cd4e6 100644 --- a/src/xrt/drivers/vive/vive_device.c +++ b/src/xrt/drivers/vive/vive_device.c @@ -337,8 +337,17 @@ update_imu(struct vive_device *d, struct vive_imu_report *report) VIVE_SPEW(d, "GYRO %f %f %f", angular_velocity.x, angular_velocity.y, angular_velocity.z); - // flip x axis - angular_velocity.x = -angular_velocity.x; + switch (d->variant) { + case VIVE_VARIANT_VIVE: + // flip x axis + angular_velocity.x = -angular_velocity.x; + break; + case VIVE_VARIANT_PRO: + // flip y axis + angular_velocity.y = -angular_velocity.y; + break; + default: VIVE_ERROR("Unhandled Vive variant\n"); return; + } math_quat_integrate_velocity( &d->rot_filtered, &angular_velocity, @@ -488,6 +497,8 @@ vive_sensros_get_imu_range_report(struct vive_device *d) VIVE_ERROR("Gyroscope or accelerometer range too large."); VIVE_ERROR("Gyroscope: %d", report.gyro_range); VIVE_ERROR("Aaccelerometer: %d", report.accel_range); + d->imu.gyro_range = 8.726646f; + d->imu.acc_range = 39.226600f; return -1; } @@ -640,10 +651,23 @@ vive_parse_config(struct vive_device *d, char *json_string) const nx_json *json = nx_json_parse(json_string, 0); if (json) { - _json_get_vec3(json, "acc_bias", &d->imu.acc_bias); - _json_get_vec3(json, "acc_scale", &d->imu.acc_scale); - _json_get_vec3(json, "gyro_bias", &d->imu.gyro_bias); - _json_get_vec3(json, "gyro_scale", &d->imu.gyro_scale); + + switch (d->variant) { + case VIVE_VARIANT_VIVE: + _json_get_vec3(json, "acc_bias", &d->imu.acc_bias); + _json_get_vec3(json, "acc_scale", &d->imu.acc_scale); + _json_get_vec3(json, "gyro_bias", &d->imu.gyro_bias); + _json_get_vec3(json, "gyro_scale", &d->imu.gyro_scale); + break; + case VIVE_VARIANT_PRO: { + const nx_json *imu = nx_json_get(json, "imu"); + _json_get_vec3(imu, "acc_bias", &d->imu.acc_bias); + _json_get_vec3(imu, "acc_scale", &d->imu.acc_scale); + _json_get_vec3(imu, "gyro_bias", &d->imu.gyro_bias); + _json_get_vec3(imu, "gyro_scale", &d->imu.gyro_scale); + } break; + default: VIVE_ERROR("Unknown Vive variant.\n"); return false; + } d->firmware.model_number = _json_get_string(json, "model_number"); diff --git a/src/xrt/drivers/vive/vive_prober.c b/src/xrt/drivers/vive/vive_prober.c index 6d6737582..76cf01b9f 100644 --- a/src/xrt/drivers/vive/vive_prober.c +++ b/src/xrt/drivers/vive/vive_prober.c @@ -16,6 +16,7 @@ #include "vive_prober.h" static const char VIVE_PRODUCT_STRING[] = "HTC Vive"; +static const char VIVE_PRO_PRODUCT_STRING[] = "VIVE Pro"; static const char VIVE_MANUFACTURER_STRING[] = "HTC"; DEBUG_GET_ONCE_BOOL_OPTION(vive_debug, "VIVE_PRINT_DEBUG", false) @@ -111,6 +112,73 @@ init_vive1(struct xrt_prober *xp, return 1; } +static int +init_vive_pro(struct xrt_prober *xp, + struct xrt_prober_device *dev, + struct xrt_prober_device **devices, + size_t num_devices, + bool print_debug, + struct xrt_device **out_xdev) +{ + if (print_debug) + _print_device_info(xp, dev); + + if (!xrt_prober_match_string(xp, dev, XRT_PROBER_STRING_MANUFACTURER, + VIVE_MANUFACTURER_STRING) || + !xrt_prober_match_string(xp, dev, XRT_PROBER_STRING_PRODUCT, + VIVE_PRO_PRODUCT_STRING)) { + if (print_debug) + printf("Vive Pro manufactuer string did not match.\n"); + return -1; + } + + struct os_hid_device *sensors_dev = NULL; + for (uint32_t i = 0; i < num_devices; i++) { + struct xrt_prober_device *d = devices[i]; + + if (d->vendor_id != VALVE_VID && + d->product_id != VIVE_PRO_LHR_PID) + continue; + + if (print_debug) + _print_device_info(xp, d); + + int result = + xrt_prober_open_hid_interface(xp, d, 0, &sensors_dev); + if (result != 0) { + VIVE_ERROR("Could not open Vive sensors device."); + return -1; + } + break; + } + + if (sensors_dev == NULL) { + VIVE_ERROR("Could not find Vive Pro sensors device."); + return -1; + } + + struct os_hid_device *mainboard_dev = NULL; + + int result = xrt_prober_open_hid_interface(xp, dev, 0, &mainboard_dev); + if (result != 0) { + VIVE_ERROR("Could not open Vive mainboard device."); + free(sensors_dev); + return -1; + } + struct vive_device *d = + vive_device_create(mainboard_dev, sensors_dev, VIVE_VARIANT_PRO); + if (d == NULL) { + free(sensors_dev); + free(mainboard_dev); + return -1; + } + + *out_xdev = &d->base; + + return 1; +} + + int vive_found(struct xrt_prober *xp, struct xrt_prober_device **devices, @@ -134,6 +202,9 @@ vive_found(struct xrt_prober *xp, case VIVE_PID: return init_vive1(xp, dev, devices, num_devices, print_debug, out_xdev); + case VIVE_PRO_MAINBOARD_PID: + return init_vive_pro(xp, dev, devices, num_devices, print_debug, + out_xdev); default: VIVE_ERROR("No product ids matched %.4x\n", dev->product_id); return -1; diff --git a/src/xrt/targets/common/target_lists.c b/src/xrt/targets/common/target_lists.c index 8b6f49f8b..9b76b9b24 100644 --- a/src/xrt/targets/common/target_lists.c +++ b/src/xrt/targets/common/target_lists.c @@ -67,6 +67,7 @@ struct xrt_prober_entry target_entry_list[] = { #ifdef XRT_BUILD_DRIVER_VIVE {HTC_VID, VIVE_PID, vive_found, "HTC Vive"}, + {HTC_VID, VIVE_PRO_MAINBOARD_PID, vive_found, "HTC Vive Pro"}, #endif {0x0000, 0x0000, NULL, NULL}, // Terminate