1 // Copyright 2013, Fredrik Hultin.
2 // Copyright 2013, Jakob Bornecrantz.
3 // Copyright 2013, Joey Ferwerda.
4 // SPDX-License-Identifier: BSL-1.0
5 /*
6 * OpenHMD - Free and Open Source API and drivers for immersive technology.
7 */
8
9 /* HTC Vive Driver */
10
11
12 #define FEATURE_BUFFER_SIZE 256
13
14 #define HTC_ID 0x0bb4
15 #define VIVE_HMD 0x2c87
16 #define VIVE_PRO_HMD 0x0309
17
18 #define VALVE_ID 0x28de
19 #define VIVE_WATCHMAN_DONGLE 0x2101
20 #define VIVE_LIGHTHOUSE_FPGA_RX 0x2000
read8(const unsigned char ** buffer)21 #define VIVE_LHR 0x2300 // VIVE PRO
22
23 #define VIVE_CLOCK_FREQ 48000000.0f // Hz = 48 MHz
24
25 #include <string.h>
26 #include <wchar.h>
27 #include <hidapi.h>
28 #include <assert.h>
29 #include <limits.h>
30 #include <stdint.h>
31 #include <stdbool.h>
32
33 #include "vive.h"
34
35 typedef enum {
36 REV_VIVE,
37 REV_VIVE_PRO
38 } vive_revision;
39
40 typedef struct {
41 ohmd_device base;
42
43 hid_device* hmd_handle;
vive_decode_sensor_packet(vive_headset_imu_packet * pkt,const unsigned char * buffer,int size)44 hid_device* imu_handle;
45 fusion sensor_fusion;
46 vec3f raw_accel, raw_gyro;
47 uint32_t last_ticks;
48 uint8_t last_seq;
49
50 vec3f gyro_error;
51 filter_queue gyro_q;
52
53 vive_revision revision;
54
55 vive_imu_config imu_config;
56
57 } vive_priv;
58
59 void vec3f_from_vive_vec_accel(const vive_imu_config* config,
60 const int16_t* smp,
61 vec3f* out)
62 {
63 float range = config->acc_range / 32768.0f;
64 out->x = range * config->acc_scale.x * (float) smp[0] - config->acc_bias.x;
65 out->y = range * config->acc_scale.y * (float) smp[1] - config->acc_bias.y;
66 out->z = range * config->acc_scale.z * (float) smp[2] - config->acc_bias.z;
67 }
68
69 void vec3f_from_vive_vec_gyro(const vive_imu_config* config,
70 const int16_t* smp,
71 vec3f* out)
72 {
trim(const char * src,char * buff,const unsigned int sizeBuff)73 float range = config->gyro_range / 32768.0f;
74 out->x = range * config->gyro_scale.x * (float)smp[0] - config->gyro_bias.x;
75 out->y = range * config->gyro_scale.y * (float)smp[1] - config->gyro_bias.x;
76 out->z = range * config->gyro_scale.z * (float)smp[2] - config->gyro_bias.x;
77 }
78
79 static bool process_error(vive_priv* priv)
80 {
81 if(priv->gyro_q.at >= priv->gyro_q.size - 1)
82 return true;
83
84 ofq_add(&priv->gyro_q, &priv->raw_gyro);
85
86 if(priv->gyro_q.at >= priv->gyro_q.size - 1){
87 ofq_get_mean(&priv->gyro_q, &priv->gyro_error);
88 LOGE("gyro error: %f, %f, %f\n",
get_vec3f_from_json(const nx_json * json,const char * name,vec3f * result)89 priv->gyro_error.x, priv->gyro_error.y, priv->gyro_error.z);
90 }
91
92 return false;
93 }
94
95 vive_headset_imu_sample* get_next_sample(vive_headset_imu_packet* pkt,
96 int last_seq)
97 {
98 int diff[3];
print_vec3f(const char * title,vec3f * vec)99
100 for(int i = 0; i < 3; i++)
101 {
102 diff[i] = (int)pkt->samples[i].seq - last_seq;
103
vive_decode_config_packet(vive_imu_config * result,const unsigned char * buffer,uint16_t size)104 if(diff[i] < -128){
105 diff[i] += 256;
106 }
107 }
108
109 int closest_diff = INT_MAX;
110 int closest_idx = -1;
111
112 for(int i = 0; i < 3; i++)
113 {
114 if(diff[i] < closest_diff && diff[i] > 0 && diff[i] < 128){
115 closest_diff = diff[i];
116 closest_idx = i;
117 }
118 }
119
120 if(closest_idx != -1)
121 return pkt->samples + closest_idx;
122
123 return NULL;
124 }
125
126 static void handle_imu_packet(vive_priv* priv, unsigned char *buffer, int size)
127 {
128 vive_headset_imu_packet pkt;
129 vive_decode_sensor_packet(&pkt, buffer, size);
130
131 vive_headset_imu_sample* smp = NULL;
132
133 while((smp = get_next_sample(&pkt, priv->last_seq)) != NULL)
134 {
135 if(priv->last_ticks == 0)
136 priv->last_ticks = smp->time_ticks;
137
138 uint32_t t1, t2;
139 t1 = smp->time_ticks;
140 t2 = priv->last_ticks;
141
142 float dt = (t1 - t2) / VIVE_CLOCK_FREQ;
143
144 priv->last_ticks = smp->time_ticks;
145
146 vec3f_from_vive_vec_accel(&priv->imu_config, smp->acc, &priv->raw_accel);
147 vec3f_from_vive_vec_gyro(&priv->imu_config, smp->rot, &priv->raw_gyro);
148
149 // Fix imu orientation
150 switch (priv->revision) {
151 case REV_VIVE:
152 priv->raw_accel.y *= -1;
153 priv->raw_accel.z *= -1;
154 priv->raw_gyro.y *= -1;
155 priv->raw_gyro.z *= -1;
156 break;
157 case REV_VIVE_PRO:
158 priv->raw_accel.x *= -1;
159 priv->raw_accel.z *= -1;
160 priv->raw_gyro.x *= -1;
161 priv->raw_gyro.z *= -1;
162 break;
163 default:
164 LOGE("Unknown VIVE revision.\n");
165 }
166
167 if(process_error(priv)){
168 vec3f mag = {{0.0f, 0.0f, 0.0f}};
169 vec3f gyro;
170 ovec3f_subtract(&priv->raw_gyro, &priv->gyro_error, &gyro);
171
172 ofusion_update(&priv->sensor_fusion, dt,
173 &gyro, &priv->raw_accel, &mag);
174 }
175
176 priv->last_seq = smp->seq;
177 }
178 }
179
180 static void update_device(ohmd_device* device)
181 {
182 vive_priv* priv = (vive_priv*)device;
183
184 int size = 0;
185
186 unsigned char buffer[FEATURE_BUFFER_SIZE];
187
188 while((size = hid_read(priv->imu_handle, buffer, FEATURE_BUFFER_SIZE)) > 0) {
189 if(buffer[0] == VIVE_HMD_IMU_PACKET_ID){
190 handle_imu_packet(priv, buffer, size);
191 }else{
192 LOGE("unknown message type: %u", buffer[0]);
193 }
194 }
195
196 if(size < 0){
197 LOGE("error reading from device");
198 }
199 }
200
201 static int getf(ohmd_device* device, ohmd_float_value type, float* out)
202 {
203 vive_priv* priv = (vive_priv*)device;
204
205 switch(type){
206 case OHMD_ROTATION_QUAT:
207 *(quatf*)out = priv->sensor_fusion.orient;
208 break;
209
210 case OHMD_POSITION_VECTOR:
211 out[0] = out[1] = out[2] = 0;
212 break;
213
214 case OHMD_DISTORTION_K:
215 // TODO this should be set to the equivalent of no distortion
216 memset(out, 0, sizeof(float) * 6);
217 break;
218
219 default:
220 ohmd_set_error(priv->base.ctx, "invalid type given to getf (%ud)", type);
221 return -1;
222 break;
223 }
224
225 return 0;
226 }
227
228 static void close_device(ohmd_device* device)
229 {
230 int hret = 0;
231 vive_priv* priv = (vive_priv*)device;
232
233 LOGD("closing HTC Vive device");
234
235 // turn the display off
236 switch (priv->revision) {
237 case REV_VIVE:
238 hret = hid_send_feature_report(priv->hmd_handle,
239 vive_magic_power_off1,
240 sizeof(vive_magic_power_off1));
241 LOGI("power off magic 1: %d\n", hret);
242
243 hret = hid_send_feature_report(priv->hmd_handle,
244 vive_magic_power_off2,
245 sizeof(vive_magic_power_off2));
246 LOGI("power off magic 2: %d\n", hret);
247 break;
248 case REV_VIVE_PRO:
249 hret = hid_send_feature_report(priv->hmd_handle,
250 vive_pro_magic_power_off,
251 sizeof(vive_pro_magic_power_off));
252 LOGI("vive pro power off magic: %d\n", hret);
253 break;
254 default:
255 LOGE("Unknown VIVE revision.\n");
256 }
257
258 hid_close(priv->hmd_handle);
259 hid_close(priv->imu_handle);
260
261 free(device);
262 }
263
264 #if 0
265 static void dump_indexed_string(hid_device* device, int index)
266 {
267 wchar_t wbuffer[512] = {0};
268 char buffer[1024] = {0};
269
270 int hret = hid_get_indexed_string(device, index, wbuffer, 511);
271
272 if(hret == 0){
273 wcstombs(buffer, wbuffer, sizeof(buffer));
274 LOGD("indexed string 0x%02x: '%s'\n", index, buffer);
275 }
276 }
277 #endif
278
279 static void dump_info_string(int (*fun)(hid_device*, wchar_t*, size_t),
280 const char* what, hid_device* device)
281 {
282 wchar_t wbuffer[512] = {0};
283 char buffer[1024] = {0};
284
285 int hret = fun(device, wbuffer, 511);
286
287 if(hret == 0){
288 wcstombs(buffer, wbuffer, sizeof(buffer));
289 LOGI("%s: '%s'\n", what, buffer);
290 }
291 }
292
293 #if 0
294 static void dumpbin(const char* label, const unsigned char* data, int length)
295 {
296 printf("%s:\n", label);
297 for(int i = 0; i < length; i++){
298 printf("%02x ", data[i]);
299 if((i % 16) == 15)
300 printf("\n");
301 }
302 printf("\n");
303 }
304 #endif
305
306 static hid_device* open_device_idx(int manufacturer, int product, int iface,
307 int iface_tot, int device_index)
308 {
309 struct hid_device_info* devs = hid_enumerate(manufacturer, product);
310 struct hid_device_info* cur_dev = devs;
311
312 int idx = 0;
313 int iface_cur = 0;
314 hid_device* ret = NULL;
315
316 while (cur_dev) {
317 LOGI("%04x:%04x %s\n", manufacturer, product, cur_dev->path);
318
319 if(idx == device_index && iface == iface_cur){
320 ret = hid_open_path(cur_dev->path);
321 LOGI("opening\n");
322 }
323
324 cur_dev = cur_dev->next;
325
326 iface_cur++;
327
328 if(iface_cur >= iface_tot){
329 idx++;
330 iface_cur = 0;
331 }
332 }
333
334 hid_free_enumeration(devs);
335
336 return ret;
337 }
338
339 int vive_read_firmware(hid_device* device)
340 {
341 vive_firmware_version_packet packet = {
342 .id = VIVE_FIRMWARE_VERSION_PACKET_ID,
343 };
344
345 int bytes;
346
347 LOGI("Getting vive_firmware_version_packet...");
348 bytes = hid_get_feature_report(device,
349 (unsigned char*) &packet,
350 sizeof(packet));
351
352 if (bytes < 0)
353 {
354 LOGE("Could not get vive_firmware_version_packet: %d", bytes);
355 return bytes;
356 }
357
358 LOGI("Firmware version %u %s@%s FPGA %u.%u\n",
359 packet.firmware_version, packet.string1,
360 packet.string2, packet.fpga_version_major,
361 packet.fpga_version_minor);
362 LOGI("Hardware revision: %d rev %d.%d.%d\n",
363 packet.hardware_revision, packet.hardware_version_major,
364 packet.hardware_version_minor, packet.hardware_version_micro);
365
366 return 0;
367 }
368
369 int vive_read_config(vive_priv* priv)
370 {
371 vive_config_start_packet start_packet = {
372 .id = VIVE_CONFIG_START_PACKET_ID,
373 };
374
375 int bytes;
376
377 LOGI("Getting vive_config_start_packet...");
378 bytes = hid_get_feature_report(priv->imu_handle,
379 (unsigned char*) &start_packet,
380 sizeof(start_packet));
381
382 if (bytes < 0)
383 {
384 LOGE("Could not get vive_config_start_packet: %ls (%d)",
385 hid_error(priv->imu_handle), bytes);
386 return bytes;
387 }
388
389 LOGI("Config packet size is %i bytes.", bytes);
390
391 vive_config_read_packet read_packet = {
392 .id = VIVE_CONFIG_READ_PACKET_ID,
393 };
394
395 unsigned char* packet_buffer = malloc(4096);
396
397 int offset = 0;
398 do {
399 bytes = hid_get_feature_report(priv->imu_handle,
400 (unsigned char*) &read_packet,
401 sizeof(read_packet));
402
403 memcpy((uint8_t*)packet_buffer + offset,
404 &read_packet.payload,
405 read_packet.length);
406 offset += read_packet.length;
407 } while (read_packet.length);
408 packet_buffer[offset] = '\0';
409 vive_decode_config_packet(&priv->imu_config, packet_buffer, offset);
410
411 free(packet_buffer);
412
413 return 0;
414 }
415
416 #define OHMD_GRAVITY_EARTH 9.80665 // m/s²
417
418 int vive_get_range_packet(vive_priv* priv)
419 {
420 int ret;
421
422 vive_imu_range_modes_packet packet = {
423 .id = VIVE_IMU_RANGE_MODES_PACKET_ID
424 };
425
426 ret = hid_get_feature_report(priv->imu_handle,
427 (unsigned char*) &packet,
428 sizeof(packet));
429
430 if (ret < 0)
431 {
432 LOGE("Could not get feature report %d.", packet.id);
433 return ret;
434 }
435
436 if (!packet.gyro_range || !packet.accel_range)
437 {
438 LOGW("Invalid gyroscope and accelerometer data. Trying to fetch again.");
439 ret = hid_get_feature_report(priv->imu_handle,
440 (unsigned char*) &packet,
441 sizeof(packet));
442 if (ret < 0)
443 {
444 LOGE("Could not get feature report %d.", packet.id);
445 return ret;
446 }
447
448 if (!packet.gyro_range || !packet.accel_range)
449 {
450 LOGE("Unexpected range mode report: %02x %02x %02x",
451 packet.id, packet.gyro_range, packet.accel_range);
452 for (int i = 0; i < 61; i++)
453 printf(" %02x", packet.unknown[i]);
454 printf("\n");
455 return -1;
456 }
457 }
458
459 if (packet.gyro_range > 4 || packet.accel_range > 4)
460 {
461 LOGE("Gyroscope or accelerometer range too large.");
462 return -1;
463 }
464
465 /*
466 * Convert MPU-6500 gyro full scale range (+/-250°/s, +/-500°/s,
467 * +/-1000°/s, or +/-2000°/s) into rad/s, accel full scale range
468 * (+/-2g, +/-4g, +/-8g, or +/-16g) into m/s².
469 */
470
471 double gyro_range = M_PI / 180.0 * (250 << packet.gyro_range);
472 priv->imu_config.gyro_range = (float) gyro_range;
473 LOGI("Vive gyroscope range %f", gyro_range);
474
475 double acc_range = OHMD_GRAVITY_EARTH * (2 << packet.accel_range);
476 priv->imu_config.acc_range = (float) acc_range;
477 LOGI("Vive accelerometer range %f", acc_range);
478 return 0;
479 }
480
481 static ohmd_device* open_device(ohmd_driver* driver, ohmd_device_desc* desc)
482 {
483 vive_priv* priv = ohmd_alloc(driver->ctx, sizeof(vive_priv));
484
485 if(!priv)
486 return NULL;
487
488 int hret = 0;
489
490 priv->revision = desc->revision;
491
492 priv->base.ctx = driver->ctx;
493
494 int idx = atoi(desc->path);
495
496 // Open the HMD device
497 switch (desc->revision) {
498 case REV_VIVE:
499 priv->hmd_handle = open_device_idx(HTC_ID, VIVE_HMD, 0, 1, idx);
500 break;
501 case REV_VIVE_PRO:
502 priv->hmd_handle = open_device_idx(HTC_ID, VIVE_PRO_HMD, 0, 1, idx);
503 break;
504 default:
505 LOGE("Unknown VIVE revision.\n");
506 }
507
508 if(!priv->hmd_handle)
509 goto cleanup;
510
511 if(hid_set_nonblocking(priv->hmd_handle, 1) == -1){
512 ohmd_set_error(driver->ctx, "failed to set non-blocking on device");
513 goto cleanup;
514 }
515
516 switch (desc->revision) {
517 case REV_VIVE:
518 priv->imu_handle = open_device_idx(VALVE_ID,
519 VIVE_LIGHTHOUSE_FPGA_RX, 0, 2, idx);
520 break;
521 case REV_VIVE_PRO:
522 priv->imu_handle = open_device_idx(VALVE_ID, VIVE_LHR, 0, 1, idx);
523 break;
524 default:
525 LOGE("Unknown VIVE revision.\n");
526 }
527
528 if(!priv->imu_handle)
529 goto cleanup;
530
531 if(hid_set_nonblocking(priv->imu_handle, 1) == -1){
532 ohmd_set_error(driver->ctx, "failed to set non-blocking on device");
533 goto cleanup;
534 }
535
536 dump_info_string(hid_get_manufacturer_string,
537 "manufacturer", priv->hmd_handle);
538 dump_info_string(hid_get_product_string, "product", priv->hmd_handle);
539 dump_info_string(hid_get_serial_number_string,
540 "serial number", priv->hmd_handle);
541
542 #if 0
543 // enable lighthouse
544 hret = hid_send_feature_report(priv->hmd_handle,
545 vive_magic_enable_lighthouse,
546 sizeof(vive_magic_enable_lighthouse));
547 LOGD("enable lighthouse magic: %d\n", hret);
548 #endif
549
550 /* IMU config defaults */
551 priv->imu_config.acc_bias.x = 0;
552 priv->imu_config.acc_bias.y = 0;
553 priv->imu_config.acc_bias.z = 0;
554
555 priv->imu_config.acc_scale.x = 1.0f;
556 priv->imu_config.acc_scale.y = 1.0f;
557 priv->imu_config.acc_scale.z = 1.0f;
558
559 priv->imu_config.gyro_bias.x = 0;
560 priv->imu_config.gyro_bias.y = 0;
561 priv->imu_config.gyro_bias.z = 0;
562
563 priv->imu_config.gyro_scale.x = 1.0f;
564 priv->imu_config.gyro_scale.y = 1.0f;
565 priv->imu_config.gyro_scale.z = 1.0f;
566
567 priv->imu_config.gyro_range = 8.726646f;
568 priv->imu_config.acc_range = 39.226600f;
569
570 switch (desc->revision) {
571 case REV_VIVE:
572 if (vive_read_config(priv) != 0)
573 {
574 LOGW("Could not read config. Using defaults.\n");
575 }
576
577 if (vive_get_range_packet(priv) != 0)
578 {
579 LOGW("Could not get range packet.\n");
580 }
581
582 if (vive_read_firmware(priv->imu_handle) != 0)
583 {
584 LOGE("Could not get headset firmware version!");
585 }
586
587 // turn the display on
588 hret = hid_send_feature_report(priv->hmd_handle,
589 vive_magic_power_on,
590 sizeof(vive_magic_power_on));
591 LOGI("power on magic: %d\n", hret);
592
593 break;
594 case REV_VIVE_PRO:
595 // turn the display on
596 hret = hid_send_feature_report(priv->hmd_handle,
597 vive_pro_magic_power_on,
598 sizeof(vive_pro_magic_power_on));
599 LOGI("power on magic: %d\n", hret);
600
601 // Enable VIVE Pro IMU
602 hret = hid_send_feature_report(priv->imu_handle,
603 vive_pro_enable_imu,
604 sizeof(vive_pro_enable_imu));
605 LOGI("Enable Pro IMU magic: %d\n", hret);
606 break;
607 default:
608 LOGE("Unknown VIVE revision.\n");
609 }
610
611 // Set default device properties
612 ohmd_set_default_device_properties(&priv->base.properties);
613
614 // Set device properties TODO: Get from device
615 switch (desc->revision) {
616 case REV_VIVE:
617 priv->base.properties.hres = 2160;
618 priv->base.properties.vres = 1200;
619 priv->base.properties.ratio = (2160.0f / 1200.0f) / 2.0f;
620 break;
621 case REV_VIVE_PRO:
622 priv->base.properties.hres = 2880;
623 priv->base.properties.vres = 1600;
624 priv->base.properties.ratio = (2880.0f / 1600.0f) / 2.0f;
625 break;
626 default:
627 LOGE("Unknown VIVE revision.\n");
628 }
629
630 //TODO: Confirm exact mesurements. Get for VIVE Pro.
631 priv->base.properties.hsize = 0.122822f;
632 priv->base.properties.vsize = 0.068234f;
633
634 /*
635 * calculated from here:
636 * https://www.gamedev.net/topic/683698-projection-matrix-model-of-the-htc-vive/
637 */
638 priv->base.properties.lens_sep = 0.057863;
639 priv->base.properties.lens_vpos = 0.033896;
640
641 float eye_to_screen_distance = 0.023226876441867737;
642 priv->base.properties.fov = 2 * atan2f(
643 priv->base.properties.hsize / 2 - priv->base.properties.lens_sep / 2,
644 eye_to_screen_distance);
645
646 /* calculate projection eye projection matrices from the device properties */
647 ohmd_calc_default_proj_matrices(&priv->base.properties);
648
649 // set up device callbacks
650 priv->base.update = update_device;
651 priv->base.close = close_device;
652 priv->base.getf = getf;
653
654 ofusion_init(&priv->sensor_fusion);
655
656 ofq_init(&priv->gyro_q, 128);
657
658 return (ohmd_device*)priv;
659
660 cleanup:
661 if(priv)
662 free(priv);
663
664 return NULL;
665 }
666
667 static void get_device_list(ohmd_driver* driver, ohmd_device_list* list)
668 {
669 vive_revision rev;
670 struct hid_device_info* devs = hid_enumerate(HTC_ID, VIVE_HMD);
671
672 if (devs != NULL) {
673 rev = REV_VIVE;
674 } else {
675 devs = hid_enumerate(HTC_ID, VIVE_PRO_HMD);
676 if (devs != NULL)
677 rev = REV_VIVE_PRO;
678 }
679
680 struct hid_device_info* cur_dev = devs;
681
682 int idx = 0;
683 while (cur_dev) {
684 ohmd_device_desc* desc = &list->devices[list->num_devices++];
685
686 strcpy(desc->driver, "OpenHMD HTC Vive Driver");
687 strcpy(desc->vendor, "HTC/Valve");
688 strcpy(desc->product, "HTC Vive");
689
690 desc->revision = rev;
691
692 snprintf(desc->path, OHMD_STR_SIZE, "%d", idx);
693
694 desc->driver_ptr = driver;
695 desc->device_class = OHMD_DEVICE_CLASS_HMD;
696 desc->device_flags = OHMD_DEVICE_FLAGS_ROTATIONAL_TRACKING;
697
698 cur_dev = cur_dev->next;
699 idx++;
700 }
701
702 hid_free_enumeration(devs);
703 }
704
705 static void destroy_driver(ohmd_driver* drv)
706 {
707 LOGD("shutting down HTC Vive driver");
708 free(drv);
709 }
710
711 ohmd_driver* ohmd_create_htc_vive_drv(ohmd_context* ctx)
712 {
713 ohmd_driver* drv = ohmd_alloc(ctx, sizeof(ohmd_driver));
714
715 if(!drv)
716 return NULL;
717
718 drv->get_device_list = get_device_list;
719 drv->open_device = open_device;
720 drv->destroy = destroy_driver;
721 drv->ctx = ctx;
722
723 return drv;
724 }
725