mirror of
https://gitlab.freedesktop.org/monado/monado.git
synced 2024-12-29 11:06:18 +00:00
d/vive: Make common functions available through vive_protocol.h
Move common code to vive_protocol.c
This commit is contained in:
Christoph Haag
Jakob Bornecrantz
parent
427808d09a
commit
04ebc42666
|
|
@ -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
|
||||
)
|
||||
|
||||
|
|
|
|||
|
|
@ -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',
|
||||
|
|
|
|||
|
|
@ -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);
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
|
|
|
|||
231
src/xrt/drivers/vive/vive_protocol.c
Normal file
231
src/xrt/drivers/vive/vive_protocol.c
Normal file
|
|
@ -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 <christoph.haag@collabora.com>
|
||||
* @author Lubosz Sarnecki <lubosz.sarnecki@collabora.com>
|
||||
* @ingroup drv_vive
|
||||
*/
|
||||
|
||||
#include <stdio.h>
|
||||
#include <zlib.h>
|
||||
#include <math.h>
|
||||
#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;
|
||||
}
|
||||
|
|
@ -12,6 +12,7 @@
|
|||
|
||||
#include <asm/byteorder.h>
|
||||
#include <stdint.h>
|
||||
#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);
|
||||
|
|
|
|||
Loading…
Reference in a new issue