Merge branch 'master' into HMD_viewport
[blender.git] / extern / openhmd / src / drv_htc_vive / vive.c
1 /*
2  * OpenHMD - Free and Open Source API and drivers for immersive technology.
3  * Copyright (C) 2013 Fredrik Hultin.
4  * Copyright (C) 2013 Jakob Bornecrantz.
5  * Distributed under the Boost 1.0 licence, see LICENSE for full text.
6  */
7
8 /* HTC Vive Driver */
9
10 #define FEATURE_BUFFER_SIZE 256
11
12 #define HTC_ID                   0x0bb4
13 #define VIVE_HMD                 0x2c87
14
15 #define VALVE_ID                 0x28de
16 #define VIVE_WATCHMAN_DONGLE     0x2101
17 #define VIVE_LIGHTHOUSE_FPGA_RX  0x2000
18
19 #define VIVE_TIME_DIV 48000000.0f
20
21 #include <string.h>
22 #include <wchar.h>
23 #include <hidapi.h>
24 #include <assert.h>
25 #include <limits.h>
26 #include <stdint.h>
27 #include <stdbool.h>
28
29 #include "vive.h"
30
31 typedef struct {
32         ohmd_device base;
33
34         hid_device* hmd_handle;
35         hid_device* imu_handle;
36         fusion sensor_fusion;
37         vec3f raw_accel, raw_gyro;
38         uint32_t last_ticks;
39         uint8_t last_seq;
40
41         vec3f gyro_error;
42         filter_queue gyro_q;
43 } vive_priv;
44
45 void vec3f_from_vive_vec_accel(const int16_t* smp, vec3f* out_vec)
46 {
47         float gravity = 9.81f;
48         float scaler = 4.0f * gravity / 32768.0f;
49
50         out_vec->x = (float)smp[0] * scaler;
51         out_vec->y = (float)smp[1] * scaler * -1;
52         out_vec->z = (float)smp[2] * scaler * -1;
53 }
54
55 void vec3f_from_vive_vec_gyro(const int16_t* smp, vec3f* out_vec)
56 {
57         float scaler = 8.7f / 32768.0f;
58         out_vec->x = (float)smp[0] * scaler;
59         out_vec->y = (float)smp[1] * scaler * -1;
60         out_vec->z = (float)smp[2] * scaler * -1;
61 }
62
63 static bool process_error(vive_priv* priv)
64 {
65         if(priv->gyro_q.at >= priv->gyro_q.size - 1)
66                 return true;
67
68         ofq_add(&priv->gyro_q, &priv->raw_gyro);
69
70         if(priv->gyro_q.at >= priv->gyro_q.size - 1){
71                 ofq_get_mean(&priv->gyro_q, &priv->gyro_error);
72                 printf("gyro error: %f, %f, %f\n", priv->gyro_error.x, priv->gyro_error.y, priv->gyro_error.z);
73         }
74
75         return false;
76 }
77
78 vive_sensor_sample* get_next_sample(vive_sensor_packet* pkt, int last_seq)
79 {
80         int diff[3];
81
82         for(int i = 0; i < 3; i++)
83         {
84                 diff[i] = (int)pkt->samples[i].seq - last_seq;
85
86                 if(diff[i] < -128){
87                         diff[i] += 256;
88                 }
89         }
90
91         int closest_diff = INT_MAX;
92         int closest_idx = -1;
93
94         for(int i = 0; i < 3; i++)
95         {
96                 if(diff[i] < closest_diff && diff[i] > 0 && diff[i] < 128){
97                         closest_diff = diff[i];
98                         closest_idx = i;
99                 }
100         }
101
102         if(closest_idx != -1)
103                 return pkt->samples + closest_idx;
104
105         return NULL;
106 }
107
108 static void update_device(ohmd_device* device)
109 {
110         vive_priv* priv = (vive_priv*)device;
111
112         int size = 0;
113         unsigned char buffer[FEATURE_BUFFER_SIZE];
114
115         while((size = hid_read(priv->imu_handle, buffer, FEATURE_BUFFER_SIZE)) > 0){
116                 if(buffer[0] == VIVE_IRQ_SENSORS){
117                         vive_sensor_packet pkt;
118                         vive_decode_sensor_packet(&pkt, buffer, size);
119
120                         vive_sensor_sample* smp = NULL;
121
122                         while((smp = get_next_sample(&pkt, priv->last_seq)) != NULL)
123                         {
124                                 if(priv->last_ticks == 0)
125                                         priv->last_ticks = smp->time_ticks;
126
127                                 uint32_t t1, t2;
128                                 t1 = smp->time_ticks;
129                                 t2 = priv->last_ticks;
130
131                                 float dt = (t1 - t2) / VIVE_TIME_DIV;
132
133                                 priv->last_ticks = smp->time_ticks;
134
135                                 vec3f_from_vive_vec_accel(smp->acc, &priv->raw_accel);
136                                 vec3f_from_vive_vec_gyro(smp->rot, &priv->raw_gyro);
137
138                                 if(process_error(priv)){
139                                         vec3f mag = {{0.0f, 0.0f, 0.0f}};
140                                         vec3f gyro;
141                                         ovec3f_subtract(&priv->raw_gyro, &priv->gyro_error, &gyro);
142
143                                         ofusion_update(&priv->sensor_fusion, dt, &gyro, &priv->raw_accel, &mag);
144                                 }
145
146                                 priv->last_seq = smp->seq;
147                         }
148                 }else{
149                         LOGE("unknown message type: %u", buffer[0]);
150                 }
151         }
152
153         if(size < 0){
154                 LOGE("error reading from device");
155         }
156 }
157
158 static int getf(ohmd_device* device, ohmd_float_value type, float* out)
159 {
160         vive_priv* priv = (vive_priv*)device;
161
162         switch(type){
163         case OHMD_ROTATION_QUAT:
164                 *(quatf*)out = priv->sensor_fusion.orient;
165                 break;
166
167         case OHMD_POSITION_VECTOR:
168                 out[0] = out[1] = out[2] = 0;
169                 break;
170
171         case OHMD_DISTORTION_K:
172                 // TODO this should be set to the equivalent of no distortion
173                 memset(out, 0, sizeof(float) * 6);
174                 break;
175
176         default:
177                 ohmd_set_error(priv->base.ctx, "invalid type given to getf (%ud)", type);
178                 return -1;
179                 break;
180         }
181
182         return 0;
183 }
184
185 static void close_device(ohmd_device* device)
186 {
187         int hret = 0;
188         vive_priv* priv = (vive_priv*)device;
189
190         LOGD("closing HTC Vive device");
191
192         // turn the display off
193         hret = hid_send_feature_report(priv->hmd_handle, vive_magic_power_off1, sizeof(vive_magic_power_off1));
194         //printf("power off magic 1: %d\n", hret);
195
196         hret = hid_send_feature_report(priv->hmd_handle, vive_magic_power_off2, sizeof(vive_magic_power_off2));
197         //printf("power off magic 2: %d\n", hret);
198
199         hid_close(priv->hmd_handle);
200         hid_close(priv->imu_handle);
201
202         free(device);
203 }
204
205 #if 0
206 static void dump_indexed_string(hid_device* device, int index)
207 {
208         wchar_t wbuffer[512] = {0};
209         char buffer[1024] = {0};
210
211         int hret = hid_get_indexed_string(device, index, wbuffer, 511);
212
213         if(hret == 0){
214                 wcstombs(buffer, wbuffer, sizeof(buffer));
215                 printf("indexed string 0x%02x: '%s'\n", index, buffer);
216         }
217 }
218 #endif
219
220 static void dump_info_string(int (*fun)(hid_device*, wchar_t*, size_t), const char* what, hid_device* device)
221 {
222         wchar_t wbuffer[512] = {0};
223         char buffer[1024] = {0};
224
225         int hret = fun(device, wbuffer, 511);
226
227         if(hret == 0){
228                 wcstombs(buffer, wbuffer, sizeof(buffer));
229                 printf("%s: '%s'\n", what, buffer);
230         }
231 }
232
233 #if 0
234 static void dumpbin(const char* label, const unsigned char* data, int length)
235 {
236         printf("%s:\n", label);
237         for(int i = 0; i < length; i++){
238                 printf("%02x ", data[i]);
239                 if((i % 16) == 15)
240                         printf("\n");
241         }
242         printf("\n");
243 }
244 #endif
245
246 static hid_device* open_device_idx(int manufacturer, int product, int iface, int iface_tot, int device_index)
247 {
248         struct hid_device_info* devs = hid_enumerate(manufacturer, product);
249         struct hid_device_info* cur_dev = devs;
250
251         int idx = 0;
252         int iface_cur = 0;
253         hid_device* ret = NULL;
254
255         while (cur_dev) {
256                 printf("%04x:%04x %s\n", manufacturer, product, cur_dev->path);
257
258                 if(idx == device_index && iface == iface_cur){
259                         ret = hid_open_path(cur_dev->path);
260                         printf("opening\n");
261                 }
262
263                 cur_dev = cur_dev->next;
264
265                 iface_cur++;
266
267                 if(iface_cur >= iface_tot){
268                         idx++;
269                         iface_cur = 0;
270                 }
271         }
272
273         hid_free_enumeration(devs);
274
275         return ret;
276 }
277
278 static ohmd_device* open_device(ohmd_driver* driver, ohmd_device_desc* desc)
279 {
280         vive_priv* priv = ohmd_alloc(driver->ctx, sizeof(vive_priv));
281
282         if(!priv)
283                 return NULL;
284
285         int hret = 0;
286
287         priv->base.ctx = driver->ctx;
288
289         int idx = atoi(desc->path);
290
291         // Open the HMD device
292         priv->hmd_handle = open_device_idx(HTC_ID, VIVE_HMD, 0, 1, idx);
293
294         if(!priv->hmd_handle)
295                 goto cleanup;
296
297         if(hid_set_nonblocking(priv->hmd_handle, 1) == -1){
298                 ohmd_set_error(driver->ctx, "failed to set non-blocking on device");
299                 goto cleanup;
300         }
301
302         // Open the lighthouse device
303         priv->imu_handle = open_device_idx(VALVE_ID, VIVE_LIGHTHOUSE_FPGA_RX, 0, 2, idx);
304
305         if(!priv->imu_handle)
306                 goto cleanup;
307
308         if(hid_set_nonblocking(priv->imu_handle, 1) == -1){
309                 ohmd_set_error(driver->ctx, "failed to set non-blocking on device");
310                 goto cleanup;
311         }
312
313         dump_info_string(hid_get_manufacturer_string, "manufacturer", priv->hmd_handle);
314         dump_info_string(hid_get_product_string , "product", priv->hmd_handle);
315         dump_info_string(hid_get_serial_number_string, "serial number", priv->hmd_handle);
316
317         // turn the display on
318         hret = hid_send_feature_report(priv->hmd_handle, vive_magic_power_on, sizeof(vive_magic_power_on));
319         printf("power on magic: %d\n", hret);
320
321         // enable lighthouse
322         //hret = hid_send_feature_report(priv->hmd_handle, vive_magic_enable_lighthouse, sizeof(vive_magic_enable_lighthouse));
323         //printf("enable lighthouse magic: %d\n", hret);
324
325         // Set default device properties
326         ohmd_set_default_device_properties(&priv->base.properties);
327
328         // Set device properties TODO: Get from device
329         priv->base.properties.hsize = 0.122822f;
330         priv->base.properties.vsize = 0.068234f;
331         priv->base.properties.hres = 2160;
332         priv->base.properties.vres = 1200;
333     /*
334     // calculated from here: https://www.gamedev.net/topic/683698-projection-matrix-model-of-the-htc-vive/
335         priv->base.properties.lens_sep = 0.057863;
336         priv->base.properties.lens_vpos = 0.033896;
337     */
338     // estimated 'by eye' on jsarret's vive
339         priv->base.properties.lens_sep = 0.056;
340         priv->base.properties.lens_vpos = 0.032;
341     float eye_to_screen_distance = 0.023226876441867737;
342         //priv->base.properties.fov = DEG_TO_RAD(111.435f); //TODO: Confirm exact mesurements
343         priv->base.properties.ratio = (2160.0f / 1200.0f) / 2.0f;
344
345         /*
346     ohmd_set_universal_distortion_k(&(priv->base.properties), 0.394119, -0.508383, 0.323322, 0.790942);
347     */
348         ohmd_set_universal_distortion_k(&(priv->base.properties), 1.318397, -1.490242, 0.663824, 0.508021);
349         ohmd_set_universal_aberration_k(&(priv->base.properties), 1.00010147892f, 1.000f, 1.00019614479f);
350
351
352         // calculate projection eye projection matrices from the device properties
353         //ohmd_calc_default_proj_matrices(&priv->base.properties);
354         float l,r,t,b,n,f;
355         // left eye screen bounds
356         l = -1.0f * (priv->base.properties.hsize/2 - priv->base.properties.lens_sep/2);
357         r = priv->base.properties.lens_sep/2;
358         t = priv->base.properties.vsize - priv->base.properties.lens_vpos;
359         b = -1.0f * priv->base.properties.lens_vpos;
360         n = eye_to_screen_distance;
361         f = n*10e6;
362         //LOGD("l: %0.3f, r: %0.3f, b: %0.3f, t: %0.3f, n: %0.3f, f: %0.3f", l,r,b,t,n,f);
363         /* eye separation is handled by IPD in the Modelview matrix */
364         omat4x4f_init_frustum(&priv->base.properties.proj_left, l, r, b, t, n, f);
365         //right eye screen bounds
366         l = -1.0f * priv->base.properties.lens_sep/2;
367         r = priv->base.properties.hsize/2 - priv->base.properties.lens_sep/2;
368         n = eye_to_screen_distance;
369         f = n*10e6;
370         //LOGD("l: %0.3f, r: %0.3f, b: %0.3f, t: %0.3f, n: %0.3f, f: %0.3f", l,r,b,t,n,f);
371         /* eye separation is handled by IPD in the Modelview matrix */
372         omat4x4f_init_frustum(&priv->base.properties.proj_right, l, r, b, t, n, f);
373
374         priv->base.properties.fov = 2 * atan2f(
375                         priv->base.properties.hsize/2 - priv->base.properties.lens_sep/2,
376                         eye_to_screen_distance);
377
378         // set up device callbacks
379         priv->base.update = update_device;
380         priv->base.close = close_device;
381         priv->base.getf = getf;
382
383         ofusion_init(&priv->sensor_fusion);
384
385         ofq_init(&priv->gyro_q, 128);
386
387         return (ohmd_device*)priv;
388
389 cleanup:
390         if(priv)
391                 free(priv);
392
393         return NULL;
394 }
395
396 static void get_device_list(ohmd_driver* driver, ohmd_device_list* list)
397 {
398         struct hid_device_info* devs = hid_enumerate(HTC_ID, VIVE_HMD);
399         struct hid_device_info* cur_dev = devs;
400
401         int idx = 0;
402         while (cur_dev) {
403                 ohmd_device_desc* desc = &list->devices[list->num_devices++];
404
405                 strcpy(desc->driver, "OpenHMD HTC Vive Driver");
406                 strcpy(desc->vendor, "HTC/Valve");
407                 strcpy(desc->product, "HTC Vive");
408
409                 desc->revision = 0;
410
411                 snprintf(desc->path, OHMD_STR_SIZE, "%d", idx);
412
413                 desc->driver_ptr = driver;
414
415                 cur_dev = cur_dev->next;
416                 idx++;
417         }
418
419         hid_free_enumeration(devs);
420 }
421
422 static void destroy_driver(ohmd_driver* drv)
423 {
424         LOGD("shutting down HTC Vive driver");
425         free(drv);
426 }
427
428 ohmd_driver* ohmd_create_htc_vive_drv(ohmd_context* ctx)
429 {
430         ohmd_driver* drv = ohmd_alloc(ctx, sizeof(ohmd_driver));
431
432         if(!drv)
433                 return NULL;
434
435         drv->get_device_list = get_device_list;
436         drv->open_device = open_device;
437         drv->destroy = destroy_driver;
438         drv->ctx = ctx;
439
440         return drv;
441 }