matrixfurry.com
The open source OpenXR runtime
1// Copyright 2019-2023, Collabora, Ltd.
2// SPDX-License-Identifier: BSL-1.0
3/*!
4 * @file
5 * @brief Adapter to Libsurvive.
6 * @author Christoph Haag <christoph.haag@collabora.com>
7 * @author Jakob Bornecrantz <jakob@collabora.com>
8 * @author Moshi Turner <moshiturner@protonmail.com>
9 * @ingroup drv_survive
10 */
11
12#include <math.h>
13#include <stdio.h>
14#include <stdlib.h>
15#include <string.h>
16#include <assert.h>
17#include <string.h>
18
19#include "math/m_api.h"
20#include "math/m_space.h"
21#include "tracking/t_tracking.h"
22#include "xrt/xrt_device.h"
23#include "util/u_debug.h"
24#include "util/u_device.h"
25#include "util/u_misc.h"
26#include "util/u_var.h"
27#include "util/u_time.h"
28#include "util/u_device.h"
29#include "util/u_distortion_mesh.h"
30#include "util/u_config_json.h"
31
32#include "os/os_threading.h"
33
34#include "os/os_time.h"
35
36#include "xrt/xrt_prober.h"
37#include "survive_interface.h"
38
39#include "survive_api.h"
40
41#include "util/u_json.h"
42
43#include "util/u_hand_tracking.h"
44#include "util/u_hand_simulation.h"
45#include "util/u_logging.h"
46#include "math/m_relation_history.h"
47
48#include "math/m_predict.h"
49
50#include "vive/vive_config.h"
51#include "vive/vive_bindings.h"
52#include "vive/vive_poses.h"
53
54#include "util/u_trace_marker.h"
55
56// If we haven't gotten a config for devices this long after startup, just start without those devices
57#define DEFAULT_WAIT_TIMEOUT 3.5f
58
59// index in sys->controllers[] array
60#define SURVIVE_LEFT_CONTROLLER_INDEX 0
61#define SURVIVE_RIGHT_CONTROLLER_INDEX 1
62#define SURVIVE_NON_CONTROLLER_START 2
63
64//! excl HMD we support 16 devices (controllers, trackers, ...)
65#define MAX_TRACKED_DEVICE_COUNT 16
66
67DEBUG_GET_ONCE_BOOL_OPTION(survive_disable_hand_emulation, "SURVIVE_DISABLE_HAND_EMULATION", false)
68DEBUG_GET_ONCE_BOOL_OPTION(survive_default_ipd, "SURVIVE_DEFAULT_IPD", false)
69DEBUG_GET_ONCE_FLOAT_OPTION(survive_timecode_offset_ms, "SURVIVE_TIMECODE_OFFSET_MS", 0.0)
70
71#define SURVIVE_TRACE(d, ...) U_LOG_XDEV_IFL_T(&d->base, d->sys->log_level, __VA_ARGS__)
72#define SURVIVE_DEBUG(d, ...) U_LOG_XDEV_IFL_D(&d->base, d->sys->log_level, __VA_ARGS__)
73#define SURVIVE_INFO(d, ...) U_LOG_XDEV_IFL_I(&d->base, d->sys->log_level, __VA_ARGS__)
74#define SURVIVE_WARN(d, ...) U_LOG_XDEV_IFL_W(&d->base, d->sys->log_level, __VA_ARGS__)
75#define SURVIVE_ERROR(d, ...) U_LOG_XDEV_IFL_E(&d->base, d->sys->log_level, __VA_ARGS__)
76
77struct survive_system;
78
79enum input_index
80{
81 // common inputs
82 VIVE_CONTROLLER_AIM_POSE = 0,
83 VIVE_CONTROLLER_GRIP_POSE,
84 VIVE_CONTROLLER_SYSTEM_CLICK,
85 VIVE_CONTROLLER_TRIGGER_CLICK,
86 VIVE_CONTROLLER_TRIGGER_VALUE,
87 VIVE_CONTROLLER_TRACKPAD,
88 VIVE_CONTROLLER_TRACKPAD_TOUCH,
89
90 // Vive Wand specific inputs
91 VIVE_CONTROLLER_SQUEEZE_CLICK,
92 VIVE_CONTROLLER_MENU_CLICK,
93 VIVE_CONTROLLER_TRACKPAD_CLICK,
94
95 // Valve Index specific inputs
96 VIVE_CONTROLLER_THUMBSTICK,
97 VIVE_CONTROLLER_A_CLICK,
98 VIVE_CONTROLLER_B_CLICK,
99 VIVE_CONTROLLER_THUMBSTICK_CLICK,
100 VIVE_CONTROLLER_THUMBSTICK_TOUCH,
101 VIVE_CONTROLLER_SYSTEM_TOUCH,
102 VIVE_CONTROLLER_A_TOUCH,
103 VIVE_CONTROLLER_B_TOUCH,
104 VIVE_CONTROLLER_SQUEEZE_VALUE,
105 VIVE_CONTROLLER_SQUEEZE_FORCE,
106 VIVE_CONTROLLER_TRIGGER_TOUCH,
107 VIVE_CONTROLLER_TRACKPAD_FORCE,
108
109 VIVE_CONTROLLER_HAND_TRACKING,
110
111 VIVE_TRACKER_POSE,
112
113 VIVE_CONTROLLER_MAX_INDEX,
114};
115
116enum DeviceType
117{
118 DEVICE_TYPE_HMD,
119 DEVICE_TYPE_CONTROLLER
120};
121
122/*!
123 * @implements xrt_device
124 */
125struct survive_device
126{
127 struct xrt_device base;
128 struct survive_system *sys;
129 const SurviveSimpleObject *survive_obj;
130
131 struct m_relation_history *relation_hist;
132
133 //! Number of inputs.
134 size_t num_last_inputs;
135 //! Array of input structs.
136 struct xrt_input *last_inputs;
137
138 enum DeviceType device_type;
139
140 union {
141 struct
142 {
143 float proximity; // [0,1]
144 float ipd;
145 bool use_default_ipd;
146
147 struct vive_config config;
148 } hmd;
149
150 struct
151 {
152 float curl[XRT_FINGER_COUNT];
153 uint64_t curl_ts[XRT_FINGER_COUNT];
154 struct u_hand_tracking hand_tracking;
155
156 struct vive_controller_config config;
157 } ctrl;
158 };
159};
160
161/*!
162 * @extends xrt_tracking_origin
163 */
164struct survive_system
165{
166 struct xrt_tracking_origin base;
167 SurviveSimpleContext *ctx;
168 struct survive_device *hmd;
169 struct survive_device *controllers[MAX_TRACKED_DEVICE_COUNT];
170 enum u_logging_level log_level;
171
172 float wait_timeout;
173 struct u_var_draggable_f32 timecode_offset_ms;
174
175 struct os_thread_helper event_thread;
176 struct os_mutex lock;
177};
178
179static void
180survive_device_destroy(struct xrt_device *xdev)
181{
182 if (!xdev) {
183 return;
184 }
185
186 U_LOG_D("destroying survive device");
187 struct survive_device *survive = (struct survive_device *)xdev;
188
189 if (survive == survive->sys->hmd) {
190 vive_config_teardown(&survive->hmd.config);
191 survive->sys->hmd = NULL;
192 }
193 for (int i = 0; i < MAX_TRACKED_DEVICE_COUNT; i++) {
194 if (survive == survive->sys->controllers[i]) {
195 survive->sys->controllers[i] = NULL;
196 }
197 }
198
199 bool all_null = true;
200 for (int i = 0; i < MAX_TRACKED_DEVICE_COUNT; i++) {
201 if (survive->sys->controllers[i] != 0) {
202 all_null = false;
203 }
204 }
205
206 if (survive->sys->hmd == NULL && all_null) {
207 U_LOG_D("Tearing down libsurvive context");
208
209 // Remove the variable tracking.
210 u_var_remove_root(survive->sys);
211
212 // Destroy also stops the thread.
213 os_thread_helper_destroy(&survive->sys->event_thread);
214
215 // Now that the thread is not running we can destroy the lock.
216 os_mutex_destroy(&survive->sys->lock);
217
218 U_LOG_D("Stopped libsurvive event thread");
219
220 survive_simple_close(survive->sys->ctx);
221 free(survive->sys);
222 }
223 m_relation_history_destroy(&survive->relation_hist);
224
225 // Remove the variable tracking.
226 u_var_remove_root(survive);
227
228 free(survive->last_inputs);
229 u_device_free(&survive->base);
230}
231
232// libsurvive timecode may not be exactly comparable with monotonic ns.
233// see OGGetAbsoluteTimeUS in libsurvive redist/os_generic.unix.h
234static double
235survive_timecode_now_s(void)
236{
237 struct timeval tv;
238 gettimeofday(&tv, 0);
239 return ((double)tv.tv_usec) / 1000000. + (tv.tv_sec);
240}
241
242static timepoint_ns
243survive_timecode_to_monotonic(struct survive_device *survive, double timecode)
244{
245 timepoint_ns timecode_ns = time_s_to_ns(timecode);
246 timepoint_ns survive_now_ns = time_s_to_ns(survive_timecode_now_s());
247
248 timepoint_ns timecode_age_ns = survive_now_ns - timecode_ns;
249
250 timepoint_ns now = os_monotonic_get_ns();
251 timepoint_ns timestamp = now - timecode_age_ns + (uint64_t)(survive->sys->timecode_offset_ms.val * 1000000.0);
252
253 return timestamp;
254}
255
256static void
257pose_to_relation(const SurvivePose *pose, const SurviveVelocity *vel, struct xrt_space_relation *out_relation)
258{
259 struct xrt_quat out_rot = {.x = pose->Rot[1], .y = pose->Rot[2], .z = pose->Rot[3], .w = pose->Rot[0]};
260
261 /* libsurvive looks down when it should be looking forward, so
262 * rotate the quat.
263 * because the HMD quat is the opposite of the in world
264 * rotation, we rotate down. */
265
266 struct xrt_quat down_rot;
267 down_rot.x = sqrtf(2) / 2.;
268 down_rot.y = 0;
269 down_rot.z = 0;
270 down_rot.w = -sqrtf(2) / 2.;
271
272 math_quat_rotate(&down_rot, &out_rot, &out_rot);
273
274 // just to be sure
275 math_quat_normalize(&out_rot);
276
277 out_relation->pose.orientation = out_rot;
278
279 /* switch -y, z axes to go from libsurvive coordinate system to ours */
280 out_relation->pose.position.x = pose->Pos[0];
281 out_relation->pose.position.y = pose->Pos[2];
282 out_relation->pose.position.z = -pose->Pos[1];
283
284 struct xrt_vec3 linear_vel = {.x = vel->Pos[0], .y = vel->Pos[2], .z = -vel->Pos[1]};
285
286 struct xrt_vec3 angular_vel = {
287 .x = vel->AxisAngleRot[0], .y = vel->AxisAngleRot[2], .z = -vel->AxisAngleRot[1]};
288
289 if (math_quat_validate(&out_rot)) {
290 out_relation->relation_flags |=
291 XRT_SPACE_RELATION_ORIENTATION_VALID_BIT | XRT_SPACE_RELATION_ORIENTATION_TRACKED_BIT;
292
293 // everything else is invalid if orientation is not valid
294
295 if (math_vec3_validate(&out_relation->pose.position)) {
296 out_relation->relation_flags |=
297 XRT_SPACE_RELATION_POSITION_VALID_BIT | XRT_SPACE_RELATION_POSITION_TRACKED_BIT;
298 }
299
300
301 out_relation->linear_velocity = linear_vel;
302 if (math_vec3_validate(&out_relation->linear_velocity)) {
303 out_relation->relation_flags |= XRT_SPACE_RELATION_LINEAR_VELOCITY_VALID_BIT;
304 }
305
306 out_relation->angular_velocity = angular_vel;
307 if (math_vec3_validate(&out_relation->angular_velocity)) {
308 out_relation->relation_flags |= XRT_SPACE_RELATION_ANGULAR_VELOCITY_VALID_BIT;
309 }
310 }
311}
312
313static bool
314verify_device_name(struct survive_device *survive, enum xrt_input_name name)
315{
316
317 switch (survive->device_type) {
318 case DEVICE_TYPE_HMD: return name == XRT_INPUT_GENERIC_HEAD_POSE;
319 case DEVICE_TYPE_CONTROLLER:
320 return name == XRT_INPUT_INDEX_AIM_POSE || name == XRT_INPUT_INDEX_GRIP_POSE ||
321 name == XRT_INPUT_VIVE_AIM_POSE || name == XRT_INPUT_VIVE_GRIP_POSE ||
322 name == XRT_INPUT_GENERIC_TRACKER_POSE;
323 };
324 return false;
325}
326
327static xrt_result_t
328survive_device_get_tracked_pose(struct xrt_device *xdev,
329 enum xrt_input_name name,
330 int64_t at_timestamp_ns,
331 struct xrt_space_relation *out_relation)
332{
333 struct survive_device *survive = (struct survive_device *)xdev;
334 if (!verify_device_name(survive, name)) {
335 U_LOG_XDEV_UNSUPPORTED_INPUT(&survive->base, survive->sys->log_level, name);
336 return XRT_ERROR_INPUT_UNSUPPORTED;
337 }
338
339 if (!survive->survive_obj) {
340 // U_LOG_D("Obj not set for %p", (void*)survive);
341 return XRT_SUCCESS;
342 }
343
344 // We're pretty sure libsurvive is giving us the IMU pose here, so this works.
345 struct xrt_pose pose_offset = XRT_POSE_IDENTITY;
346 vive_poses_get_pose_offset(survive->base.name, survive->base.device_type, name, &pose_offset);
347
348 struct xrt_space_relation space_relation;
349 m_relation_history_get(survive->relation_hist, at_timestamp_ns, &space_relation);
350
351 struct xrt_relation_chain relation_chain = {0};
352 m_relation_chain_push_pose(&relation_chain, &pose_offset);
353 m_relation_chain_push_relation(&relation_chain, &space_relation);
354 m_relation_chain_resolve(&relation_chain, out_relation);
355
356 struct xrt_pose *p = &out_relation->pose;
357 SURVIVE_TRACE(survive, "GET_POSITION (%f %f %f) GET_ORIENTATION (%f, %f, %f, %f)", p->position.x, p->position.y,
358 p->position.z, p->orientation.x, p->orientation.y, p->orientation.z, p->orientation.w);
359 return XRT_SUCCESS;
360}
361
362static xrt_result_t
363survive_device_get_battery_status(struct xrt_device *xdev, bool *out_present, bool *out_charging, float *out_charge)
364{
365 struct survive_device *survive = (struct survive_device *)xdev;
366 if (!survive->survive_obj) {
367 // U_LOG_D("Obj not set for %p", (void*)survive);
368 *out_present = false;
369 return XRT_SUCCESS;
370 }
371
372 *out_present = true;
373 *out_charging = survive_simple_object_charging(survive->survive_obj);
374 *out_charge = survive_simple_object_charge_percet(survive->survive_obj) * 0.01F;
375 SURVIVE_TRACE(survive, "Charging: %s, charge: %f", *out_charging ? "true" : "false", *out_charge);
376 return XRT_SUCCESS;
377}
378
379static int
380survive_controller_haptic_pulse(struct survive_device *survive, const struct xrt_output_value *value)
381{
382 float duration_seconds;
383 if (value->vibration.duration_ns == XRT_MIN_HAPTIC_DURATION) {
384 SURVIVE_TRACE(survive, "Haptic pulse duration: using %f minimum", MIN_HAPTIC_DURATION);
385 duration_seconds = MIN_HAPTIC_DURATION;
386 } else {
387 duration_seconds = time_ns_to_s(value->vibration.duration_ns);
388 }
389
390 float frequency = value->vibration.frequency;
391
392 if (frequency == XRT_FREQUENCY_UNSPECIFIED) {
393 SURVIVE_TRACE(survive, "Haptic pulse frequency unspecified, setting to %fHz", DEFAULT_HAPTIC_FREQ);
394 frequency = DEFAULT_HAPTIC_FREQ;
395 }
396
397 float amplitude = value->vibration.amplitude;
398
399 SURVIVE_TRACE(survive, "Got Haptic pulse amp %f, %fHz, %" PRId64 "ns", value->vibration.amplitude,
400 value->vibration.frequency, value->vibration.duration_ns);
401 SURVIVE_TRACE(survive, "Doing Haptic pulse amp %f, %fHz, %fs", amplitude, frequency, duration_seconds);
402
403 return survive_simple_object_haptic((struct SurviveSimpleObject *)survive->survive_obj, frequency, amplitude,
404 duration_seconds);
405}
406
407static xrt_result_t
408survive_controller_device_set_output(struct xrt_device *xdev,
409 enum xrt_output_name name,
410 const struct xrt_output_value *value)
411{
412 struct survive_device *survive = (struct survive_device *)xdev;
413
414 if (name != XRT_OUTPUT_NAME_VIVE_HAPTIC && name != XRT_OUTPUT_NAME_INDEX_HAPTIC) {
415 U_LOG_XDEV_UNSUPPORTED_OUTPUT(&survive->base, survive->sys->log_level, name);
416 return XRT_ERROR_OUTPUT_UNSUPPORTED;
417 }
418
419 bool pulse = value->vibration.amplitude > 0.01;
420 if (!pulse) {
421 return XRT_SUCCESS;
422 }
423
424 int ret = survive_controller_haptic_pulse(survive, value);
425
426 if (ret != 0) {
427 SURVIVE_ERROR(survive, "haptic failed %d", ret);
428 return XRT_ERROR_OUTPUT_REQUEST_FAILURE;
429 }
430
431 return XRT_SUCCESS;
432}
433
434struct Button
435{
436 enum input_index click;
437 enum input_index touch;
438};
439
440struct Button buttons[255] = {
441 [SURVIVE_BUTTON_A] = {.click = VIVE_CONTROLLER_A_CLICK, .touch = VIVE_CONTROLLER_A_TOUCH},
442 [SURVIVE_BUTTON_B] = {.click = VIVE_CONTROLLER_B_CLICK, .touch = VIVE_CONTROLLER_B_TOUCH},
443
444 [SURVIVE_BUTTON_TRACKPAD] = {.click = VIVE_CONTROLLER_TRACKPAD_CLICK, .touch = VIVE_CONTROLLER_TRACKPAD_TOUCH},
445
446 [SURVIVE_BUTTON_THUMBSTICK] = {.click = VIVE_CONTROLLER_THUMBSTICK_CLICK,
447 .touch = VIVE_CONTROLLER_THUMBSTICK_TOUCH},
448
449 [SURVIVE_BUTTON_SYSTEM] = {.click = VIVE_CONTROLLER_SYSTEM_CLICK, .touch = VIVE_CONTROLLER_SYSTEM_TOUCH},
450
451 [SURVIVE_BUTTON_MENU] = {.click = VIVE_CONTROLLER_MENU_CLICK,
452 // only on vive wand without touch
453 .touch = 0},
454
455 [SURVIVE_BUTTON_GRIP] = {.click = VIVE_CONTROLLER_SQUEEZE_CLICK,
456 // only on vive wand without touch
457 .touch = 0},
458
459 [SURVIVE_BUTTON_TRIGGER] = {.click = VIVE_CONTROLLER_TRIGGER_CLICK, .touch = VIVE_CONTROLLER_TRIGGER_TOUCH},
460};
461
462static xrt_result_t
463survive_controller_get_hand_tracking(struct xrt_device *xdev,
464 enum xrt_input_name name,
465 int64_t at_timestamp_ns,
466 struct xrt_hand_joint_set *out_value,
467 int64_t *out_timestamp_ns)
468{
469 struct survive_device *survive = (struct survive_device *)xdev;
470
471 if (name != XRT_INPUT_HT_CONFORMING_LEFT && name != XRT_INPUT_HT_CONFORMING_RIGHT) {
472 U_LOG_XDEV_UNSUPPORTED_INPUT(&survive->base, survive->sys->log_level, name);
473 return XRT_ERROR_INPUT_UNSUPPORTED;
474 }
475
476
477 bool left = survive->ctrl.config.variant == CONTROLLER_INDEX_LEFT;
478 enum xrt_hand hand = left ? XRT_HAND_LEFT : XRT_HAND_RIGHT;
479
480 float thumb_curl = 0.0f;
481 //! @todo place thumb preciely on the button that is touched/pressed
482 if (survive->last_inputs[VIVE_CONTROLLER_A_TOUCH].value.boolean ||
483 survive->last_inputs[VIVE_CONTROLLER_B_TOUCH].value.boolean ||
484 survive->last_inputs[VIVE_CONTROLLER_THUMBSTICK_TOUCH].value.boolean ||
485 survive->last_inputs[VIVE_CONTROLLER_TRACKPAD_TOUCH].value.boolean) {
486 thumb_curl = 1.0;
487 }
488
489 if (survive->last_inputs[buttons[SURVIVE_BUTTON_TRIGGER].click].value.boolean) {
490 survive->ctrl.curl[XRT_FINGER_INDEX] = 1.0;
491 thumb_curl = 1.0;
492 }
493
494 struct u_hand_tracking_curl_values values = {.little = survive->ctrl.curl[XRT_FINGER_LITTLE],
495 .ring = survive->ctrl.curl[XRT_FINGER_RING],
496 .middle = survive->ctrl.curl[XRT_FINGER_MIDDLE],
497 .index = survive->ctrl.curl[XRT_FINGER_INDEX],
498 .thumb = thumb_curl};
499
500
501 struct xrt_space_relation hand_relation;
502
503 m_relation_history_get(survive->relation_hist, at_timestamp_ns, &hand_relation);
504
505
506 u_hand_sim_simulate_for_valve_index_knuckles(&values, hand, &hand_relation, out_value);
507
508
509 struct xrt_relation_chain chain = {0};
510
511 // We're pretty sure libsurvive is giving us the IMU pose here, so this works.
512 struct xrt_pose pose_offset = XRT_POSE_IDENTITY;
513 vive_poses_get_pose_offset(survive->base.name, survive->base.device_type, name, &pose_offset);
514
515 m_relation_chain_push_pose(&chain, &pose_offset);
516 m_relation_chain_push_relation(&chain, &hand_relation);
517 m_relation_chain_resolve(&chain, &out_value->hand_pose);
518
519 // This is the truth - we pose-predicted or interpolated all the way up to `at_timestamp_ns`.
520 *out_timestamp_ns = at_timestamp_ns;
521
522 // This is a lie - apparently libsurvive doesn't report controller tracked/untracked state, so just say that the
523 // hand is being tracked
524 out_value->is_active = true;
525
526 return XRT_SUCCESS;
527}
528
529static xrt_result_t
530survive_device_get_view_poses(struct xrt_device *xdev,
531 const struct xrt_vec3 *default_eye_relation,
532 int64_t at_timestamp_ns,
533 enum xrt_view_type view_type,
534 uint32_t view_count,
535 struct xrt_space_relation *out_head_relation,
536 struct xrt_fov *out_fovs,
537 struct xrt_pose *out_poses)
538{
539 XRT_TRACE_MARKER();
540
541 // Only supports two views.
542 assert(view_count <= 2);
543
544 struct survive_device *survive = (struct survive_device *)xdev;
545
546 struct xrt_vec3 eye_relation = {0};
547
548 if (survive->hmd.use_default_ipd || survive->hmd.ipd == 0.f) {
549 eye_relation = *default_eye_relation;
550 } else {
551 eye_relation.x = survive->hmd.ipd;
552 }
553
554 xrt_result_t xret = u_device_get_view_poses( //
555 xdev, //
556 &eye_relation, //
557 at_timestamp_ns, //
558 view_type, //
559 view_count, //
560 out_head_relation, //
561 out_fovs, //
562 out_poses); //
563 if (xret != XRT_SUCCESS) {
564 return xret;
565 }
566
567 // This is for the Index' canted displays, on the Vive [Pro] they are identity.
568 for (uint32_t i = 0; i < view_count && i < ARRAY_SIZE(survive->hmd.config.display.rot); i++) {
569 out_poses[i].orientation = survive->hmd.config.display.rot[i];
570 }
571
572 return XRT_SUCCESS;
573}
574
575enum InputComponent
576{
577 COMP_1D,
578 COMP_2DX,
579 COMP_2DY
580};
581
582struct Axis
583{
584 enum input_index input;
585 enum InputComponent comp;
586};
587
588static struct Axis axes[255] = {
589 [SURVIVE_AXIS_TRIGGER] =
590 {
591 .input = VIVE_CONTROLLER_TRIGGER_VALUE,
592 .comp = COMP_1D,
593 },
594 [SURVIVE_AXIS_TRACKPAD_X] =
595 {
596 .input = VIVE_CONTROLLER_TRACKPAD,
597 .comp = COMP_2DX,
598 },
599 [SURVIVE_AXIS_TRACKPAD_Y] =
600 {
601 .input = VIVE_CONTROLLER_TRACKPAD,
602 .comp = COMP_2DY,
603 },
604 [SURVIVE_AXIS_JOYSTICK_X] =
605 {
606 .input = VIVE_CONTROLLER_THUMBSTICK,
607 .comp = COMP_2DX,
608 },
609 [SURVIVE_AXIS_JOYSTICK_Y] =
610 {
611 .input = VIVE_CONTROLLER_THUMBSTICK,
612 .comp = COMP_2DY,
613 },
614 [SURVIVE_AXIS_GRIP_FORCE] =
615 {
616 .input = VIVE_CONTROLLER_SQUEEZE_FORCE,
617 .comp = COMP_1D,
618 },
619 [SURVIVE_AXIS_TRACKPAD_FORCE] =
620 {
621 .input = VIVE_CONTROLLER_TRACKPAD_FORCE,
622 .comp = COMP_1D,
623 },
624};
625
626static bool
627update_axis(struct survive_device *survive, struct Axis *axis, const SurviveSimpleButtonEvent *e, int i, uint64_t now)
628{
629 if (axis->input == 0) {
630 return false;
631 }
632
633 struct xrt_input *in = &survive->last_inputs[axis->input];
634
635 float fval = e->axis_val[i];
636
637 switch (axis->comp) {
638 case COMP_1D: in->value.vec1.x = fval; break;
639 case COMP_2DX: in->value.vec2.x = fval; break;
640 case COMP_2DY: in->value.vec2.y = fval; break;
641 default: SURVIVE_WARN(survive, "Unknown axis component %d", axis->comp);
642 }
643
644 // SURVIVE_DEBUG("input %u Axis %d: %f", axis->input, i, fval);
645
646 in->timestamp = now;
647 return true;
648}
649
650
651
652static bool
653update_button(struct survive_device *survive, const struct SurviveSimpleButtonEvent *e, timepoint_ns ts)
654{
655 if (e->event_type == SURVIVE_INPUT_EVENT_NONE) {
656 return true;
657 }
658
659 enum SurviveButton btn_id = e->button_id;
660 enum SurviveInputEvent e_type = e->event_type;
661
662
663 if (e_type == SURVIVE_INPUT_EVENT_BUTTON_UP) {
664 enum input_index index = buttons[btn_id].click;
665 struct xrt_input *input = &survive->last_inputs[index];
666 input->value.boolean = false;
667 input->timestamp = ts;
668 } else if (e_type == SURVIVE_INPUT_EVENT_BUTTON_DOWN) {
669 enum input_index index = buttons[btn_id].click;
670 struct xrt_input *input = &survive->last_inputs[index];
671 input->value.boolean = true;
672 input->timestamp = ts;
673 } else if (e_type == SURVIVE_INPUT_EVENT_TOUCH_UP) {
674 enum input_index index = buttons[btn_id].touch;
675 struct xrt_input *input = &survive->last_inputs[index];
676 input->value.boolean = false;
677 input->timestamp = ts;
678 } else if (e_type == SURVIVE_INPUT_EVENT_TOUCH_DOWN) {
679 enum input_index index = buttons[btn_id].touch;
680 struct xrt_input *input = &survive->last_inputs[index];
681 input->value.boolean = true;
682 input->timestamp = ts;
683 }
684
685 return true;
686}
687
688static float
689_calculate_squeeze_value(struct survive_device *survive)
690{
691 /*! @todo find a good formula for squeeze value */
692 float val = 0;
693 val = fmaxf(val, survive->ctrl.curl[XRT_FINGER_LITTLE]);
694 val = fmaxf(val, survive->ctrl.curl[XRT_FINGER_RING]);
695 val = fmaxf(val, survive->ctrl.curl[XRT_FINGER_MIDDLE]);
696 return val;
697}
698
699static void
700_process_button_event(struct survive_device *survive, const struct SurviveSimpleButtonEvent *e)
701{
702 timepoint_ns ts = survive_timecode_to_monotonic(survive, e->time);
703 ;
704 if (e->event_type == SURVIVE_INPUT_EVENT_AXIS_CHANGED) {
705 for (int i = 0; i < e->axis_count; i++) {
706
707 struct Axis *axis = &axes[e->axis_ids[i]];
708 float val = e->axis_val[i];
709
710 if (update_axis(survive, axis, e, i, ts)) {
711
712
713 } else if (e->axis_ids[i] == SURVIVE_AXIS_TRIGGER_FINGER_PROXIMITY) {
714 survive->ctrl.curl[XRT_FINGER_INDEX] = val;
715 survive->ctrl.curl_ts[XRT_FINGER_INDEX] = ts;
716 } else if (e->axis_ids[i] == SURVIVE_AXIS_MIDDLE_FINGER_PROXIMITY) {
717 survive->ctrl.curl[XRT_FINGER_MIDDLE] = val;
718 survive->ctrl.curl_ts[XRT_FINGER_MIDDLE] = ts;
719 } else if (e->axis_ids[i] == SURVIVE_AXIS_RING_FINGER_PROXIMITY) {
720 survive->ctrl.curl[XRT_FINGER_RING] = val;
721 survive->ctrl.curl_ts[XRT_FINGER_RING] = ts;
722 } else if (e->axis_ids[i] == SURVIVE_AXIS_PINKY_FINGER_PROXIMITY) {
723 survive->ctrl.curl[XRT_FINGER_LITTLE] = val;
724 survive->ctrl.curl_ts[XRT_FINGER_LITTLE] = ts;
725 } else {
726 SURVIVE_DEBUG(survive, "axis id: %d val %f", e->axis_ids[i], e->axis_val[i]);
727 }
728 }
729 struct xrt_input *squeeze_value_in = &survive->last_inputs[VIVE_CONTROLLER_SQUEEZE_VALUE];
730 float prev_squeeze_value = squeeze_value_in->value.vec1.x;
731 float squeeze_value = _calculate_squeeze_value(survive);
732 if (prev_squeeze_value != squeeze_value) {
733 squeeze_value_in->value.vec1.x = squeeze_value;
734 squeeze_value_in->timestamp = ts;
735 }
736 }
737
738 update_button(survive, e, ts);
739}
740
741static void
742_process_hmd_button_event(struct survive_device *survive, const struct SurviveSimpleButtonEvent *e)
743{
744 if (e->event_type == SURVIVE_INPUT_EVENT_AXIS_CHANGED) {
745 for (int i = 0; i < e->axis_count; i++) {
746 float val = e->axis_val[i];
747
748 if (e->axis_ids[i] == SURVIVE_AXIS_IPD) {
749 float ipd = val;
750
751 // arbitrary default values
752 float max = 70;
753 float min = 60;
754 if (survive->hmd.config.variant == VIVE_VARIANT_INDEX) {
755 max = INDEX_MAX_IPD;
756 min = INDEX_MIN_IPD;
757 } else if (survive->hmd.config.variant == VIVE_VARIANT_VIVE) {
758 max = VIVE_MAX_IPD;
759 min = VIVE_MIN_IPD;
760 } else {
761 if (!survive->hmd.use_default_ipd) {
762 SURVIVE_WARN(survive,
763 "No IPD range for this HMD, falling back to default");
764 survive->hmd.use_default_ipd = true;
765 }
766 }
767
768 float range = max - min;
769 ipd *= range;
770 ipd += min;
771 survive->hmd.ipd = ipd;
772
773 // SURVIVE_DEBUG(survive, "ipd: %f meter", ipd);
774 } else if (e->axis_ids[i] == SURVIVE_AXIS_FACE_PROXIMITY) {
775 // Valve Index:
776 // >0.003 not wearing hmd
777 // 0.03-0.035 wearing hmd
778 const float threshold = 0.02;
779
780 float proximity = val;
781
782 // extreme closeup may overflow?
783 if (proximity < 0) {
784 proximity = 1.0;
785 }
786
787 float curr = survive->hmd.proximity;
788 bool engagement = (curr <= threshold && proximity > threshold) ||
789 (curr >= threshold && proximity < threshold);
790
791 if (engagement) {
792 //! @todo engagement changed
793 }
794 // SURVIVE_DEBUG(survive, "Proximity %f",
795 // proximity);
796
797 survive->hmd.proximity = proximity;
798 } else {
799 SURVIVE_DEBUG(survive, "axis id: %d val %f", e->axis_ids[i], e->axis_val[i]);
800 }
801 }
802 }
803}
804
805static struct survive_device *
806get_device_by_object(struct survive_system *sys, const SurviveSimpleObject *object)
807{
808 if (sys->hmd != NULL && sys->hmd->survive_obj == object) {
809 return sys->hmd;
810 }
811
812 for (int i = 0; i < MAX_TRACKED_DEVICE_COUNT; i++) {
813 if (sys->controllers[i] == NULL) {
814 continue;
815 }
816
817 if (sys->controllers[i]->survive_obj == object) {
818 return sys->controllers[i];
819 }
820 }
821 return NULL;
822}
823
824static void
825add_device(struct survive_system *ss, const struct SurviveSimpleConfigEvent *e);
826
827static void
828_process_pose_event(struct survive_device *survive, const struct SurviveSimplePoseUpdatedEvent *e)
829{
830 struct xrt_space_relation rel;
831 timepoint_ns ts;
832 pose_to_relation(&e->pose, &e->velocity, &rel);
833 ts = survive_timecode_to_monotonic(survive, e->time);
834 m_relation_history_push(survive->relation_hist, &rel, ts);
835
836 SURVIVE_TRACE(survive, "Process pose event for %s", survive->base.str);
837}
838
839static void
840_process_event(struct survive_system *ss, struct SurviveSimpleEvent *event)
841{
842 switch (event->event_type) {
843 case SurviveSimpleEventType_ButtonEvent: {
844 const struct SurviveSimpleButtonEvent *e = survive_simple_get_button_event(event);
845
846 struct survive_device *event_device = get_device_by_object(ss, e->object);
847 if (event_device == NULL) {
848 U_LOG_IFL_I(ss->log_level, "Event for unknown object not handled");
849 return;
850 }
851
852 // hmd & controller axes have overlapping enum indices
853 if (event_device == ss->hmd) {
854 _process_hmd_button_event(event_device, e);
855 } else {
856 _process_button_event(event_device, e);
857 }
858
859 break;
860 }
861 case SurviveSimpleEventType_ConfigEvent: {
862 const struct SurviveSimpleConfigEvent *e = survive_simple_get_config_event(event);
863 enum SurviveSimpleObject_type t = survive_simple_object_get_type(e->object);
864 const char *name = survive_simple_object_name(e->object);
865 U_LOG_IFL_D(ss->log_level, "Processing config for object name %s: type %d", name, t);
866 add_device(ss, e);
867 break;
868 }
869 case SurviveSimpleEventType_PoseUpdateEvent: {
870 const struct SurviveSimplePoseUpdatedEvent *e = survive_simple_get_pose_updated_event(event);
871
872 struct survive_device *event_device = get_device_by_object(ss, e->object);
873 if (event_device == NULL) {
874 U_LOG_IFL_E(ss->log_level, "Event for unknown object not handled");
875 return;
876 }
877
878 _process_pose_event(event_device, e);
879 break;
880 }
881 case SurviveSimpleEventType_DeviceAdded: {
882 U_LOG_IFL_W(ss->log_level, "Device added event, but hotplugging not implemented yet");
883 break;
884 }
885 case SurviveSimpleEventType_None: break;
886 default: U_LOG_IFL_E(ss->log_level, "Unknown event %d", event->event_type);
887 }
888}
889
890static xrt_result_t
891survive_device_update_inputs(struct xrt_device *xdev)
892{
893 struct survive_device *survive = (struct survive_device *)xdev;
894
895 os_mutex_lock(&survive->sys->lock);
896
897 for (size_t i = 0; i < survive->base.input_count; i++) {
898 survive->base.inputs[i] = survive->last_inputs[i];
899 }
900
901 os_mutex_unlock(&survive->sys->lock);
902
903 return XRT_SUCCESS;
904}
905
906static xrt_result_t
907compute_distortion(struct xrt_device *xdev, uint32_t view, float u, float v, struct xrt_uv_triplet *result)
908{
909 struct survive_device *d = (struct survive_device *)xdev;
910 u_compute_distortion_vive(&d->hmd.config.distortion.values[view], u, v, result);
911
912 if (d->hmd.config.variant == VIVE_VARIANT_PRO2) {
913 // Flip Y coordinates
914 result->r.y = 1.0f - result->r.y;
915 result->g.y = 1.0f - result->g.y;
916 result->b.y = 1.0f - result->b.y;
917 }
918 return XRT_SUCCESS;
919}
920
921static bool
922_create_hmd_device(struct survive_system *sys, const struct SurviveSimpleObject *sso, char *conf_str)
923{
924
925 enum u_device_alloc_flags flags = (enum u_device_alloc_flags)U_DEVICE_ALLOC_HMD;
926 int inputs = 1;
927 int outputs = 0;
928
929 struct survive_device *survive = U_DEVICE_ALLOCATE(struct survive_device, flags, inputs, outputs);
930
931 if (!vive_config_parse(&survive->hmd.config, conf_str, sys->log_level)) {
932 free(survive);
933 return false;
934 }
935
936 sys->hmd = survive;
937 survive->sys = sys;
938 survive->survive_obj = sso;
939 survive->device_type = DEVICE_TYPE_HMD;
940
941 survive->base.name = XRT_DEVICE_GENERIC_HMD;
942 survive->base.destroy = survive_device_destroy;
943 survive->base.update_inputs = survive_device_update_inputs;
944 survive->base.get_tracked_pose = survive_device_get_tracked_pose;
945 survive->base.get_view_poses = survive_device_get_view_poses;
946 survive->base.tracking_origin = &sys->base;
947
948 SURVIVE_INFO(survive, "survive HMD present");
949 m_relation_history_create(&survive->relation_hist);
950
951
952 size_t idx = 0;
953 survive->base.hmd->blend_modes[idx++] = XRT_BLEND_MODE_OPAQUE;
954 survive->base.hmd->blend_mode_count = idx;
955
956 switch (survive->hmd.config.variant) {
957 case VIVE_VARIANT_VIVE: snprintf(survive->base.str, XRT_DEVICE_NAME_LEN, "HTC Vive (libsurvive)"); break;
958 case VIVE_VARIANT_PRO: snprintf(survive->base.str, XRT_DEVICE_NAME_LEN, "HTC Vive Pro (libsurvive)"); break;
959 case VIVE_VARIANT_INDEX: snprintf(survive->base.str, XRT_DEVICE_NAME_LEN, "Valve Index (libsurvive)"); break;
960 case VIVE_VARIANT_PRO2: snprintf(survive->base.str, XRT_DEVICE_NAME_LEN, "HTC Vive Pro 2 (libsurvive)"); break;
961 case VIVE_VARIANT_BEYOND:
962 snprintf(survive->base.str, XRT_DEVICE_NAME_LEN, "Bigscreen Beyond (libsurvive)");
963 break;
964 case VIVE_UNKNOWN: snprintf(survive->base.str, XRT_DEVICE_NAME_LEN, "Unknown HMD (libsurvive)"); break;
965 }
966 snprintf(survive->base.serial, XRT_DEVICE_NAME_LEN, "%s", survive->hmd.config.firmware.device_serial_number);
967
968 // Per-view size.
969 uint32_t w_pixels = survive->hmd.config.display.eye_target_width_in_pixels;
970 uint32_t h_pixels = survive->hmd.config.display.eye_target_height_in_pixels;
971
972 SURVIVE_DEBUG(survive, "display: %dx%d", w_pixels, h_pixels);
973
974 // Main display.
975 survive->base.hmd->screens[0].w_pixels = (int)w_pixels * 2;
976 survive->base.hmd->screens[0].h_pixels = (int)h_pixels;
977
978 if (survive->hmd.config.variant == VIVE_VARIANT_INDEX) {
979 survive->base.hmd->screens[0].nominal_frame_interval_ns = (uint64_t)time_s_to_ns(1.0f / 144.0f);
980 } else {
981 survive->base.hmd->screens[0].nominal_frame_interval_ns = (uint64_t)time_s_to_ns(1.0f / 90.0f);
982 }
983
984 if (survive->hmd.config.variant == VIVE_VARIANT_PRO) {
985 survive->base.hmd->screens[0].scanout_direction = XRT_SCANOUT_DIRECTION_TOP_TO_BOTTOM;
986 survive->base.hmd->screens[0].scanout_time_ns = survive->base.hmd->screens[0].nominal_frame_interval_ns;
987 // Compensate for the length of vblank.
988 survive->base.hmd->screens[0].scanout_time_ns *= 1600.0 / 1624.0;
989 } else {
990 survive->base.hmd->screens[0].scanout_direction = XRT_SCANOUT_DIRECTION_NONE;
991 survive->base.hmd->screens[0].scanout_time_ns = 0;
992 }
993
994 for (uint8_t eye = 0; eye < 2; eye++) {
995 struct xrt_view *v = &survive->base.hmd->views[eye];
996 v->display.w_pixels = w_pixels;
997 v->display.h_pixels = h_pixels;
998 v->viewport.w_pixels = w_pixels;
999 v->viewport.h_pixels = h_pixels;
1000 v->viewport.x_pixels = eye == 0 ? 0 : w_pixels;
1001 v->viewport.y_pixels = 0;
1002 v->rot = u_device_rotation_ident;
1003 }
1004
1005 // FoV values from config.
1006 survive->base.hmd->distortion.fov[0] = survive->hmd.config.distortion.fov[0];
1007 survive->base.hmd->distortion.fov[1] = survive->hmd.config.distortion.fov[1];
1008
1009 // Distortion params.
1010 survive->base.hmd->distortion.models = XRT_DISTORTION_MODEL_COMPUTE;
1011 survive->base.hmd->distortion.preferred = XRT_DISTORTION_MODEL_COMPUTE;
1012 survive->base.compute_distortion = compute_distortion;
1013 survive->base.get_battery_status = survive_device_get_battery_status;
1014
1015 survive->base.supported.orientation_tracking = true;
1016 survive->base.supported.position_tracking = true;
1017 survive->base.supported.battery_status = true;
1018 survive->base.device_type = XRT_DEVICE_TYPE_HMD;
1019
1020 survive->base.inputs[0].name = XRT_INPUT_GENERIC_HEAD_POSE;
1021
1022 survive->last_inputs = U_TYPED_ARRAY_CALLOC(struct xrt_input, survive->base.input_count);
1023 survive->num_last_inputs = survive->base.input_count;
1024 for (size_t i = 0; i < survive->base.input_count; i++) {
1025 survive->last_inputs[i] = survive->base.inputs[i];
1026 }
1027
1028 survive->hmd.use_default_ipd = debug_get_bool_option_survive_default_ipd();
1029
1030 u_var_add_root(survive, "Survive HMD Device", true);
1031 u_var_add_bool(survive, &survive->hmd.use_default_ipd, "Use default IPD");
1032 u_var_add_f32(survive, &survive->hmd.ipd, "IPD");
1033
1034 u_var_add_f32(survive, &survive->base.hmd->distortion.fov[0].angle_down, "View 0 FovAngleDown");
1035 u_var_add_f32(survive, &survive->base.hmd->distortion.fov[0].angle_left, "View 0 FovAngleLeft");
1036 u_var_add_f32(survive, &survive->base.hmd->distortion.fov[0].angle_right, "View 0 FovAngleRight");
1037 u_var_add_f32(survive, &survive->base.hmd->distortion.fov[0].angle_up, "View 0 FovAngleUp");
1038
1039 u_var_add_f32(survive, &survive->base.hmd->distortion.fov[1].angle_down, "View 1 FovAngleDown");
1040 u_var_add_f32(survive, &survive->base.hmd->distortion.fov[1].angle_left, "View 1 FovAngleLeft");
1041 u_var_add_f32(survive, &survive->base.hmd->distortion.fov[1].angle_right, "View 1 FovAngleRight");
1042 u_var_add_f32(survive, &survive->base.hmd->distortion.fov[1].angle_up, "View 1 FovAngleUp");
1043
1044 return true;
1045}
1046
1047#define SET_WAND_INPUT(NAME, NAME2) \
1048 do { \
1049 (survive->base.inputs[VIVE_CONTROLLER_##NAME].name = XRT_INPUT_VIVE_##NAME2); \
1050 } while (0)
1051
1052#define SET_INDEX_INPUT(NAME, NAME2) \
1053 do { \
1054 (survive->base.inputs[VIVE_CONTROLLER_##NAME].name = XRT_INPUT_INDEX_##NAME2); \
1055 } while (0)
1056
1057static bool
1058_create_controller_device(struct survive_system *sys,
1059 const SurviveSimpleObject *sso,
1060 struct vive_controller_config *config)
1061{
1062
1063 enum VIVE_CONTROLLER_VARIANT variant = config->variant;
1064
1065 int idx = -1;
1066 if (variant == CONTROLLER_VIVE_WAND) {
1067 if (sys->controllers[SURVIVE_LEFT_CONTROLLER_INDEX] == NULL) {
1068 idx = SURVIVE_LEFT_CONTROLLER_INDEX;
1069 } else if (sys->controllers[SURVIVE_RIGHT_CONTROLLER_INDEX] == NULL) {
1070 idx = SURVIVE_RIGHT_CONTROLLER_INDEX;
1071 } else {
1072 U_LOG_IFL_E(sys->log_level, "Only creating 2 controllers!");
1073 return false;
1074 }
1075 } else if (variant == CONTROLLER_INDEX_LEFT) {
1076 if (sys->controllers[SURVIVE_LEFT_CONTROLLER_INDEX] == NULL) {
1077 idx = SURVIVE_LEFT_CONTROLLER_INDEX;
1078 } else {
1079 U_LOG_IFL_E(sys->log_level, "Only creating 1 left controller!");
1080 return false;
1081 }
1082 } else if (variant == CONTROLLER_INDEX_RIGHT) {
1083 if (sys->controllers[SURVIVE_RIGHT_CONTROLLER_INDEX] == NULL) {
1084 idx = SURVIVE_RIGHT_CONTROLLER_INDEX;
1085 } else {
1086 U_LOG_IFL_E(sys->log_level, "Only creating 1 right controller!");
1087 return false;
1088 }
1089 } else if (variant == CONTROLLER_TRACKER_GEN1 || variant == CONTROLLER_TRACKER_GEN2 ||
1090 variant == CONTROLLER_TRACKER_GEN3 || variant == CONTROLLER_TRACKER_TUNDRA) {
1091 for (int i = SURVIVE_NON_CONTROLLER_START; i < MAX_TRACKED_DEVICE_COUNT; i++) {
1092 if (sys->controllers[i] == NULL) {
1093 idx = i;
1094 break;
1095 }
1096 }
1097 }
1098
1099 if (idx == -1) {
1100 U_LOG_IFL_E(sys->log_level, "Skipping survive device we couldn't assign: %s!",
1101 config->firmware.model_number);
1102 return false;
1103 }
1104
1105 enum u_device_alloc_flags flags = 0;
1106
1107 int inputs = VIVE_CONTROLLER_MAX_INDEX;
1108 int outputs = 1;
1109 struct survive_device *survive = U_DEVICE_ALLOCATE(struct survive_device, flags, inputs, outputs);
1110 survive->ctrl.config = *config;
1111 m_relation_history_create(&survive->relation_hist);
1112
1113 sys->controllers[idx] = survive;
1114 survive->sys = sys;
1115 survive->survive_obj = sso;
1116 survive->device_type = DEVICE_TYPE_CONTROLLER;
1117
1118 survive->base.tracking_origin = &sys->base;
1119
1120 survive->base.destroy = survive_device_destroy;
1121 survive->base.update_inputs = survive_device_update_inputs;
1122 survive->base.get_tracked_pose = survive_device_get_tracked_pose;
1123 survive->base.set_output = survive_controller_device_set_output;
1124 survive->base.get_battery_status = survive_device_get_battery_status;
1125 snprintf(survive->base.serial, XRT_DEVICE_NAME_LEN, "%s", survive->ctrl.config.firmware.device_serial_number);
1126
1127 if (variant == CONTROLLER_INDEX_LEFT || variant == CONTROLLER_INDEX_RIGHT) {
1128 survive->base.name = XRT_DEVICE_INDEX_CONTROLLER;
1129
1130 SET_INDEX_INPUT(SYSTEM_CLICK, SYSTEM_CLICK);
1131 SET_INDEX_INPUT(A_CLICK, A_CLICK);
1132 SET_INDEX_INPUT(B_CLICK, B_CLICK);
1133 SET_INDEX_INPUT(TRIGGER_CLICK, TRIGGER_CLICK);
1134 SET_INDEX_INPUT(TRIGGER_VALUE, TRIGGER_VALUE);
1135 SET_INDEX_INPUT(TRACKPAD, TRACKPAD);
1136 SET_INDEX_INPUT(TRACKPAD_TOUCH, TRACKPAD_TOUCH);
1137 SET_INDEX_INPUT(THUMBSTICK, THUMBSTICK);
1138 SET_INDEX_INPUT(THUMBSTICK_CLICK, THUMBSTICK_CLICK);
1139
1140 SET_INDEX_INPUT(THUMBSTICK_TOUCH, THUMBSTICK_TOUCH);
1141 SET_INDEX_INPUT(SYSTEM_TOUCH, SYSTEM_TOUCH);
1142 SET_INDEX_INPUT(A_TOUCH, A_TOUCH);
1143 SET_INDEX_INPUT(B_TOUCH, B_TOUCH);
1144 SET_INDEX_INPUT(SQUEEZE_VALUE, SQUEEZE_VALUE);
1145 SET_INDEX_INPUT(SQUEEZE_FORCE, SQUEEZE_FORCE);
1146 SET_INDEX_INPUT(TRIGGER_TOUCH, TRIGGER_TOUCH);
1147 SET_INDEX_INPUT(TRACKPAD_FORCE, TRACKPAD_FORCE);
1148
1149 SET_INDEX_INPUT(AIM_POSE, AIM_POSE);
1150 SET_INDEX_INPUT(GRIP_POSE, GRIP_POSE);
1151
1152 if (variant == CONTROLLER_INDEX_LEFT) {
1153 survive->base.device_type = XRT_DEVICE_TYPE_LEFT_HAND_CONTROLLER;
1154 survive->base.inputs[VIVE_CONTROLLER_HAND_TRACKING].name = XRT_INPUT_HT_CONFORMING_LEFT;
1155 snprintf(survive->base.str, XRT_DEVICE_NAME_LEN, "Valve Index Left Controller (libsurvive)");
1156 } else if (variant == CONTROLLER_INDEX_RIGHT) {
1157 survive->base.device_type = XRT_DEVICE_TYPE_RIGHT_HAND_CONTROLLER;
1158 survive->base.inputs[VIVE_CONTROLLER_HAND_TRACKING].name = XRT_INPUT_HT_CONFORMING_RIGHT;
1159 snprintf(survive->base.str, XRT_DEVICE_NAME_LEN, "Valve Index Right Controller (libsurvive)");
1160 }
1161
1162 survive->base.outputs[0].name = XRT_OUTPUT_NAME_INDEX_HAPTIC;
1163
1164 survive->base.binding_profiles = vive_binding_profiles_index;
1165 survive->base.binding_profile_count = vive_binding_profiles_index_count;
1166
1167 survive->base.get_hand_tracking = survive_controller_get_hand_tracking;
1168 survive->base.supported.hand_tracking = !debug_get_bool_option_survive_disable_hand_emulation();
1169
1170 } else if (survive->ctrl.config.variant == CONTROLLER_VIVE_WAND) {
1171 survive->base.name = XRT_DEVICE_VIVE_WAND;
1172 snprintf(survive->base.str, XRT_DEVICE_NAME_LEN, "Vive Wand Controller (libsurvive)");
1173
1174 SET_WAND_INPUT(SYSTEM_CLICK, SYSTEM_CLICK);
1175 SET_WAND_INPUT(SQUEEZE_CLICK, SQUEEZE_CLICK);
1176 SET_WAND_INPUT(MENU_CLICK, MENU_CLICK);
1177 SET_WAND_INPUT(TRIGGER_CLICK, TRIGGER_CLICK);
1178 SET_WAND_INPUT(TRIGGER_VALUE, TRIGGER_VALUE);
1179 SET_WAND_INPUT(TRACKPAD, TRACKPAD);
1180 SET_WAND_INPUT(TRACKPAD_CLICK, TRACKPAD_CLICK);
1181 SET_WAND_INPUT(TRACKPAD_TOUCH, TRACKPAD_TOUCH);
1182
1183 SET_WAND_INPUT(AIM_POSE, AIM_POSE);
1184 SET_WAND_INPUT(GRIP_POSE, GRIP_POSE);
1185
1186 survive->base.outputs[0].name = XRT_OUTPUT_NAME_VIVE_HAPTIC;
1187
1188 survive->base.binding_profiles = vive_binding_profiles_wand;
1189 survive->base.binding_profile_count = vive_binding_profiles_wand_count;
1190
1191 survive->base.device_type = XRT_DEVICE_TYPE_ANY_HAND_CONTROLLER;
1192 } else if (survive->ctrl.config.variant == CONTROLLER_TRACKER_GEN1 ||
1193 survive->ctrl.config.variant == CONTROLLER_TRACKER_GEN2 ||
1194 survive->ctrl.config.variant == CONTROLLER_TRACKER_GEN3 ||
1195 survive->ctrl.config.variant == CONTROLLER_TRACKER_TUNDRA) {
1196 if (survive->ctrl.config.variant == CONTROLLER_TRACKER_GEN1) {
1197 survive->base.name = XRT_DEVICE_VIVE_TRACKER_GEN1;
1198 snprintf(survive->base.str, XRT_DEVICE_NAME_LEN, "Vive Tracker Gen1 (libsurvive)");
1199 } else if (survive->ctrl.config.variant == CONTROLLER_TRACKER_GEN2) {
1200 survive->base.name = XRT_DEVICE_VIVE_TRACKER_GEN2;
1201 snprintf(survive->base.str, XRT_DEVICE_NAME_LEN, "Vive Tracker Gen2 (libsurvive)");
1202 } else if (survive->ctrl.config.variant == CONTROLLER_TRACKER_GEN3) {
1203 survive->base.name = XRT_DEVICE_VIVE_TRACKER_GEN3;
1204 snprintf(survive->base.str, XRT_DEVICE_NAME_LEN, "Vive Tracker Gen3 (libsurvive)");
1205 } else if (survive->ctrl.config.variant == CONTROLLER_TRACKER_TUNDRA) {
1206 survive->base.name = XRT_DEVICE_VIVE_TRACKER_TUNDRA;
1207 snprintf(survive->base.str, XRT_DEVICE_NAME_LEN, "Tundra Tracker Gen3 (libsurvive)");
1208 }
1209
1210 survive->base.device_type = XRT_DEVICE_TYPE_GENERIC_TRACKER;
1211
1212 survive->base.inputs[VIVE_TRACKER_POSE].name = XRT_INPUT_GENERIC_TRACKER_POSE;
1213 }
1214
1215 survive->base.supported.orientation_tracking = true;
1216 survive->base.supported.position_tracking = true;
1217 survive->base.supported.battery_status = true;
1218
1219 survive->last_inputs = U_TYPED_ARRAY_CALLOC(struct xrt_input, survive->base.input_count);
1220 survive->num_last_inputs = survive->base.input_count;
1221 for (size_t i = 0; i < survive->base.input_count; i++) {
1222 survive->last_inputs[i] = survive->base.inputs[i];
1223 }
1224
1225 SURVIVE_DEBUG(survive, "Created Controller %d", idx);
1226
1227 u_var_add_root(survive, "Survive Device", true);
1228
1229 return true;
1230}
1231
1232DEBUG_GET_ONCE_LOG_OPTION(survive_log, "SURVIVE_LOG", U_LOGGING_WARN)
1233DEBUG_GET_ONCE_OPTION(survive_lh_gen, "SURVIVE_LH_GEN", "0")
1234
1235static void
1236add_device(struct survive_system *ss, const struct SurviveSimpleConfigEvent *e)
1237{
1238 struct SurviveSimpleObject *sso = e->object;
1239
1240 U_LOG_IFL_D(ss->log_level, "Got device config from survive");
1241
1242 enum SurviveSimpleObject_type type = survive_simple_object_get_type(sso);
1243
1244 char *conf_str = (char *)survive_simple_json_config(sso);
1245
1246 if (type == SurviveSimpleObject_HMD) {
1247
1248 _create_hmd_device(ss, sso, conf_str);
1249
1250 } else if (type == SurviveSimpleObject_OBJECT) {
1251 struct vive_controller_config config = {0};
1252 vive_config_parse_controller(&config, conf_str, ss->log_level);
1253
1254 switch (config.variant) {
1255 case CONTROLLER_VIVE_WAND:
1256 case CONTROLLER_INDEX_LEFT:
1257 case CONTROLLER_INDEX_RIGHT:
1258 case CONTROLLER_TRACKER_GEN1:
1259 case CONTROLLER_TRACKER_GEN2:
1260 case CONTROLLER_TRACKER_GEN3:
1261 case CONTROLLER_TRACKER_TUNDRA:
1262 U_LOG_IFL_D(ss->log_level, "Adding controller: %s.", config.firmware.model_number);
1263 _create_controller_device(ss, sso, &config);
1264 break;
1265 default:
1266 U_LOG_IFL_D(ss->log_level, "Skip non controller obj %s.", config.firmware.model_number);
1267 U_LOG_IFL_T(ss->log_level, "json: %s", conf_str);
1268 break;
1269 }
1270 } else {
1271 U_LOG_IFL_D(ss->log_level, "Skip non OBJECT obj.");
1272 }
1273}
1274
1275static bool
1276add_connected_devices(struct survive_system *ss)
1277{
1278 /** @todo We don't know how many device added events we will get.
1279 * After 25ms Index HMD + Controllers are added here. So 250ms should be a safe value.
1280 * Device added just means libsurvive knows the usb devices, the config will then be loaded asynchronously.
1281 */
1282 os_nanosleep(250 * 1000 * 1000);
1283
1284 size_t objs = survive_simple_get_object_count(ss->ctx);
1285 U_LOG_IFL_D(ss->log_level, "Object count: %zu", objs);
1286
1287 timepoint_ns start = os_monotonic_get_ns();
1288
1289 // First count how many non-lighthouse objects libsurvive knows.
1290 // Then poll events until we have gotten configs for this many, or until timeout.
1291 int configs_to_wait_for = 0;
1292 int configs_gotten = 0;
1293
1294 for (const SurviveSimpleObject *sso = survive_simple_get_first_object(ss->ctx); sso;
1295 sso = survive_simple_get_next_object(ss->ctx, sso)) {
1296 enum SurviveSimpleObject_type t = survive_simple_object_get_type(sso);
1297 const char *name = survive_simple_object_name(sso);
1298 U_LOG_IFL_D(ss->log_level, "Object name %s: type %d", name, t);
1299
1300 // we only want to wait for configs of HMDs and controllers / trackers.
1301 // Note: HMDs will be of type SurviveSimpleObject_OBJECT until the config is loaded.
1302 if (t == SurviveSimpleObject_HMD || t == SurviveSimpleObject_OBJECT) {
1303 configs_to_wait_for++;
1304 }
1305 }
1306
1307 U_LOG_IFL_D(ss->log_level, "Waiting for %d configs", configs_to_wait_for);
1308 while (configs_gotten < configs_to_wait_for) {
1309 struct SurviveSimpleEvent event = {0};
1310 while (survive_simple_next_event(ss->ctx, &event) != SurviveSimpleEventType_None) {
1311 if (event.event_type == SurviveSimpleEventType_ConfigEvent) {
1312 _process_event(ss, &event);
1313 configs_gotten++;
1314 U_LOG_IFL_D(ss->log_level, "Got config from device: %d/%d", configs_gotten,
1315 configs_to_wait_for);
1316 } else {
1317 U_LOG_IFL_T(ss->log_level, "Skipping event type %d", event.event_type);
1318 }
1319 }
1320
1321 if (time_ns_to_s(os_monotonic_get_ns() - start) > ss->wait_timeout) {
1322 U_LOG_IFL_D(ss->log_level, "Timed out after getting configs for %d/%d devices", configs_gotten,
1323 configs_to_wait_for);
1324 break;
1325 }
1326 os_nanosleep(500 * 1000);
1327 }
1328 U_LOG_IFL_D(ss->log_level, "Waiting for configs took %f ms", time_ns_to_ms_f(os_monotonic_get_ns() - start));
1329 return true;
1330}
1331
1332static void *
1333run_event_thread(void *ptr)
1334{
1335 struct survive_system *ss = (struct survive_system *)ptr;
1336
1337 os_thread_helper_lock(&ss->event_thread);
1338 while (os_thread_helper_is_running_locked(&ss->event_thread)) {
1339 os_thread_helper_unlock(&ss->event_thread);
1340
1341 // one event queue for all devices. _process_events() updates all devices
1342 struct SurviveSimpleEvent event = {0};
1343 survive_simple_wait_for_event(ss->ctx, &event);
1344
1345 os_mutex_lock(&ss->lock);
1346 _process_event(ss, &event);
1347 os_mutex_unlock(&ss->lock);
1348
1349 // Just keep swimming.
1350 os_thread_helper_lock(&ss->event_thread);
1351 }
1352
1353 os_thread_helper_unlock(&ss->event_thread);
1354
1355 return NULL;
1356}
1357
1358int
1359survive_get_devices(struct xrt_device **out_xdevs, struct vive_config **out_vive_config)
1360{
1361 SurviveSimpleContext *actx = NULL;
1362#if 1
1363 char *survive_args[] = {
1364 "Monado-libsurvive", "--lighthouse-gen", (char *)debug_get_option_survive_lh_gen(),
1365 //"--time-window", "1500000"
1366 //"--use-imu", "0",
1367 //"--use-kalman", "0"
1368 };
1369 actx = survive_simple_init(sizeof(survive_args) / sizeof(survive_args[0]), survive_args);
1370#else
1371 actx = survive_simple_init(0, 0);
1372#endif
1373
1374 if (!actx) {
1375 U_LOG_E("failed to init survive");
1376 return 0;
1377 }
1378
1379 struct survive_system *ss = U_TYPED_CALLOC(struct survive_system);
1380
1381 survive_simple_start_thread(actx);
1382
1383 ss->ctx = actx;
1384 ss->base.type = XRT_TRACKING_TYPE_LIGHTHOUSE;
1385 snprintf(ss->base.name, XRT_TRACKING_NAME_LEN, "%s", "Libsurvive Tracking");
1386 ss->base.initial_offset.position.x = 0.0f;
1387 ss->base.initial_offset.position.y = 0.0f;
1388 ss->base.initial_offset.position.z = 0.0f;
1389 ss->base.initial_offset.orientation.w = 1.0f;
1390 ss->timecode_offset_ms = (struct u_var_draggable_f32){
1391 .val = debug_get_float_option_survive_timecode_offset_ms(),
1392 .min = -20.0,
1393 .step = 0.1,
1394 .max = +20.0,
1395 };
1396
1397 ss->log_level = debug_get_log_option_survive_log();
1398
1399 ss->wait_timeout = DEFAULT_WAIT_TIMEOUT;
1400
1401
1402 while (!add_connected_devices(ss)) {
1403 U_LOG_IFL_E(ss->log_level, "Failed to get device config from survive");
1404 continue;
1405 }
1406
1407 if (ss->log_level <= U_LOGGING_DEBUG) {
1408 if (ss->hmd) {
1409 u_device_dump_config(&ss->hmd->base, __func__, "libsurvive");
1410 }
1411 }
1412
1413 int out_idx = 0;
1414
1415 if (ss->hmd != NULL) {
1416 out_xdevs[out_idx++] = &ss->hmd->base;
1417 *out_vive_config = &ss->hmd->hmd.config;
1418 }
1419
1420 for (int i = 0; i < MAX_TRACKED_DEVICE_COUNT; i++) {
1421
1422 if (out_idx == XRT_MAX_DEVICES_PER_PROBE - 1) {
1423 U_LOG_IFL_W(ss->log_level, "Probed max of %d devices, ignoring further devices",
1424 XRT_MAX_DEVICES_PER_PROBE);
1425 return out_idx;
1426 }
1427
1428 if (ss->controllers[i] != NULL) {
1429 out_xdevs[out_idx++] = &ss->controllers[i]->base;
1430 }
1431 }
1432
1433 // Mutex before thread.
1434 int ret = os_mutex_init(&ss->lock);
1435 if (ret != 0) {
1436 U_LOG_IFL_E(ss->log_level, "Failed to init mutex!");
1437 survive_device_destroy((struct xrt_device *)ss->hmd);
1438 for (int i = 0; i < MAX_TRACKED_DEVICE_COUNT; i++) {
1439 survive_device_destroy((struct xrt_device *)ss->controllers[i]);
1440 }
1441 return 0;
1442 }
1443
1444 os_thread_helper_init(&ss->event_thread);
1445 ret = os_thread_helper_start(&ss->event_thread, run_event_thread, ss);
1446 if (ret != 0) {
1447 U_LOG_IFL_E(ss->log_level, "Failed to start event thread!");
1448 survive_device_destroy((struct xrt_device *)ss->hmd);
1449 for (int i = 0; i < MAX_TRACKED_DEVICE_COUNT; i++) {
1450 survive_device_destroy((struct xrt_device *)ss->controllers[i]);
1451 }
1452 return 0;
1453 }
1454
1455 u_var_add_root(ss, "Survive system", true);
1456 u_var_add_draggable_f32(ss, &ss->timecode_offset_ms, "Timecode offset(ms)");
1457
1458 return out_idx;
1459}