matrixfurry.com
The open source OpenXR runtime
1// Copyright 2019, Collabora, Ltd.
2// Copyright 2011, Iowa State University
3// SPDX-License-Identifier: BSL-1.0
4/*!
5 * @file
6 * @brief Razer Hydra prober and driver code
7 * @author Rylie Pavlik <rylie.pavlik@collabora.com>
8 * @author Jakob Bornecrantz <jakob@collabora.com>
9 *
10 * Portions based on the VRPN Razer Hydra driver,
11 * originally written by Rylie Pavlik and available under the BSL-1.0.
12 */
13
14
15#include <assert.h>
16#include <stdio.h>
17#include <stdlib.h>
18#include <string.h>
19
20#include "xrt/xrt_prober.h"
21
22#include "os/os_hid.h"
23#include "os/os_time.h"
24#include "os/os_threading.h"
25#include "xrt/xrt_byte_order.h"
26
27#include "math/m_api.h"
28#include "math/m_filter_one_euro.h"
29#include "math/m_relation_history.h"
30#include "math/m_space.h"
31
32#include "util/u_debug.h"
33#include "util/u_device.h"
34#include "util/u_misc.h"
35#include "util/u_time.h"
36#include "util/u_logging.h"
37#include "util/u_linux.h"
38#include "util/u_trace_marker.h"
39#include "util/u_var.h"
40
41#include "hydra_interface.h"
42
43
44
45/*
46 *
47 * Defines & structs.
48 *
49 */
50
51#define HD_TRACE(d, ...) U_LOG_XDEV_IFL_T(&d->base, d->sys->log_level, __VA_ARGS__)
52#define HD_DEBUG(d, ...) U_LOG_XDEV_IFL_D(&d->base, d->sys->log_level, __VA_ARGS__)
53#define HD_INFO(d, ...) U_LOG_XDEV_IFL_I(&d->base, d->sys->log_level, __VA_ARGS__)
54#define HD_WARN(d, ...) U_LOG_XDEV_IFL_W(&d->base, d->sys->log_level, __VA_ARGS__)
55#define HD_ERROR(d, ...) U_LOG_XDEV_IFL_E(&d->base, d->sys->log_level, __VA_ARGS__)
56
57#define HS_TRACE(d, ...) U_LOG_IFL_T(d->log_level, __VA_ARGS__)
58#define HS_DEBUG(d, ...) U_LOG_IFL_D(d->log_level, __VA_ARGS__)
59#define HS_INFO(d, ...) U_LOG_IFL_I(d->log_level, __VA_ARGS__)
60#define HS_WARN(d, ...) U_LOG_IFL_W(d->log_level, __VA_ARGS__)
61#define HS_ERROR(d, ...) U_LOG_IFL_E(d->log_level, __VA_ARGS__)
62
63DEBUG_GET_ONCE_LOG_OPTION(hydra_log, "HYDRA_LOG", U_LOGGING_WARN)
64
65enum hydra_input_index
66{
67 HYDRA_INDEX_1_CLICK,
68 HYDRA_INDEX_2_CLICK,
69 HYDRA_INDEX_3_CLICK,
70 HYDRA_INDEX_4_CLICK,
71 HYDRA_INDEX_MIDDLE_CLICK,
72 HYDRA_INDEX_BUMPER_CLICK,
73 HYDRA_INDEX_JOYSTICK_CLICK,
74 HYDRA_INDEX_JOYSTICK_VALUE,
75 HYDRA_INDEX_TRIGGER_VALUE,
76 HYDRA_INDEX_GRIP_POSE,
77 HYDRA_INDEX_AIM_POSE,
78 HYDRA_MAX_CONTROLLER_INDEX
79};
80
81/* Yes this is a bizarre bit mask. Mysteries of the Hydra. */
82enum hydra_button_bit
83{
84 HYDRA_BUTTON_BIT_BUMPER = (1 << 0),
85
86 HYDRA_BUTTON_BIT_3 = (1 << 1),
87 HYDRA_BUTTON_BIT_1 = (1 << 2),
88 HYDRA_BUTTON_BIT_2 = (1 << 3),
89 HYDRA_BUTTON_BIT_4 = (1 << 4),
90
91 HYDRA_BUTTON_BIT_MIDDLE = (1 << 5),
92 HYDRA_BUTTON_BIT_JOYSTICK = (1 << 6),
93};
94
95static const uint8_t HYDRA_REPORT_START_MOTION[] = {
96
97 0x00, // first byte must be report type
98 0x00, 0x00, 0x00, 0x00, 0x00, 0x01, 0x00, 0x04, 0x03, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
99 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
100 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
101 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
102 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x06, 0x00};
103
104static const uint8_t HYDRA_REPORT_START_GAMEPAD[] = {
105 0x00, // first byte must be report type
106 0x00, 0x00, 0x00, 0x00, 0x00, 0x01, 0x00, 0x04, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
107 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
108 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
109 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
110 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x05, 0x00};
111
112struct hydra_controller_state
113{
114 struct m_relation_history *relation_history;
115 struct
116 {
117 struct m_filter_euro_vec3 position;
118 struct m_filter_euro_quat orientation;
119 } motion_vector_filters;
120
121 struct xrt_vec2 js;
122 float trigger;
123 uint8_t buttons;
124};
125/*!
126 * The states of the finite-state machine controlling the Hydra.
127 */
128enum hydra_sm_state
129{
130 HYDRA_SM_LISTENING_AFTER_CONNECT = 0,
131 HYDRA_SM_LISTENING_AFTER_SET_FEATURE,
132 HYDRA_SM_REPORTING
133};
134
135/*!
136 * The details of the Hydra state machine in a convenient package.
137 */
138struct hydra_state_machine
139{
140 enum hydra_sm_state current_state;
141
142 //! Time of the last (non-trivial) state transition
143 timepoint_ns transition_time;
144};
145
146struct hydra_device;
147
148/*!
149 * A Razer Hydra system containing two controllers.
150 *
151 * @ingroup drv_hydra
152 * @extends xrt_tracking_origin
153 */
154struct hydra_system
155{
156 struct xrt_tracking_origin base;
157 struct os_hid_device *data_hid;
158 struct os_hid_device *command_hid;
159
160 struct os_thread_helper usb_thread;
161
162 struct os_mutex data_mutex;
163
164 struct hydra_state_machine sm;
165 struct hydra_device *devs[2];
166
167 int16_t report_counter;
168
169 //! Last time that we received a report
170 timepoint_ns report_time;
171
172 /*!
173 * Reference count of the number of devices still alive using this
174 * system
175 */
176 uint8_t refs;
177
178 /*!
179 * Was the hydra in gamepad mode at start?
180 *
181 * If it was, we set it back to gamepad on destruction.
182 */
183 bool was_in_gamepad_mode;
184 int motion_attempt_number;
185
186 enum u_logging_level log_level;
187};
188
189/*!
190 * A Razer Hydra device, representing just a single controller.
191 *
192 * @ingroup drv_hydra
193 * @implements xrt_device
194 */
195struct hydra_device
196{
197 struct xrt_device base;
198 struct hydra_system *sys;
199
200 //! Last time that we updated inputs
201 timepoint_ns input_time;
202
203 // bool calibration_done;
204 // int mirror;
205 // int sign_x;
206
207 struct hydra_controller_state state;
208
209 //! Which hydra controller in the system are we?
210 size_t index;
211};
212
213/*
214 *
215 * Internal functions.
216 *
217 */
218
219static void
220hydra_device_parse_controller(struct hydra_device *hd, uint8_t *buf, int64_t now);
221
222static inline struct hydra_device *
223hydra_device(struct xrt_device *xdev)
224{
225 assert(xdev);
226 struct hydra_device *ret = (struct hydra_device *)xdev;
227 assert(ret->sys != NULL);
228 return ret;
229}
230
231static inline struct hydra_system *
232hydra_system(struct xrt_tracking_origin *xtrack)
233{
234 assert(xtrack);
235 struct hydra_system *ret = (struct hydra_system *)xtrack;
236 return ret;
237}
238
239/*!
240 * Reports the number of seconds since the most recent change of state.
241 *
242 * @relates hydra_sm
243 */
244static float
245hydra_sm_seconds_since_transition(struct hydra_state_machine *hsm, timepoint_ns now)
246{
247
248 if (hsm->transition_time == 0) {
249 hsm->transition_time = now;
250 return 0.f;
251 }
252
253 float state_duration_s = time_ns_to_s(now - hsm->transition_time);
254 return state_duration_s;
255}
256/*!
257 * Performs a state transition, updating the transition time if the state
258 * actually changed.
259 *
260 * @relates hydra_sm
261 */
262static void
263hydra_sm_transition(struct hydra_state_machine *hsm, enum hydra_sm_state new_state, timepoint_ns now)
264{
265 if (hsm->transition_time == 0) {
266 hsm->transition_time = now;
267 }
268 if (new_state != hsm->current_state) {
269 hsm->current_state = new_state;
270 hsm->transition_time = now;
271 }
272}
273static inline uint8_t
274hydra_read_uint8(uint8_t **bufptr)
275{
276 uint8_t ret = **bufptr;
277 (*bufptr)++;
278 return ret;
279}
280static inline int16_t
281hydra_read_int16_le(uint8_t **bufptr)
282{
283 uint8_t *buf = *bufptr;
284#ifdef XRT_BIG_ENDIAN
285 uint8_t bytes[2] = {buf[1], buf[0]};
286#else
287 uint8_t bytes[2] = {buf[0], buf[1]};
288#endif // XRT_BIG_ENDIAN
289 (*bufptr) += 2;
290 int16_t ret;
291 memcpy(&ret, bytes, sizeof(ret));
292 return ret;
293}
294
295/*!
296 * Parse the controller-specific part of a buffer into a hydra device.
297 */
298static void
299hydra_device_parse_controller(struct hydra_device *hd, uint8_t *buf, int64_t now)
300{
301 struct hydra_controller_state *state = &hd->state;
302
303 static const float SCALE_MM_TO_METER = 0.001f;
304 static const float SCALE_INT16_TO_FLOAT_PLUSMINUS_1 = 1.0f / 32768.0f;
305 static const float SCALE_UINT8_TO_FLOAT_0_TO_1 = 1.0f / 255.0f;
306
307 struct xrt_pose pose;
308
309 pose.position.x = hydra_read_int16_le(&buf) * SCALE_MM_TO_METER;
310 pose.position.z = hydra_read_int16_le(&buf) * SCALE_MM_TO_METER;
311 pose.position.y = -hydra_read_int16_le(&buf) * SCALE_MM_TO_METER;
312
313 // the negatives are to fix handedness
314 pose.orientation.w = hydra_read_int16_le(&buf) * SCALE_INT16_TO_FLOAT_PLUSMINUS_1;
315 pose.orientation.x = hydra_read_int16_le(&buf) * SCALE_INT16_TO_FLOAT_PLUSMINUS_1;
316 pose.orientation.y = hydra_read_int16_le(&buf) * SCALE_INT16_TO_FLOAT_PLUSMINUS_1;
317 pose.orientation.z = hydra_read_int16_le(&buf) * SCALE_INT16_TO_FLOAT_PLUSMINUS_1;
318
319 //! @todo the presence of this suggest we're not decoding the
320 //! orientation right.
321 math_quat_normalize(&pose.orientation);
322
323 struct xrt_quat fixed = {
324 .x = pose.orientation.x,
325 .y = -pose.orientation.z,
326 .z = pose.orientation.y,
327 .w = pose.orientation.w,
328 };
329
330 struct xrt_quat adjustment = {.x = 0, .y = 1, .z = 0, .w = 0};
331 math_quat_rotate(&fixed, &adjustment, &fixed);
332
333 adjustment = (struct xrt_quat){.x = 0, .y = 0, .z = 1, .w = 0};
334 math_quat_rotate(&fixed, &adjustment, &fixed);
335
336 pose.orientation = fixed;
337
338 struct xrt_space_relation space_relation = {0};
339 m_filter_euro_vec3_run(&state->motion_vector_filters.position, now, &pose.position,
340 &space_relation.pose.position);
341 m_filter_euro_quat_run(&state->motion_vector_filters.orientation, now, &pose.orientation,
342 &space_relation.pose.orientation);
343
344 space_relation.relation_flags =
345 (XRT_SPACE_RELATION_ORIENTATION_TRACKED_BIT | XRT_SPACE_RELATION_ORIENTATION_VALID_BIT) |
346 (XRT_SPACE_RELATION_POSITION_TRACKED_BIT | XRT_SPACE_RELATION_POSITION_VALID_BIT);
347
348 m_relation_history_estimate_motion(state->relation_history, &space_relation, now, &space_relation);
349
350 m_relation_history_push(state->relation_history, &space_relation, now);
351
352 state->buttons = hydra_read_uint8(&buf);
353
354 state->js.x = hydra_read_int16_le(&buf) * SCALE_INT16_TO_FLOAT_PLUSMINUS_1;
355 state->js.y = hydra_read_int16_le(&buf) * SCALE_INT16_TO_FLOAT_PLUSMINUS_1;
356
357 state->trigger = hydra_read_uint8(&buf) * SCALE_UINT8_TO_FLOAT_0_TO_1;
358
359 HD_TRACE(hd,
360 "\n\t"
361 "controller: %i\n\t"
362 "position: (%-1.2f, %-1.2f, %-1.2f)\n\t"
363 "orientation: (%-1.2f, %-1.2f, %-1.2f, %-1.2f)\n\t"
364 "buttons: %08x\n\t"
365 "joystick: (%-1.2f, %-1.2f)\n\t"
366 "trigger: %01.2f\n",
367 (int)hd->index, pose.position.x, pose.position.y, pose.position.z, pose.orientation.x,
368 pose.orientation.y, pose.orientation.z, pose.orientation.w, state->buttons, state->js.x, state->js.y,
369 state->trigger);
370}
371
372static int
373hydra_system_read_data_hid(struct hydra_system *hs)
374{
375 assert(hs);
376 uint8_t buffer[128];
377
378 int ret = os_hid_read(hs->data_hid, buffer, sizeof(buffer),
379 20); // 20ms is a generous number above the 16.66ms we expect to receive reports at (60hz)
380
381 timepoint_ns now = os_monotonic_get_ns();
382
383 // we dont care if we get no data
384 if (ret <= 0) {
385 return ret;
386 }
387
388 if (ret != 52) {
389 HS_ERROR(hs, "Unexpected data report of size %d", ret);
390 return -1;
391 }
392
393 os_mutex_lock(&hs->data_mutex);
394
395 uint8_t new_counter = buffer[7];
396 bool missed = false;
397 if (hs->report_counter != -1) {
398 uint8_t expected_counter = ((hs->report_counter + 1) & 0xff);
399 missed = new_counter != expected_counter;
400 }
401 hs->report_counter = new_counter;
402
403
404 if (hs->devs[0] != NULL) {
405 hydra_device_parse_controller(hs->devs[0], buffer + 8, now);
406 }
407 if (hs->devs[1] != NULL) {
408 hydra_device_parse_controller(hs->devs[1], buffer + 30, now);
409 }
410
411 hs->report_time = now;
412
413 os_mutex_unlock(&hs->data_mutex);
414
415 HS_TRACE(hs,
416 "\n\t"
417 "missed: %s\n\t"
418 "seq_no: %x\n",
419 missed ? "yes" : "no", new_counter);
420
421 return ret;
422}
423
424
425/*!
426 * Switch to motion controller mode.
427 */
428static void
429hydra_system_enter_motion_control(struct hydra_system *hs, timepoint_ns now)
430{
431 assert(hs);
432
433 hs->was_in_gamepad_mode = true;
434 hs->motion_attempt_number++;
435 HS_DEBUG(hs,
436 "Setting feature report to start motion-controller mode, "
437 "attempt %d",
438 hs->motion_attempt_number);
439
440 os_hid_set_feature(hs->command_hid, HYDRA_REPORT_START_MOTION, sizeof(HYDRA_REPORT_START_MOTION));
441
442 // Doing a throwaway get-feature now.
443 uint8_t buf[91] = {0};
444 os_hid_get_feature(hs->command_hid, 0, buf, sizeof(buf));
445
446 hydra_sm_transition(&hs->sm, HYDRA_SM_LISTENING_AFTER_SET_FEATURE, now);
447}
448/*!
449 * Update the internal state of the Hydra driver.
450 *
451 * Reads devices, checks the state machine and timeouts, etc.
452 *
453 */
454static int
455hydra_system_update(struct hydra_system *hs)
456{
457 assert(hs);
458
459 // In all states of the state machine:
460 // Try reading a report: will only return >0 if we get a full motion
461 // report.
462 int received = hydra_system_read_data_hid(hs);
463
464 // we got an error
465 if (received < 0) {
466 return received;
467 }
468
469 os_mutex_lock(&hs->data_mutex);
470
471 timepoint_ns now = os_monotonic_get_ns();
472
473 // if we got data, transition to "reporting" mode
474 if (received > 0) {
475 hydra_sm_transition(&hs->sm, HYDRA_SM_REPORTING, now);
476 }
477
478 switch (hs->sm.current_state) {
479 case HYDRA_SM_LISTENING_AFTER_CONNECT: {
480 float state_duration_s = hydra_sm_seconds_since_transition(&hs->sm, now);
481 if (state_duration_s > 1.0f) {
482 // only waiting 1 second for the initial report after
483 // connect
484 hydra_system_enter_motion_control(hs, now);
485 }
486 } break;
487 case HYDRA_SM_LISTENING_AFTER_SET_FEATURE: {
488 float state_duration_s = hydra_sm_seconds_since_transition(&hs->sm, now);
489 if (state_duration_s > 5.0f) {
490 // giving each motion control attempt 5 seconds to work.
491 hydra_system_enter_motion_control(hs, now);
492 }
493 } break;
494 default: break;
495 }
496
497 os_mutex_unlock(&hs->data_mutex);
498
499 return 0;
500}
501
502static void
503hydra_device_update_input_click(struct hydra_device *hd, timepoint_ns now, int index, uint32_t bit)
504{
505 assert(hd);
506 hd->base.inputs[index].timestamp = now;
507 hd->base.inputs[index].value.boolean = (hd->state.buttons & bit) != 0;
508}
509
510static void *
511hydra_usb_thread_run(void *user_data)
512{
513 struct hydra_system *hs = (struct hydra_system *)user_data;
514
515 const char *thread_name = "Hydra USB";
516
517 U_TRACE_SET_THREAD_NAME(thread_name);
518 os_thread_helper_name(&hs->usb_thread, thread_name);
519
520#ifdef XRT_OS_LINUX
521 // Try to raise priority of this thread.
522 u_linux_try_to_set_realtime_priority_on_thread(hs->log_level, thread_name);
523#endif
524
525 os_thread_helper_lock(&hs->usb_thread);
526
527 int result = 0;
528#if 0
529 int ticks = 0;
530#endif
531
532 while (os_thread_helper_is_running_locked(&hs->usb_thread) && result >= 0) {
533 os_thread_helper_unlock(&hs->usb_thread);
534
535 result = hydra_system_update(hs);
536
537 os_thread_helper_lock(&hs->usb_thread);
538#if 0
539 ticks += 1;
540#endif
541 }
542
543 os_thread_helper_unlock(&hs->usb_thread);
544
545 return NULL;
546}
547
548/*
549 *
550 * Device functions.
551 *
552 */
553
554static xrt_result_t
555hydra_device_update_inputs(struct xrt_device *xdev)
556{
557 struct hydra_device *hd = hydra_device(xdev);
558 struct hydra_system *hs = hydra_system(xdev->tracking_origin);
559
560 os_mutex_lock(&hs->data_mutex);
561
562 if (hd->input_time != hs->report_time) {
563 timepoint_ns now = hs->report_time;
564 hd->input_time = now;
565
566 hydra_device_update_input_click(hd, now, HYDRA_INDEX_1_CLICK, HYDRA_BUTTON_BIT_1);
567 hydra_device_update_input_click(hd, now, HYDRA_INDEX_2_CLICK, HYDRA_BUTTON_BIT_2);
568 hydra_device_update_input_click(hd, now, HYDRA_INDEX_3_CLICK, HYDRA_BUTTON_BIT_3);
569 hydra_device_update_input_click(hd, now, HYDRA_INDEX_4_CLICK, HYDRA_BUTTON_BIT_4);
570
571 hydra_device_update_input_click(hd, now, HYDRA_INDEX_MIDDLE_CLICK, HYDRA_BUTTON_BIT_MIDDLE);
572 hydra_device_update_input_click(hd, now, HYDRA_INDEX_BUMPER_CLICK, HYDRA_BUTTON_BIT_BUMPER);
573 hydra_device_update_input_click(hd, now, HYDRA_INDEX_JOYSTICK_CLICK, HYDRA_INDEX_JOYSTICK_CLICK);
574
575 struct xrt_input *inputs = hd->base.inputs;
576 struct hydra_controller_state *state = &(hd->state);
577
578 inputs[HYDRA_INDEX_JOYSTICK_VALUE].timestamp = now;
579 inputs[HYDRA_INDEX_JOYSTICK_VALUE].value.vec2 = state->js;
580
581 inputs[HYDRA_INDEX_TRIGGER_VALUE].timestamp = now;
582 inputs[HYDRA_INDEX_TRIGGER_VALUE].value.vec1.x = state->trigger;
583 }
584
585 os_mutex_unlock(&hs->data_mutex);
586
587 return XRT_SUCCESS;
588}
589
590static xrt_result_t
591hydra_device_get_tracked_pose(struct xrt_device *xdev,
592 enum xrt_input_name name,
593 int64_t at_timestamp_ns,
594 struct xrt_space_relation *out_relation)
595{
596 struct hydra_device *hd = hydra_device(xdev);
597
598 struct xrt_relation_chain xrc = {0};
599
600 switch (name) {
601 case XRT_INPUT_HYDRA_AIM_POSE: {
602 const struct xrt_pose aim_offset = {
603 .position = {0, 0.045, -0.08},
604 .orientation = {-0.258819, 0, 0, 0.9659258},
605 };
606 m_relation_chain_push_pose(&xrc, &aim_offset);
607 break;
608 }
609 case XRT_INPUT_HYDRA_GRIP_POSE:
610 default: break;
611 }
612
613 struct xrt_space_relation device_relation = {0};
614 m_relation_history_get(hd->state.relation_history, at_timestamp_ns, &device_relation);
615
616 m_relation_chain_push_relation(&xrc, &device_relation);
617
618 m_relation_chain_resolve(&xrc, out_relation);
619
620 return XRT_SUCCESS;
621}
622
623static void
624hydra_system_remove_child(struct hydra_system *hs, struct hydra_device *hd)
625{
626 assert(hydra_system(hd->base.tracking_origin) == hs);
627 assert(hd->index == 0 || hd->index == 1);
628
629 // Make the device not point to the system
630 hd->sys = NULL;
631
632 // Make the system not point to the device
633 assert(hs->devs[hd->index] == hd);
634 hs->devs[hd->index] = NULL;
635
636 // Decrease ref count of system
637 hs->refs--;
638
639 if (hs->refs == 0) {
640 os_thread_helper_destroy(&hs->usb_thread);
641
642 os_mutex_destroy(&hs->data_mutex);
643
644 // No more children, destroy system.
645 if (hs->data_hid != NULL && hs->command_hid != NULL && hs->sm.current_state == HYDRA_SM_REPORTING &&
646 hs->was_in_gamepad_mode) {
647
648 HS_DEBUG(hs,
649 "Sending command to re-enter gamepad mode "
650 "and pausing while it takes effect.");
651
652 os_hid_set_feature(hs->command_hid, HYDRA_REPORT_START_GAMEPAD,
653 sizeof(HYDRA_REPORT_START_GAMEPAD));
654 os_nanosleep(2 * 1000 * 1000 * 1000);
655 }
656 if (hs->data_hid != NULL) {
657 os_hid_destroy(hs->data_hid);
658 hs->data_hid = NULL;
659 }
660 if (hs->command_hid != NULL) {
661 os_hid_destroy(hs->command_hid);
662 hs->command_hid = NULL;
663 }
664 free(hs);
665 }
666}
667
668static void
669hydra_device_destroy(struct xrt_device *xdev)
670{
671 struct hydra_device *hd = hydra_device(xdev);
672 struct hydra_system *hs = hydra_system(xdev->tracking_origin);
673
674 m_relation_history_destroy(&hd->state.relation_history);
675
676 hydra_system_remove_child(hs, hd);
677
678 free(hd);
679}
680
681/*
682 *
683 * Prober functions.
684 *
685 */
686#define SET_INPUT(NAME) \
687 do { \
688 (hd->base.inputs[HYDRA_INDEX_##NAME].name = XRT_INPUT_HYDRA_##NAME); \
689 } while (0)
690
691static struct xrt_binding_input_pair touch_inputs[19] = {
692 {XRT_INPUT_TOUCH_X_CLICK, XRT_INPUT_HYDRA_2_CLICK},
693 {XRT_INPUT_TOUCH_X_TOUCH, XRT_INPUT_HYDRA_2_CLICK},
694 {XRT_INPUT_TOUCH_Y_CLICK, XRT_INPUT_HYDRA_4_CLICK},
695 {XRT_INPUT_TOUCH_Y_TOUCH, XRT_INPUT_HYDRA_4_CLICK},
696 {XRT_INPUT_TOUCH_MENU_CLICK, XRT_INPUT_HYDRA_MIDDLE_CLICK},
697 {XRT_INPUT_TOUCH_A_CLICK, XRT_INPUT_HYDRA_1_CLICK},
698 {XRT_INPUT_TOUCH_A_TOUCH, XRT_INPUT_HYDRA_1_CLICK},
699 {XRT_INPUT_TOUCH_B_CLICK, XRT_INPUT_HYDRA_3_CLICK},
700 {XRT_INPUT_TOUCH_B_TOUCH, XRT_INPUT_HYDRA_3_CLICK},
701 {XRT_INPUT_TOUCH_SYSTEM_CLICK, XRT_INPUT_HYDRA_MIDDLE_CLICK},
702 {XRT_INPUT_TOUCH_SQUEEZE_VALUE, XRT_INPUT_HYDRA_BUMPER_CLICK},
703 {XRT_INPUT_TOUCH_TRIGGER_TOUCH, XRT_INPUT_HYDRA_TRIGGER_VALUE},
704 {XRT_INPUT_TOUCH_TRIGGER_VALUE, XRT_INPUT_HYDRA_TRIGGER_VALUE},
705 {XRT_INPUT_TOUCH_THUMBSTICK_CLICK, XRT_INPUT_HYDRA_JOYSTICK_CLICK},
706 {XRT_INPUT_TOUCH_THUMBSTICK, XRT_INPUT_HYDRA_JOYSTICK_VALUE},
707 {XRT_INPUT_TOUCH_GRIP_POSE, XRT_INPUT_HYDRA_GRIP_POSE},
708 {XRT_INPUT_TOUCH_AIM_POSE, XRT_INPUT_HYDRA_AIM_POSE},
709};
710
711static struct xrt_binding_input_pair simple_inputs[4] = {
712 {XRT_INPUT_SIMPLE_SELECT_CLICK, XRT_INPUT_HYDRA_TRIGGER_VALUE},
713 {XRT_INPUT_SIMPLE_MENU_CLICK, XRT_INPUT_HYDRA_MIDDLE_CLICK},
714 {XRT_INPUT_SIMPLE_GRIP_POSE, XRT_INPUT_HYDRA_GRIP_POSE},
715 {XRT_INPUT_SIMPLE_AIM_POSE, XRT_INPUT_HYDRA_AIM_POSE},
716};
717
718static struct xrt_binding_profile binding_profiles[2] = {
719 {
720 .name = XRT_DEVICE_TOUCH_CONTROLLER,
721 .inputs = touch_inputs,
722 .input_count = ARRAY_SIZE(touch_inputs),
723 },
724 {
725 .name = XRT_DEVICE_SIMPLE_CONTROLLER,
726 .inputs = simple_inputs,
727 .input_count = ARRAY_SIZE(simple_inputs),
728 },
729};
730
731int
732hydra_found(struct xrt_prober *xp,
733 struct xrt_prober_device **devices,
734 size_t device_count,
735 size_t index,
736 cJSON *attached_data,
737 struct xrt_device **out_xdevs)
738{
739 struct xrt_prober_device *dev = devices[index];
740 int ret;
741
742 struct os_hid_device *data_hid = NULL;
743 ret = xrt_prober_open_hid_interface(xp, dev, 0, &data_hid);
744 if (ret != 0) {
745 return -1;
746 }
747 struct os_hid_device *command_hid = NULL;
748 ret = xrt_prober_open_hid_interface(xp, dev, 1, &command_hid);
749 if (ret != 0) {
750 os_hid_destroy(data_hid);
751 return -1;
752 }
753
754 // Create the system
755 struct hydra_system *hs = U_TYPED_CALLOC(struct hydra_system);
756 hs->base.type = XRT_TRACKING_TYPE_MAGNETIC;
757 snprintf(hs->base.name, XRT_TRACKING_NAME_LEN, "%s", "Sixense Magnetic Tracking");
758 // Arbitrary static transform from local space to base.
759 hs->base.initial_offset.position.y = 1.0f;
760 hs->base.initial_offset.position.z = -0.25f;
761 hs->base.initial_offset.orientation.w = 1.0f;
762
763 ret = os_thread_helper_init(&hs->usb_thread);
764 if (ret < 0) {
765 HS_ERROR(hs, "Failed to init USB thread.");
766 after_system_err:
767 free(hs);
768 os_hid_destroy(command_hid);
769 os_hid_destroy(data_hid);
770 return -1;
771 }
772
773 ret = os_mutex_init(&hs->data_mutex);
774 if (ret < 0) {
775 HS_ERROR(hs, "Failed to init data mutex.");
776 os_thread_helper_destroy(&hs->usb_thread);
777 goto after_system_err;
778 }
779
780 hs->data_hid = data_hid;
781 hs->command_hid = command_hid;
782
783 enum u_device_alloc_flags flags = (enum u_device_alloc_flags)0;
784 hs->devs[0] = U_DEVICE_ALLOCATE(struct hydra_device, flags, HYDRA_MAX_CONTROLLER_INDEX, 0);
785 hs->devs[1] = U_DEVICE_ALLOCATE(struct hydra_device, flags, HYDRA_MAX_CONTROLLER_INDEX, 0);
786
787 hs->report_counter = -1;
788 hs->refs = 2;
789
790 hs->log_level = debug_get_log_option_hydra_log();
791
792 u_var_add_root(hs, "Razer Hydra System", false);
793 u_var_add_log_level(hs, &hs->log_level, "Log Level");
794 u_var_add_bool(hs, &hs->was_in_gamepad_mode, "Was In Gamepad Mode");
795 u_var_add_i32(hs, &hs->motion_attempt_number, "Motion Attempt Number");
796 u_var_add_ro_i16(hs, &hs->report_counter, "Report Counter");
797
798 // Populate the individual devices
799 for (size_t i = 0; i < 2; ++i) {
800 struct hydra_device *hd = hs->devs[i];
801
802 hd->base.destroy = hydra_device_destroy;
803 hd->base.update_inputs = hydra_device_update_inputs;
804 hd->base.get_tracked_pose = hydra_device_get_tracked_pose;
805 hd->base.set_output = u_device_ni_set_output;
806 hd->base.name = XRT_DEVICE_HYDRA;
807 snprintf(hd->base.str, XRT_DEVICE_NAME_LEN, "%s %i", "Razer Hydra Controller", (int)(i + 1));
808 snprintf(hd->base.serial, XRT_DEVICE_NAME_LEN, "%s%i", "RZRHDRC", (int)(i + 1));
809 SET_INPUT(1_CLICK);
810 SET_INPUT(2_CLICK);
811 SET_INPUT(3_CLICK);
812 SET_INPUT(4_CLICK);
813 SET_INPUT(MIDDLE_CLICK);
814 SET_INPUT(BUMPER_CLICK);
815 SET_INPUT(JOYSTICK_CLICK);
816 SET_INPUT(JOYSTICK_VALUE);
817 SET_INPUT(TRIGGER_VALUE);
818 SET_INPUT(GRIP_POSE);
819 SET_INPUT(AIM_POSE);
820 hd->index = i;
821 hd->sys = hs;
822
823 const float fc_min = 9.f;
824 const float fc_min_d = 9.f;
825 const float beta = 0.1f;
826
827 m_filter_euro_vec3_init(&hd->state.motion_vector_filters.position, fc_min, fc_min_d, beta);
828 m_filter_euro_quat_init(&hd->state.motion_vector_filters.orientation, fc_min, fc_min_d, beta);
829
830 m_relation_history_create(&hd->state.relation_history);
831
832 hd->base.binding_profiles = binding_profiles;
833 hd->base.binding_profile_count = ARRAY_SIZE(binding_profiles);
834
835 hd->base.tracking_origin = &hs->base;
836
837 hd->base.supported.position_tracking = true;
838 hd->base.supported.orientation_tracking = true;
839 hd->base.device_type = XRT_DEVICE_TYPE_ANY_HAND_CONTROLLER;
840
841 out_xdevs[i] = &(hd->base);
842 }
843
844 ret = os_thread_helper_start(&hs->usb_thread, hydra_usb_thread_run, hs);
845 if (ret < 0) {
846 HS_ERROR(hs, "Failed to start USB thread.");
847
848 // doing this will destroy the system as well
849 xrt_device_destroy((struct xrt_device **)&hs->devs[0]);
850 xrt_device_destroy((struct xrt_device **)&hs->devs[1]);
851
852 return ret;
853 }
854
855 HS_INFO(hs, "Opened Razer Hydra!");
856 return 2;
857}