The open source OpenXR runtime
d/vive_controller: Add basic 3dof vive_controller_driver
v2: Add basic 3DOF Index Controller support
Only the "gen1" watchman usb protocol is supported (almost same as vive wand).
Note: Lighthouse v2 data is not properly ignored, the IMU data is only parsed correctly
when controller is not hit by lighthouse lasers.
Supported:
* trigger value
* trigger button
* trackpad x, y
* trackpad touch
* thumbstick x, y
* thumbstick click
* system click
* a click,
* b click
Not supported:
* trigger touch
* touchpad force
* thumbstick touch
* system touch
* a touch
* b touch
* grip squeeze
* finger tracking
authored by
Christoph Haag
and committed by
Jakob Bornecrantz
d4e4a690
74a328e9
+1276
-1
+2
src/xrt/drivers/CMakeLists.txt
+2
src/xrt/drivers/CMakeLists.txt
+2
src/xrt/drivers/meson.build
+2
src/xrt/drivers/meson.build
+1219
src/xrt/drivers/vive/vive_controller_driver.c
+1219
src/xrt/drivers/vive/vive_controller_driver.c
···
1
+
// Copyright 2020, Collabora, Ltd.
2
+
// Copyright 2016 Philipp Zabel
3
+
// SPDX-License-Identifier: BSL-1.0
4
+
/*!
5
+
* @file
6
+
* @brief Vive Controller prober and driver code
7
+
* @author Christoph Haag <christoph.gaag@collabora.com>
8
+
* @author Lubosz Sarnecki <lubosz.sarnecki@collabora.com>
9
+
* @author Ryan Pavlik <ryan.pavlik@collabora.com>
10
+
* @author Jakob Bornecrantz <jakob@collabora.com>
11
+
*
12
+
* Portions based on the VRPN Razer Hydra driver,
13
+
* originally written by Ryan Pavlik and available under the BSL-1.0.
14
+
*/
15
+
16
+
17
+
#include <assert.h>
18
+
#include <stdio.h>
19
+
#include <stdlib.h>
20
+
#include <string.h>
21
+
22
+
#include "xrt/xrt_prober.h"
23
+
24
+
#include "math/m_api.h"
25
+
#include "util/u_debug.h"
26
+
#include "util/u_device.h"
27
+
#include "util/u_json.h"
28
+
#include "util/u_misc.h"
29
+
#include "util/u_time.h"
30
+
#include "os/os_hid.h"
31
+
#include "os/os_threading.h"
32
+
#include "os/os_time.h"
33
+
34
+
#include "../vive/vive_protocol.h"
35
+
#include "vive_controller_interface.h"
36
+
37
+
#include "math/m_imu_3dof.h"
38
+
39
+
#ifdef XRT_OS_LINUX
40
+
#include <unistd.h>
41
+
#include <math.h>
42
+
#endif
43
+
44
+
/*
45
+
*
46
+
* Defines & structs.
47
+
*
48
+
*/
49
+
50
+
#define VIVE_CONTROLLER_SPEW(p, ...) \
51
+
do { \
52
+
if (p->print_spew) { \
53
+
fprintf(stderr, "%s - ", __func__); \
54
+
fprintf(stderr, __VA_ARGS__); \
55
+
fprintf(stderr, "\n"); \
56
+
} \
57
+
} while (false)
58
+
59
+
#define VIVE_CONTROLLER_DEBUG(p, ...) \
60
+
do { \
61
+
if (p->print_debug) { \
62
+
fprintf(stderr, "%s - ", __func__); \
63
+
fprintf(stderr, __VA_ARGS__); \
64
+
fprintf(stderr, "\n"); \
65
+
} \
66
+
} while (false)
67
+
68
+
#define VIVE_CONTROLLER_ERROR(p, ...) \
69
+
do { \
70
+
fprintf(stderr, "%s - ", __func__); \
71
+
fprintf(stderr, __VA_ARGS__); \
72
+
fprintf(stderr, "\n"); \
73
+
} while (false)
74
+
75
+
DEBUG_GET_ONCE_BOOL_OPTION(vive_controller_spew,
76
+
"VIVE_CONTROLLER_PRINT_SPEW",
77
+
false)
78
+
DEBUG_GET_ONCE_BOOL_OPTION(vive_controller_debug,
79
+
"VIVE_CONTROLLER_PRINT_DEBUG",
80
+
false)
81
+
enum vive_controller_input_index
82
+
{
83
+
// common inputs
84
+
VIVE_CONTROLLER_INDEX_AIM_POSE = 0,
85
+
VIVE_CONTROLLER_INDEX_GRIP_POSE,
86
+
VIVE_CONTROLLER_INDEX_SYSTEM_CLICK,
87
+
VIVE_CONTROLLER_INDEX_TRIGGER_CLICK,
88
+
VIVE_CONTROLLER_INDEX_TRIGGER_VALUE,
89
+
VIVE_CONTROLLER_INDEX_TRACKPAD_X,
90
+
VIVE_CONTROLLER_INDEX_TRACKPAD_Y,
91
+
VIVE_CONTROLLER_INDEX_TRACKPAD_TOUCH,
92
+
93
+
// Vive Wand specific inputs
94
+
VIVE_CONTROLLER_INDEX_SQUEEZE_CLICK,
95
+
VIVE_CONTROLLER_INDEX_MENU_CLICK,
96
+
VIVE_CONTROLLER_INDEX_TRACKPAD_CLICK,
97
+
98
+
// Valve Index specific inputs
99
+
VIVE_CONTROLLER_INDEX_THUMBSTICK_X,
100
+
VIVE_CONTROLLER_INDEX_THUMBSTICK_Y,
101
+
VIVE_CONTROLLER_INDEX_A_CLICK,
102
+
VIVE_CONTROLLER_INDEX_B_CLICK,
103
+
VIVE_CONTROLLER_INDEX_THUMBSTICK_CLICK,
104
+
105
+
VIVE_CONTROLLER_MAX_INDEX,
106
+
};
107
+
108
+
#define VIVE_CLOCK_FREQ 48000000.0f // Hz = 48 MHz
109
+
110
+
enum watchman_gen
111
+
{
112
+
WATCHMAN_GEN1,
113
+
WATCHMAN_GEN2,
114
+
WATCHMAN_GEN_UNKNOWN
115
+
};
116
+
117
+
enum controller_variant
118
+
{
119
+
CONTROLLER_VIVE_WAND,
120
+
CONTROLLER_INDEX_LEFT,
121
+
CONTROLLER_INDEX_RIGHT,
122
+
CONTROLLER_UNKNOWN
123
+
};
124
+
125
+
#define DEFAULT_HAPTIC_FREQ 150.0f
126
+
#define MIN_HAPTIC_DURATION 0.05f
127
+
128
+
/*!
129
+
* A Vive Controller device, representing just a single controller.
130
+
*
131
+
* @ingroup drv_vive
132
+
*/
133
+
struct vive_controller_device
134
+
{
135
+
struct xrt_device base;
136
+
137
+
struct os_hid_device *controller_hid;
138
+
struct os_thread_helper controller_thread;
139
+
140
+
struct
141
+
{
142
+
uint64_t time_ns;
143
+
uint32_t last_sample_time_raw;
144
+
double acc_range;
145
+
double gyro_range;
146
+
struct xrt_vec3 acc_bias;
147
+
struct xrt_vec3 acc_scale;
148
+
struct xrt_vec3 gyro_bias;
149
+
struct xrt_vec3 gyro_scale;
150
+
151
+
//! IMU position in tracking space.
152
+
struct xrt_pose trackref;
153
+
} imu;
154
+
155
+
struct m_imu_3dof fusion;
156
+
157
+
struct
158
+
{
159
+
struct xrt_vec3 acc;
160
+
struct xrt_vec3 gyro;
161
+
} last;
162
+
163
+
struct xrt_quat rot_filtered;
164
+
165
+
bool print_spew;
166
+
bool print_debug;
167
+
168
+
uint32_t last_ticks;
169
+
170
+
//! Which vive controller in the system are we?
171
+
size_t index;
172
+
173
+
struct
174
+
{
175
+
struct xrt_vec2 trackpad;
176
+
float trigger;
177
+
uint8_t buttons;
178
+
uint8_t last_buttons;
179
+
180
+
bool charging;
181
+
uint8_t battery;
182
+
} state;
183
+
184
+
struct
185
+
{
186
+
uint32_t firmware_version;
187
+
uint8_t hardware_revision;
188
+
uint8_t hardware_version_micro;
189
+
uint8_t hardware_version_minor;
190
+
uint8_t hardware_version_major;
191
+
char *mb_serial_number;
192
+
char *model_number;
193
+
char *device_serial_number;
194
+
} firmware;
195
+
196
+
enum watchman_gen watchman_gen;
197
+
enum controller_variant variant;
198
+
};
199
+
200
+
static inline struct vive_controller_device *
201
+
vive_controller_device(struct xrt_device *xdev)
202
+
{
203
+
assert(xdev);
204
+
struct vive_controller_device *ret =
205
+
(struct vive_controller_device *)xdev;
206
+
return ret;
207
+
}
208
+
209
+
static void
210
+
vive_controller_device_destroy(struct xrt_device *xdev)
211
+
{
212
+
struct vive_controller_device *d = vive_controller_device(xdev);
213
+
214
+
os_thread_helper_destroy(&d->controller_thread);
215
+
216
+
m_imu_3dof_close(&d->fusion);
217
+
218
+
if (d->controller_hid)
219
+
os_hid_destroy(d->controller_hid);
220
+
221
+
free(d);
222
+
}
223
+
224
+
static void
225
+
vive_controller_device_update_wand_inputs(struct xrt_device *xdev)
226
+
{
227
+
struct vive_controller_device *d = vive_controller_device(xdev);
228
+
229
+
os_thread_helper_lock(&d->controller_thread);
230
+
uint8_t buttons = d->state.buttons;
231
+
232
+
/*
233
+
int i = 8;
234
+
while(i--) {
235
+
putchar('0' + ((buttons >> i) & 1));
236
+
}
237
+
printf("\n");
238
+
*/
239
+
240
+
241
+
uint64_t now = os_monotonic_get_ns();
242
+
243
+
/* d->state.buttons is bitmask of currently pressed buttons.
244
+
* (index n) nth bit in the bitmask -> input "name"
245
+
*/
246
+
const int button_index_map[] = {VIVE_CONTROLLER_INDEX_TRIGGER_CLICK,
247
+
VIVE_CONTROLLER_INDEX_TRACKPAD_TOUCH,
248
+
VIVE_CONTROLLER_INDEX_TRACKPAD_CLICK,
249
+
VIVE_CONTROLLER_INDEX_SYSTEM_CLICK,
250
+
VIVE_CONTROLLER_INDEX_SQUEEZE_CLICK,
251
+
VIVE_CONTROLLER_INDEX_MENU_CLICK};
252
+
253
+
int button_count = ARRAY_SIZE(button_index_map);
254
+
for (int i = 0; i < button_count; i++) {
255
+
256
+
bool pressed = (buttons >> i) & 1;
257
+
bool last_pressed = (d->state.last_buttons >> i) & 1;
258
+
259
+
if (pressed != last_pressed) {
260
+
struct xrt_input *input =
261
+
&d->base.inputs[button_index_map[i]];
262
+
263
+
input->timestamp = now;
264
+
input->value.boolean = pressed;
265
+
266
+
VIVE_CONTROLLER_DEBUG(d, "button %d %s\n", i,
267
+
pressed ? "pressed" : "released");
268
+
}
269
+
}
270
+
271
+
if (d->state.trackpad.x != 0) {
272
+
struct xrt_input *input =
273
+
&d->base.inputs[VIVE_CONTROLLER_INDEX_TRACKPAD_X];
274
+
input->timestamp = now;
275
+
input->value.vec1.x = d->state.trackpad.x;
276
+
}
277
+
278
+
if (d->state.trackpad.y != 0) {
279
+
struct xrt_input *input =
280
+
&d->base.inputs[VIVE_CONTROLLER_INDEX_TRACKPAD_Y];
281
+
input->timestamp = now;
282
+
input->value.vec1.x = d->state.trackpad.y;
283
+
}
284
+
285
+
if (d->state.trackpad.x != 0 || d->state.trackpad.y != 0)
286
+
VIVE_CONTROLLER_DEBUG(d, "Trackpad: %f, %f",
287
+
d->state.trackpad.x, d->state.trackpad.y);
288
+
289
+
if (d->state.trigger != 0) {
290
+
struct xrt_input *input =
291
+
&d->base.inputs[VIVE_CONTROLLER_INDEX_TRIGGER_VALUE];
292
+
input->timestamp = now;
293
+
input->value.vec1.x = d->state.trigger;
294
+
VIVE_CONTROLLER_DEBUG(d, "Trigger: %f", d->state.trigger);
295
+
}
296
+
297
+
d->state.last_buttons = d->state.buttons;
298
+
os_thread_helper_unlock(&d->controller_thread);
299
+
}
300
+
301
+
static void
302
+
vive_controller_device_update_index_inputs(struct xrt_device *xdev)
303
+
{
304
+
struct vive_controller_device *d = vive_controller_device(xdev);
305
+
306
+
os_thread_helper_lock(&d->controller_thread);
307
+
uint8_t buttons = d->state.buttons;
308
+
309
+
/*
310
+
int i = 8;
311
+
while(i--) {
312
+
putchar('0' + ((buttons >> i) & 1));
313
+
}
314
+
printf("\n");
315
+
*/
316
+
317
+
uint64_t now = os_monotonic_get_ns();
318
+
319
+
/* d->state.buttons is bitmask of currently pressed buttons.
320
+
* (index n) nth bit in the bitmask -> input "name"
321
+
*/
322
+
const int button_index_map[] = {VIVE_CONTROLLER_INDEX_TRIGGER_CLICK,
323
+
VIVE_CONTROLLER_INDEX_TRACKPAD_TOUCH,
324
+
VIVE_CONTROLLER_INDEX_THUMBSTICK_CLICK,
325
+
VIVE_CONTROLLER_INDEX_SYSTEM_CLICK,
326
+
VIVE_CONTROLLER_INDEX_A_CLICK,
327
+
VIVE_CONTROLLER_INDEX_B_CLICK};
328
+
329
+
int button_count = ARRAY_SIZE(button_index_map);
330
+
for (int i = 0; i < button_count; i++) {
331
+
332
+
bool pressed = (buttons >> i) & 1;
333
+
bool last_pressed = (d->state.last_buttons >> i) & 1;
334
+
335
+
if (pressed != last_pressed) {
336
+
struct xrt_input *input =
337
+
&d->base.inputs[button_index_map[i]];
338
+
339
+
input->timestamp = now;
340
+
input->value.boolean = pressed;
341
+
342
+
VIVE_CONTROLLER_DEBUG(d, "button %d %s\n", i,
343
+
pressed ? "pressed" : "released");
344
+
}
345
+
}
346
+
347
+
/* trackpad and thumbstick position are the same usb events.
348
+
* report trackpad position when trackpad has been touched last, and
349
+
* thumbstick position when trackpad touch has been released
350
+
*/
351
+
if (d->state.trackpad.x != 0) {
352
+
struct xrt_input *input;
353
+
if (d->base.inputs[VIVE_CONTROLLER_INDEX_TRACKPAD_TOUCH]
354
+
.value.boolean)
355
+
input =
356
+
&d->base.inputs[VIVE_CONTROLLER_INDEX_TRACKPAD_X];
357
+
else
358
+
input =
359
+
&d->base.inputs[VIVE_CONTROLLER_INDEX_THUMBSTICK_X];
360
+
input->timestamp = now;
361
+
input->value.vec1.x = d->state.trackpad.x;
362
+
}
363
+
364
+
if (d->state.trackpad.y != 0) {
365
+
struct xrt_input *input;
366
+
if (d->base.inputs[VIVE_CONTROLLER_INDEX_TRACKPAD_TOUCH]
367
+
.value.boolean)
368
+
input =
369
+
&d->base.inputs[VIVE_CONTROLLER_INDEX_TRACKPAD_X];
370
+
else
371
+
input =
372
+
&d->base.inputs[VIVE_CONTROLLER_INDEX_THUMBSTICK_X];
373
+
input->timestamp = now;
374
+
input->value.vec1.x = d->state.trackpad.y;
375
+
}
376
+
377
+
if (d->state.trackpad.x != 0 || d->state.trackpad.y != 0) {
378
+
const char *component =
379
+
d->base.inputs[VIVE_CONTROLLER_INDEX_TRACKPAD_TOUCH]
380
+
.value.boolean
381
+
? "Trackpad"
382
+
: "Thumbstick";
383
+
VIVE_CONTROLLER_DEBUG(d, "%s: %f, %f", component,
384
+
d->state.trackpad.x, d->state.trackpad.y);
385
+
}
386
+
387
+
if (d->state.trigger != 0) {
388
+
struct xrt_input *input =
389
+
&d->base.inputs[VIVE_CONTROLLER_INDEX_TRIGGER_VALUE];
390
+
input->timestamp = now;
391
+
input->value.vec1.x = d->state.trigger;
392
+
VIVE_CONTROLLER_DEBUG(d, "Trigger: %f", d->state.trigger);
393
+
}
394
+
395
+
d->state.last_buttons = d->state.buttons;
396
+
os_thread_helper_unlock(&d->controller_thread);
397
+
}
398
+
399
+
static void
400
+
vive_controller_device_get_tracked_pose(struct xrt_device *xdev,
401
+
enum xrt_input_name name,
402
+
uint64_t at_timestamp_ns,
403
+
uint64_t *out_relation_timestamp_ns,
404
+
struct xrt_space_relation *out_relation)
405
+
{
406
+
struct vive_controller_device *d = vive_controller_device(xdev);
407
+
408
+
// printf("input name %d %d\n", name, XRT_INPUT_VIVE_GRIP_POSE);
409
+
if (name != XRT_INPUT_VIVE_AIM_POSE &&
410
+
name != XRT_INPUT_VIVE_GRIP_POSE &&
411
+
name != XRT_INPUT_INDEX_AIM_POSE &&
412
+
name != XRT_INPUT_INDEX_GRIP_POSE) {
413
+
VIVE_CONTROLLER_ERROR(d, "unknown input name");
414
+
return;
415
+
}
416
+
417
+
// Clear out the relation.
418
+
U_ZERO(out_relation);
419
+
420
+
uint64_t now = os_monotonic_get_ns();
421
+
*out_relation_timestamp_ns = now;
422
+
423
+
os_thread_helper_lock(&d->controller_thread);
424
+
425
+
// Don't do anything if we have stopped.
426
+
if (!os_thread_helper_is_running_locked(&d->controller_thread)) {
427
+
os_thread_helper_unlock(&d->controller_thread);
428
+
return;
429
+
}
430
+
431
+
out_relation->pose.orientation = d->rot_filtered;
432
+
433
+
//! @todo assuming that orientation is actually currently tracked.
434
+
out_relation->relation_flags = (enum xrt_space_relation_flags)(
435
+
XRT_SPACE_RELATION_POSITION_VALID_BIT |
436
+
XRT_SPACE_RELATION_POSITION_TRACKED_BIT |
437
+
XRT_SPACE_RELATION_ORIENTATION_VALID_BIT |
438
+
XRT_SPACE_RELATION_ORIENTATION_TRACKED_BIT);
439
+
440
+
os_thread_helper_unlock(&d->controller_thread);
441
+
442
+
struct xrt_vec3 pos = out_relation->pose.position;
443
+
struct xrt_quat quat = out_relation->pose.orientation;
444
+
VIVE_CONTROLLER_SPEW(
445
+
d, "GET_TRACKED_POSE (%f, %f, %f) (%f, %f, %f, %f) ", pos.x, pos.y,
446
+
pos.z, quat.x, quat.y, quat.z, quat.w);
447
+
}
448
+
449
+
static int
450
+
vive_controller_haptic_pulse(struct vive_controller_device *d,
451
+
union xrt_output_value *value)
452
+
{
453
+
float duration_seconds;
454
+
//! @todo: proper min duration value
455
+
if (value->vibration.duration == -1) {
456
+
VIVE_CONTROLLER_SPEW(d,
457
+
"Haptic pulse duration: using %f minimum",
458
+
MIN_HAPTIC_DURATION);
459
+
duration_seconds = 0.1;
460
+
} else {
461
+
duration_seconds = time_ns_to_s(value->vibration.duration);
462
+
}
463
+
464
+
VIVE_CONTROLLER_SPEW(d, "Haptic pulse amp %f, %fHz, %fs",
465
+
value->vibration.amplitude,
466
+
value->vibration.frequency, duration_seconds);
467
+
float frequency = value->vibration.frequency;
468
+
469
+
//! @todo: proper unspecified value
470
+
if (frequency == 0) {
471
+
VIVE_CONTROLLER_SPEW(
472
+
d, "Haptic pulse frequency unspecified, setting to %fHz",
473
+
DEFAULT_HAPTIC_FREQ);
474
+
frequency = 200;
475
+
}
476
+
477
+
478
+
/* haptic pulse for Vive Controller:
479
+
* desired_frequency = 1000 * 1000 / (high + low).
480
+
* => (high + low) = 1000 * 1000 / desired_frequency
481
+
* repeat = desired_duration_in_seconds * desired_frequency.
482
+
*
483
+
* I think:
484
+
* Lowest amplitude: 1, high+low-1
485
+
* Highest amplitude: (high+low)/2, / (high+low)/2
486
+
*/
487
+
488
+
float high_plus_low = 1000.f * 1000.f / frequency;
489
+
uint16_t pulse_low =
490
+
(uint16_t)(value->vibration.amplitude * high_plus_low / 2.);
491
+
492
+
/* Vive Controller doesn't vibrate with value == 0.
493
+
* Not sure if this actually happens, but let's fix it anyway. */
494
+
if (pulse_low == 0)
495
+
pulse_low = 1;
496
+
497
+
uint16_t pulse_high = high_plus_low - pulse_low;
498
+
499
+
uint16_t repeat_count = duration_seconds * frequency;
500
+
501
+
const struct vive_controller_haptic_pulse_report report = {
502
+
.id = VIVE_CONTROLLER_COMMAND_REPORT_ID,
503
+
.command = VIVE_CONTROLLER_HAPTIC_PULSE_COMMAND,
504
+
.len = 7,
505
+
.zero = 0x00,
506
+
.pulse_high = __cpu_to_le16(pulse_high),
507
+
.pulse_low = __cpu_to_le16(pulse_low),
508
+
.repeat_count = __cpu_to_le16(repeat_count),
509
+
};
510
+
511
+
return os_hid_set_feature(d->controller_hid, (uint8_t *)&report,
512
+
sizeof(report));
513
+
}
514
+
515
+
static void
516
+
vive_controller_device_set_output(struct xrt_device *xdev,
517
+
enum xrt_output_name name,
518
+
union xrt_output_value *value)
519
+
{
520
+
struct vive_controller_device *d = vive_controller_device(xdev);
521
+
522
+
if (name != XRT_OUTPUT_NAME_VIVE_HAPTIC &&
523
+
name != XRT_OUTPUT_NAME_INDEX_HAPTIC) {
524
+
VIVE_CONTROLLER_ERROR(d, "Unknown output\n");
525
+
return;
526
+
}
527
+
528
+
bool pulse = value->vibration.amplitude > 0.01;
529
+
if (!pulse) {
530
+
return;
531
+
}
532
+
533
+
vive_controller_haptic_pulse(d, value);
534
+
}
535
+
536
+
static void
537
+
controller_handle_battery(struct vive_controller_device *d, uint8_t battery)
538
+
{
539
+
uint8_t charge_percent = battery & VIVE_CONTROLLER_BATTERY_CHARGE_MASK;
540
+
bool charging = battery & VIVE_CONTROLLER_BATTERY_CHARGING;
541
+
VIVE_CONTROLLER_DEBUG(d, "Charging %d, percent %d\n", charging,
542
+
charge_percent);
543
+
d->state.charging = charging;
544
+
d->state.battery = charge_percent;
545
+
}
546
+
547
+
static void
548
+
controller_handle_buttons(struct vive_controller_device *d, uint8_t buttons)
549
+
{
550
+
d->state.buttons = buttons;
551
+
}
552
+
553
+
static void
554
+
controller_handle_touch_position(struct vive_controller_device *d, uint8_t *buf)
555
+
{
556
+
int16_t x = __le16_to_cpup((__le16 *)buf);
557
+
int16_t y = __le16_to_cpup((__le16 *)(buf + 2));
558
+
d->state.trackpad.x = (float)x / INT16_MAX;
559
+
d->state.trackpad.y = (float)y / INT16_MAX;
560
+
if (d->state.trackpad.x != 0 || d->state.trackpad.y != 0)
561
+
VIVE_CONTROLLER_SPEW(d, "Trackpad %f,%f\n", d->state.trackpad.x,
562
+
d->state.trackpad.y);
563
+
}
564
+
565
+
static void
566
+
controller_handle_analog_trigger(struct vive_controller_device *d,
567
+
uint8_t analog)
568
+
{
569
+
d->state.trigger = (float)analog / UINT8_MAX;
570
+
VIVE_CONTROLLER_SPEW(d, "Trigger %f\n", d->state.trigger);
571
+
}
572
+
573
+
static inline uint32_t
574
+
calc_dt_raw_and_handle_overflow(struct vive_controller_device *d,
575
+
uint32_t sample_time)
576
+
{
577
+
uint64_t dt_raw =
578
+
(uint64_t)sample_time - (uint64_t)d->imu.last_sample_time_raw;
579
+
d->imu.last_sample_time_raw = sample_time;
580
+
581
+
// The 32-bit tick counter has rolled over,
582
+
// adjust the "negative" value to be positive.
583
+
// It's easiest to do this with 64-bits.
584
+
if (dt_raw > 0xFFFFFFFF) {
585
+
dt_raw += 0x100000000;
586
+
}
587
+
588
+
return (uint32_t)dt_raw;
589
+
}
590
+
591
+
static inline uint64_t
592
+
cald_dt_ns(uint32_t dt_raw)
593
+
{
594
+
double f = (double)(dt_raw) / VIVE_CLOCK_FREQ;
595
+
uint64_t diff_ns = (uint64_t)(f * 1000.0 * 1000.0 * 1000.0);
596
+
return diff_ns;
597
+
}
598
+
599
+
static void
600
+
vive_controller_handle_imu_sample(struct vive_controller_device *d,
601
+
struct vive_imu_report *report)
602
+
{
603
+
/* Time in 48 MHz ticks, but we are missing the low byte */
604
+
uint32_t time_raw = d->last_ticks | report->id;
605
+
uint32_t dt_raw = calc_dt_raw_and_handle_overflow(d, time_raw);
606
+
uint64_t dt_ns = cald_dt_ns(dt_raw);
607
+
608
+
int16_t acc[3] = {
609
+
__le16_to_cpu(report->sample->acc[0]),
610
+
__le16_to_cpu(report->sample->acc[1]),
611
+
__le16_to_cpu(report->sample->acc[2]),
612
+
};
613
+
614
+
int16_t gyro[3] = {
615
+
__le16_to_cpu(report->sample->gyro[0]),
616
+
__le16_to_cpu(report->sample->gyro[1]),
617
+
__le16_to_cpu(report->sample->gyro[2]),
618
+
};
619
+
620
+
float scale = (float)d->imu.acc_range / 32768.0f;
621
+
struct xrt_vec3 acceleration = {
622
+
scale * d->imu.acc_scale.x * acc[0] - d->imu.acc_bias.x,
623
+
scale * d->imu.acc_scale.y * acc[1] - d->imu.acc_bias.y,
624
+
scale * d->imu.acc_scale.z * acc[2] - d->imu.acc_bias.z,
625
+
};
626
+
627
+
scale = (float)d->imu.gyro_range / 32768.0f;
628
+
struct xrt_vec3 angular_velocity = {
629
+
scale * d->imu.gyro_scale.x * gyro[0] - d->imu.gyro_bias.x,
630
+
scale * d->imu.gyro_scale.y * gyro[1] - d->imu.gyro_bias.y,
631
+
scale * d->imu.gyro_scale.z * gyro[2] - d->imu.gyro_bias.z,
632
+
};
633
+
634
+
/*
635
+
VIVE_CONTROLLER_SPEW(d, "ACC %f %f %f", acceleration.x,
636
+
acceleration.y, acceleration.z); VIVE_CONTROLLER_SPEW(d, "GYRO %f %f
637
+
%f", angular_velocity.x, angular_velocity.y, angular_velocity.z);
638
+
*/
639
+
640
+
if (d->variant == CONTROLLER_VIVE_WAND) {
641
+
acceleration.x *= -1;
642
+
float temp_accel = acceleration.y;
643
+
acceleration.y = -acceleration.z;
644
+
acceleration.z = -temp_accel;
645
+
646
+
angular_velocity.x *= -1;
647
+
float temp_ang = angular_velocity.y;
648
+
angular_velocity.y = -angular_velocity.z;
649
+
angular_velocity.z = -temp_ang;
650
+
} else if (d->variant == CONTROLLER_INDEX_LEFT ||
651
+
d->variant == CONTROLLER_INDEX_RIGHT) {
652
+
float temp_accel = acceleration.x;
653
+
acceleration.x = acceleration.z;
654
+
acceleration.y = -acceleration.y;
655
+
acceleration.z = temp_accel;
656
+
657
+
float temp_ang = angular_velocity.x;
658
+
angular_velocity.x = angular_velocity.z;
659
+
angular_velocity.y = -angular_velocity.y;
660
+
angular_velocity.z = temp_ang;
661
+
}
662
+
663
+
d->imu.time_ns += dt_ns;
664
+
d->last.acc = acceleration;
665
+
d->last.gyro = angular_velocity;
666
+
667
+
m_imu_3dof_update(&d->fusion, d->imu.time_ns, &acceleration,
668
+
&angular_velocity);
669
+
670
+
d->rot_filtered = d->fusion.rot;
671
+
672
+
// VIVE_CONTROLLER_SPEW(d, "Rot %f %f %f", d->rot_filtered.x,
673
+
// d->rot_filtered.y, d->rot_filtered.z);
674
+
}
675
+
676
+
static void
677
+
vive_controller_handle_lighthousev1(uint8_t *buf, uint8_t len)
678
+
{
679
+
// stub
680
+
}
681
+
682
+
/*
683
+
* Handles battery, imu, trigger, buttons, trackpad.
684
+
* Then hands off to vive_controller_handle_lighthousev1().
685
+
*/
686
+
static void
687
+
vive_controller_decode_watchmanv1(struct vive_controller_device *d,
688
+
struct vive_controller_message *message)
689
+
{
690
+
uint8_t *buf = message->payload;
691
+
uint8_t *end = message->payload + message->len - 1;
692
+
693
+
/*
694
+
for (int i = 0; i < message->len; i++) {
695
+
//printf("%02x ", buf[i]);
696
+
int j = 8;
697
+
while(j--) {
698
+
putchar('0' + ((buf[i] >> j) & 1));
699
+
}
700
+
putchar(' ');
701
+
}
702
+
printf("\n");
703
+
*/
704
+
705
+
/* payload starts with "event flags" byte.
706
+
* If it does not start with 111, it contains only lighthouse data.
707
+
* If it starts with 111, events follow in this order, each of them
708
+
* optional:
709
+
* - battery: 1 byte (1110???1)
710
+
* - trigger: 1 byte (1111?1??)
711
+
* - trackpad: 4 byte (1111??1?)
712
+
* - buttons: 1 byte (1111???1)
713
+
* - imu: 13 byte (111?1???)
714
+
* There may be another input event after a battery event.
715
+
* Lighthouse data may follow in the rest of the payload.
716
+
*/
717
+
718
+
// input events have first three bits set
719
+
while ((*buf & 0xe0) == 0xe0 && buf < end) {
720
+
721
+
// clang-format off
722
+
723
+
// battery follows when 1110???1
724
+
bool has_battery = (*buf & 0x10) != 0x10 && (*buf & 0x1) == 0x1;
725
+
726
+
// input follows when 1111?<trigger><trackpad><buttons>
727
+
bool has_trigger = (*buf & 0x10) == 0x10 && (*buf & 0x4) == 0x4;
728
+
bool has_trackpad = (*buf & 0x10) == 0x10 && (*buf & 0x2) == 0x2;
729
+
bool has_buttons = (*buf & 0x10) == 0x10 && (*buf & 0x1) == 0x1;
730
+
731
+
// imu event follows when 111?1???
732
+
// there are imu-only messages, and imu-after-battery
733
+
bool has_imu = (*buf & 0x08) == 0x8;
734
+
735
+
// clang-format on
736
+
737
+
buf++;
738
+
739
+
if (has_battery) {
740
+
controller_handle_battery(d, *buf++);
741
+
}
742
+
743
+
if (has_buttons) {
744
+
controller_handle_buttons(d, *buf++);
745
+
}
746
+
if (has_trigger) {
747
+
controller_handle_analog_trigger(d, *buf++);
748
+
}
749
+
if (has_trackpad) {
750
+
controller_handle_touch_position(d, buf);
751
+
buf += 4;
752
+
}
753
+
if (has_imu) {
754
+
vive_controller_handle_imu_sample(
755
+
d, (struct vive_imu_report *)buf);
756
+
buf += 13;
757
+
}
758
+
}
759
+
760
+
if (buf > end)
761
+
VIVE_CONTROLLER_ERROR(d, "overshoot: %ld\n", buf - end);
762
+
763
+
if (buf < end)
764
+
vive_controller_handle_lighthousev1(buf, end - buf);
765
+
}
766
+
767
+
/*
768
+
* Handles battery, imu, trigger, buttons, trackpad.
769
+
* Then hands off to vive_controller_handle_lighthousev1().
770
+
*/
771
+
static void
772
+
vive_controller_decode_watchmanv2(struct vive_controller_device *d,
773
+
struct vive_controller_message *message)
774
+
{
775
+
uint8_t *buf = message->payload;
776
+
uint8_t *end = message->payload + message->len - 1;
777
+
778
+
/*
779
+
for (int i = 0; i < message->len; i++) {
780
+
//printf("%02x ", buf[i]);
781
+
int j = 8;
782
+
while(j--) {
783
+
putchar('0' + ((buf[i] >> j) & 1));
784
+
}
785
+
putchar(' ');
786
+
}
787
+
printf("\n");
788
+
*/
789
+
790
+
/* payload starts with "event flags" byte.
791
+
* If it does not start with 111, it contains only lighthouse data,
792
+
* and possibly gen2 events.
793
+
* If it starts with 111, events follow in this order, each of them
794
+
* optional:
795
+
* - battery: 1 byte (1110???1)
796
+
* - trigger: 1 byte (1111?1??)
797
+
* - trackpad: 4 byte (1111??1?)
798
+
* - buttons: 1 byte (1111???1)
799
+
* - imu: 13 byte (111?1???)
800
+
* There may be another input event after a battery event.
801
+
*/
802
+
803
+
// input events have first three bits set
804
+
if ((*buf & 0xe0) == 0xe0 && buf < end) {
805
+
806
+
// clang-format off
807
+
808
+
// battery follows when 1110???1
809
+
bool has_battery = (*buf & 0x10) != 0x10 && (*buf & 0x1) == 0x1;
810
+
811
+
// input follows when 1111?<trigger><trackpad><buttons>
812
+
bool has_trigger = (*buf & 0x10) == 0x10 && (*buf & 0x4) == 0x4;
813
+
bool has_trackpad = (*buf & 0x10) == 0x10 && (*buf & 0x2) == 0x2;
814
+
bool has_buttons = (*buf & 0x10) == 0x10 && (*buf & 0x1) == 0x1;
815
+
816
+
// imu event follows when 11101???
817
+
// there are imu-only messages, and imu-after-battery
818
+
bool has_imu = (*buf & 0x08) == 0x8 && (*buf & 0x10) != 0x10;
819
+
820
+
//! @todo: Confirm that messages 4th bit == 1 have no valid
821
+
// imu data that we erroneously drop
822
+
823
+
// clang-format on
824
+
825
+
buf++;
826
+
827
+
if (has_battery) {
828
+
controller_handle_battery(d, *buf++);
829
+
}
830
+
831
+
if (has_buttons) {
832
+
controller_handle_buttons(d, *buf++);
833
+
}
834
+
if (has_trigger) {
835
+
controller_handle_analog_trigger(d, *buf++);
836
+
}
837
+
if (has_trackpad) {
838
+
controller_handle_touch_position(d, buf);
839
+
buf += 4;
840
+
}
841
+
if (has_imu) {
842
+
vive_controller_handle_imu_sample(
843
+
d, (struct vive_imu_report *)buf);
844
+
}
845
+
}
846
+
847
+
if (buf > end)
848
+
VIVE_CONTROLLER_ERROR(d, "overshoot: %ld\n", buf - end);
849
+
850
+
//! @todo: Parse lighthouse v2 data
851
+
}
852
+
/*
853
+
* Decodes multiplexed Wireless Receiver messages.
854
+
*/
855
+
static void
856
+
vive_controller_decode_message(struct vive_controller_device *d,
857
+
struct vive_controller_message *message)
858
+
{
859
+
d->last_ticks =
860
+
(message->timestamp_hi << 24) | (message->timestamp_lo << 16);
861
+
862
+
//! @todo: Check if Vive controller on watchman2 is correctly handled
863
+
//! with watchman2 codepath
864
+
switch (d->watchman_gen) {
865
+
case WATCHMAN_GEN1:
866
+
vive_controller_decode_watchmanv1(d, message);
867
+
break;
868
+
case WATCHMAN_GEN2:
869
+
vive_controller_decode_watchmanv2(d, message);
870
+
break;
871
+
default: VIVE_CONTROLLER_ERROR(d, "Can't decode unknown watchman gen");
872
+
}
873
+
}
874
+
875
+
#define FEATURE_BUFFER_SIZE 256
876
+
877
+
static int
878
+
vive_controller_device_update(struct vive_controller_device *d)
879
+
{
880
+
uint8_t buf[FEATURE_BUFFER_SIZE];
881
+
do {
882
+
int ret = os_hid_read(d->controller_hid, buf, sizeof(buf), 0);
883
+
if (ret < 0) {
884
+
return ret;
885
+
}
886
+
if (ret == 0) {
887
+
// controller off
888
+
return true;
889
+
}
890
+
891
+
if (buf[0] == VIVE_CONTROLLER_REPORT1_ID) {
892
+
struct vive_controller_report1 *pkt =
893
+
(struct vive_controller_report1 *)buf;
894
+
vive_controller_decode_message(d, &pkt->message);
895
+
} else if (buf[0] == VIVE_CONTROLLER_REPORT2_ID) {
896
+
struct vive_controller_report2 *pkt =
897
+
(struct vive_controller_report2 *)buf;
898
+
vive_controller_decode_message(d, &pkt->message[0]);
899
+
vive_controller_decode_message(d, &pkt->message[1]);
900
+
} else if (buf[0] == VIVE_CONTROLLER_DISCONNECT_REPORT_ID) {
901
+
VIVE_CONTROLLER_DEBUG(d, "Controller disconnected.");
902
+
} else {
903
+
VIVE_CONTROLLER_ERROR(
904
+
d, "Unknown controller message type: %u", buf[0]);
905
+
}
906
+
907
+
} while (true);
908
+
909
+
return 0;
910
+
}
911
+
912
+
static void *
913
+
vive_controller_run_thread(void *ptr)
914
+
{
915
+
struct vive_controller_device *d = (struct vive_controller_device *)ptr;
916
+
917
+
uint8_t buf[FEATURE_BUFFER_SIZE];
918
+
while (os_hid_read(d->controller_hid, buf, sizeof(buf), 0) > 0) {
919
+
// Empty queue first
920
+
}
921
+
922
+
os_thread_helper_lock(&d->controller_thread);
923
+
while (os_thread_helper_is_running_locked(&d->controller_thread)) {
924
+
os_thread_helper_unlock(&d->controller_thread);
925
+
926
+
if (!vive_controller_device_update(d)) {
927
+
return NULL;
928
+
}
929
+
930
+
// Just keep swimming.
931
+
os_thread_helper_lock(&d->controller_thread);
932
+
}
933
+
934
+
return NULL;
935
+
}
936
+
937
+
static char *
938
+
_json_get_string(const cJSON *json, const char *name)
939
+
{
940
+
const cJSON *item = cJSON_GetObjectItemCaseSensitive(json, name);
941
+
return strdup(item->valuestring);
942
+
}
943
+
944
+
static void
945
+
print_vec3(const char *title, struct xrt_vec3 *vec)
946
+
{
947
+
printf("%s = %f %f %f\n", title, (double)vec->x, (double)vec->y,
948
+
(double)vec->z);
949
+
}
950
+
951
+
static void
952
+
_get_pose_from_pos_x_z(const cJSON *obj, struct xrt_pose *pose)
953
+
{
954
+
struct xrt_vec3 plus_x, plus_z;
955
+
u_json_get_vec3(u_json_get(obj, "plus_x"), &plus_x);
956
+
u_json_get_vec3(u_json_get(obj, "plus_z"), &plus_z);
957
+
u_json_get_vec3(u_json_get(obj, "position"), &pose->position);
958
+
959
+
math_quat_from_plus_x_z(&plus_x, &plus_z, &pose->orientation);
960
+
}
961
+
962
+
static bool
963
+
vive_controller_parse_config(struct vive_controller_device *d,
964
+
char *json_string)
965
+
{
966
+
VIVE_CONTROLLER_DEBUG(d, "JSON config:\n%s\n", json_string);
967
+
968
+
cJSON *json = cJSON_Parse(json_string);
969
+
if (!cJSON_IsObject(json)) {
970
+
VIVE_CONTROLLER_ERROR(d, "Could not parse JSON data.");
971
+
return false;
972
+
}
973
+
974
+
d->firmware.model_number = _json_get_string(json, "model_number");
975
+
if (strcmp(d->firmware.model_number, "Vive. Controller MV") == 0) {
976
+
d->variant = CONTROLLER_VIVE_WAND;
977
+
VIVE_CONTROLLER_DEBUG(d, "Found Vive Wand controller");
978
+
} else if (strcmp(d->firmware.model_number, "Knuckles Right") == 0) {
979
+
d->variant = CONTROLLER_INDEX_RIGHT;
980
+
VIVE_CONTROLLER_DEBUG(d, "Found Knuckles Right controller");
981
+
} else if (strcmp(d->firmware.model_number, "Knuckles Left") == 0) {
982
+
d->variant = CONTROLLER_INDEX_LEFT;
983
+
VIVE_CONTROLLER_DEBUG(d, "Found Knuckles Left controller");
984
+
} else {
985
+
VIVE_CONTROLLER_ERROR(d, "Failed to parse controller variant");
986
+
}
987
+
988
+
switch (d->variant) {
989
+
case CONTROLLER_VIVE_WAND: {
990
+
u_json_get_vec3(u_json_get(json, "acc_bias"), &d->imu.acc_bias);
991
+
u_json_get_vec3(u_json_get(json, "acc_scale"),
992
+
&d->imu.acc_scale);
993
+
u_json_get_vec3(u_json_get(json, "gyro_bias"),
994
+
&d->imu.gyro_bias);
995
+
u_json_get_vec3(u_json_get(json, "gyro_scale"),
996
+
&d->imu.gyro_scale);
997
+
d->firmware.mb_serial_number =
998
+
_json_get_string(json, "mb_serial_number");
999
+
} break;
1000
+
case CONTROLLER_INDEX_LEFT:
1001
+
case CONTROLLER_INDEX_RIGHT: {
1002
+
const cJSON *imu = u_json_get(json, "imu");
1003
+
_get_pose_from_pos_x_z(imu, &d->imu.trackref);
1004
+
1005
+
u_json_get_vec3(u_json_get(imu, "acc_bias"), &d->imu.acc_bias);
1006
+
u_json_get_vec3(u_json_get(imu, "acc_scale"),
1007
+
&d->imu.acc_scale);
1008
+
u_json_get_vec3(u_json_get(imu, "gyro_bias"),
1009
+
&d->imu.gyro_bias);
1010
+
} break;
1011
+
default:
1012
+
VIVE_CONTROLLER_ERROR(d, "Unknown Vive watchman variant.\n");
1013
+
return false;
1014
+
}
1015
+
1016
+
d->firmware.device_serial_number =
1017
+
_json_get_string(json, "device_serial_number");
1018
+
1019
+
cJSON_Delete(json);
1020
+
1021
+
// clang-format off
1022
+
VIVE_CONTROLLER_DEBUG(d, "= Vive controller configuration =");
1023
+
1024
+
VIVE_CONTROLLER_DEBUG(d, "model_number: %s", d->firmware.model_number);
1025
+
VIVE_CONTROLLER_DEBUG(d, "mb_serial_number: %s", d->firmware.mb_serial_number);
1026
+
VIVE_CONTROLLER_DEBUG(d, "device_serial_number: %s", d->firmware.device_serial_number);
1027
+
1028
+
if (d->print_debug) {
1029
+
print_vec3("acc_bias", &d->imu.acc_bias);
1030
+
print_vec3("acc_scale", &d->imu.acc_scale);
1031
+
print_vec3("gyro_bias", &d->imu.gyro_bias);
1032
+
print_vec3("gyro_scale", &d->imu.gyro_scale);
1033
+
}
1034
+
1035
+
// clang-format on
1036
+
1037
+
return true;
1038
+
}
1039
+
1040
+
/*
1041
+
*
1042
+
* Prober functions.
1043
+
*
1044
+
*/
1045
+
#define SET_WAND_INPUT(NAME, NAME2) \
1046
+
do { \
1047
+
(d->base.inputs[VIVE_CONTROLLER_INDEX_##NAME].name = \
1048
+
XRT_INPUT_VIVE_##NAME2); \
1049
+
} while (0)
1050
+
1051
+
#define SET_INDEX_INPUT(NAME, NAME2) \
1052
+
do { \
1053
+
(d->base.inputs[VIVE_CONTROLLER_INDEX_##NAME].name = \
1054
+
XRT_INPUT_INDEX_##NAME2); \
1055
+
} while (0)
1056
+
int
1057
+
vive_controller_found(struct xrt_prober *xp,
1058
+
struct xrt_prober_device **devices,
1059
+
size_t num_devices,
1060
+
size_t index,
1061
+
cJSON *attached_data,
1062
+
struct xrt_device **out_xdevs)
1063
+
{
1064
+
struct xrt_prober_device *dev = devices[index];
1065
+
int ret;
1066
+
1067
+
static int controller_num = 0;
1068
+
1069
+
struct os_hid_device *controller_hid = NULL;
1070
+
ret = xp->open_hid_interface(xp, dev, 0, &controller_hid);
1071
+
if (ret != 0) {
1072
+
return -1;
1073
+
}
1074
+
1075
+
enum u_device_alloc_flags flags = U_DEVICE_ALLOC_TRACKING_NONE;
1076
+
struct vive_controller_device *d = U_DEVICE_ALLOCATE(
1077
+
struct vive_controller_device, flags, VIVE_CONTROLLER_MAX_INDEX, 1);
1078
+
1079
+
d->watchman_gen = WATCHMAN_GEN_UNKNOWN;
1080
+
d->variant = CONTROLLER_UNKNOWN;
1081
+
1082
+
if (dev->vendor_id == VALVE_VID &&
1083
+
dev->product_id == VIVE_WATCHMAN_DONGLE) {
1084
+
d->watchman_gen = WATCHMAN_GEN1;
1085
+
VIVE_CONTROLLER_DEBUG(d, "Found watchman gen 1");
1086
+
} else if (dev->vendor_id == VALVE_VID &&
1087
+
dev->product_id == VIVE_WATCHMAN_DONGLE_GEN2) {
1088
+
d->watchman_gen = WATCHMAN_GEN2;
1089
+
VIVE_CONTROLLER_DEBUG(d, "Found watchman gen 2");
1090
+
} else {
1091
+
VIVE_CONTROLLER_ERROR(d, "Unknown watchman gen");
1092
+
}
1093
+
1094
+
m_imu_3dof_init(&d->fusion, M_IMU_3DOF_USE_GRAVITY_DUR_20MS);
1095
+
1096
+
/* default values, will be queried from device */
1097
+
d->imu.gyro_range = 8.726646f;
1098
+
d->imu.acc_range = 39.226600f;
1099
+
1100
+
d->imu.acc_scale.x = 1.0f;
1101
+
d->imu.acc_scale.y = 1.0f;
1102
+
d->imu.acc_scale.z = 1.0f;
1103
+
d->imu.gyro_scale.x = 1.0f;
1104
+
d->imu.gyro_scale.y = 1.0f;
1105
+
d->imu.gyro_scale.z = 1.0f;
1106
+
1107
+
d->imu.acc_bias.x = 0.0f;
1108
+
d->imu.acc_bias.y = 0.0f;
1109
+
d->imu.acc_bias.z = 0.0f;
1110
+
d->imu.gyro_bias.x = 0.0f;
1111
+
d->imu.gyro_bias.y = 0.0f;
1112
+
d->imu.gyro_bias.z = 0.0f;
1113
+
1114
+
d->print_spew = debug_get_bool_option_vive_controller_spew();
1115
+
d->print_debug = debug_get_bool_option_vive_controller_debug();
1116
+
1117
+
d->controller_hid = controller_hid;
1118
+
1119
+
d->base.destroy = vive_controller_device_destroy;
1120
+
d->base.get_tracked_pose = vive_controller_device_get_tracked_pose;
1121
+
d->base.set_output = vive_controller_device_set_output;
1122
+
1123
+
snprintf(d->base.str, XRT_DEVICE_NAME_LEN, "%s %i", "Vive Controller",
1124
+
(int)(controller_num));
1125
+
1126
+
d->index = controller_num;
1127
+
1128
+
//! @todo: reading range report fails for powered off controller
1129
+
if (vive_get_imu_range_report(d->controller_hid, &d->imu.gyro_range,
1130
+
&d->imu.acc_range) != 0) {
1131
+
VIVE_CONTROLLER_ERROR(
1132
+
d, "Could not get watchman IMU range packet!");
1133
+
free(d);
1134
+
return 0;
1135
+
}
1136
+
1137
+
VIVE_CONTROLLER_DEBUG(d, "Vive controller gyroscope range %f",
1138
+
d->imu.gyro_range);
1139
+
VIVE_CONTROLLER_DEBUG(d, "Vive controller accelerometer range %f",
1140
+
d->imu.acc_range);
1141
+
1142
+
// successful config parsing determines d->variant
1143
+
char *config = vive_read_config(d->controller_hid);
1144
+
if (config != NULL) {
1145
+
vive_controller_parse_config(d, config);
1146
+
free(config);
1147
+
} else {
1148
+
VIVE_CONTROLLER_ERROR(d,
1149
+
"Could not get Vive controller config\n");
1150
+
free(d);
1151
+
return 0;
1152
+
}
1153
+
1154
+
if (d->variant == CONTROLLER_VIVE_WAND) {
1155
+
d->base.name = XRT_DEVICE_VIVE_WAND;
1156
+
1157
+
SET_WAND_INPUT(SYSTEM_CLICK, SYSTEM_CLICK);
1158
+
SET_WAND_INPUT(SQUEEZE_CLICK, SQUEEZE_CLICK);
1159
+
SET_WAND_INPUT(MENU_CLICK, MENU_CLICK);
1160
+
SET_WAND_INPUT(TRIGGER_CLICK, TRIGGER_CLICK);
1161
+
SET_WAND_INPUT(TRIGGER_VALUE, TRIGGER_VALUE);
1162
+
SET_WAND_INPUT(TRACKPAD_X, TRACKPAD_X);
1163
+
SET_WAND_INPUT(TRACKPAD_Y, TRACKPAD_Y);
1164
+
SET_WAND_INPUT(TRACKPAD_CLICK, TRACKPAD_CLICK);
1165
+
SET_WAND_INPUT(TRACKPAD_TOUCH, TRACKPAD_TOUCH);
1166
+
1167
+
SET_WAND_INPUT(AIM_POSE, AIM_POSE);
1168
+
SET_WAND_INPUT(GRIP_POSE, GRIP_POSE);
1169
+
1170
+
d->base.outputs[0].name = XRT_OUTPUT_NAME_VIVE_HAPTIC;
1171
+
1172
+
d->base.update_inputs =
1173
+
vive_controller_device_update_wand_inputs;
1174
+
} else if (d->variant == CONTROLLER_INDEX_LEFT ||
1175
+
d->variant == CONTROLLER_INDEX_RIGHT) {
1176
+
d->base.name = XRT_DEVICE_INDEX_CONTROLLER;
1177
+
1178
+
SET_INDEX_INPUT(SYSTEM_CLICK, SYSTEM_CLICK);
1179
+
SET_INDEX_INPUT(A_CLICK, A_CLICK);
1180
+
SET_INDEX_INPUT(B_CLICK, B_CLICK);
1181
+
SET_INDEX_INPUT(TRIGGER_CLICK, TRIGGER_CLICK);
1182
+
SET_INDEX_INPUT(TRIGGER_VALUE, TRIGGER_VALUE);
1183
+
SET_INDEX_INPUT(TRACKPAD_X, TRACKPAD_X);
1184
+
SET_INDEX_INPUT(TRACKPAD_Y, TRACKPAD_Y);
1185
+
SET_INDEX_INPUT(TRACKPAD_TOUCH, TRACKPAD_TOUCH);
1186
+
SET_INDEX_INPUT(THUMBSTICK_X, THUMBSTICK_X);
1187
+
SET_INDEX_INPUT(THUMBSTICK_Y, THUMBSTICK_Y);
1188
+
SET_INDEX_INPUT(THUMBSTICK_CLICK, THUMBSTICK_CLICK);
1189
+
1190
+
SET_INDEX_INPUT(AIM_POSE, AIM_POSE);
1191
+
SET_INDEX_INPUT(GRIP_POSE, GRIP_POSE);
1192
+
1193
+
d->base.outputs[0].name = XRT_OUTPUT_NAME_INDEX_HAPTIC;
1194
+
1195
+
d->base.update_inputs =
1196
+
vive_controller_device_update_index_inputs;
1197
+
} else {
1198
+
d->base.name = XRT_DEVICE_GENERIC_HMD;
1199
+
VIVE_CONTROLLER_ERROR(d,
1200
+
"Failed to assign update input function");
1201
+
}
1202
+
1203
+
if (d->controller_hid) {
1204
+
ret = os_thread_helper_start(&d->controller_thread,
1205
+
vive_controller_run_thread, d);
1206
+
if (ret != 0) {
1207
+
VIVE_CONTROLLER_ERROR(
1208
+
d, "Failed to start mainboard thread!");
1209
+
vive_controller_device_destroy((struct xrt_device *)d);
1210
+
return 0;
1211
+
}
1212
+
}
1213
+
1214
+
out_xdevs[0] = &(d->base);
1215
+
VIVE_CONTROLLER_DEBUG(d, "Opened vive controller!\n");
1216
+
1217
+
controller_num++;
1218
+
return 1;
1219
+
}
+43
src/xrt/drivers/vive/vive_controller_interface.h
+43
src/xrt/drivers/vive/vive_controller_interface.h
···
1
+
// Copyright 2020, Collabora, Ltd.
2
+
// SPDX-License-Identifier: BSL-1.0
3
+
/*!
4
+
* @file
5
+
* @brief Interface to @ref drv_vive
6
+
* @author Christoph Haag <christoph.haag@collabora.com>
7
+
* @author Ryan Pavlik <ryan.pavlik@collabora.com>
8
+
* @ingroup drv_vive
9
+
*/
10
+
11
+
#pragma once
12
+
13
+
#include <stdlib.h>
14
+
15
+
#ifdef __cplusplus
16
+
extern "C" {
17
+
#endif
18
+
/*!
19
+
* @ingroup drv_vive
20
+
*
21
+
* @brief Driver for the HTC Vive and Valve Index controllers.
22
+
*/
23
+
24
+
#define VALVE_VID 0x28de
25
+
#define VIVE_WATCHMAN_DONGLE 0x2101
26
+
#define VIVE_WATCHMAN_DONGLE_GEN2 0x2102
27
+
28
+
/*!
29
+
* Probing function for HTC Vive and Valve Index devices.
30
+
*
31
+
* @ingroup drv_vive
32
+
*/
33
+
int
34
+
vive_controller_found(struct xrt_prober *xp,
35
+
struct xrt_prober_device **devices,
36
+
size_t num_devices,
37
+
size_t index,
38
+
cJSON *attached_data,
39
+
struct xrt_device **out_xdevs);
40
+
41
+
#ifdef __cplusplus
42
+
}
43
+
#endif
+1
src/xrt/drivers/vive/vive_protocol.c
+1
src/xrt/drivers/vive/vive_protocol.c
+4
-1
src/xrt/drivers/vive/vive_protocol.h
+4
-1
src/xrt/drivers/vive/vive_protocol.h
+5
src/xrt/targets/common/target_lists.c
+5
src/xrt/targets/common/target_lists.c
···
44
44
45
45
#ifdef XRT_BUILD_DRIVER_VIVE
46
46
#include "vive/vive_prober.h"
47
+
#include "vive/vive_controller_interface.h"
47
48
#endif
48
49
49
50
#ifdef XRT_BUILD_DRIVER_DAYDREAM
···
85
86
{HTC_VID, VIVE_PID, vive_found, "HTC Vive"},
86
87
{HTC_VID, VIVE_PRO_MAINBOARD_PID, vive_found, "HTC Vive Pro"},
87
88
{VALVE_VID, VIVE_PRO_LHR_PID, vive_found, "Valve Index"},
89
+
{VALVE_VID, VIVE_WATCHMAN_DONGLE, vive_controller_found,
90
+
"HTC Vive Watchman Wireless Device"},
91
+
{VALVE_VID, VIVE_WATCHMAN_DONGLE_GEN2, vive_controller_found,
92
+
"Valve Watchman Wireless Device"},
88
93
#endif
89
94
90
95
{0x0000, 0x0000, NULL, NULL}, // Terminate