matrixfurry.com
The open source OpenXR runtime
1// Copyright 2016 Philipp Zabel
2// Copyright 2020-2023, Collabora, Ltd.
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 Rylie Pavlik <rylie.pavlik@collabora.com>
10 * @author Jakob Bornecrantz <jakob@collabora.com>
11 *
12 * Portions based on the VRPN Razer Hydra driver,
13 * originally written by Rylie Pavlik and available under the BSL-1.0.
14 */
15
16#include "xrt/xrt_defines.h"
17#include "xrt/xrt_prober.h"
18
19#include "math/m_imu_3dof.h"
20#include "math/m_relation_history.h"
21
22#include "util/u_var.h"
23
24#include "os/os_hid.h"
25#include "os/os_threading.h"
26#include "os/os_time.h"
27
28#include "math/m_api.h"
29#include "math/m_space.h"
30#include "math/m_predict.h"
31
32#include "util/u_json.h"
33#include "util/u_misc.h"
34#include "util/u_time.h"
35#include "util/u_debug.h"
36#include "util/u_device.h"
37#include "util/u_trace_marker.h"
38
39#include "vive/vive_config.h"
40#include "vive/vive_bindings.h"
41#include "vive/vive_poses.h"
42
43#include "vive.h"
44#include "vive_protocol.h"
45#include "vive_controller.h"
46#include "util/u_hand_simulation.h"
47
48#include <assert.h>
49#include <stdio.h>
50#include <stdlib.h>
51#include <string.h>
52
53#ifdef XRT_OS_LINUX
54#include <unistd.h>
55#include <math.h>
56#endif
57
58
59/*
60 *
61 * Defines & structs.
62 *
63 */
64
65enum vive_controller_input_index
66{
67 // common inputs
68 VIVE_CONTROLLER_INDEX_AIM_POSE = 0,
69 VIVE_CONTROLLER_INDEX_GRIP_POSE,
70 VIVE_CONTROLLER_INDEX_SYSTEM_CLICK,
71 VIVE_CONTROLLER_INDEX_TRIGGER_CLICK,
72 VIVE_CONTROLLER_INDEX_TRIGGER_VALUE,
73 VIVE_CONTROLLER_INDEX_TRACKPAD,
74 VIVE_CONTROLLER_INDEX_TRACKPAD_TOUCH,
75
76 // Vive Wand specific inputs
77 VIVE_CONTROLLER_INDEX_SQUEEZE_CLICK,
78 VIVE_CONTROLLER_INDEX_MENU_CLICK,
79 VIVE_CONTROLLER_INDEX_TRACKPAD_CLICK,
80
81 // Valve Index specific inputs
82 VIVE_CONTROLLER_INDEX_THUMBSTICK,
83 VIVE_CONTROLLER_INDEX_A_CLICK,
84 VIVE_CONTROLLER_INDEX_B_CLICK,
85 VIVE_CONTROLLER_INDEX_THUMBSTICK_CLICK,
86 VIVE_CONTROLLER_INDEX_THUMBSTICK_TOUCH,
87 VIVE_CONTROLLER_INDEX_SYSTEM_TOUCH,
88 VIVE_CONTROLLER_INDEX_A_TOUCH,
89 VIVE_CONTROLLER_INDEX_B_TOUCH,
90 VIVE_CONTROLLER_INDEX_SQUEEZE_VALUE,
91 VIVE_CONTROLLER_INDEX_SQUEEZE_FORCE,
92 VIVE_CONTROLLER_INDEX_TRIGGER_TOUCH,
93 VIVE_CONTROLLER_INDEX_TRACKPAD_FORCE,
94
95 VIVE_CONTROLLER_HAND_TRACKING,
96
97 VIVE_CONTROLLER_MAX_INDEX,
98};
99
100
101
102#define DEFAULT_HAPTIC_FREQ 150.0f
103#define MIN_HAPTIC_DURATION 0.05f
104
105// Debug define(s), always off.
106#undef WATCHMAN2_PRINT_HID
107
108
109/*
110 *
111 * Helper functions.
112 *
113 */
114
115static inline struct vive_controller_device *
116vive_controller_device(struct xrt_device *xdev)
117{
118 assert(xdev);
119 struct vive_controller_device *ret = (struct vive_controller_device *)xdev;
120 return ret;
121}
122
123static inline void
124get_pose(struct vive_controller_device *d,
125 enum xrt_input_name name,
126 int64_t at_timestamp_ns,
127 struct xrt_space_relation *out_relation)
128{
129 struct xrt_space_relation imu_relation = {0};
130 imu_relation.relation_flags = XRT_SPACE_RELATION_BITMASK_ALL;
131 m_relation_history_get(d->fusion.relation_hist, at_timestamp_ns, &imu_relation);
132 imu_relation.relation_flags = XRT_SPACE_RELATION_BITMASK_ALL; // Needed after history_get
133
134 // Get the offset to the pose (this is from libsurvive's reporting position currently)
135 struct xrt_pose pose_offset = XRT_POSE_IDENTITY;
136 vive_poses_get_pose_offset(d->base.name, d->base.device_type, name, &pose_offset);
137
138 // We want this to make grip the center of rotation.
139 struct xrt_pose grip = XRT_POSE_IDENTITY;
140 enum xrt_input_name grip_name = XRT_INPUT_INDEX_GRIP_POSE; //! @todo Vive poses only have index poses.
141 vive_poses_get_pose_offset(d->base.name, d->base.device_type, grip_name, &grip);
142
143 // Build proper relation.
144 struct xrt_relation_chain chain = {0};
145 m_relation_chain_push_pose(&chain, &pose_offset);
146 m_relation_chain_push_inverted_pose_if_not_identity(&chain, &grip);
147 m_relation_chain_push_relation(&chain, &imu_relation);
148 m_relation_chain_push_pose_if_not_identity(&chain, &d->offset);
149
150 // And resolve it.
151 struct xrt_space_relation relation = {0};
152 m_relation_chain_resolve(&chain, &relation);
153
154 relation.linear_velocity = (struct xrt_vec3){0, 0, 0};
155
156 *out_relation = relation;
157}
158
159
160/*
161 *
162 * Member functions.
163 *
164 */
165
166static void
167vive_controller_device_destroy(struct xrt_device *xdev)
168{
169 struct vive_controller_device *d = vive_controller_device(xdev);
170
171 os_thread_helper_destroy(&d->controller_thread);
172
173 // Now that the thread is not running we can destroy the lock.
174 os_mutex_destroy(&d->lock);
175
176 os_mutex_destroy(&d->fusion.mutex);
177 m_relation_history_destroy(&d->fusion.relation_hist);
178 m_imu_3dof_close(&d->fusion.i3dof);
179
180 if (d->controller_hid)
181 os_hid_destroy(d->controller_hid);
182
183 free(d);
184}
185
186static xrt_result_t
187vive_controller_device_wand_update_inputs(struct xrt_device *xdev)
188{
189 struct vive_controller_device *d = vive_controller_device(xdev);
190
191 os_mutex_lock(&d->lock);
192
193 uint8_t buttons = d->state.buttons;
194
195 /*
196 int i = 8;
197 while(i--) {
198 putchar('0' + ((buttons >> i) & 1));
199 }
200 printf("\n");
201 */
202
203 uint64_t now = os_monotonic_get_ns();
204
205 /* d->state.buttons is bitmask of currently pressed buttons.
206 * (index n) nth bit in the bitmask -> input "name"
207 */
208 const int button_index_map[] = {VIVE_CONTROLLER_INDEX_TRIGGER_CLICK, VIVE_CONTROLLER_INDEX_TRACKPAD_TOUCH,
209 VIVE_CONTROLLER_INDEX_TRACKPAD_CLICK, VIVE_CONTROLLER_INDEX_SYSTEM_CLICK,
210 VIVE_CONTROLLER_INDEX_SQUEEZE_CLICK, VIVE_CONTROLLER_INDEX_MENU_CLICK};
211
212 int button_count = ARRAY_SIZE(button_index_map);
213 for (int i = 0; i < button_count; i++) {
214
215 bool pressed = (buttons >> i) & 1;
216 bool last_pressed = (d->state.last_buttons >> i) & 1;
217
218 if (pressed != last_pressed) {
219 struct xrt_input *input = &d->base.inputs[button_index_map[i]];
220
221 input->timestamp = now;
222 input->value.boolean = pressed;
223
224 VIVE_DEBUG(d, "button %d %s\n", i, pressed ? "pressed" : "released");
225 }
226 }
227 d->state.last_buttons = d->state.buttons;
228
229
230 struct xrt_input *trackpad_input = &d->base.inputs[VIVE_CONTROLLER_INDEX_TRACKPAD];
231 trackpad_input->timestamp = now;
232 trackpad_input->value.vec2.x = d->state.trackpad.x;
233 trackpad_input->value.vec2.y = d->state.trackpad.y;
234 VIVE_TRACE(d, "Trackpad: %f, %f", d->state.trackpad.x, d->state.trackpad.y);
235
236
237 struct xrt_input *trigger_input = &d->base.inputs[VIVE_CONTROLLER_INDEX_TRIGGER_VALUE];
238 trigger_input->timestamp = now;
239 trigger_input->value.vec1.x = d->state.trigger;
240 VIVE_TRACE(d, "Trigger: %f", d->state.trigger);
241
242 os_mutex_unlock(&d->lock);
243
244 return XRT_SUCCESS;
245}
246
247static xrt_result_t
248vive_controller_device_index_update_inputs(struct xrt_device *xdev)
249{
250 XRT_TRACE_MARKER();
251
252 struct vive_controller_device *d = vive_controller_device(xdev);
253
254 os_mutex_lock(&d->lock);
255 uint8_t buttons = d->state.buttons;
256
257 /*
258 int i = 8;
259 while(i--) {
260 putchar('0' + ((buttons >> i) & 1));
261 }
262 printf("\n");
263 */
264
265 bool was_trackpad_touched = d->base.inputs[VIVE_CONTROLLER_INDEX_TRACKPAD_TOUCH].value.boolean;
266
267 uint64_t now = os_monotonic_get_ns();
268
269 /* d->state.buttons is bitmask of currently pressed buttons.
270 * (index n) nth bit in the bitmask -> input "name"
271 */
272 const int button_index_map[] = {VIVE_CONTROLLER_INDEX_TRIGGER_CLICK, VIVE_CONTROLLER_INDEX_TRACKPAD_TOUCH,
273 VIVE_CONTROLLER_INDEX_THUMBSTICK_CLICK, VIVE_CONTROLLER_INDEX_SYSTEM_CLICK,
274 VIVE_CONTROLLER_INDEX_A_CLICK, VIVE_CONTROLLER_INDEX_B_CLICK};
275
276 int button_count = ARRAY_SIZE(button_index_map);
277 for (int i = 0; i < button_count; i++) {
278
279 bool pressed = (buttons >> i) & 1;
280 bool last_pressed = (d->state.last_buttons >> i) & 1;
281
282 if (pressed != last_pressed) {
283 struct xrt_input *input = &d->base.inputs[button_index_map[i]];
284
285 input->timestamp = now;
286 input->value.boolean = pressed;
287
288 VIVE_DEBUG(d, "button %d %s\n", i, pressed ? "pressed" : "released");
289 }
290 }
291 d->state.last_buttons = d->state.buttons;
292
293 bool is_trackpad_touched = d->base.inputs[VIVE_CONTROLLER_INDEX_TRACKPAD_TOUCH].value.boolean;
294
295 /* trackpad and thumbstick position are the same usb events.
296 * report trackpad position when trackpad has been touched last, and
297 * thumbstick position when trackpad touch has been released
298 */
299 struct xrt_input *thumb_input;
300
301 // after releasing trackpad, next 0,0 position still goes to trackpad
302 if (is_trackpad_touched || was_trackpad_touched)
303 thumb_input = &d->base.inputs[VIVE_CONTROLLER_INDEX_TRACKPAD];
304 else
305 thumb_input = &d->base.inputs[VIVE_CONTROLLER_INDEX_THUMBSTICK];
306 thumb_input->timestamp = now;
307 thumb_input->value.vec2.x = d->state.trackpad.x;
308 thumb_input->value.vec2.y = d->state.trackpad.y;
309
310 const char *component = is_trackpad_touched || was_trackpad_touched ? "Trackpad" : "Thumbstick";
311 VIVE_TRACE(d, "%s: %f, %f", component, d->state.trackpad.x, d->state.trackpad.y);
312
313
314 struct xrt_input *trigger_input = &d->base.inputs[VIVE_CONTROLLER_INDEX_TRIGGER_VALUE];
315
316 trigger_input->timestamp = now;
317 trigger_input->value.vec1.x = d->state.trigger;
318
319 VIVE_TRACE(d, "Trigger: %f", d->state.trigger);
320
321
322 /* d->state.touch is bitmask of currently touched buttons.
323 * (index n) nth bit in the bitmask -> input "name"
324 */
325 const int touched_button_index_map[] = {0,
326 0,
327 0,
328 VIVE_CONTROLLER_INDEX_SYSTEM_TOUCH,
329 VIVE_CONTROLLER_INDEX_A_TOUCH,
330 VIVE_CONTROLLER_INDEX_B_TOUCH,
331 VIVE_CONTROLLER_INDEX_THUMBSTICK_TOUCH};
332 int touch_button_count = ARRAY_SIZE(touched_button_index_map);
333 uint8_t touch_buttons = d->state.touch;
334 for (int i = 0; i < touch_button_count; i++) {
335
336 bool touched = (touch_buttons >> i) & 1;
337 bool last_touched = (d->state.last_touch >> i) & 1;
338
339 if (touched != last_touched) {
340 struct xrt_input *input = &d->base.inputs[touched_button_index_map[i]];
341
342 input->timestamp = now;
343 input->value.boolean = touched;
344
345 VIVE_DEBUG(d, "button %d %s\n", i, touched ? "touched" : "untouched");
346 }
347 }
348 d->state.last_touch = d->state.touch;
349
350 d->base.inputs[VIVE_CONTROLLER_INDEX_SQUEEZE_FORCE].value.vec1.x = (float)d->state.squeeze_force / UINT8_MAX;
351 d->base.inputs[VIVE_CONTROLLER_INDEX_SQUEEZE_FORCE].timestamp = now;
352 if (d->state.squeeze_force > 0) {
353 VIVE_DEBUG(d, "Squeeze force: %f\n", (float)d->state.squeeze_force / UINT8_MAX);
354 }
355
356 d->base.inputs[VIVE_CONTROLLER_INDEX_TRACKPAD_FORCE].value.vec1.x = (float)d->state.trackpad_force / UINT8_MAX;
357 d->base.inputs[VIVE_CONTROLLER_INDEX_TRACKPAD_FORCE].timestamp = now;
358 if (d->state.trackpad_force > 0) {
359 VIVE_DEBUG(d, "Trackpad force: %f\n", (float)d->state.trackpad_force / UINT8_MAX);
360 }
361
362 os_mutex_unlock(&d->lock);
363
364 return XRT_SUCCESS;
365}
366
367static xrt_result_t
368vive_controller_get_hand_tracking(struct xrt_device *xdev,
369 enum xrt_input_name name,
370 int64_t requested_timestamp_ns,
371 struct xrt_hand_joint_set *out_value,
372 int64_t *out_timestamp_ns)
373{
374 XRT_TRACE_MARKER();
375
376 struct vive_controller_device *d = vive_controller_device(xdev);
377
378 if (name != XRT_INPUT_HT_CONFORMING_LEFT && name != XRT_INPUT_HT_CONFORMING_RIGHT) {
379 U_LOG_XDEV_UNSUPPORTED_INPUT(&d->base, d->log_level, name);
380 return XRT_ERROR_INPUT_UNSUPPORTED;
381 }
382
383 enum xrt_hand hand = d->config.variant == CONTROLLER_INDEX_LEFT ? XRT_HAND_LEFT : XRT_HAND_RIGHT;
384
385 float thumb_curl = 0.0f;
386 //! @todo place thumb preciely on the button that is touched/pressed
387 if (d->base.inputs[VIVE_CONTROLLER_INDEX_A_TOUCH].value.boolean ||
388 d->base.inputs[VIVE_CONTROLLER_INDEX_B_TOUCH].value.boolean ||
389 d->base.inputs[VIVE_CONTROLLER_INDEX_THUMBSTICK_TOUCH].value.boolean ||
390 d->base.inputs[VIVE_CONTROLLER_INDEX_TRACKPAD_TOUCH].value.boolean) {
391 thumb_curl = 1.0;
392 }
393
394 struct u_hand_tracking_curl_values values = {
395 .little = (float)d->state.pinky_finger_handle / UINT8_MAX,
396 .ring = (float)d->state.ring_finger_handle / UINT8_MAX,
397 .middle = (float)d->state.middle_finger_handle / UINT8_MAX,
398 .index = (float)d->state.index_finger_trigger / UINT8_MAX,
399 .thumb = thumb_curl,
400 };
401
402 struct xrt_space_relation hand_relation;
403 get_pose(d, name, requested_timestamp_ns, &hand_relation);
404
405 u_hand_sim_simulate_for_valve_index_knuckles(&values, hand, &hand_relation, out_value);
406
407 // This is the truth - we pose-predicted or interpolated all the way up to `at_timestamp_ns`.
408 *out_timestamp_ns = requested_timestamp_ns;
409
410 out_value->is_active = true;
411 return XRT_SUCCESS;
412}
413
414static xrt_result_t
415vive_controller_device_get_tracked_pose(struct xrt_device *xdev,
416 enum xrt_input_name name,
417 int64_t at_timestamp_ns,
418 struct xrt_space_relation *out_relation)
419{
420 struct vive_controller_device *d = vive_controller_device(xdev);
421
422 // U_LOG_D("input name %d %d", name, XRT_INPUT_VIVE_GRIP_POSE);
423 if (name != XRT_INPUT_VIVE_AIM_POSE && name != XRT_INPUT_VIVE_GRIP_POSE && name != XRT_INPUT_INDEX_AIM_POSE &&
424 name != XRT_INPUT_INDEX_GRIP_POSE) {
425 U_LOG_XDEV_UNSUPPORTED_INPUT(&d->base, d->log_level, name);
426 return XRT_ERROR_INPUT_UNSUPPORTED;
427 }
428
429 get_pose(d, name, at_timestamp_ns, out_relation);
430
431 return XRT_SUCCESS;
432}
433
434static int
435vive_controller_haptic_pulse(struct vive_controller_device *d, const struct xrt_output_value *value)
436{
437 float duration_seconds;
438 if (value->vibration.duration_ns == XRT_MIN_HAPTIC_DURATION) {
439 VIVE_TRACE(d, "Haptic pulse duration: using %f minimum", MIN_HAPTIC_DURATION);
440 duration_seconds = MIN_HAPTIC_DURATION;
441 } else {
442 duration_seconds = time_ns_to_s(value->vibration.duration_ns);
443 }
444
445 VIVE_TRACE(d, "Haptic pulse amp %f, %fHz, %fs", value->vibration.amplitude, value->vibration.frequency,
446 duration_seconds);
447 float frequency = value->vibration.frequency;
448
449 if (frequency == XRT_FREQUENCY_UNSPECIFIED) {
450 VIVE_TRACE(d, "Haptic pulse frequency unspecified, setting to %fHz", DEFAULT_HAPTIC_FREQ);
451 frequency = DEFAULT_HAPTIC_FREQ;
452 }
453
454
455 /* haptic pulse for Vive Controller:
456 * desired_frequency = 1000 * 1000 / (high + low).
457 * => (high + low) = 1000 * 1000 / desired_frequency
458 * repeat = desired_duration_in_seconds * desired_frequency.
459 *
460 * I think:
461 * Lowest amplitude: 1, high+low-1
462 * Highest amplitude: (high+low)/2, / (high+low)/2
463 */
464
465 float high_plus_low = 1000.f * 1000.f / frequency;
466 uint16_t pulse_low = (uint16_t)(value->vibration.amplitude * high_plus_low / 2.);
467
468 /* Vive Controller doesn't vibrate with value == 0.
469 * Not sure if this actually happens, but let's fix it anyway. */
470 if (pulse_low == 0)
471 pulse_low = 1;
472
473 uint16_t pulse_high = high_plus_low - pulse_low;
474
475 uint16_t repeat_count = duration_seconds * frequency;
476
477 const struct vive_controller_haptic_pulse_report report = {
478 .id = VIVE_CONTROLLER_COMMAND_REPORT_ID,
479 .command = VIVE_CONTROLLER_HAPTIC_PULSE_COMMAND,
480 .len = 7,
481 .zero = 0x00,
482 .pulse_high = __cpu_to_le16(pulse_high),
483 .pulse_low = __cpu_to_le16(pulse_low),
484 .repeat_count = __cpu_to_le16(repeat_count),
485 };
486
487 return os_hid_set_feature(d->controller_hid, (uint8_t *)&report, sizeof(report));
488}
489
490static xrt_result_t
491vive_controller_device_set_output(struct xrt_device *xdev,
492 enum xrt_output_name name,
493 const struct xrt_output_value *value)
494{
495 struct vive_controller_device *d = vive_controller_device(xdev);
496
497 if (name != XRT_OUTPUT_NAME_VIVE_HAPTIC && name != XRT_OUTPUT_NAME_INDEX_HAPTIC) {
498 U_LOG_XDEV_UNSUPPORTED_OUTPUT(&d->base, d->log_level, name);
499 return XRT_ERROR_OUTPUT_UNSUPPORTED;
500 }
501
502 if (value->vibration.amplitude > 0.01) {
503 os_mutex_lock(&d->lock);
504 vive_controller_haptic_pulse(d, value);
505 os_mutex_unlock(&d->lock);
506 }
507
508 return XRT_SUCCESS;
509}
510
511
512/*
513 *
514 * Misc functions.
515 *
516 */
517
518static void
519controller_handle_battery(struct vive_controller_device *d, struct vive_controller_battery_sample *sample)
520{
521 uint8_t charge_percent = sample->battery & VIVE_CONTROLLER_BATTERY_CHARGE_MASK;
522 bool charging = sample->battery & VIVE_CONTROLLER_BATTERY_CHARGING;
523 VIVE_DEBUG(d, "Charging %d, percent %d\n", charging, charge_percent);
524 d->state.charging = charging;
525 d->state.battery = charge_percent;
526}
527
528static void
529controller_handle_buttons(struct vive_controller_device *d, struct vive_controller_button_sample *sample)
530{
531 d->state.buttons = sample->buttons;
532}
533
534static void
535controller_handle_touch_position(struct vive_controller_device *d, struct vive_controller_touch_sample *sample)
536{
537 int16_t x = __le16_to_cpu(sample->touch[0]);
538 int16_t y = __le16_to_cpu(sample->touch[1]);
539 d->state.trackpad.x = (float)x / INT16_MAX;
540 d->state.trackpad.y = (float)y / INT16_MAX;
541 if (d->state.trackpad.x != 0 || d->state.trackpad.y != 0)
542 VIVE_TRACE(d, "Trackpad %f,%f\n", d->state.trackpad.x, d->state.trackpad.y);
543}
544
545static void
546controller_handle_analog_trigger(struct vive_controller_device *d, struct vive_controller_trigger_sample *sample)
547{
548 d->state.trigger = (float)sample->trigger / UINT8_MAX;
549 VIVE_TRACE(d, "Trigger %f\n", d->state.trigger);
550}
551
552
553static void
554vive_controller_handle_imu_sample(struct vive_controller_device *d, struct watchman_imu_sample *sample)
555{
556 XRT_TRACE_MARKER();
557
558 uint64_t now_ns = os_monotonic_get_ns();
559
560 /* ouvrt: "Time in 48 MHz ticks, but we are missing the low byte" */
561 uint32_t time_raw = d->last_ticks | (sample->timestamp_hi << 8);
562 ticks_to_ns(time_raw, &d->imu.last_sample_ticks, &d->imu.last_sample_ts_ns);
563
564 int16_t acc[3] = {
565 __le16_to_cpu(sample->acc[0]),
566 __le16_to_cpu(sample->acc[1]),
567 __le16_to_cpu(sample->acc[2]),
568 };
569
570 int16_t gyro[3] = {
571 __le16_to_cpu(sample->gyro[0]),
572 __le16_to_cpu(sample->gyro[1]),
573 __le16_to_cpu(sample->gyro[2]),
574 };
575
576 float scale = (float)d->config.imu.acc_range / 32768.0f;
577 struct xrt_vec3 acceleration = {
578 scale * d->config.imu.acc_scale.x * acc[0] - d->config.imu.acc_bias.x,
579 scale * d->config.imu.acc_scale.y * acc[1] - d->config.imu.acc_bias.y,
580 scale * d->config.imu.acc_scale.z * acc[2] - d->config.imu.acc_bias.z,
581 };
582
583 scale = (float)d->config.imu.gyro_range / 32768.0f;
584 struct xrt_vec3 angular_velocity = {
585 scale * d->config.imu.gyro_scale.x * gyro[0] - d->config.imu.gyro_bias.x,
586 scale * d->config.imu.gyro_scale.y * gyro[1] - d->config.imu.gyro_bias.y,
587 scale * d->config.imu.gyro_scale.z * gyro[2] - d->config.imu.gyro_bias.z,
588 };
589
590 VIVE_TRACE(d, "ACC %f %f %f", acceleration.x, acceleration.y, acceleration.z);
591 VIVE_TRACE(d, "GYRO %f %f %f", angular_velocity.x, angular_velocity.y, angular_velocity.z);
592 /*
593 */
594
595 if (d->config.variant == CONTROLLER_VIVE_WAND) {
596 struct xrt_vec3 fixed_acceleration = {.x = -acceleration.x, .y = -acceleration.z, .z = -acceleration.y};
597 acceleration = fixed_acceleration;
598
599 struct xrt_vec3 fixed_angular_velocity = {
600 .x = -angular_velocity.x, .y = -angular_velocity.z, .z = -angular_velocity.y};
601 angular_velocity = fixed_angular_velocity;
602 } else if (d->config.variant == CONTROLLER_INDEX_RIGHT) {
603 struct xrt_vec3 fixed_acceleration = {.x = acceleration.z, .y = -acceleration.y, .z = acceleration.x};
604 acceleration = fixed_acceleration;
605
606 struct xrt_vec3 fixed_angular_velocity = {
607 .x = angular_velocity.z, .y = -angular_velocity.y, .z = angular_velocity.x};
608 angular_velocity = fixed_angular_velocity;
609 } else if (d->config.variant == CONTROLLER_INDEX_LEFT) {
610 struct xrt_vec3 fixed_acceleration = {.x = -acceleration.z, .y = acceleration.x, .z = -acceleration.y};
611 acceleration = fixed_acceleration;
612
613 struct xrt_vec3 fixed_angular_velocity = {
614 .x = -angular_velocity.z, .y = angular_velocity.x, .z = -angular_velocity.y};
615 angular_velocity = fixed_angular_velocity;
616 }
617
618 d->last.acc = acceleration;
619 d->last.gyro = angular_velocity;
620
621 struct xrt_space_relation rel = {0};
622 rel.relation_flags = XRT_SPACE_RELATION_ORIENTATION_VALID_BIT | XRT_SPACE_RELATION_ORIENTATION_TRACKED_BIT;
623
624 os_mutex_lock(&d->fusion.mutex);
625 m_imu_3dof_update(&d->fusion.i3dof, d->imu.last_sample_ts_ns, &acceleration, &angular_velocity);
626 rel.pose.orientation = d->fusion.i3dof.rot;
627 os_mutex_unlock(&d->fusion.mutex);
628
629 m_relation_history_push(d->fusion.relation_hist, &rel, now_ns);
630
631 // Update the pose we show in the GUI.
632 d->pose = rel.pose;
633}
634
635static void
636controller_handle_touch_force(struct vive_controller_device *d, struct watchman_touch_force *sample)
637{
638 d->state.touch = sample->touch;
639
640 d->state.middle_finger_handle = sample->middle_finger_handle;
641 d->state.ring_finger_handle = sample->ring_finger_handle;
642 d->state.pinky_finger_handle = sample->pinky_finger_handle;
643 d->state.index_finger_trigger = sample->index_finger_trigger;
644
645 d->state.squeeze_force = sample->squeeze_force;
646 d->state.trackpad_force = sample->trackpad_force;
647}
648
649static void
650vive_controller_handle_lighthousev1(struct vive_controller_device *d, uint8_t *buf, uint8_t len)
651{
652 VIVE_TRACE(d, "Got lighthouse message with len %d.\n", len);
653}
654
655/*
656 * Handles battery, imu, trigger, buttons, trackpad.
657 * Then hands off to vive_controller_handle_lighthousev1().
658 */
659static void
660vive_controller_decode_watchmanv1(struct vive_controller_device *d, struct vive_controller_message *message)
661{
662 uint8_t *buf = message->payload;
663 uint8_t *end = message->payload + message->len - 1;
664
665 /*
666 for (int i = 0; i < message->len; i++) {
667 //printf("%02x ", buf[i]);
668 int j = 8;
669 while(j--) {
670 putchar('0' + ((buf[i] >> j) & 1));
671 }
672 putchar(' ');
673 }
674 printf("\n");
675 */
676
677 /* payload starts with "event flags" byte.
678 * If it does not start with 111, it contains only lighthouse data.
679 * If it starts with 111, events follow in this order, each of them
680 * optional:
681 * - battery: 1 byte (1110???1)
682 * - trigger: 1 byte (1111?1??)
683 * - trackpad: 4 byte (1111??1?)
684 * - buttons: 1 byte (1111???1)
685 * - imu: 13 byte (111?1???)
686 * There may be another input event after a battery event.
687 * Lighthouse data may follow in the rest of the payload.
688 */
689
690 // input events have first three bits set
691 if ((*buf & 0xe0) == 0xe0 && buf < end) {
692
693 // clang-format off
694
695 // battery follows when 1110???1
696 bool has_battery = (*buf & 0x10) != 0x10 && (*buf & 0x1) == 0x1;
697
698 // input follows when 1111?<trigger><trackpad><buttons>
699 bool has_trigger = (*buf & 0x10) == 0x10 && (*buf & 0x4) == 0x4;
700 bool has_trackpad = (*buf & 0x10) == 0x10 && (*buf & 0x2) == 0x2;
701 bool has_buttons = (*buf & 0x10) == 0x10 && (*buf & 0x1) == 0x1;
702
703 // imu event follows when 111?1???
704 // there are imu-only messages, and imu-after-battery
705 bool has_imu = (*buf & 0x08) == 0x8;
706
707 // clang-format on
708
709 VIVE_TRACE(d,
710 "battery %d trigger %d trackpad %d "
711 "buttons %d imu %d",
712 has_battery, has_trigger, has_trackpad, has_buttons, has_imu);
713
714 buf++;
715
716 if (has_battery) {
717 controller_handle_battery(d, (struct vive_controller_battery_sample *)buf);
718 buf += sizeof(struct vive_controller_battery_sample);
719 }
720
721 if (has_buttons) {
722 controller_handle_buttons(d, (struct vive_controller_button_sample *)buf);
723 buf += sizeof(struct vive_controller_button_sample);
724 }
725 if (has_trigger) {
726 controller_handle_analog_trigger(d, (struct vive_controller_trigger_sample *)buf);
727 buf += sizeof(struct vive_controller_trigger_sample);
728 }
729 if (has_trackpad) {
730 controller_handle_touch_position(d, (struct vive_controller_touch_sample *)buf);
731 buf += 4;
732 }
733 if (has_imu) {
734 vive_controller_handle_imu_sample(d, (struct watchman_imu_sample *)buf);
735 buf += sizeof(struct watchman_imu_sample);
736 }
737 }
738
739 if (buf > end)
740 VIVE_ERROR(d, "overshoot: %td\n", (ptrdiff_t)(buf - end));
741
742 if (buf < end)
743 vive_controller_handle_lighthousev1(d, buf, end - buf);
744}
745
746/*
747 * Handles battery, imu, trigger, buttons, trackpad.
748 * Then hands off to vive_controller_handle_lighthousev1().
749 */
750static void
751vive_controller_decode_watchmanv2(struct vive_controller_device *d, struct vive_controller_message *message)
752{
753 uint8_t *buf = message->payload;
754 uint8_t *end = message->payload + message->len - 1;
755
756#ifdef WATCHMAN2_PRINT_HID
757 for (int i = 0; i < message->len; i++) {
758 int j = 8;
759 while (j--) {
760 putchar('0' + ((buf[i] >> j) & 1));
761 }
762 putchar(' ');
763 }
764 printf("\n");
765 for (int i = 0; i < message->len; i++) {
766 printf("%8.02x ", buf[i]);
767 }
768 printf("\n");
769#endif
770
771
772 /* payload starts with "event flags" byte. */
773
774 /*
775 * If flags == 0xe1 == 11100001, battery follows.
776 * Battery is always at the beginning of the payload.
777 * after battery there may be another payload.
778 * careful: 0xe1 often comes alone without actual data.
779 */
780 if (*buf == 0xe1 && buf < end) {
781 buf++;
782 controller_handle_battery(d, (struct vive_controller_battery_sample *)buf);
783 buf += sizeof(struct vive_controller_battery_sample);
784
785#ifdef WATCHMAN2_PRINT_HID
786 printf(
787 " "
788 " battery");
789#endif
790 }
791
792
793 /*
794 * If flags == 0xf0 == 11110000, 8 bytes touch+force follow.
795 * This package is always at the beginning of the payload.
796 */
797 if (*buf == 0xf0 && buf < end) {
798 buf++;
799 controller_handle_touch_force(d, (struct watchman_touch_force *)buf);
800 size_t s = sizeof(struct watchman_touch_force);
801 buf += s;
802
803#ifdef WATCHMAN2_PRINT_HID
804 printf(" ");
805 for (size_t i = 0; i < s; i++)
806 printf(" t&force");
807#endif
808 }
809
810 /*
811 * If flags == 0xe8 == 11101000, imu data follows.
812 * This package can be at the beginning of the payload or after battery.
813 */
814 // TODO: it's possible we misparse non-im udata as imu data
815 if (*buf == 0xe8 && buf < end) {
816 buf++;
817 vive_controller_handle_imu_sample(d, (struct watchman_imu_sample *)buf);
818 size_t s = sizeof(struct watchman_imu_sample);
819 buf += s;
820
821#ifdef WATCHMAN2_PRINT_HID
822 printf(" ");
823 for (size_t i = 0; i < s; i++)
824 printf(" imu");
825#endif
826 }
827
828 /*
829 * If flags starts with 1111, events follow in this order,
830 * each of them optional:
831 * - trigger: 1 byte (1111?1??)
832 * - trackpad: 4 byte (1111??1?)
833 * - buttons: 1 byte (1111???1)
834 * - touch&force+imu or imu: 8+13 or 13 byte (11111???)
835 * There may be another input event after a battery event.
836 */
837 if ((*buf & 0xf0) == 0xf0 && buf < end - 1) {
838
839 // clang-format off
840
841 // input flags 1111<touch_force><trigger><trackpad><buttons>
842 bool has_touch_force = (*buf & 0x8) == 0x8;
843 bool has_trigger = (*buf & 0x4) == 0x4;
844 bool has_trackpad = (*buf & 0x2) == 0x2;
845 bool has_buttons = (*buf & 0x1) == 0x1;
846
847 // clang-format on
848
849 buf++;
850
851#ifdef WATCHMAN2_PRINT_HID
852 printf(" ");
853#endif
854
855 if (has_buttons) {
856 controller_handle_buttons(d, (struct vive_controller_button_sample *)buf);
857 buf += sizeof(struct vive_controller_button_sample);
858#ifdef WATCHMAN2_PRINT_HID
859 printf(" buttons");
860#endif
861 }
862 if (has_trigger) {
863 controller_handle_analog_trigger(d, (struct vive_controller_trigger_sample *)buf);
864 buf += sizeof(struct vive_controller_trigger_sample);
865#ifdef WATCHMAN2_PRINT_HID
866 printf(" trigger");
867#endif
868 }
869 if (has_trackpad) {
870 controller_handle_touch_position(d, (struct vive_controller_touch_sample *)buf);
871 buf += sizeof(struct vive_controller_touch_sample);
872#ifdef WATCHMAN2_PRINT_HID
873 for (unsigned long i = 0; i < sizeof(struct vive_controller_touch_sample); i++)
874 printf(" trackpad");
875#endif
876 }
877 if (has_touch_force) {
878 uint8_t type_flag = *buf;
879 if (type_flag == TYPE_FLAG_TOUCH_FORCE) {
880 controller_handle_touch_force(d, (struct watchman_touch_force *)buf);
881 size_t s = sizeof(struct watchman_touch_force);
882 buf += s;
883#ifdef WATCHMAN2_PRINT_HID
884 for (unsigned long i = 0; i < sizeof(struct watchman_touch_force); i++)
885 printf(" t&force");
886#endif
887 }
888 }
889 // if something still follows, usually imu
890 // sometimes it's 5 unknown bytes'
891 if (buf < end && end - buf >= (long)sizeof(struct watchman_imu_sample)) {
892 vive_controller_handle_imu_sample(d, (struct watchman_imu_sample *)buf);
893 size_t s = sizeof(struct watchman_imu_sample);
894 buf += s;
895#ifdef WATCHMAN2_PRINT_HID
896 for (unsigned long i = 0; i < sizeof(struct watchman_imu_sample); i++)
897 printf(" imu");
898#endif
899 }
900 }
901
902
903#ifdef WATCHMAN2_PRINT_HID
904 printf("\n");
905#endif
906
907 if (buf < end) {
908 VIVE_TRACE(d, "%td bytes unparsed data in message\n",
909 (ptrdiff_t)(message->len - (buf - message->payload) - 1));
910 }
911 if (buf > end)
912 VIVE_ERROR(d, "overshoot: %td\n", (ptrdiff_t)(buf - end));
913
914 //! @todo: Parse lighthouse v2 data
915}
916/*
917 * Decodes multiplexed Wireless Receiver messages.
918 */
919static void
920vive_controller_decode_message(struct vive_controller_device *d, struct vive_controller_message *message)
921{
922 d->last_ticks = (message->timestamp_hi << 24) | (message->timestamp_lo << 16);
923
924 //! @todo: Check if Vive controller on watchman2 is correctly handled
925 //! with watchman2 codepath
926 switch (d->watchman_gen) {
927 case WATCHMAN_GEN1: vive_controller_decode_watchmanv1(d, message); break;
928 case WATCHMAN_GEN2: vive_controller_decode_watchmanv2(d, message); break;
929 default: VIVE_ERROR(d, "Can't decode unknown watchman gen");
930 }
931}
932
933#define FEATURE_BUFFER_SIZE 256
934
935static int
936vive_controller_device_update(struct vive_controller_device *d)
937{
938 uint8_t buf[FEATURE_BUFFER_SIZE];
939
940 int ret = os_hid_read(d->controller_hid, buf, sizeof(buf), 1000);
941 if (ret == 0) {
942 // controller off
943 return true;
944 }
945
946 if (ret < 0) {
947 VIVE_ERROR(d, "Failed to read device '%i'!", ret);
948 return false;
949 }
950
951 switch (buf[0]) {
952 case VIVE_CONTROLLER_REPORT1_ID:
953 os_mutex_lock(&d->lock);
954 vive_controller_decode_message(d, &((struct vive_controller_report1 *)buf)->message);
955 os_mutex_unlock(&d->lock);
956 break;
957
958 case VIVE_CONTROLLER_REPORT2_ID:
959 os_mutex_lock(&d->lock);
960 vive_controller_decode_message(d, &((struct vive_controller_report2 *)buf)->message[0]);
961 vive_controller_decode_message(d, &((struct vive_controller_report2 *)buf)->message[1]);
962 os_mutex_unlock(&d->lock);
963 break;
964 case VIVE_CONTROLLER_DISCONNECT_REPORT_ID: VIVE_DEBUG(d, "Controller disconnected."); break;
965 default: VIVE_ERROR(d, "Unknown controller message type: %u", buf[0]);
966 }
967
968 return true;
969}
970
971static void *
972vive_controller_run_thread(void *ptr)
973{
974 struct vive_controller_device *d = (struct vive_controller_device *)ptr;
975
976 uint8_t buf[FEATURE_BUFFER_SIZE];
977 while (os_hid_read(d->controller_hid, buf, sizeof(buf), 0) > 0) {
978 // Empty queue first
979 }
980
981 os_thread_helper_lock(&d->controller_thread);
982 while (os_thread_helper_is_running_locked(&d->controller_thread)) {
983 os_thread_helper_unlock(&d->controller_thread);
984
985 if (!vive_controller_device_update(d)) {
986 return NULL;
987 }
988
989 // Just keep swimming.
990 os_thread_helper_lock(&d->controller_thread);
991 }
992
993 return NULL;
994}
995
996void
997vive_controller_reset_pose_cb(void *ptr)
998{
999 struct vive_controller_device *d = (struct vive_controller_device *)ptr;
1000 os_mutex_lock(&d->fusion.mutex);
1001 m_imu_3dof_reset(&d->fusion.i3dof);
1002 d->pose = (struct xrt_pose)XRT_POSE_IDENTITY;
1003 os_mutex_unlock(&d->fusion.mutex);
1004}
1005
1006static void
1007vive_controller_setup_ui(struct vive_controller_device *d)
1008{
1009 char tmp[256] = {0};
1010 snprintf(tmp, sizeof(tmp), "Vive Controller %zu", d->index);
1011
1012 u_var_add_root(d, tmp, false);
1013 u_var_add_log_level(d, &d->log_level, "Log level");
1014
1015 u_var_add_gui_header(d, NULL, "Tracking");
1016 u_var_add_pose(d, &d->pose, "Tracked Pose");
1017 u_var_add_pose(d, &d->offset, "Pose Offset");
1018
1019 d->gui.reset_pose_btn.cb = vive_controller_reset_pose_cb;
1020 d->gui.reset_pose_btn.ptr = d;
1021 u_var_add_button(d, &d->gui.reset_pose_btn, "Reset pose");
1022
1023 u_var_add_gui_header(d, NULL, "3DoF Tracking");
1024 m_imu_3dof_add_vars(&d->fusion.i3dof, d, "");
1025 u_var_add_gui_header(d, NULL, "Calibration");
1026 u_var_add_vec3_f32(d, &d->config.imu.acc_scale, "acc_scale");
1027 u_var_add_vec3_f32(d, &d->config.imu.acc_bias, "acc_bias");
1028 u_var_add_vec3_f32(d, &d->config.imu.gyro_scale, "gyro_scale");
1029 u_var_add_vec3_f32(d, &d->config.imu.gyro_bias, "gyro_bias");
1030}
1031
1032/*
1033 *
1034 * 'Exported' function(s).
1035 *
1036 */
1037
1038#define SET_WAND_INPUT(NAME, NAME2) \
1039 do { \
1040 (d->base.inputs[VIVE_CONTROLLER_INDEX_##NAME].name = XRT_INPUT_VIVE_##NAME2); \
1041 } while (0)
1042
1043#define SET_INDEX_INPUT(NAME, NAME2) \
1044 do { \
1045 (d->base.inputs[VIVE_CONTROLLER_INDEX_##NAME].name = XRT_INPUT_INDEX_##NAME2); \
1046 } while (0)
1047
1048struct vive_controller_device *
1049vive_controller_create(struct os_hid_device *controller_hid, enum watchman_gen watchman_gen, int controller_num)
1050{
1051
1052 enum u_device_alloc_flags flags = U_DEVICE_ALLOC_TRACKING_NONE;
1053 struct vive_controller_device *d =
1054 U_DEVICE_ALLOCATE(struct vive_controller_device, flags, VIVE_CONTROLLER_MAX_INDEX, 1);
1055
1056 d->log_level = debug_get_log_option_vive_log();
1057 d->watchman_gen = WATCHMAN_GEN_UNKNOWN;
1058 d->config.variant = CONTROLLER_UNKNOWN;
1059 d->index = controller_num;
1060 d->pose = (struct xrt_pose)XRT_POSE_IDENTITY;
1061 d->offset = (struct xrt_pose)XRT_POSE_IDENTITY;
1062
1063 d->watchman_gen = watchman_gen;
1064
1065 m_imu_3dof_init(&d->fusion.i3dof, M_IMU_3DOF_USE_GRAVITY_DUR_20MS);
1066 m_relation_history_create(&d->fusion.relation_hist);
1067 int ret = os_mutex_init(&d->fusion.mutex);
1068 if (ret != 0) {
1069 VIVE_ERROR(d, "Failed to init 3dof mutex");
1070 return NULL;
1071 }
1072
1073 /* default values, will be queried from device */
1074 d->config.imu.gyro_range = 8.726646f;
1075 d->config.imu.acc_range = 39.226600f;
1076
1077 d->config.imu.acc_scale.x = 1.0f;
1078 d->config.imu.acc_scale.y = 1.0f;
1079 d->config.imu.acc_scale.z = 1.0f;
1080 d->config.imu.gyro_scale.x = 1.0f;
1081 d->config.imu.gyro_scale.y = 1.0f;
1082 d->config.imu.gyro_scale.z = 1.0f;
1083
1084 d->config.imu.acc_bias.x = 0.0f;
1085 d->config.imu.acc_bias.y = 0.0f;
1086 d->config.imu.acc_bias.z = 0.0f;
1087 d->config.imu.gyro_bias.x = 0.0f;
1088 d->config.imu.gyro_bias.y = 0.0f;
1089 d->config.imu.gyro_bias.z = 0.0f;
1090
1091 d->controller_hid = controller_hid;
1092
1093 d->base.destroy = vive_controller_device_destroy;
1094 d->base.get_tracked_pose = vive_controller_device_get_tracked_pose;
1095 d->base.set_output = vive_controller_device_set_output;
1096
1097 // Have to init before destroy is called.
1098 os_mutex_init(&d->lock);
1099 os_thread_helper_init(&d->controller_thread);
1100
1101 if (vive_get_imu_range_report(d->controller_hid, &d->config.imu.gyro_range, &d->config.imu.acc_range) != 0) {
1102 // reading range report fails for powered off controller
1103 vive_controller_device_destroy(&d->base);
1104 return NULL;
1105 }
1106
1107 VIVE_DEBUG(d, "Vive controller gyroscope range %f", d->config.imu.gyro_range);
1108 VIVE_DEBUG(d, "Vive controller accelerometer range %f", d->config.imu.acc_range);
1109
1110 // successful config parsing determines d->config.variant
1111 char *config = vive_read_config(d->controller_hid);
1112
1113 if (config != NULL) {
1114 vive_config_parse_controller(&d->config, config, d->log_level);
1115 free(config);
1116 } else {
1117 VIVE_ERROR(d, "Could not get Vive controller config\n");
1118 vive_controller_device_destroy(&d->base);
1119 return NULL;
1120 }
1121
1122 snprintf(d->base.serial, XRT_DEVICE_NAME_LEN, "%s", d->config.firmware.device_serial_number);
1123
1124 if (d->config.variant == CONTROLLER_VIVE_WAND) {
1125 d->base.name = XRT_DEVICE_VIVE_WAND;
1126 snprintf(d->base.str, XRT_DEVICE_NAME_LEN, "Vive Wand Controller (vive)");
1127
1128 SET_WAND_INPUT(SYSTEM_CLICK, SYSTEM_CLICK);
1129 SET_WAND_INPUT(SQUEEZE_CLICK, SQUEEZE_CLICK);
1130 SET_WAND_INPUT(MENU_CLICK, MENU_CLICK);
1131 SET_WAND_INPUT(TRIGGER_CLICK, TRIGGER_CLICK);
1132 SET_WAND_INPUT(TRIGGER_VALUE, TRIGGER_VALUE);
1133 SET_WAND_INPUT(TRACKPAD, TRACKPAD);
1134 SET_WAND_INPUT(TRACKPAD_CLICK, TRACKPAD_CLICK);
1135 SET_WAND_INPUT(TRACKPAD_TOUCH, TRACKPAD_TOUCH);
1136
1137 SET_WAND_INPUT(AIM_POSE, AIM_POSE);
1138 SET_WAND_INPUT(GRIP_POSE, GRIP_POSE);
1139
1140 d->base.outputs[0].name = XRT_OUTPUT_NAME_VIVE_HAPTIC;
1141
1142 d->base.update_inputs = vive_controller_device_wand_update_inputs;
1143
1144 d->base.binding_profiles = vive_binding_profiles_wand;
1145 d->base.binding_profile_count = vive_binding_profiles_wand_count;
1146
1147 d->base.device_type = XRT_DEVICE_TYPE_ANY_HAND_CONTROLLER;
1148 } else if (d->config.variant == CONTROLLER_INDEX_LEFT || d->config.variant == CONTROLLER_INDEX_RIGHT) {
1149 d->base.name = XRT_DEVICE_INDEX_CONTROLLER;
1150
1151 SET_INDEX_INPUT(SYSTEM_CLICK, SYSTEM_CLICK);
1152 SET_INDEX_INPUT(A_CLICK, A_CLICK);
1153 SET_INDEX_INPUT(B_CLICK, B_CLICK);
1154 SET_INDEX_INPUT(TRIGGER_CLICK, TRIGGER_CLICK);
1155 SET_INDEX_INPUT(TRIGGER_VALUE, TRIGGER_VALUE);
1156 SET_INDEX_INPUT(TRACKPAD, TRACKPAD);
1157 SET_INDEX_INPUT(TRACKPAD_TOUCH, TRACKPAD_TOUCH);
1158 SET_INDEX_INPUT(THUMBSTICK, THUMBSTICK);
1159 SET_INDEX_INPUT(THUMBSTICK_CLICK, THUMBSTICK_CLICK);
1160
1161 SET_INDEX_INPUT(THUMBSTICK_TOUCH, THUMBSTICK_TOUCH);
1162 SET_INDEX_INPUT(SYSTEM_TOUCH, SYSTEM_TOUCH);
1163 SET_INDEX_INPUT(A_TOUCH, A_TOUCH);
1164 SET_INDEX_INPUT(B_TOUCH, B_TOUCH);
1165 SET_INDEX_INPUT(SQUEEZE_VALUE, SQUEEZE_VALUE);
1166 SET_INDEX_INPUT(SQUEEZE_FORCE, SQUEEZE_FORCE);
1167 SET_INDEX_INPUT(TRIGGER_TOUCH, TRIGGER_TOUCH);
1168 SET_INDEX_INPUT(TRACKPAD_FORCE, TRACKPAD_FORCE);
1169
1170 SET_INDEX_INPUT(AIM_POSE, AIM_POSE);
1171 SET_INDEX_INPUT(GRIP_POSE, GRIP_POSE);
1172
1173 d->base.outputs[0].name = XRT_OUTPUT_NAME_INDEX_HAPTIC;
1174
1175 d->base.update_inputs = vive_controller_device_index_update_inputs;
1176
1177 d->base.get_hand_tracking = vive_controller_get_hand_tracking;
1178
1179 d->base.binding_profiles = vive_binding_profiles_index;
1180 d->base.binding_profile_count = vive_binding_profiles_index_count;
1181
1182 if (d->config.variant == CONTROLLER_INDEX_LEFT) {
1183 d->base.device_type = XRT_DEVICE_TYPE_LEFT_HAND_CONTROLLER;
1184 d->base.inputs[VIVE_CONTROLLER_HAND_TRACKING].name = XRT_INPUT_HT_CONFORMING_LEFT;
1185 snprintf(d->base.str, XRT_DEVICE_NAME_LEN, "Valve Index Left Controller (vive)");
1186 } else if (d->config.variant == CONTROLLER_INDEX_RIGHT) {
1187 d->base.device_type = XRT_DEVICE_TYPE_RIGHT_HAND_CONTROLLER;
1188 d->base.inputs[VIVE_CONTROLLER_HAND_TRACKING].name = XRT_INPUT_HT_CONFORMING_RIGHT;
1189 snprintf(d->base.str, XRT_DEVICE_NAME_LEN, "Valve Index Right Controller (vive)");
1190 }
1191 } else if (d->config.variant == CONTROLLER_TRACKER_GEN1) {
1192 d->base.name = XRT_DEVICE_VIVE_TRACKER_GEN1;
1193 d->base.update_inputs = u_device_noop_update_inputs;
1194 d->base.device_type = XRT_DEVICE_TYPE_GENERIC_TRACKER;
1195 snprintf(d->base.str, XRT_DEVICE_NAME_LEN, "Vive Tracker Gen1 (vive)");
1196 } else if (d->config.variant == CONTROLLER_TRACKER_GEN2) {
1197 d->base.name = XRT_DEVICE_VIVE_TRACKER_GEN2;
1198 d->base.update_inputs = u_device_noop_update_inputs;
1199 d->base.device_type = XRT_DEVICE_TYPE_GENERIC_TRACKER;
1200 snprintf(d->base.str, XRT_DEVICE_NAME_LEN, "Vive Tracker Gen2 (vive)");
1201 } else if (d->config.variant == CONTROLLER_TRACKER_GEN3) {
1202 d->base.name = XRT_DEVICE_VIVE_TRACKER_GEN3;
1203 d->base.update_inputs = u_device_noop_update_inputs;
1204 d->base.device_type = XRT_DEVICE_TYPE_GENERIC_TRACKER;
1205 snprintf(d->base.str, XRT_DEVICE_NAME_LEN, "Vive Tracker Gen3 (vive)");
1206 } else if (d->config.variant == CONTROLLER_TRACKER_TUNDRA) {
1207 d->base.name = XRT_DEVICE_VIVE_TRACKER_TUNDRA;
1208 d->base.update_inputs = u_device_noop_update_inputs;
1209 d->base.device_type = XRT_DEVICE_TYPE_GENERIC_TRACKER;
1210 snprintf(d->base.str, XRT_DEVICE_NAME_LEN, "Tundra Tracker Gen3 (vive)");
1211 } else {
1212 d->base.name = XRT_DEVICE_GENERIC_HMD;
1213 d->base.device_type = XRT_DEVICE_TYPE_GENERIC_TRACKER;
1214 VIVE_ERROR(d, "Failed to assign update input function");
1215 }
1216
1217 if (d->controller_hid) {
1218 int ret = os_thread_helper_start(&d->controller_thread, vive_controller_run_thread, d);
1219 if (ret != 0) {
1220 VIVE_ERROR(d, "Failed to start mainboard thread!");
1221 vive_controller_device_destroy(&d->base);
1222 return NULL;
1223 }
1224 }
1225
1226 VIVE_DEBUG(d, "Opened vive controller!\n");
1227 d->base.supported.orientation_tracking = true;
1228 d->base.supported.position_tracking = false;
1229 d->base.supported.hand_tracking =
1230 d->config.variant == CONTROLLER_INDEX_LEFT || d->config.variant == CONTROLLER_INDEX_RIGHT;
1231
1232 vive_controller_setup_ui(d);
1233
1234 return d;
1235}