From 04ebc426663d9d28be0857f17c816f430f654eda Mon Sep 17 00:00:00 2001 From: Christoph Haag Date: Tue, 14 Apr 2020 16:18:58 +0200 Subject: [PATCH] d/vive: Make common functions available through vive_protocol.h Move common code to vive_protocol.c --- src/xrt/drivers/CMakeLists.txt | 1 + src/xrt/drivers/meson.build | 1 + src/xrt/drivers/vive/vive_device.c | 196 +++-------------------- src/xrt/drivers/vive/vive_device.h | 3 + src/xrt/drivers/vive/vive_protocol.c | 231 +++++++++++++++++++++++++++ src/xrt/drivers/vive/vive_protocol.h | 53 ++---- 6 files changed, 277 insertions(+), 208 deletions(-) create mode 100644 src/xrt/drivers/vive/vive_protocol.c diff --git a/src/xrt/drivers/CMakeLists.txt b/src/xrt/drivers/CMakeLists.txt index b8a15c3bc..d0afbfebc 100644 --- a/src/xrt/drivers/CMakeLists.txt +++ b/src/xrt/drivers/CMakeLists.txt @@ -139,6 +139,7 @@ if(BUILD_DRIVER_VIVE) vive/vive_device.c vive/vive_prober.h vive/vive_prober.c + vive/vive_protocol.c vive/vive_protocol.h ) diff --git a/src/xrt/drivers/meson.build b/src/xrt/drivers/meson.build index 6f4bf685f..bae8b9033 100644 --- a/src/xrt/drivers/meson.build +++ b/src/xrt/drivers/meson.build @@ -119,6 +119,7 @@ lib_drv_vive = static_library( files( 'vive/vive_device.c', 'vive/vive_device.h', + 'vive/vive_protocol.c', 'vive/vive_protocol.h', 'vive/vive_prober.h', 'vive/vive_prober.c', diff --git a/src/xrt/drivers/vive/vive_device.c b/src/xrt/drivers/vive/vive_device.c index 2f4386612..5c79f5be8 100644 --- a/src/xrt/drivers/vive/vive_device.c +++ b/src/xrt/drivers/vive/vive_device.c @@ -518,95 +518,7 @@ vive_sensors_run_thread(void *ptr) return NULL; } - -int -vive_sensors_read_firmware(struct vive_device *d) -{ - struct vive_firmware_version_report report = { - .id = VIVE_FIRMWARE_VERSION_REPORT_ID, - }; - - int ret; - ret = os_hid_get_feature(d->sensors_dev, report.id, (uint8_t *)&report, - sizeof(report)); - if (ret < 0) - return ret; - - d->firmware.firmware_version = __le32_to_cpu(report.firmware_version); - d->firmware.hardware_revision = report.hardware_revision; - - VIVE_DEBUG(d, "Firmware version %u %s@%s FPGA %u.%u", - d->firmware.firmware_version, report.string1, report.string2, - report.fpga_version_major, report.fpga_version_minor); - VIVE_DEBUG(d, "Hardware revision: %d rev %d.%d.%d", - d->firmware.hardware_revision, report.hardware_version_major, - report.hardware_version_minor, - report.hardware_version_micro); - - return 0; -} - -int -vive_sensors_get_imu_range_report(struct vive_device *d) -{ - struct vive_imu_range_modes_report report = { - .id = VIVE_IMU_RANGE_MODES_REPORT_ID}; - - int ret; - ret = os_hid_get_feature(d->sensors_dev, report.id, (uint8_t *)&report, - sizeof(report)); - if (ret < 0) { - printf("Could not get range report!\n"); - return ret; - } - - if (!report.gyro_range || !report.accel_range) { - VIVE_ERROR( - "Invalid gyroscope and accelerometer data. Trying to fetch " - "again."); - ret = os_hid_get_feature(d->sensors_dev, report.id, - (uint8_t *)&report, sizeof(report)); - if (ret < 0) { - VIVE_ERROR("Could not get feature report %d.", - report.id); - return ret; - } - - if (!report.gyro_range || !report.accel_range) { - VIVE_ERROR( - "Unexpected range mode report: %02x %02x %02x", - report.id, report.gyro_range, report.accel_range); - for (int i = 0; i < 61; i++) - printf(" %02x", report.unknown[i]); - printf("\n"); - return -1; - } - } - - if (report.gyro_range > 4 || report.accel_range > 4) { - VIVE_ERROR("Gyroscope or accelerometer range too large."); - VIVE_ERROR("Gyroscope: %d", report.gyro_range); - VIVE_ERROR("Accelerometer: %d", report.accel_range); - return -1; - } - - /* - * Convert MPU-6500 gyro full scale range (+/-250°/s, +/-500°/s, - * +/-1000°/s, or +/-2000°/s) into rad/s, accel full scale range - * (+/-2g, +/-4g, +/-8g, or +/-16g) into m/s². - */ - - d->imu.gyro_range = M_PI / 180.0 * (250 << report.gyro_range); - VIVE_DEBUG(d, "Vive gyroscope range %f", d->imu.gyro_range); - - d->imu.acc_range = MATH_GRAVITY_M_S2 * (2 << report.accel_range); - VIVE_DEBUG(d, "Vive accelerometer range %f", d->imu.acc_range); - - return 0; -} - - -void +static void print_vec3(const char *title, struct xrt_vec3 *vec) { printf("%s = %f %f %f\n", title, (double)vec->x, (double)vec->y, @@ -1048,86 +960,6 @@ vive_parse_config(struct vive_device *d, char *json_string) return true; } -char * -vive_sensors_read_config(struct vive_device *d) -{ - struct vive_config_start_report start_report = { - .id = VIVE_CONFIG_START_REPORT_ID, - }; - - int ret = os_hid_get_feature_timeout(d->sensors_dev, &start_report, - sizeof(start_report), 100); - if (ret < 0) { - VIVE_ERROR("Could not get config start report."); - return NULL; - } - - struct vive_config_read_report report = { - .id = VIVE_CONFIG_READ_REPORT_ID, - }; - - unsigned char *config_z = U_TYPED_ARRAY_CALLOC(unsigned char, 4096); - - uint32_t count = 0; - do { - ret = os_hid_get_feature_timeout(d->sensors_dev, &report, - sizeof(report), 100); - if (ret < 0) { - VIVE_ERROR("Read error after %d bytes: %d", count, ret); - free(config_z); - return NULL; - } - - if (report.len > 62) { - VIVE_ERROR("Invalid configuration data at %d", count); - free(config_z); - return NULL; - } - - if (count + report.len > 4096) { - VIVE_ERROR("Configuration data too large"); - free(config_z); - return NULL; - } - - memcpy(config_z + count, report.payload, report.len); - count += report.len; - } while (report.len); - - unsigned char *config_json = U_TYPED_ARRAY_CALLOC(unsigned char, 32768); - - z_stream strm = { - .next_in = config_z, - .avail_in = count, - .next_out = config_json, - .avail_out = 32768, - .zalloc = Z_NULL, - .zfree = Z_NULL, - .opaque = Z_NULL, - }; - - ret = inflateInit(&strm); - if (ret != Z_OK) { - VIVE_ERROR("inflate_init failed: %d", ret); - free(config_z); - free(config_json); - return NULL; - } - - ret = inflate(&strm, Z_FINISH); - free(config_z); - if (ret != Z_STREAM_END) { - VIVE_ERROR("Failed to inflate configuration data: %d", ret); - free(config_json); - return NULL; - } - - config_json[strm.total_out] = '\0'; - - U_ARRAY_REALLOC_OR_FREE(config_json, unsigned char, strm.total_out + 1); - return (char *)config_json; -} - struct vive_device * vive_device_create(struct os_hid_device *mainboard_dev, struct os_hid_device *sensors_dev, @@ -1172,11 +1004,31 @@ vive_device_create(struct os_hid_device *mainboard_dev, vive_mainboard_power_on(d); vive_mainboard_get_device_info(d); } - vive_sensors_read_firmware(d); + vive_read_firmware(d->sensors_dev, &d->firmware.firmware_version, + &d->firmware.hardware_revision, + &d->firmware.hardware_version_micro, + &d->firmware.hardware_version_minor, + &d->firmware.hardware_version_major); - vive_sensors_get_imu_range_report(d); + /* + VIVE_DEBUG(d, "Firmware version %u %s@%s FPGA %u.%u", + d->firmware.firmware_version, report.string1, report.string2, + report.fpga_version_major, report.fpga_version_minor); + */ - char *config = vive_sensors_read_config(d); + VIVE_DEBUG(d, "Firmware version %u", d->firmware.firmware_version); + VIVE_DEBUG(d, "Hardware revision: %d rev %d.%d.%d", + d->firmware.hardware_revision, + d->firmware.hardware_version_major, + d->firmware.hardware_version_minor, + d->firmware.hardware_version_micro); + + vive_get_imu_range_report(d->sensors_dev, &d->imu.gyro_range, + &d->imu.acc_range); + VIVE_DEBUG(d, "Vive gyroscope range %f", d->imu.gyro_range); + VIVE_DEBUG(d, "Vive accelerometer range %f", d->imu.acc_range); + + char *config = vive_read_config(d->sensors_dev); if (config != NULL) { vive_parse_config(d, config); free(config); diff --git a/src/xrt/drivers/vive/vive_device.h b/src/xrt/drivers/vive/vive_device.h index a0443a5d2..0923eae87 100644 --- a/src/xrt/drivers/vive/vive_device.h +++ b/src/xrt/drivers/vive/vive_device.h @@ -112,6 +112,9 @@ struct vive_device uint32_t display_firmware_version; uint32_t firmware_version; uint8_t hardware_revision; + uint8_t hardware_version_micro; + uint8_t hardware_version_minor; + uint8_t hardware_version_major; char *mb_serial_number; char *model_number; char *device_serial_number; diff --git a/src/xrt/drivers/vive/vive_protocol.c b/src/xrt/drivers/vive/vive_protocol.c new file mode 100644 index 000000000..c6fa7fb8e --- /dev/null +++ b/src/xrt/drivers/vive/vive_protocol.c @@ -0,0 +1,231 @@ +// Copyright 2016-2019, Philipp Zabel +// Copyright 2019, Collabora, Ltd. +// SPDX-License-Identifier: BSL-1.0 +/*! + * @file + * @brief Vive USB HID reports + * @author Christoph Haag + * @author Lubosz Sarnecki + * @ingroup drv_vive + */ + +#include +#include +#include +#include "math/m_api.h" + +#include "vive_protocol.h" + +#include "util/u_debug.h" +#include "util/u_misc.h" + +#define VIVE_ERROR(...) \ + do { \ + fprintf(stderr, "%s - ", __func__); \ + fprintf(stderr, __VA_ARGS__); \ + fprintf(stderr, "\n"); \ + } while (false) + +const struct vive_headset_power_report power_on_report = { + .id = VIVE_HEADSET_POWER_REPORT_ID, + .type = __cpu_to_le16(VIVE_HEADSET_POWER_REPORT_TYPE), + .len = 56, + .unknown1 = + { + 0x01, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x02, + 0x00, + 0x01, + }, + .unknown2 = 0x7a, +}; + +const struct vive_headset_power_report power_off_report = { + .id = VIVE_HEADSET_POWER_REPORT_ID, + .type = __cpu_to_le16(VIVE_HEADSET_POWER_REPORT_TYPE), + .len = 56, + .unknown1 = + { + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x02, + 0x00, + 0x00, + }, + .unknown2 = 0x7c, +}; + + +char * +vive_read_config(struct os_hid_device *hid_dev) +{ + struct vive_config_start_report start_report = { + .id = VIVE_CONFIG_START_REPORT_ID, + }; + + int ret = os_hid_get_feature_timeout(hid_dev, &start_report, + sizeof(start_report), 100); + if (ret < 0) { + VIVE_ERROR("Could not get config start report."); + return NULL; + } + + struct vive_config_read_report report = { + .id = VIVE_CONFIG_READ_REPORT_ID, + }; + + unsigned char *config_z = U_TYPED_ARRAY_CALLOC(unsigned char, 4096); + + uint32_t count = 0; + do { + ret = os_hid_get_feature_timeout(hid_dev, &report, + sizeof(report), 100); + if (ret < 0) { + VIVE_ERROR("Read error after %d bytes: %d", count, ret); + free(config_z); + return NULL; + } + + if (report.len > 62) { + VIVE_ERROR("Invalid configuration data at %d", count); + free(config_z); + return NULL; + } + + if (count + report.len > 4096) { + VIVE_ERROR("Configuration data too large"); + free(config_z); + return NULL; + } + + memcpy(config_z + count, report.payload, report.len); + count += report.len; + } while (report.len); + + unsigned char *config_json = U_TYPED_ARRAY_CALLOC(unsigned char, 32768); + + z_stream strm = { + .next_in = config_z, + .avail_in = count, + .next_out = config_json, + .avail_out = 32768, + .zalloc = Z_NULL, + .zfree = Z_NULL, + .opaque = Z_NULL, + }; + + ret = inflateInit(&strm); + if (ret != Z_OK) { + VIVE_ERROR("inflate_init failed: %d", ret); + free(config_z); + free(config_json); + return NULL; + } + + ret = inflate(&strm, Z_FINISH); + free(config_z); + if (ret != Z_STREAM_END) { + VIVE_ERROR("Failed to inflate configuration data: %d", ret); + free(config_json); + return NULL; + } + + config_json[strm.total_out] = '\0'; + + U_ARRAY_REALLOC_OR_FREE(config_json, unsigned char, strm.total_out + 1); + return (char *)config_json; +} + +int +vive_get_imu_range_report(struct os_hid_device *hid_dev, + double *gyro_range, + double *acc_range) +{ + struct vive_imu_range_modes_report report = { + .id = VIVE_IMU_RANGE_MODES_REPORT_ID}; + + int ret; + + ret = os_hid_get_feature_timeout(hid_dev, &report, sizeof(report), 100); + if (ret < 0) { + printf("Could not get range report!\n"); + return ret; + } + + if (!report.gyro_range || !report.accel_range) { + VIVE_ERROR( + "Invalid gyroscope and accelerometer data. Trying to fetch " + "again."); + ret = os_hid_get_feature(hid_dev, report.id, (uint8_t *)&report, + sizeof(report)); + if (ret < 0) { + VIVE_ERROR("Could not get feature report %d.", + report.id); + return ret; + } + + if (!report.gyro_range || !report.accel_range) { + VIVE_ERROR( + "Unexpected range mode report: %02x %02x %02x", + report.id, report.gyro_range, report.accel_range); + for (int i = 0; i < 61; i++) + printf(" %02x", report.unknown[i]); + printf("\n"); + return -1; + } + } + + if (report.gyro_range > 4 || report.accel_range > 4) { + VIVE_ERROR("Gyroscope or accelerometer range too large."); + VIVE_ERROR("Gyroscope: %d", report.gyro_range); + VIVE_ERROR("Accelerometer: %d", report.accel_range); + return -1; + } + + /* + * Convert MPU-6500 gyro full scale range (+/-250°/s, +/-500°/s, + * +/-1000°/s, or +/-2000°/s) into rad/s, accel full scale range + * (+/-2g, +/-4g, +/-8g, or +/-16g) into m/s². + */ + + *gyro_range = M_PI / 180.0 * (250 << report.gyro_range); + *acc_range = MATH_GRAVITY_M_S2 * (2 << report.accel_range); + + return 0; +} + +int +vive_read_firmware(struct os_hid_device *hid_dev, + uint32_t *firmware_version, + uint8_t *hardware_revision, + uint8_t *hardware_version_micro, + uint8_t *hardware_version_minor, + uint8_t *hardware_version_major) +{ + struct vive_firmware_version_report report = { + .id = VIVE_FIRMWARE_VERSION_REPORT_ID, + }; + + int ret; + ret = os_hid_get_feature(hid_dev, report.id, (uint8_t *)&report, + sizeof(report)); + if (ret < 0) + return ret; + + *firmware_version = __le32_to_cpu(report.firmware_version); + *hardware_revision = report.hardware_revision; + *hardware_version_major = report.hardware_version_major; + *hardware_version_minor = report.hardware_version_minor; + *hardware_version_micro = report.hardware_version_micro; + + return 0; +} diff --git a/src/xrt/drivers/vive/vive_protocol.h b/src/xrt/drivers/vive/vive_protocol.h index 82689fc9a..73a2caed0 100644 --- a/src/xrt/drivers/vive/vive_protocol.h +++ b/src/xrt/drivers/vive/vive_protocol.h @@ -12,6 +12,7 @@ #include #include +#include "os/os_hid.h" #define VIVE_CONTROLLER_BUTTON_REPORT_ID 0x01 @@ -246,41 +247,21 @@ struct vive_controller_poweroff_report uint8_t magic[4]; } __attribute__((packed)); +const struct vive_headset_power_report power_on_report; +const struct vive_headset_power_report power_off_report; -const struct vive_headset_power_report power_on_report = { - .id = VIVE_HEADSET_POWER_REPORT_ID, - .type = __cpu_to_le16(VIVE_HEADSET_POWER_REPORT_TYPE), - .len = 56, - .unknown1 = - { - 0x01, - 0x00, - 0x00, - 0x00, - 0x00, - 0x00, - 0x02, - 0x00, - 0x01, - }, - .unknown2 = 0x7a, -}; +char * +vive_read_config(struct os_hid_device *hid_dev); -const struct vive_headset_power_report power_off_report = { - .id = VIVE_HEADSET_POWER_REPORT_ID, - .type = __cpu_to_le16(VIVE_HEADSET_POWER_REPORT_TYPE), - .len = 56, - .unknown1 = - { - 0x00, - 0x00, - 0x00, - 0x00, - 0x00, - 0x00, - 0x02, - 0x00, - 0x00, - }, - .unknown2 = 0x7c, -}; +int +vive_get_imu_range_report(struct os_hid_device *hid_dev, + double *gyro_range, + double *acc_range); + +int +vive_read_firmware(struct os_hid_device *hid_dev, + uint32_t *firmware_version, + uint8_t *hardware_revision, + uint8_t *hardware_version_micro, + uint8_t *hardware_version_minor, + uint8_t *hardware_version_major);