// Copyright 2019, Collabora, Ltd. // Copyright 2011, Iowa State University // SPDX-License-Identifier: BSL-1.0 /*! * @file * @brief Razer Hydra prober and driver code * @author Rylie Pavlik * @author Jakob Bornecrantz * * Portions based on the VRPN Razer Hydra driver, * originally written by Rylie Pavlik and available under the BSL-1.0. */ #include #include #include #include #include "xrt/xrt_prober.h" #include "os/os_hid.h" #include "os/os_time.h" #include "os/os_threading.h" #include "xrt/xrt_byte_order.h" #include "math/m_api.h" #include "math/m_filter_one_euro.h" #include "math/m_relation_history.h" #include "math/m_space.h" #include "util/u_debug.h" #include "util/u_device.h" #include "util/u_misc.h" #include "util/u_time.h" #include "util/u_logging.h" #include "util/u_linux.h" #include "util/u_trace_marker.h" #include "util/u_var.h" #include "hydra_interface.h" /* * * Defines & structs. * */ #define HD_TRACE(d, ...) U_LOG_XDEV_IFL_T(&d->base, d->sys->log_level, __VA_ARGS__) #define HD_DEBUG(d, ...) U_LOG_XDEV_IFL_D(&d->base, d->sys->log_level, __VA_ARGS__) #define HD_INFO(d, ...) U_LOG_XDEV_IFL_I(&d->base, d->sys->log_level, __VA_ARGS__) #define HD_WARN(d, ...) U_LOG_XDEV_IFL_W(&d->base, d->sys->log_level, __VA_ARGS__) #define HD_ERROR(d, ...) U_LOG_XDEV_IFL_E(&d->base, d->sys->log_level, __VA_ARGS__) #define HS_TRACE(d, ...) U_LOG_IFL_T(d->log_level, __VA_ARGS__) #define HS_DEBUG(d, ...) U_LOG_IFL_D(d->log_level, __VA_ARGS__) #define HS_INFO(d, ...) U_LOG_IFL_I(d->log_level, __VA_ARGS__) #define HS_WARN(d, ...) U_LOG_IFL_W(d->log_level, __VA_ARGS__) #define HS_ERROR(d, ...) U_LOG_IFL_E(d->log_level, __VA_ARGS__) DEBUG_GET_ONCE_LOG_OPTION(hydra_log, "HYDRA_LOG", U_LOGGING_WARN) enum hydra_input_index { HYDRA_INDEX_1_CLICK, HYDRA_INDEX_2_CLICK, HYDRA_INDEX_3_CLICK, HYDRA_INDEX_4_CLICK, HYDRA_INDEX_MIDDLE_CLICK, HYDRA_INDEX_BUMPER_CLICK, HYDRA_INDEX_JOYSTICK_CLICK, HYDRA_INDEX_JOYSTICK_VALUE, HYDRA_INDEX_TRIGGER_VALUE, HYDRA_INDEX_GRIP_POSE, HYDRA_INDEX_AIM_POSE, HYDRA_MAX_CONTROLLER_INDEX }; /* Yes this is a bizarre bit mask. Mysteries of the Hydra. */ enum hydra_button_bit { HYDRA_BUTTON_BIT_BUMPER = (1 << 0), HYDRA_BUTTON_BIT_3 = (1 << 1), HYDRA_BUTTON_BIT_1 = (1 << 2), HYDRA_BUTTON_BIT_2 = (1 << 3), HYDRA_BUTTON_BIT_4 = (1 << 4), HYDRA_BUTTON_BIT_MIDDLE = (1 << 5), HYDRA_BUTTON_BIT_JOYSTICK = (1 << 6), }; static const uint8_t HYDRA_REPORT_START_MOTION[] = { 0x00, // first byte must be report type 0x00, 0x00, 0x00, 0x00, 0x00, 0x01, 0x00, 0x04, 0x03, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x06, 0x00}; static const uint8_t HYDRA_REPORT_START_GAMEPAD[] = { 0x00, // first byte must be report type 0x00, 0x00, 0x00, 0x00, 0x00, 0x01, 0x00, 0x04, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x05, 0x00}; struct hydra_controller_state { struct m_relation_history *relation_history; struct { struct m_filter_euro_vec3 position; struct m_filter_euro_quat orientation; } motion_vector_filters; struct xrt_vec2 js; float trigger; uint8_t buttons; }; /*! * The states of the finite-state machine controlling the Hydra. */ enum hydra_sm_state { HYDRA_SM_LISTENING_AFTER_CONNECT = 0, HYDRA_SM_LISTENING_AFTER_SET_FEATURE, HYDRA_SM_REPORTING }; /*! * The details of the Hydra state machine in a convenient package. */ struct hydra_state_machine { enum hydra_sm_state current_state; //! Time of the last (non-trivial) state transition timepoint_ns transition_time; }; struct hydra_device; /*! * A Razer Hydra system containing two controllers. * * @ingroup drv_hydra * @extends xrt_tracking_origin */ struct hydra_system { struct xrt_tracking_origin base; struct os_hid_device *data_hid; struct os_hid_device *command_hid; struct os_thread_helper usb_thread; struct os_mutex data_mutex; struct hydra_state_machine sm; struct hydra_device *devs[2]; int16_t report_counter; //! Last time that we received a report timepoint_ns report_time; /*! * Reference count of the number of devices still alive using this * system */ uint8_t refs; /*! * Was the hydra in gamepad mode at start? * * If it was, we set it back to gamepad on destruction. */ bool was_in_gamepad_mode; int motion_attempt_number; enum u_logging_level log_level; }; /*! * A Razer Hydra device, representing just a single controller. * * @ingroup drv_hydra * @implements xrt_device */ struct hydra_device { struct xrt_device base; struct hydra_system *sys; //! Last time that we updated inputs timepoint_ns input_time; // bool calibration_done; // int mirror; // int sign_x; struct hydra_controller_state state; //! Which hydra controller in the system are we? size_t index; }; /* * * Internal functions. * */ static void hydra_device_parse_controller(struct hydra_device *hd, uint8_t *buf, int64_t now); static inline struct hydra_device * hydra_device(struct xrt_device *xdev) { assert(xdev); struct hydra_device *ret = (struct hydra_device *)xdev; assert(ret->sys != NULL); return ret; } static inline struct hydra_system * hydra_system(struct xrt_tracking_origin *xtrack) { assert(xtrack); struct hydra_system *ret = (struct hydra_system *)xtrack; return ret; } /*! * Reports the number of seconds since the most recent change of state. * * @relates hydra_sm */ static float hydra_sm_seconds_since_transition(struct hydra_state_machine *hsm, timepoint_ns now) { if (hsm->transition_time == 0) { hsm->transition_time = now; return 0.f; } float state_duration_s = time_ns_to_s(now - hsm->transition_time); return state_duration_s; } /*! * Performs a state transition, updating the transition time if the state * actually changed. * * @relates hydra_sm */ static void hydra_sm_transition(struct hydra_state_machine *hsm, enum hydra_sm_state new_state, timepoint_ns now) { if (hsm->transition_time == 0) { hsm->transition_time = now; } if (new_state != hsm->current_state) { hsm->current_state = new_state; hsm->transition_time = now; } } static inline uint8_t hydra_read_uint8(uint8_t **bufptr) { uint8_t ret = **bufptr; (*bufptr)++; return ret; } static inline int16_t hydra_read_int16_le(uint8_t **bufptr) { uint8_t *buf = *bufptr; #ifdef XRT_BIG_ENDIAN uint8_t bytes[2] = {buf[1], buf[0]}; #else uint8_t bytes[2] = {buf[0], buf[1]}; #endif // XRT_BIG_ENDIAN (*bufptr) += 2; int16_t ret; memcpy(&ret, bytes, sizeof(ret)); return ret; } /*! * Parse the controller-specific part of a buffer into a hydra device. */ static void hydra_device_parse_controller(struct hydra_device *hd, uint8_t *buf, int64_t now) { struct hydra_controller_state *state = &hd->state; static const float SCALE_MM_TO_METER = 0.001f; static const float SCALE_INT16_TO_FLOAT_PLUSMINUS_1 = 1.0f / 32768.0f; static const float SCALE_UINT8_TO_FLOAT_0_TO_1 = 1.0f / 255.0f; struct xrt_pose pose; pose.position.x = hydra_read_int16_le(&buf) * SCALE_MM_TO_METER; pose.position.z = hydra_read_int16_le(&buf) * SCALE_MM_TO_METER; pose.position.y = -hydra_read_int16_le(&buf) * SCALE_MM_TO_METER; // the negatives are to fix handedness pose.orientation.w = hydra_read_int16_le(&buf) * SCALE_INT16_TO_FLOAT_PLUSMINUS_1; pose.orientation.x = hydra_read_int16_le(&buf) * SCALE_INT16_TO_FLOAT_PLUSMINUS_1; pose.orientation.y = hydra_read_int16_le(&buf) * SCALE_INT16_TO_FLOAT_PLUSMINUS_1; pose.orientation.z = hydra_read_int16_le(&buf) * SCALE_INT16_TO_FLOAT_PLUSMINUS_1; //! @todo the presence of this suggest we're not decoding the //! orientation right. math_quat_normalize(&pose.orientation); struct xrt_quat fixed = { .x = pose.orientation.x, .y = -pose.orientation.z, .z = pose.orientation.y, .w = pose.orientation.w, }; struct xrt_quat adjustment = {.x = 0, .y = 1, .z = 0, .w = 0}; math_quat_rotate(&fixed, &adjustment, &fixed); adjustment = (struct xrt_quat){.x = 0, .y = 0, .z = 1, .w = 0}; math_quat_rotate(&fixed, &adjustment, &fixed); pose.orientation = fixed; struct xrt_space_relation space_relation = {0}; m_filter_euro_vec3_run(&state->motion_vector_filters.position, now, &pose.position, &space_relation.pose.position); m_filter_euro_quat_run(&state->motion_vector_filters.orientation, now, &pose.orientation, &space_relation.pose.orientation); space_relation.relation_flags = (XRT_SPACE_RELATION_ORIENTATION_TRACKED_BIT | XRT_SPACE_RELATION_ORIENTATION_VALID_BIT) | (XRT_SPACE_RELATION_POSITION_TRACKED_BIT | XRT_SPACE_RELATION_POSITION_VALID_BIT); m_relation_history_estimate_motion(state->relation_history, &space_relation, now, &space_relation); m_relation_history_push(state->relation_history, &space_relation, now); state->buttons = hydra_read_uint8(&buf); state->js.x = hydra_read_int16_le(&buf) * SCALE_INT16_TO_FLOAT_PLUSMINUS_1; state->js.y = hydra_read_int16_le(&buf) * SCALE_INT16_TO_FLOAT_PLUSMINUS_1; state->trigger = hydra_read_uint8(&buf) * SCALE_UINT8_TO_FLOAT_0_TO_1; HD_TRACE(hd, "\n\t" "controller: %i\n\t" "position: (%-1.2f, %-1.2f, %-1.2f)\n\t" "orientation: (%-1.2f, %-1.2f, %-1.2f, %-1.2f)\n\t" "buttons: %08x\n\t" "joystick: (%-1.2f, %-1.2f)\n\t" "trigger: %01.2f\n", (int)hd->index, pose.position.x, pose.position.y, pose.position.z, pose.orientation.x, pose.orientation.y, pose.orientation.z, pose.orientation.w, state->buttons, state->js.x, state->js.y, state->trigger); } static int hydra_system_read_data_hid(struct hydra_system *hs) { assert(hs); uint8_t buffer[128]; int ret = os_hid_read(hs->data_hid, buffer, sizeof(buffer), 20); // 20ms is a generous number above the 16.66ms we expect to receive reports at (60hz) timepoint_ns now = os_monotonic_get_ns(); // we dont care if we get no data if (ret <= 0) { return ret; } if (ret != 52) { HS_ERROR(hs, "Unexpected data report of size %d", ret); return -1; } os_mutex_lock(&hs->data_mutex); uint8_t new_counter = buffer[7]; bool missed = false; if (hs->report_counter != -1) { uint8_t expected_counter = ((hs->report_counter + 1) & 0xff); missed = new_counter != expected_counter; } hs->report_counter = new_counter; if (hs->devs[0] != NULL) { hydra_device_parse_controller(hs->devs[0], buffer + 8, now); } if (hs->devs[1] != NULL) { hydra_device_parse_controller(hs->devs[1], buffer + 30, now); } hs->report_time = now; os_mutex_unlock(&hs->data_mutex); HS_TRACE(hs, "\n\t" "missed: %s\n\t" "seq_no: %x\n", missed ? "yes" : "no", new_counter); return ret; } /*! * Switch to motion controller mode. */ static void hydra_system_enter_motion_control(struct hydra_system *hs, timepoint_ns now) { assert(hs); hs->was_in_gamepad_mode = true; hs->motion_attempt_number++; HS_DEBUG(hs, "Setting feature report to start motion-controller mode, " "attempt %d", hs->motion_attempt_number); os_hid_set_feature(hs->command_hid, HYDRA_REPORT_START_MOTION, sizeof(HYDRA_REPORT_START_MOTION)); // Doing a throwaway get-feature now. uint8_t buf[91] = {0}; os_hid_get_feature(hs->command_hid, 0, buf, sizeof(buf)); hydra_sm_transition(&hs->sm, HYDRA_SM_LISTENING_AFTER_SET_FEATURE, now); } /*! * Update the internal state of the Hydra driver. * * Reads devices, checks the state machine and timeouts, etc. * */ static int hydra_system_update(struct hydra_system *hs) { assert(hs); // In all states of the state machine: // Try reading a report: will only return >0 if we get a full motion // report. int received = hydra_system_read_data_hid(hs); // we got an error if (received < 0) { return received; } os_mutex_lock(&hs->data_mutex); timepoint_ns now = os_monotonic_get_ns(); // if we got data, transition to "reporting" mode if (received > 0) { hydra_sm_transition(&hs->sm, HYDRA_SM_REPORTING, now); } switch (hs->sm.current_state) { case HYDRA_SM_LISTENING_AFTER_CONNECT: { float state_duration_s = hydra_sm_seconds_since_transition(&hs->sm, now); if (state_duration_s > 1.0f) { // only waiting 1 second for the initial report after // connect hydra_system_enter_motion_control(hs, now); } } break; case HYDRA_SM_LISTENING_AFTER_SET_FEATURE: { float state_duration_s = hydra_sm_seconds_since_transition(&hs->sm, now); if (state_duration_s > 5.0f) { // giving each motion control attempt 5 seconds to work. hydra_system_enter_motion_control(hs, now); } } break; default: break; } os_mutex_unlock(&hs->data_mutex); return 0; } static void hydra_device_update_input_click(struct hydra_device *hd, timepoint_ns now, int index, uint32_t bit) { assert(hd); hd->base.inputs[index].timestamp = now; hd->base.inputs[index].value.boolean = (hd->state.buttons & bit) != 0; } static void * hydra_usb_thread_run(void *user_data) { struct hydra_system *hs = (struct hydra_system *)user_data; const char *thread_name = "Hydra USB"; U_TRACE_SET_THREAD_NAME(thread_name); os_thread_helper_name(&hs->usb_thread, thread_name); #ifdef XRT_OS_LINUX // Try to raise priority of this thread. u_linux_try_to_set_realtime_priority_on_thread(hs->log_level, thread_name); #endif os_thread_helper_lock(&hs->usb_thread); int result = 0; #if 0 int ticks = 0; #endif while (os_thread_helper_is_running_locked(&hs->usb_thread) && result >= 0) { os_thread_helper_unlock(&hs->usb_thread); result = hydra_system_update(hs); os_thread_helper_lock(&hs->usb_thread); #if 0 ticks += 1; #endif } os_thread_helper_unlock(&hs->usb_thread); return NULL; } /* * * Device functions. * */ static xrt_result_t hydra_device_update_inputs(struct xrt_device *xdev) { struct hydra_device *hd = hydra_device(xdev); struct hydra_system *hs = hydra_system(xdev->tracking_origin); os_mutex_lock(&hs->data_mutex); if (hd->input_time != hs->report_time) { timepoint_ns now = hs->report_time; hd->input_time = now; hydra_device_update_input_click(hd, now, HYDRA_INDEX_1_CLICK, HYDRA_BUTTON_BIT_1); hydra_device_update_input_click(hd, now, HYDRA_INDEX_2_CLICK, HYDRA_BUTTON_BIT_2); hydra_device_update_input_click(hd, now, HYDRA_INDEX_3_CLICK, HYDRA_BUTTON_BIT_3); hydra_device_update_input_click(hd, now, HYDRA_INDEX_4_CLICK, HYDRA_BUTTON_BIT_4); hydra_device_update_input_click(hd, now, HYDRA_INDEX_MIDDLE_CLICK, HYDRA_BUTTON_BIT_MIDDLE); hydra_device_update_input_click(hd, now, HYDRA_INDEX_BUMPER_CLICK, HYDRA_BUTTON_BIT_BUMPER); hydra_device_update_input_click(hd, now, HYDRA_INDEX_JOYSTICK_CLICK, HYDRA_INDEX_JOYSTICK_CLICK); struct xrt_input *inputs = hd->base.inputs; struct hydra_controller_state *state = &(hd->state); inputs[HYDRA_INDEX_JOYSTICK_VALUE].timestamp = now; inputs[HYDRA_INDEX_JOYSTICK_VALUE].value.vec2 = state->js; inputs[HYDRA_INDEX_TRIGGER_VALUE].timestamp = now; inputs[HYDRA_INDEX_TRIGGER_VALUE].value.vec1.x = state->trigger; } os_mutex_unlock(&hs->data_mutex); return XRT_SUCCESS; } static xrt_result_t hydra_device_get_tracked_pose(struct xrt_device *xdev, enum xrt_input_name name, int64_t at_timestamp_ns, struct xrt_space_relation *out_relation) { struct hydra_device *hd = hydra_device(xdev); struct xrt_relation_chain xrc = {0}; switch (name) { case XRT_INPUT_HYDRA_AIM_POSE: { const struct xrt_pose aim_offset = { .position = {0, 0.045, -0.08}, .orientation = {-0.258819, 0, 0, 0.9659258}, }; m_relation_chain_push_pose(&xrc, &aim_offset); break; } case XRT_INPUT_HYDRA_GRIP_POSE: default: break; } struct xrt_space_relation device_relation = {0}; m_relation_history_get(hd->state.relation_history, at_timestamp_ns, &device_relation); m_relation_chain_push_relation(&xrc, &device_relation); m_relation_chain_resolve(&xrc, out_relation); return XRT_SUCCESS; } static void hydra_system_remove_child(struct hydra_system *hs, struct hydra_device *hd) { assert(hydra_system(hd->base.tracking_origin) == hs); assert(hd->index == 0 || hd->index == 1); // Make the device not point to the system hd->sys = NULL; // Make the system not point to the device assert(hs->devs[hd->index] == hd); hs->devs[hd->index] = NULL; // Decrease ref count of system hs->refs--; if (hs->refs == 0) { os_thread_helper_destroy(&hs->usb_thread); os_mutex_destroy(&hs->data_mutex); // No more children, destroy system. if (hs->data_hid != NULL && hs->command_hid != NULL && hs->sm.current_state == HYDRA_SM_REPORTING && hs->was_in_gamepad_mode) { HS_DEBUG(hs, "Sending command to re-enter gamepad mode " "and pausing while it takes effect."); os_hid_set_feature(hs->command_hid, HYDRA_REPORT_START_GAMEPAD, sizeof(HYDRA_REPORT_START_GAMEPAD)); os_nanosleep(2 * 1000 * 1000 * 1000); } if (hs->data_hid != NULL) { os_hid_destroy(hs->data_hid); hs->data_hid = NULL; } if (hs->command_hid != NULL) { os_hid_destroy(hs->command_hid); hs->command_hid = NULL; } free(hs); } } static void hydra_device_destroy(struct xrt_device *xdev) { struct hydra_device *hd = hydra_device(xdev); struct hydra_system *hs = hydra_system(xdev->tracking_origin); m_relation_history_destroy(&hd->state.relation_history); hydra_system_remove_child(hs, hd); free(hd); } /* * * Prober functions. * */ #define SET_INPUT(NAME) \ do { \ (hd->base.inputs[HYDRA_INDEX_##NAME].name = XRT_INPUT_HYDRA_##NAME); \ } while (0) static struct xrt_binding_input_pair touch_inputs[19] = { {XRT_INPUT_TOUCH_X_CLICK, XRT_INPUT_HYDRA_2_CLICK}, {XRT_INPUT_TOUCH_X_TOUCH, XRT_INPUT_HYDRA_2_CLICK}, {XRT_INPUT_TOUCH_Y_CLICK, XRT_INPUT_HYDRA_4_CLICK}, {XRT_INPUT_TOUCH_Y_TOUCH, XRT_INPUT_HYDRA_4_CLICK}, {XRT_INPUT_TOUCH_MENU_CLICK, XRT_INPUT_HYDRA_MIDDLE_CLICK}, {XRT_INPUT_TOUCH_A_CLICK, XRT_INPUT_HYDRA_1_CLICK}, {XRT_INPUT_TOUCH_A_TOUCH, XRT_INPUT_HYDRA_1_CLICK}, {XRT_INPUT_TOUCH_B_CLICK, XRT_INPUT_HYDRA_3_CLICK}, {XRT_INPUT_TOUCH_B_TOUCH, XRT_INPUT_HYDRA_3_CLICK}, {XRT_INPUT_TOUCH_SYSTEM_CLICK, XRT_INPUT_HYDRA_MIDDLE_CLICK}, {XRT_INPUT_TOUCH_SQUEEZE_VALUE, XRT_INPUT_HYDRA_BUMPER_CLICK}, {XRT_INPUT_TOUCH_TRIGGER_TOUCH, XRT_INPUT_HYDRA_TRIGGER_VALUE}, {XRT_INPUT_TOUCH_TRIGGER_VALUE, XRT_INPUT_HYDRA_TRIGGER_VALUE}, {XRT_INPUT_TOUCH_THUMBSTICK_CLICK, XRT_INPUT_HYDRA_JOYSTICK_CLICK}, {XRT_INPUT_TOUCH_THUMBSTICK, XRT_INPUT_HYDRA_JOYSTICK_VALUE}, {XRT_INPUT_TOUCH_GRIP_POSE, XRT_INPUT_HYDRA_GRIP_POSE}, {XRT_INPUT_TOUCH_AIM_POSE, XRT_INPUT_HYDRA_AIM_POSE}, }; static struct xrt_binding_input_pair simple_inputs[4] = { {XRT_INPUT_SIMPLE_SELECT_CLICK, XRT_INPUT_HYDRA_TRIGGER_VALUE}, {XRT_INPUT_SIMPLE_MENU_CLICK, XRT_INPUT_HYDRA_MIDDLE_CLICK}, {XRT_INPUT_SIMPLE_GRIP_POSE, XRT_INPUT_HYDRA_GRIP_POSE}, {XRT_INPUT_SIMPLE_AIM_POSE, XRT_INPUT_HYDRA_AIM_POSE}, }; static struct xrt_binding_profile binding_profiles[2] = { { .name = XRT_DEVICE_TOUCH_CONTROLLER, .inputs = touch_inputs, .input_count = ARRAY_SIZE(touch_inputs), }, { .name = XRT_DEVICE_SIMPLE_CONTROLLER, .inputs = simple_inputs, .input_count = ARRAY_SIZE(simple_inputs), }, }; int hydra_found(struct xrt_prober *xp, struct xrt_prober_device **devices, size_t device_count, size_t index, cJSON *attached_data, struct xrt_device **out_xdevs) { struct xrt_prober_device *dev = devices[index]; int ret; struct os_hid_device *data_hid = NULL; ret = xrt_prober_open_hid_interface(xp, dev, 0, &data_hid); if (ret != 0) { return -1; } struct os_hid_device *command_hid = NULL; ret = xrt_prober_open_hid_interface(xp, dev, 1, &command_hid); if (ret != 0) { os_hid_destroy(data_hid); return -1; } // Create the system struct hydra_system *hs = U_TYPED_CALLOC(struct hydra_system); hs->base.type = XRT_TRACKING_TYPE_MAGNETIC; snprintf(hs->base.name, XRT_TRACKING_NAME_LEN, "%s", "Sixense Magnetic Tracking"); // Arbitrary static transform from local space to base. hs->base.initial_offset.position.y = 1.0f; hs->base.initial_offset.position.z = -0.25f; hs->base.initial_offset.orientation.w = 1.0f; ret = os_thread_helper_init(&hs->usb_thread); if (ret < 0) { HS_ERROR(hs, "Failed to init USB thread."); after_system_err: free(hs); os_hid_destroy(command_hid); os_hid_destroy(data_hid); return -1; } ret = os_mutex_init(&hs->data_mutex); if (ret < 0) { HS_ERROR(hs, "Failed to init data mutex."); os_thread_helper_destroy(&hs->usb_thread); goto after_system_err; } hs->data_hid = data_hid; hs->command_hid = command_hid; enum u_device_alloc_flags flags = (enum u_device_alloc_flags)0; hs->devs[0] = U_DEVICE_ALLOCATE(struct hydra_device, flags, HYDRA_MAX_CONTROLLER_INDEX, 0); hs->devs[1] = U_DEVICE_ALLOCATE(struct hydra_device, flags, HYDRA_MAX_CONTROLLER_INDEX, 0); hs->report_counter = -1; hs->refs = 2; hs->log_level = debug_get_log_option_hydra_log(); u_var_add_root(hs, "Razer Hydra System", false); u_var_add_log_level(hs, &hs->log_level, "Log Level"); u_var_add_bool(hs, &hs->was_in_gamepad_mode, "Was In Gamepad Mode"); u_var_add_i32(hs, &hs->motion_attempt_number, "Motion Attempt Number"); u_var_add_ro_i16(hs, &hs->report_counter, "Report Counter"); // Populate the individual devices for (size_t i = 0; i < 2; ++i) { struct hydra_device *hd = hs->devs[i]; hd->base.destroy = hydra_device_destroy; hd->base.update_inputs = hydra_device_update_inputs; hd->base.get_tracked_pose = hydra_device_get_tracked_pose; hd->base.set_output = u_device_ni_set_output; hd->base.name = XRT_DEVICE_HYDRA; snprintf(hd->base.str, XRT_DEVICE_NAME_LEN, "%s %i", "Razer Hydra Controller", (int)(i + 1)); snprintf(hd->base.serial, XRT_DEVICE_NAME_LEN, "%s%i", "RZRHDRC", (int)(i + 1)); SET_INPUT(1_CLICK); SET_INPUT(2_CLICK); SET_INPUT(3_CLICK); SET_INPUT(4_CLICK); SET_INPUT(MIDDLE_CLICK); SET_INPUT(BUMPER_CLICK); SET_INPUT(JOYSTICK_CLICK); SET_INPUT(JOYSTICK_VALUE); SET_INPUT(TRIGGER_VALUE); SET_INPUT(GRIP_POSE); SET_INPUT(AIM_POSE); hd->index = i; hd->sys = hs; const float fc_min = 9.f; const float fc_min_d = 9.f; const float beta = 0.1f; m_filter_euro_vec3_init(&hd->state.motion_vector_filters.position, fc_min, fc_min_d, beta); m_filter_euro_quat_init(&hd->state.motion_vector_filters.orientation, fc_min, fc_min_d, beta); m_relation_history_create(&hd->state.relation_history); hd->base.binding_profiles = binding_profiles; hd->base.binding_profile_count = ARRAY_SIZE(binding_profiles); hd->base.tracking_origin = &hs->base; hd->base.supported.position_tracking = true; hd->base.supported.orientation_tracking = true; hd->base.device_type = XRT_DEVICE_TYPE_ANY_HAND_CONTROLLER; out_xdevs[i] = &(hd->base); } ret = os_thread_helper_start(&hs->usb_thread, hydra_usb_thread_run, hs); if (ret < 0) { HS_ERROR(hs, "Failed to start USB thread."); // doing this will destroy the system as well xrt_device_destroy((struct xrt_device **)&hs->devs[0]); xrt_device_destroy((struct xrt_device **)&hs->devs[1]); return ret; } HS_INFO(hs, "Opened Razer Hydra!"); return 2; }