The open source OpenXR runtime
d/vive: Make common functions available through vive_protocol.h
Move common code to vive_protocol.c
authored by
Christoph Haag
and committed by
Jakob Bornecrantz
04ebc426
427808d0
+277
-208
+1
src/xrt/drivers/CMakeLists.txt
+1
src/xrt/drivers/CMakeLists.txt
+1
src/xrt/drivers/meson.build
+1
src/xrt/drivers/meson.build
+24
-172
src/xrt/drivers/vive/vive_device.c
+24
-172
src/xrt/drivers/vive/vive_device.c
···
518
518
return NULL;
519
519
}
520
520
521
-
522
-
int
523
-
vive_sensors_read_firmware(struct vive_device *d)
524
-
{
525
-
struct vive_firmware_version_report report = {
526
-
.id = VIVE_FIRMWARE_VERSION_REPORT_ID,
527
-
};
528
-
529
-
int ret;
530
-
ret = os_hid_get_feature(d->sensors_dev, report.id, (uint8_t *)&report,
531
-
sizeof(report));
532
-
if (ret < 0)
533
-
return ret;
534
-
535
-
d->firmware.firmware_version = __le32_to_cpu(report.firmware_version);
536
-
d->firmware.hardware_revision = report.hardware_revision;
537
-
538
-
VIVE_DEBUG(d, "Firmware version %u %s@%s FPGA %u.%u",
539
-
d->firmware.firmware_version, report.string1, report.string2,
540
-
report.fpga_version_major, report.fpga_version_minor);
541
-
VIVE_DEBUG(d, "Hardware revision: %d rev %d.%d.%d",
542
-
d->firmware.hardware_revision, report.hardware_version_major,
543
-
report.hardware_version_minor,
544
-
report.hardware_version_micro);
545
-
546
-
return 0;
547
-
}
548
-
549
-
int
550
-
vive_sensors_get_imu_range_report(struct vive_device *d)
551
-
{
552
-
struct vive_imu_range_modes_report report = {
553
-
.id = VIVE_IMU_RANGE_MODES_REPORT_ID};
554
-
555
-
int ret;
556
-
ret = os_hid_get_feature(d->sensors_dev, report.id, (uint8_t *)&report,
557
-
sizeof(report));
558
-
if (ret < 0) {
559
-
printf("Could not get range report!\n");
560
-
return ret;
561
-
}
562
-
563
-
if (!report.gyro_range || !report.accel_range) {
564
-
VIVE_ERROR(
565
-
"Invalid gyroscope and accelerometer data. Trying to fetch "
566
-
"again.");
567
-
ret = os_hid_get_feature(d->sensors_dev, report.id,
568
-
(uint8_t *)&report, sizeof(report));
569
-
if (ret < 0) {
570
-
VIVE_ERROR("Could not get feature report %d.",
571
-
report.id);
572
-
return ret;
573
-
}
574
-
575
-
if (!report.gyro_range || !report.accel_range) {
576
-
VIVE_ERROR(
577
-
"Unexpected range mode report: %02x %02x %02x",
578
-
report.id, report.gyro_range, report.accel_range);
579
-
for (int i = 0; i < 61; i++)
580
-
printf(" %02x", report.unknown[i]);
581
-
printf("\n");
582
-
return -1;
583
-
}
584
-
}
585
-
586
-
if (report.gyro_range > 4 || report.accel_range > 4) {
587
-
VIVE_ERROR("Gyroscope or accelerometer range too large.");
588
-
VIVE_ERROR("Gyroscope: %d", report.gyro_range);
589
-
VIVE_ERROR("Accelerometer: %d", report.accel_range);
590
-
return -1;
591
-
}
592
-
593
-
/*
594
-
* Convert MPU-6500 gyro full scale range (+/-250°/s, +/-500°/s,
595
-
* +/-1000°/s, or +/-2000°/s) into rad/s, accel full scale range
596
-
* (+/-2g, +/-4g, +/-8g, or +/-16g) into m/s².
597
-
*/
598
-
599
-
d->imu.gyro_range = M_PI / 180.0 * (250 << report.gyro_range);
600
-
VIVE_DEBUG(d, "Vive gyroscope range %f", d->imu.gyro_range);
601
-
602
-
d->imu.acc_range = MATH_GRAVITY_M_S2 * (2 << report.accel_range);
603
-
VIVE_DEBUG(d, "Vive accelerometer range %f", d->imu.acc_range);
604
-
605
-
return 0;
606
-
}
607
-
608
-
609
-
void
521
+
static void
610
522
print_vec3(const char *title, struct xrt_vec3 *vec)
611
523
{
612
524
printf("%s = %f %f %f\n", title, (double)vec->x, (double)vec->y,
···
1048
960
return true;
1049
961
}
1050
962
1051
-
char *
1052
-
vive_sensors_read_config(struct vive_device *d)
1053
-
{
1054
-
struct vive_config_start_report start_report = {
1055
-
.id = VIVE_CONFIG_START_REPORT_ID,
1056
-
};
1057
-
1058
-
int ret = os_hid_get_feature_timeout(d->sensors_dev, &start_report,
1059
-
sizeof(start_report), 100);
1060
-
if (ret < 0) {
1061
-
VIVE_ERROR("Could not get config start report.");
1062
-
return NULL;
1063
-
}
1064
-
1065
-
struct vive_config_read_report report = {
1066
-
.id = VIVE_CONFIG_READ_REPORT_ID,
1067
-
};
1068
-
1069
-
unsigned char *config_z = U_TYPED_ARRAY_CALLOC(unsigned char, 4096);
1070
-
1071
-
uint32_t count = 0;
1072
-
do {
1073
-
ret = os_hid_get_feature_timeout(d->sensors_dev, &report,
1074
-
sizeof(report), 100);
1075
-
if (ret < 0) {
1076
-
VIVE_ERROR("Read error after %d bytes: %d", count, ret);
1077
-
free(config_z);
1078
-
return NULL;
1079
-
}
1080
-
1081
-
if (report.len > 62) {
1082
-
VIVE_ERROR("Invalid configuration data at %d", count);
1083
-
free(config_z);
1084
-
return NULL;
1085
-
}
1086
-
1087
-
if (count + report.len > 4096) {
1088
-
VIVE_ERROR("Configuration data too large");
1089
-
free(config_z);
1090
-
return NULL;
1091
-
}
1092
-
1093
-
memcpy(config_z + count, report.payload, report.len);
1094
-
count += report.len;
1095
-
} while (report.len);
1096
-
1097
-
unsigned char *config_json = U_TYPED_ARRAY_CALLOC(unsigned char, 32768);
1098
-
1099
-
z_stream strm = {
1100
-
.next_in = config_z,
1101
-
.avail_in = count,
1102
-
.next_out = config_json,
1103
-
.avail_out = 32768,
1104
-
.zalloc = Z_NULL,
1105
-
.zfree = Z_NULL,
1106
-
.opaque = Z_NULL,
1107
-
};
1108
-
1109
-
ret = inflateInit(&strm);
1110
-
if (ret != Z_OK) {
1111
-
VIVE_ERROR("inflate_init failed: %d", ret);
1112
-
free(config_z);
1113
-
free(config_json);
1114
-
return NULL;
1115
-
}
1116
-
1117
-
ret = inflate(&strm, Z_FINISH);
1118
-
free(config_z);
1119
-
if (ret != Z_STREAM_END) {
1120
-
VIVE_ERROR("Failed to inflate configuration data: %d", ret);
1121
-
free(config_json);
1122
-
return NULL;
1123
-
}
1124
-
1125
-
config_json[strm.total_out] = '\0';
1126
-
1127
-
U_ARRAY_REALLOC_OR_FREE(config_json, unsigned char, strm.total_out + 1);
1128
-
return (char *)config_json;
1129
-
}
1130
-
1131
963
struct vive_device *
1132
964
vive_device_create(struct os_hid_device *mainboard_dev,
1133
965
struct os_hid_device *sensors_dev,
···
1172
1004
vive_mainboard_power_on(d);
1173
1005
vive_mainboard_get_device_info(d);
1174
1006
}
1175
-
vive_sensors_read_firmware(d);
1007
+
vive_read_firmware(d->sensors_dev, &d->firmware.firmware_version,
1008
+
&d->firmware.hardware_revision,
1009
+
&d->firmware.hardware_version_micro,
1010
+
&d->firmware.hardware_version_minor,
1011
+
&d->firmware.hardware_version_major);
1176
1012
1177
-
vive_sensors_get_imu_range_report(d);
1013
+
/*
1014
+
VIVE_DEBUG(d, "Firmware version %u %s@%s FPGA %u.%u",
1015
+
d->firmware.firmware_version, report.string1, report.string2,
1016
+
report.fpga_version_major, report.fpga_version_minor);
1017
+
*/
1178
1018
1179
-
char *config = vive_sensors_read_config(d);
1019
+
VIVE_DEBUG(d, "Firmware version %u", d->firmware.firmware_version);
1020
+
VIVE_DEBUG(d, "Hardware revision: %d rev %d.%d.%d",
1021
+
d->firmware.hardware_revision,
1022
+
d->firmware.hardware_version_major,
1023
+
d->firmware.hardware_version_minor,
1024
+
d->firmware.hardware_version_micro);
1025
+
1026
+
vive_get_imu_range_report(d->sensors_dev, &d->imu.gyro_range,
1027
+
&d->imu.acc_range);
1028
+
VIVE_DEBUG(d, "Vive gyroscope range %f", d->imu.gyro_range);
1029
+
VIVE_DEBUG(d, "Vive accelerometer range %f", d->imu.acc_range);
1030
+
1031
+
char *config = vive_read_config(d->sensors_dev);
1180
1032
if (config != NULL) {
1181
1033
vive_parse_config(d, config);
1182
1034
free(config);
+3
src/xrt/drivers/vive/vive_device.h
+3
src/xrt/drivers/vive/vive_device.h
···
112
112
uint32_t display_firmware_version;
113
113
uint32_t firmware_version;
114
114
uint8_t hardware_revision;
115
+
uint8_t hardware_version_micro;
116
+
uint8_t hardware_version_minor;
117
+
uint8_t hardware_version_major;
115
118
char *mb_serial_number;
116
119
char *model_number;
117
120
char *device_serial_number;
+231
src/xrt/drivers/vive/vive_protocol.c
+231
src/xrt/drivers/vive/vive_protocol.c
···
1
+
// Copyright 2016-2019, Philipp Zabel
2
+
// Copyright 2019, Collabora, Ltd.
3
+
// SPDX-License-Identifier: BSL-1.0
4
+
/*!
5
+
* @file
6
+
* @brief Vive USB HID reports
7
+
* @author Christoph Haag <christoph.haag@collabora.com>
8
+
* @author Lubosz Sarnecki <lubosz.sarnecki@collabora.com>
9
+
* @ingroup drv_vive
10
+
*/
11
+
12
+
#include <stdio.h>
13
+
#include <zlib.h>
14
+
#include <math.h>
15
+
#include "math/m_api.h"
16
+
17
+
#include "vive_protocol.h"
18
+
19
+
#include "util/u_debug.h"
20
+
#include "util/u_misc.h"
21
+
22
+
#define VIVE_ERROR(...) \
23
+
do { \
24
+
fprintf(stderr, "%s - ", __func__); \
25
+
fprintf(stderr, __VA_ARGS__); \
26
+
fprintf(stderr, "\n"); \
27
+
} while (false)
28
+
29
+
const struct vive_headset_power_report power_on_report = {
30
+
.id = VIVE_HEADSET_POWER_REPORT_ID,
31
+
.type = __cpu_to_le16(VIVE_HEADSET_POWER_REPORT_TYPE),
32
+
.len = 56,
33
+
.unknown1 =
34
+
{
35
+
0x01,
36
+
0x00,
37
+
0x00,
38
+
0x00,
39
+
0x00,
40
+
0x00,
41
+
0x02,
42
+
0x00,
43
+
0x01,
44
+
},
45
+
.unknown2 = 0x7a,
46
+
};
47
+
48
+
const struct vive_headset_power_report power_off_report = {
49
+
.id = VIVE_HEADSET_POWER_REPORT_ID,
50
+
.type = __cpu_to_le16(VIVE_HEADSET_POWER_REPORT_TYPE),
51
+
.len = 56,
52
+
.unknown1 =
53
+
{
54
+
0x00,
55
+
0x00,
56
+
0x00,
57
+
0x00,
58
+
0x00,
59
+
0x00,
60
+
0x02,
61
+
0x00,
62
+
0x00,
63
+
},
64
+
.unknown2 = 0x7c,
65
+
};
66
+
67
+
68
+
char *
69
+
vive_read_config(struct os_hid_device *hid_dev)
70
+
{
71
+
struct vive_config_start_report start_report = {
72
+
.id = VIVE_CONFIG_START_REPORT_ID,
73
+
};
74
+
75
+
int ret = os_hid_get_feature_timeout(hid_dev, &start_report,
76
+
sizeof(start_report), 100);
77
+
if (ret < 0) {
78
+
VIVE_ERROR("Could not get config start report.");
79
+
return NULL;
80
+
}
81
+
82
+
struct vive_config_read_report report = {
83
+
.id = VIVE_CONFIG_READ_REPORT_ID,
84
+
};
85
+
86
+
unsigned char *config_z = U_TYPED_ARRAY_CALLOC(unsigned char, 4096);
87
+
88
+
uint32_t count = 0;
89
+
do {
90
+
ret = os_hid_get_feature_timeout(hid_dev, &report,
91
+
sizeof(report), 100);
92
+
if (ret < 0) {
93
+
VIVE_ERROR("Read error after %d bytes: %d", count, ret);
94
+
free(config_z);
95
+
return NULL;
96
+
}
97
+
98
+
if (report.len > 62) {
99
+
VIVE_ERROR("Invalid configuration data at %d", count);
100
+
free(config_z);
101
+
return NULL;
102
+
}
103
+
104
+
if (count + report.len > 4096) {
105
+
VIVE_ERROR("Configuration data too large");
106
+
free(config_z);
107
+
return NULL;
108
+
}
109
+
110
+
memcpy(config_z + count, report.payload, report.len);
111
+
count += report.len;
112
+
} while (report.len);
113
+
114
+
unsigned char *config_json = U_TYPED_ARRAY_CALLOC(unsigned char, 32768);
115
+
116
+
z_stream strm = {
117
+
.next_in = config_z,
118
+
.avail_in = count,
119
+
.next_out = config_json,
120
+
.avail_out = 32768,
121
+
.zalloc = Z_NULL,
122
+
.zfree = Z_NULL,
123
+
.opaque = Z_NULL,
124
+
};
125
+
126
+
ret = inflateInit(&strm);
127
+
if (ret != Z_OK) {
128
+
VIVE_ERROR("inflate_init failed: %d", ret);
129
+
free(config_z);
130
+
free(config_json);
131
+
return NULL;
132
+
}
133
+
134
+
ret = inflate(&strm, Z_FINISH);
135
+
free(config_z);
136
+
if (ret != Z_STREAM_END) {
137
+
VIVE_ERROR("Failed to inflate configuration data: %d", ret);
138
+
free(config_json);
139
+
return NULL;
140
+
}
141
+
142
+
config_json[strm.total_out] = '\0';
143
+
144
+
U_ARRAY_REALLOC_OR_FREE(config_json, unsigned char, strm.total_out + 1);
145
+
return (char *)config_json;
146
+
}
147
+
148
+
int
149
+
vive_get_imu_range_report(struct os_hid_device *hid_dev,
150
+
double *gyro_range,
151
+
double *acc_range)
152
+
{
153
+
struct vive_imu_range_modes_report report = {
154
+
.id = VIVE_IMU_RANGE_MODES_REPORT_ID};
155
+
156
+
int ret;
157
+
158
+
ret = os_hid_get_feature_timeout(hid_dev, &report, sizeof(report), 100);
159
+
if (ret < 0) {
160
+
printf("Could not get range report!\n");
161
+
return ret;
162
+
}
163
+
164
+
if (!report.gyro_range || !report.accel_range) {
165
+
VIVE_ERROR(
166
+
"Invalid gyroscope and accelerometer data. Trying to fetch "
167
+
"again.");
168
+
ret = os_hid_get_feature(hid_dev, report.id, (uint8_t *)&report,
169
+
sizeof(report));
170
+
if (ret < 0) {
171
+
VIVE_ERROR("Could not get feature report %d.",
172
+
report.id);
173
+
return ret;
174
+
}
175
+
176
+
if (!report.gyro_range || !report.accel_range) {
177
+
VIVE_ERROR(
178
+
"Unexpected range mode report: %02x %02x %02x",
179
+
report.id, report.gyro_range, report.accel_range);
180
+
for (int i = 0; i < 61; i++)
181
+
printf(" %02x", report.unknown[i]);
182
+
printf("\n");
183
+
return -1;
184
+
}
185
+
}
186
+
187
+
if (report.gyro_range > 4 || report.accel_range > 4) {
188
+
VIVE_ERROR("Gyroscope or accelerometer range too large.");
189
+
VIVE_ERROR("Gyroscope: %d", report.gyro_range);
190
+
VIVE_ERROR("Accelerometer: %d", report.accel_range);
191
+
return -1;
192
+
}
193
+
194
+
/*
195
+
* Convert MPU-6500 gyro full scale range (+/-250°/s, +/-500°/s,
196
+
* +/-1000°/s, or +/-2000°/s) into rad/s, accel full scale range
197
+
* (+/-2g, +/-4g, +/-8g, or +/-16g) into m/s².
198
+
*/
199
+
200
+
*gyro_range = M_PI / 180.0 * (250 << report.gyro_range);
201
+
*acc_range = MATH_GRAVITY_M_S2 * (2 << report.accel_range);
202
+
203
+
return 0;
204
+
}
205
+
206
+
int
207
+
vive_read_firmware(struct os_hid_device *hid_dev,
208
+
uint32_t *firmware_version,
209
+
uint8_t *hardware_revision,
210
+
uint8_t *hardware_version_micro,
211
+
uint8_t *hardware_version_minor,
212
+
uint8_t *hardware_version_major)
213
+
{
214
+
struct vive_firmware_version_report report = {
215
+
.id = VIVE_FIRMWARE_VERSION_REPORT_ID,
216
+
};
217
+
218
+
int ret;
219
+
ret = os_hid_get_feature(hid_dev, report.id, (uint8_t *)&report,
220
+
sizeof(report));
221
+
if (ret < 0)
222
+
return ret;
223
+
224
+
*firmware_version = __le32_to_cpu(report.firmware_version);
225
+
*hardware_revision = report.hardware_revision;
226
+
*hardware_version_major = report.hardware_version_major;
227
+
*hardware_version_minor = report.hardware_version_minor;
228
+
*hardware_version_micro = report.hardware_version_micro;
229
+
230
+
return 0;
231
+
}
+17
-36
src/xrt/drivers/vive/vive_protocol.h
+17
-36
src/xrt/drivers/vive/vive_protocol.h
···
12
12
13
13
#include <asm/byteorder.h>
14
14
#include <stdint.h>
15
+
#include "os/os_hid.h"
15
16
16
17
#define VIVE_CONTROLLER_BUTTON_REPORT_ID 0x01
17
18
···
246
247
uint8_t magic[4];
247
248
} __attribute__((packed));
248
249
250
+
const struct vive_headset_power_report power_on_report;
251
+
const struct vive_headset_power_report power_off_report;
249
252
250
-
const struct vive_headset_power_report power_on_report = {
251
-
.id = VIVE_HEADSET_POWER_REPORT_ID,
252
-
.type = __cpu_to_le16(VIVE_HEADSET_POWER_REPORT_TYPE),
253
-
.len = 56,
254
-
.unknown1 =
255
-
{
256
-
0x01,
257
-
0x00,
258
-
0x00,
259
-
0x00,
260
-
0x00,
261
-
0x00,
262
-
0x02,
263
-
0x00,
264
-
0x01,
265
-
},
266
-
.unknown2 = 0x7a,
267
-
};
253
+
char *
254
+
vive_read_config(struct os_hid_device *hid_dev);
268
255
269
-
const struct vive_headset_power_report power_off_report = {
270
-
.id = VIVE_HEADSET_POWER_REPORT_ID,
271
-
.type = __cpu_to_le16(VIVE_HEADSET_POWER_REPORT_TYPE),
272
-
.len = 56,
273
-
.unknown1 =
274
-
{
275
-
0x00,
276
-
0x00,
277
-
0x00,
278
-
0x00,
279
-
0x00,
280
-
0x00,
281
-
0x02,
282
-
0x00,
283
-
0x00,
284
-
},
285
-
.unknown2 = 0x7c,
286
-
};
256
+
int
257
+
vive_get_imu_range_report(struct os_hid_device *hid_dev,
258
+
double *gyro_range,
259
+
double *acc_range);
260
+
261
+
int
262
+
vive_read_firmware(struct os_hid_device *hid_dev,
263
+
uint32_t *firmware_version,
264
+
uint8_t *hardware_revision,
265
+
uint8_t *hardware_version_micro,
266
+
uint8_t *hardware_version_minor,
267
+
uint8_t *hardware_version_major);