// Copyright 2020, Collabora, Ltd. // Copyright 2016 Philipp Zabel // SPDX-License-Identifier: BSL-1.0 /*! * @file * @brief Vive Controller prober and driver code * @author Christoph Haag * @author Lubosz Sarnecki * @author Ryan Pavlik * @author Jakob Bornecrantz * * Portions based on the VRPN Razer Hydra driver, * originally written by Ryan Pavlik and available under the BSL-1.0. */ #include #include #include #include #include "xrt/xrt_prober.h" #include "math/m_api.h" #include "util/u_debug.h" #include "util/u_device.h" #include "util/u_json.h" #include "util/u_misc.h" #include "util/u_time.h" #include "os/os_hid.h" #include "os/os_threading.h" #include "os/os_time.h" #include "vive.h" #include "vive_protocol.h" #include "vive_controller.h" #include "vive_config.h" #ifdef XRT_OS_LINUX #include #include #endif /* * * Defines & structs. * */ DEBUG_GET_ONCE_LOG_OPTION(vive_log, "VIVE_LOG", U_LOGGING_WARN) enum vive_controller_input_index { // common inputs VIVE_CONTROLLER_INDEX_AIM_POSE = 0, VIVE_CONTROLLER_INDEX_GRIP_POSE, VIVE_CONTROLLER_INDEX_SYSTEM_CLICK, VIVE_CONTROLLER_INDEX_TRIGGER_CLICK, VIVE_CONTROLLER_INDEX_TRIGGER_VALUE, VIVE_CONTROLLER_INDEX_TRACKPAD, VIVE_CONTROLLER_INDEX_TRACKPAD_TOUCH, // Vive Wand specific inputs VIVE_CONTROLLER_INDEX_SQUEEZE_CLICK, VIVE_CONTROLLER_INDEX_MENU_CLICK, VIVE_CONTROLLER_INDEX_TRACKPAD_CLICK, // Valve Index specific inputs VIVE_CONTROLLER_INDEX_THUMBSTICK, VIVE_CONTROLLER_INDEX_A_CLICK, VIVE_CONTROLLER_INDEX_B_CLICK, VIVE_CONTROLLER_INDEX_THUMBSTICK_CLICK, VIVE_CONTROLLER_INDEX_THUMBSTICK_TOUCH, VIVE_CONTROLLER_INDEX_SYSTEM_TOUCH, VIVE_CONTROLLER_INDEX_A_TOUCH, VIVE_CONTROLLER_INDEX_B_TOUCH, VIVE_CONTROLLER_INDEX_SQUEEZE_VALUE, VIVE_CONTROLLER_INDEX_SQUEEZE_FORCE, VIVE_CONTROLLER_INDEX_TRIGGER_TOUCH, VIVE_CONTROLLER_INDEX_TRACKPAD_FORCE, VIVE_CONTROLLER_MAX_INDEX, }; #define VIVE_CLOCK_FREQ 48000000.0f // Hz = 48 MHz #define DEFAULT_HAPTIC_FREQ 150.0f #define MIN_HAPTIC_DURATION 0.05f static inline struct vive_controller_device * vive_controller_device(struct xrt_device *xdev) { assert(xdev); struct vive_controller_device *ret = (struct vive_controller_device *)xdev; return ret; } static void vive_controller_device_destroy(struct xrt_device *xdev) { struct vive_controller_device *d = vive_controller_device(xdev); os_thread_helper_destroy(&d->controller_thread); m_imu_3dof_close(&d->fusion); if (d->controller_hid) os_hid_destroy(d->controller_hid); free(d); } static void vive_controller_device_update_wand_inputs(struct xrt_device *xdev) { struct vive_controller_device *d = vive_controller_device(xdev); os_thread_helper_lock(&d->controller_thread); uint8_t buttons = d->state.buttons; /* int i = 8; while(i--) { putchar('0' + ((buttons >> i) & 1)); } printf("\n"); */ uint64_t now = os_monotonic_get_ns(); /* d->state.buttons is bitmask of currently pressed buttons. * (index n) nth bit in the bitmask -> input "name" */ const int button_index_map[] = {VIVE_CONTROLLER_INDEX_TRIGGER_CLICK, VIVE_CONTROLLER_INDEX_TRACKPAD_TOUCH, VIVE_CONTROLLER_INDEX_TRACKPAD_CLICK, VIVE_CONTROLLER_INDEX_SYSTEM_CLICK, VIVE_CONTROLLER_INDEX_SQUEEZE_CLICK, VIVE_CONTROLLER_INDEX_MENU_CLICK}; int button_count = ARRAY_SIZE(button_index_map); for (int i = 0; i < button_count; i++) { bool pressed = (buttons >> i) & 1; bool last_pressed = (d->state.last_buttons >> i) & 1; if (pressed != last_pressed) { struct xrt_input *input = &d->base.inputs[button_index_map[i]]; input->timestamp = now; input->value.boolean = pressed; VIVE_DEBUG(d, "button %d %s\n", i, pressed ? "pressed" : "released"); } } d->state.last_buttons = d->state.buttons; struct xrt_input *trackpad_input = &d->base.inputs[VIVE_CONTROLLER_INDEX_TRACKPAD]; trackpad_input->timestamp = now; trackpad_input->value.vec2.x = d->state.trackpad.x; trackpad_input->value.vec2.y = d->state.trackpad.y; VIVE_TRACE(d, "Trackpad: %f, %f", d->state.trackpad.x, d->state.trackpad.y); struct xrt_input *trigger_input = &d->base.inputs[VIVE_CONTROLLER_INDEX_TRIGGER_VALUE]; trigger_input->timestamp = now; trigger_input->value.vec1.x = d->state.trigger; VIVE_TRACE(d, "Trigger: %f", d->state.trigger); os_thread_helper_unlock(&d->controller_thread); } static void vive_controller_device_update_index_inputs(struct xrt_device *xdev) { struct vive_controller_device *d = vive_controller_device(xdev); os_thread_helper_lock(&d->controller_thread); uint8_t buttons = d->state.buttons; /* int i = 8; while(i--) { putchar('0' + ((buttons >> i) & 1)); } printf("\n"); */ bool was_trackpad_touched = d->base.inputs[VIVE_CONTROLLER_INDEX_TRACKPAD_TOUCH].value.boolean; uint64_t now = os_monotonic_get_ns(); /* d->state.buttons is bitmask of currently pressed buttons. * (index n) nth bit in the bitmask -> input "name" */ const int button_index_map[] = {VIVE_CONTROLLER_INDEX_TRIGGER_CLICK, VIVE_CONTROLLER_INDEX_TRACKPAD_TOUCH, VIVE_CONTROLLER_INDEX_THUMBSTICK_CLICK, VIVE_CONTROLLER_INDEX_SYSTEM_CLICK, VIVE_CONTROLLER_INDEX_A_CLICK, VIVE_CONTROLLER_INDEX_B_CLICK}; int button_count = ARRAY_SIZE(button_index_map); for (int i = 0; i < button_count; i++) { bool pressed = (buttons >> i) & 1; bool last_pressed = (d->state.last_buttons >> i) & 1; if (pressed != last_pressed) { struct xrt_input *input = &d->base.inputs[button_index_map[i]]; input->timestamp = now; input->value.boolean = pressed; VIVE_DEBUG(d, "button %d %s\n", i, pressed ? "pressed" : "released"); } } d->state.last_buttons = d->state.buttons; bool is_trackpad_touched = d->base.inputs[VIVE_CONTROLLER_INDEX_TRACKPAD_TOUCH].value.boolean; /* trackpad and thumbstick position are the same usb events. * report trackpad position when trackpad has been touched last, and * thumbstick position when trackpad touch has been released */ struct xrt_input *thumb_input; // after releasing trackpad, next 0,0 position still goes to trackpad if (is_trackpad_touched || was_trackpad_touched) thumb_input = &d->base.inputs[VIVE_CONTROLLER_INDEX_TRACKPAD]; else thumb_input = &d->base.inputs[VIVE_CONTROLLER_INDEX_THUMBSTICK]; thumb_input->timestamp = now; thumb_input->value.vec2.x = d->state.trackpad.x; thumb_input->value.vec2.y = d->state.trackpad.y; const char *component = is_trackpad_touched || was_trackpad_touched ? "Trackpad" : "Thumbstick"; VIVE_TRACE(d, "%s: %f, %f", component, d->state.trackpad.x, d->state.trackpad.y); struct xrt_input *trigger_input = &d->base.inputs[VIVE_CONTROLLER_INDEX_TRIGGER_VALUE]; trigger_input->timestamp = now; trigger_input->value.vec1.x = d->state.trigger; VIVE_TRACE(d, "Trigger: %f", d->state.trigger); /* d->state.touch is bitmask of currently touched buttons. * (index n) nth bit in the bitmask -> input "name" */ const int touched_button_index_map[] = { 0, 0, 0, VIVE_CONTROLLER_INDEX_SYSTEM_TOUCH, VIVE_CONTROLLER_INDEX_A_TOUCH, VIVE_CONTROLLER_INDEX_B_TOUCH, VIVE_CONTROLLER_INDEX_THUMBSTICK_TOUCH}; int touch_button_count = ARRAY_SIZE(touched_button_index_map); uint8_t touch_buttons = d->state.touch; for (int i = 0; i < touch_button_count; i++) { bool touched = (touch_buttons >> i) & 1; bool last_touched = (d->state.last_touch >> i) & 1; if (touched != last_touched) { struct xrt_input *input = &d->base.inputs[touched_button_index_map[i]]; input->timestamp = now; input->value.boolean = touched; VIVE_DEBUG(d, "button %d %s\n", i, touched ? "touched" : "untouched"); } } d->state.last_touch = d->state.touch; d->base.inputs[VIVE_CONTROLLER_INDEX_SQUEEZE_FORCE].value.vec1.x = (float)d->state.squeeze_force / UINT8_MAX; d->base.inputs[VIVE_CONTROLLER_INDEX_SQUEEZE_FORCE].timestamp = now; if (d->state.squeeze_force > 0) { VIVE_DEBUG(d, "Squeeze force: %f\n", (float)d->state.squeeze_force / UINT8_MAX); } d->base.inputs[VIVE_CONTROLLER_INDEX_TRACKPAD_FORCE].value.vec1.x = (float)d->state.trackpad_force / UINT8_MAX; d->base.inputs[VIVE_CONTROLLER_INDEX_TRACKPAD_FORCE].timestamp = now; if (d->state.trackpad_force > 0) { VIVE_DEBUG(d, "Trackpad force: %f\n", (float)d->state.trackpad_force / UINT8_MAX); } os_thread_helper_unlock(&d->controller_thread); } static void _update_tracker_inputs(struct xrt_device *xdev) { // Nothing to do here as the device does not send button reports. } static void vive_controller_device_get_tracked_pose(struct xrt_device *xdev, enum xrt_input_name name, uint64_t at_timestamp_ns, uint64_t *out_relation_timestamp_ns, struct xrt_space_relation *out_relation) { struct vive_controller_device *d = vive_controller_device(xdev); // printf("input name %d %d\n", name, XRT_INPUT_VIVE_GRIP_POSE); if (name != XRT_INPUT_VIVE_AIM_POSE && name != XRT_INPUT_VIVE_GRIP_POSE && name != XRT_INPUT_INDEX_AIM_POSE && name != XRT_INPUT_INDEX_GRIP_POSE) { VIVE_ERROR(d, "unknown input name"); return; } // Clear out the relation. U_ZERO(out_relation); uint64_t now = os_monotonic_get_ns(); *out_relation_timestamp_ns = now; os_thread_helper_lock(&d->controller_thread); // Don't do anything if we have stopped. if (!os_thread_helper_is_running_locked(&d->controller_thread)) { os_thread_helper_unlock(&d->controller_thread); return; } out_relation->pose.orientation = d->rot_filtered; //! @todo assuming that orientation is actually currently tracked. out_relation->relation_flags = (enum xrt_space_relation_flags)( XRT_SPACE_RELATION_POSITION_VALID_BIT | XRT_SPACE_RELATION_POSITION_TRACKED_BIT | XRT_SPACE_RELATION_ORIENTATION_VALID_BIT | XRT_SPACE_RELATION_ORIENTATION_TRACKED_BIT); os_thread_helper_unlock(&d->controller_thread); struct xrt_vec3 pos = out_relation->pose.position; struct xrt_quat quat = out_relation->pose.orientation; VIVE_TRACE(d, "GET_TRACKED_POSE (%f, %f, %f) (%f, %f, %f, %f) ", pos.x, pos.y, pos.z, quat.x, quat.y, quat.z, quat.w); } static int vive_controller_haptic_pulse(struct vive_controller_device *d, union xrt_output_value *value) { float duration_seconds; //! @todo: proper min duration value if (value->vibration.duration == -1) { VIVE_TRACE(d, "Haptic pulse duration: using %f minimum", MIN_HAPTIC_DURATION); duration_seconds = 0.1; } else { duration_seconds = time_ns_to_s(value->vibration.duration); } VIVE_TRACE(d, "Haptic pulse amp %f, %fHz, %fs", value->vibration.amplitude, value->vibration.frequency, duration_seconds); float frequency = value->vibration.frequency; //! @todo: proper unspecified value if (frequency == 0) { VIVE_TRACE( d, "Haptic pulse frequency unspecified, setting to %fHz", DEFAULT_HAPTIC_FREQ); frequency = 200; } /* haptic pulse for Vive Controller: * desired_frequency = 1000 * 1000 / (high + low). * => (high + low) = 1000 * 1000 / desired_frequency * repeat = desired_duration_in_seconds * desired_frequency. * * I think: * Lowest amplitude: 1, high+low-1 * Highest amplitude: (high+low)/2, / (high+low)/2 */ float high_plus_low = 1000.f * 1000.f / frequency; uint16_t pulse_low = (uint16_t)(value->vibration.amplitude * high_plus_low / 2.); /* Vive Controller doesn't vibrate with value == 0. * Not sure if this actually happens, but let's fix it anyway. */ if (pulse_low == 0) pulse_low = 1; uint16_t pulse_high = high_plus_low - pulse_low; uint16_t repeat_count = duration_seconds * frequency; const struct vive_controller_haptic_pulse_report report = { .id = VIVE_CONTROLLER_COMMAND_REPORT_ID, .command = VIVE_CONTROLLER_HAPTIC_PULSE_COMMAND, .len = 7, .zero = 0x00, .pulse_high = __cpu_to_le16(pulse_high), .pulse_low = __cpu_to_le16(pulse_low), .repeat_count = __cpu_to_le16(repeat_count), }; return os_hid_set_feature(d->controller_hid, (uint8_t *)&report, sizeof(report)); } static void vive_controller_device_set_output(struct xrt_device *xdev, enum xrt_output_name name, union xrt_output_value *value) { struct vive_controller_device *d = vive_controller_device(xdev); if (name != XRT_OUTPUT_NAME_VIVE_HAPTIC && name != XRT_OUTPUT_NAME_INDEX_HAPTIC) { VIVE_ERROR(d, "Unknown output\n"); return; } bool pulse = value->vibration.amplitude > 0.01; if (!pulse) { return; } vive_controller_haptic_pulse(d, value); } static void controller_handle_battery(struct vive_controller_device *d, struct vive_controller_battery_sample *sample) { uint8_t charge_percent = sample->battery & VIVE_CONTROLLER_BATTERY_CHARGE_MASK; bool charging = sample->battery & VIVE_CONTROLLER_BATTERY_CHARGING; VIVE_DEBUG(d, "Charging %d, percent %d\n", charging, charge_percent); d->state.charging = charging; d->state.battery = charge_percent; } static void controller_handle_buttons(struct vive_controller_device *d, struct vive_controller_button_sample *sample) { d->state.buttons = sample->buttons; } static void controller_handle_touch_position(struct vive_controller_device *d, struct vive_controller_touch_sample *sample) { int16_t x = __le16_to_cpu(sample->touch[0]); int16_t y = __le16_to_cpu(sample->touch[1]); d->state.trackpad.x = (float)x / INT16_MAX; d->state.trackpad.y = (float)y / INT16_MAX; if (d->state.trackpad.x != 0 || d->state.trackpad.y != 0) VIVE_TRACE(d, "Trackpad %f,%f\n", d->state.trackpad.x, d->state.trackpad.y); } static void controller_handle_analog_trigger(struct vive_controller_device *d, struct vive_controller_trigger_sample *sample) { d->state.trigger = (float)sample->trigger / UINT8_MAX; VIVE_TRACE(d, "Trigger %f\n", d->state.trigger); } static inline uint32_t calc_dt_raw_and_handle_overflow(struct vive_controller_device *d, uint32_t sample_time) { uint64_t dt_raw = (uint64_t)sample_time - (uint64_t)d->imu.last_sample_time_raw; d->imu.last_sample_time_raw = sample_time; // The 32-bit tick counter has rolled over, // adjust the "negative" value to be positive. // It's easiest to do this with 64-bits. if (dt_raw > 0xFFFFFFFF) { dt_raw += 0x100000000; } return (uint32_t)dt_raw; } static inline uint64_t cald_dt_ns(uint32_t dt_raw) { double f = (double)(dt_raw) / VIVE_CLOCK_FREQ; uint64_t diff_ns = (uint64_t)(f * 1000.0 * 1000.0 * 1000.0); return diff_ns; } static void vive_controller_handle_imu_sample(struct vive_controller_device *d, struct watchman_imu_sample *sample) { /* ouvrt: "Time in 48 MHz ticks, but we are missing the low byte" */ uint32_t time_raw = d->last_ticks | (sample->timestamp_hi << 8); uint32_t dt_raw = calc_dt_raw_and_handle_overflow(d, time_raw); uint64_t dt_ns = cald_dt_ns(dt_raw); int16_t acc[3] = { __le16_to_cpu(sample->acc[0]), __le16_to_cpu(sample->acc[1]), __le16_to_cpu(sample->acc[2]), }; int16_t gyro[3] = { __le16_to_cpu(sample->gyro[0]), __le16_to_cpu(sample->gyro[1]), __le16_to_cpu(sample->gyro[2]), }; float scale = (float)d->imu.acc_range / 32768.0f; struct xrt_vec3 acceleration = { scale * d->imu.acc_scale.x * acc[0] - d->imu.acc_bias.x, scale * d->imu.acc_scale.y * acc[1] - d->imu.acc_bias.y, scale * d->imu.acc_scale.z * acc[2] - d->imu.acc_bias.z, }; scale = (float)d->imu.gyro_range / 32768.0f; struct xrt_vec3 angular_velocity = { scale * d->imu.gyro_scale.x * gyro[0] - d->imu.gyro_bias.x, scale * d->imu.gyro_scale.y * gyro[1] - d->imu.gyro_bias.y, scale * d->imu.gyro_scale.z * gyro[2] - d->imu.gyro_bias.z, }; VIVE_TRACE(d, "ACC %f %f %f", acceleration.x, acceleration.y, acceleration.z); VIVE_TRACE(d, "GYRO %f %f %f", angular_velocity.x, angular_velocity.y, angular_velocity.z); /* */ if (d->variant == CONTROLLER_VIVE_WAND) { struct xrt_vec3 fixed_acceleration = {.x = -acceleration.x, .y = -acceleration.z, .z = -acceleration.y}; acceleration = fixed_acceleration; struct xrt_vec3 fixed_angular_velocity = { .x = -angular_velocity.x, .y = -angular_velocity.z, .z = -angular_velocity.y}; angular_velocity = fixed_angular_velocity; } else if (d->variant == CONTROLLER_INDEX_RIGHT) { struct xrt_vec3 fixed_acceleration = {.x = acceleration.z, .y = -acceleration.y, .z = acceleration.x}; acceleration = fixed_acceleration; struct xrt_vec3 fixed_angular_velocity = { .x = angular_velocity.z, .y = -angular_velocity.y, .z = angular_velocity.x}; angular_velocity = fixed_angular_velocity; } else if (d->variant == CONTROLLER_INDEX_LEFT) { struct xrt_vec3 fixed_acceleration = {.x = -acceleration.z, .y = acceleration.x, .z = -acceleration.y}; acceleration = fixed_acceleration; struct xrt_vec3 fixed_angular_velocity = { .x = -angular_velocity.z, .y = angular_velocity.x, .z = -angular_velocity.y}; angular_velocity = fixed_angular_velocity; } d->imu.time_ns += dt_ns; d->last.acc = acceleration; d->last.gyro = angular_velocity; m_imu_3dof_update(&d->fusion, d->imu.time_ns, &acceleration, &angular_velocity); d->rot_filtered = d->fusion.rot; // VIVE_TRACE(d, "Rot %f %f %f", d->rot_filtered.x, // d->rot_filtered.y, d->rot_filtered.z); } static void controller_handle_touch_force(struct vive_controller_device *d, struct watchman_touch_force *sample) { d->state.touch = sample->touch; d->state.middle_finger_handle = sample->middle_finger_handle; d->state.ring_finger_handle = sample->ring_finger_handle; d->state.pinky_finger_handle = sample->pinky_finger_handle; d->state.index_finger_trigger = sample->index_finger_trigger; d->state.squeeze_force = sample->squeeze_force; d->state.trackpad_force = sample->trackpad_force; } static void vive_controller_handle_lighthousev1(struct vive_controller_device *d, uint8_t *buf, uint8_t len) { VIVE_TRACE(d, "Got lighthouse message with len %d.\n", len); } /* * Handles battery, imu, trigger, buttons, trackpad. * Then hands off to vive_controller_handle_lighthousev1(). */ static void vive_controller_decode_watchmanv1(struct vive_controller_device *d, struct vive_controller_message *message) { uint8_t *buf = message->payload; uint8_t *end = message->payload + message->len - 1; /* for (int i = 0; i < message->len; i++) { //printf("%02x ", buf[i]); int j = 8; while(j--) { putchar('0' + ((buf[i] >> j) & 1)); } putchar(' '); } printf("\n"); */ /* payload starts with "event flags" byte. * If it does not start with 111, it contains only lighthouse data. * If it starts with 111, events follow in this order, each of them * optional: * - battery: 1 byte (1110???1) * - trigger: 1 byte (1111?1??) * - trackpad: 4 byte (1111??1?) * - buttons: 1 byte (1111???1) * - imu: 13 byte (111?1???) * There may be another input event after a battery event. * Lighthouse data may follow in the rest of the payload. */ // input events have first three bits set if ((*buf & 0xe0) == 0xe0 && buf < end) { // clang-format off // battery follows when 1110???1 bool has_battery = (*buf & 0x10) != 0x10 && (*buf & 0x1) == 0x1; // input follows when 1111? bool has_trigger = (*buf & 0x10) == 0x10 && (*buf & 0x4) == 0x4; bool has_trackpad = (*buf & 0x10) == 0x10 && (*buf & 0x2) == 0x2; bool has_buttons = (*buf & 0x10) == 0x10 && (*buf & 0x1) == 0x1; // imu event follows when 111?1??? // there are imu-only messages, and imu-after-battery bool has_imu = (*buf & 0x08) == 0x8; // clang-format on VIVE_TRACE(d, "battery %d trigger %d trackpad %d " "buttons %d imu %d", has_battery, has_trigger, has_trackpad, has_buttons, has_imu); buf++; if (has_battery) { controller_handle_battery( d, (struct vive_controller_battery_sample *)buf); buf += sizeof(struct vive_controller_battery_sample); } if (has_buttons) { controller_handle_buttons( d, (struct vive_controller_button_sample *)buf); buf += sizeof(struct vive_controller_button_sample); } if (has_trigger) { controller_handle_analog_trigger( d, (struct vive_controller_trigger_sample *)buf); buf += sizeof(struct vive_controller_trigger_sample); } if (has_trackpad) { controller_handle_touch_position( d, (struct vive_controller_touch_sample *)buf); buf += 4; } if (has_imu) { vive_controller_handle_imu_sample( d, (struct watchman_imu_sample *)buf); buf += sizeof(struct watchman_imu_sample); } } if (buf > end) VIVE_ERROR(d, "overshoot: %ld\n", buf - end); if (buf < end) vive_controller_handle_lighthousev1(d, buf, end - buf); } //#define WATCHMAN2_PRINT_HID /* * Handles battery, imu, trigger, buttons, trackpad. * Then hands off to vive_controller_handle_lighthousev1(). */ static void vive_controller_decode_watchmanv2(struct vive_controller_device *d, struct vive_controller_message *message) { uint8_t *buf = message->payload; uint8_t *end = message->payload + message->len - 1; #ifdef WATCHMAN2_PRINT_HID for (int i = 0; i < message->len; i++) { int j = 8; while (j--) { putchar('0' + ((buf[i] >> j) & 1)); } putchar(' '); } printf("\n"); for (int i = 0; i < message->len; i++) { printf("%8.02x ", buf[i]); } printf("\n"); #endif /* payload starts with "event flags" byte. */ /* * If flags == 0xe1 == 11100001, battery follows. * Battery is always at the beginning of the payload. * after battery there may be another payload. * careful: 0xe1 often comes alone without actual data. */ if (*buf == 0xe1 && buf < end) { buf++; controller_handle_battery( d, (struct vive_controller_battery_sample *)buf); buf += sizeof(struct vive_controller_battery_sample); #ifdef WATCHMAN2_PRINT_HID printf( " " " battery"); #endif } /* * If flags == 0xf0 == 11110000, 8 bytes touch+force follow. * This package is always at the beginning of the payload. */ if (*buf == 0xf0 && buf < end) { buf++; controller_handle_touch_force( d, (struct watchman_touch_force *)buf); size_t s = sizeof(struct watchman_touch_force); buf += s; #ifdef WATCHMAN2_PRINT_HID printf(" "); for (size_t i = 0; i < s; i++) printf(" t&force"); #endif } /* * If flags == 0xe8 == 11101000, imu data follows. * This package can be at the beginning of the payload or after battery. */ // TODO: it's possible we misparse non-im udata as imu data if (*buf == 0xe8 && buf < end) { buf++; vive_controller_handle_imu_sample( d, (struct watchman_imu_sample *)buf); size_t s = sizeof(struct watchman_imu_sample); buf += s; #ifdef WATCHMAN2_PRINT_HID printf(" "); for (size_t i = 0; i < s; i++) printf(" imu"); #endif } /* * If flags starts with 1111, events follow in this order, * each of them optional: * - trigger: 1 byte (1111?1??) * - trackpad: 4 byte (1111??1?) * - buttons: 1 byte (1111???1) * - touch&force+imu or imu: 8+13 or 13 byte (11111???) * There may be another input event after a battery event. */ if ((*buf & 0xf0) == 0xf0 && buf < end - 1) { // clang-format off // input flags 1111 bool has_touch_force = (*buf & 0x8) == 0x8; bool has_trigger = (*buf & 0x4) == 0x4; bool has_trackpad = (*buf & 0x2) == 0x2; bool has_buttons = (*buf & 0x1) == 0x1; // clang-format on buf++; #ifdef WATCHMAN2_PRINT_HID printf(" "); #endif if (has_buttons) { controller_handle_buttons( d, (struct vive_controller_button_sample *)buf); buf += sizeof(struct vive_controller_button_sample); #ifdef WATCHMAN2_PRINT_HID printf(" buttons"); #endif } if (has_trigger) { controller_handle_analog_trigger( d, (struct vive_controller_trigger_sample *)buf); buf += sizeof(struct vive_controller_trigger_sample); #ifdef WATCHMAN2_PRINT_HID printf(" trigger"); #endif } if (has_trackpad) { controller_handle_touch_position( d, (struct vive_controller_touch_sample *)buf); buf += sizeof(struct vive_controller_touch_sample); #ifdef WATCHMAN2_PRINT_HID for (unsigned long i = 0; i < sizeof(struct vive_controller_touch_sample); i++) printf(" trackpad"); #endif } if (has_touch_force) { uint8_t type_flag = *buf; if (type_flag == TYPE_FLAG_TOUCH_FORCE) { controller_handle_touch_force( d, (struct watchman_touch_force *)buf); size_t s = sizeof(struct watchman_touch_force); buf += s; #ifdef WATCHMAN2_PRINT_HID for (unsigned long i = 0; i < sizeof(struct watchman_touch_force); i++) printf(" t&force"); #endif } } // if something still follows, usually imu // sometimes it's 5 unknown bytes' if (buf < end && end - buf >= (long)sizeof(struct watchman_imu_sample)) { vive_controller_handle_imu_sample( d, (struct watchman_imu_sample *)buf); size_t s = sizeof(struct watchman_imu_sample); buf += s; #ifdef WATCHMAN2_PRINT_HID for (unsigned long i = 0; i < sizeof(struct watchman_imu_sample); i++) printf(" imu"); #endif } } #ifdef WATCHMAN2_PRINT_HID printf("\n"); #endif if (buf < end) { VIVE_TRACE(d, "%ld bytes unparsed data in message\n", message->len - (buf - message->payload) - 1); } if (buf > end) VIVE_ERROR(d, "overshoot: %ld\n", buf - end); //! @todo: Parse lighthouse v2 data } /* * Decodes multiplexed Wireless Receiver messages. */ static void vive_controller_decode_message(struct vive_controller_device *d, struct vive_controller_message *message) { d->last_ticks = (message->timestamp_hi << 24) | (message->timestamp_lo << 16); //! @todo: Check if Vive controller on watchman2 is correctly handled //! with watchman2 codepath switch (d->watchman_gen) { case WATCHMAN_GEN1: vive_controller_decode_watchmanv1(d, message); break; case WATCHMAN_GEN2: vive_controller_decode_watchmanv2(d, message); break; default: VIVE_ERROR(d, "Can't decode unknown watchman gen"); } } #define FEATURE_BUFFER_SIZE 256 static int vive_controller_device_update(struct vive_controller_device *d) { uint8_t buf[FEATURE_BUFFER_SIZE]; int ret = os_hid_read(d->controller_hid, buf, sizeof(buf), 1000); if (ret == 0) { // controller off return true; } if (ret < 0) { VIVE_ERROR(d, "Failed to read device '%i'!", ret); return false; } switch (buf[0]) { case VIVE_CONTROLLER_REPORT1_ID: vive_controller_decode_message( d, &((struct vive_controller_report1 *)buf)->message); break; case VIVE_CONTROLLER_REPORT2_ID: vive_controller_decode_message( d, &((struct vive_controller_report2 *)buf)->message[0]); vive_controller_decode_message( d, &((struct vive_controller_report2 *)buf)->message[1]); break; case VIVE_CONTROLLER_DISCONNECT_REPORT_ID: VIVE_DEBUG(d, "Controller disconnected."); break; default: VIVE_ERROR(d, "Unknown controller message type: %u", buf[0]); } return true; } static void * vive_controller_run_thread(void *ptr) { struct vive_controller_device *d = (struct vive_controller_device *)ptr; uint8_t buf[FEATURE_BUFFER_SIZE]; while (os_hid_read(d->controller_hid, buf, sizeof(buf), 0) > 0) { // Empty queue first } os_thread_helper_lock(&d->controller_thread); while (os_thread_helper_is_running_locked(&d->controller_thread)) { os_thread_helper_unlock(&d->controller_thread); if (!vive_controller_device_update(d)) { return NULL; } // Just keep swimming. os_thread_helper_lock(&d->controller_thread); } return NULL; } #define SET_WAND_INPUT(NAME, NAME2) \ do { \ (d->base.inputs[VIVE_CONTROLLER_INDEX_##NAME].name = \ XRT_INPUT_VIVE_##NAME2); \ } while (0) #define SET_INDEX_INPUT(NAME, NAME2) \ do { \ (d->base.inputs[VIVE_CONTROLLER_INDEX_##NAME].name = \ XRT_INPUT_INDEX_##NAME2); \ } while (0) struct vive_controller_device * vive_controller_create(struct os_hid_device *controller_hid, enum watchman_gen watchman_gen, int controller_num) { enum u_device_alloc_flags flags = U_DEVICE_ALLOC_TRACKING_NONE; struct vive_controller_device *d = U_DEVICE_ALLOCATE( struct vive_controller_device, flags, VIVE_CONTROLLER_MAX_INDEX, 1); d->ll = debug_get_log_option_vive_log(); d->watchman_gen = WATCHMAN_GEN_UNKNOWN; d->variant = CONTROLLER_UNKNOWN; d->watchman_gen = watchman_gen; m_imu_3dof_init(&d->fusion, M_IMU_3DOF_USE_GRAVITY_DUR_20MS); /* default values, will be queried from device */ d->imu.gyro_range = 8.726646f; d->imu.acc_range = 39.226600f; d->imu.acc_scale.x = 1.0f; d->imu.acc_scale.y = 1.0f; d->imu.acc_scale.z = 1.0f; d->imu.gyro_scale.x = 1.0f; d->imu.gyro_scale.y = 1.0f; d->imu.gyro_scale.z = 1.0f; d->imu.acc_bias.x = 0.0f; d->imu.acc_bias.y = 0.0f; d->imu.acc_bias.z = 0.0f; d->imu.gyro_bias.x = 0.0f; d->imu.gyro_bias.y = 0.0f; d->imu.gyro_bias.z = 0.0f; d->controller_hid = controller_hid; d->base.destroy = vive_controller_device_destroy; d->base.get_tracked_pose = vive_controller_device_get_tracked_pose; d->base.set_output = vive_controller_device_set_output; snprintf(d->base.str, XRT_DEVICE_NAME_LEN, "%s %i", "Vive Controller", (int)(controller_num)); d->index = controller_num; //! @todo: reading range report fails for powered off controller if (vive_get_imu_range_report(d->controller_hid, &d->imu.gyro_range, &d->imu.acc_range) != 0) { VIVE_ERROR(d, "Could not get watchman IMU range packet!"); free(d); return 0; } VIVE_DEBUG(d, "Vive controller gyroscope range %f", d->imu.gyro_range); VIVE_DEBUG(d, "Vive controller accelerometer range %f", d->imu.acc_range); // successful config parsing determines d->variant char *config = vive_read_config(d->controller_hid); if (config != NULL) { vive_config_parse_controller(d, config); free(config); } else { VIVE_ERROR(d, "Could not get Vive controller config\n"); free(d); return 0; } if (d->variant == CONTROLLER_VIVE_WAND) { d->base.name = XRT_DEVICE_VIVE_WAND; SET_WAND_INPUT(SYSTEM_CLICK, SYSTEM_CLICK); SET_WAND_INPUT(SQUEEZE_CLICK, SQUEEZE_CLICK); SET_WAND_INPUT(MENU_CLICK, MENU_CLICK); SET_WAND_INPUT(TRIGGER_CLICK, TRIGGER_CLICK); SET_WAND_INPUT(TRIGGER_VALUE, TRIGGER_VALUE); SET_WAND_INPUT(TRACKPAD, TRACKPAD); SET_WAND_INPUT(TRACKPAD_CLICK, TRACKPAD_CLICK); SET_WAND_INPUT(TRACKPAD_TOUCH, TRACKPAD_TOUCH); SET_WAND_INPUT(AIM_POSE, AIM_POSE); SET_WAND_INPUT(GRIP_POSE, GRIP_POSE); d->base.outputs[0].name = XRT_OUTPUT_NAME_VIVE_HAPTIC; d->base.update_inputs = vive_controller_device_update_wand_inputs; } else if (d->variant == CONTROLLER_INDEX_LEFT || d->variant == CONTROLLER_INDEX_RIGHT) { d->base.name = XRT_DEVICE_INDEX_CONTROLLER; SET_INDEX_INPUT(SYSTEM_CLICK, SYSTEM_CLICK); SET_INDEX_INPUT(A_CLICK, A_CLICK); SET_INDEX_INPUT(B_CLICK, B_CLICK); SET_INDEX_INPUT(TRIGGER_CLICK, TRIGGER_CLICK); SET_INDEX_INPUT(TRIGGER_VALUE, TRIGGER_VALUE); SET_INDEX_INPUT(TRACKPAD, TRACKPAD); SET_INDEX_INPUT(TRACKPAD_TOUCH, TRACKPAD_TOUCH); SET_INDEX_INPUT(THUMBSTICK, THUMBSTICK); SET_INDEX_INPUT(THUMBSTICK_CLICK, THUMBSTICK_CLICK); SET_INDEX_INPUT(THUMBSTICK_TOUCH, THUMBSTICK_TOUCH); SET_INDEX_INPUT(SYSTEM_TOUCH, SYSTEM_TOUCH); SET_INDEX_INPUT(A_TOUCH, A_TOUCH); SET_INDEX_INPUT(B_TOUCH, B_TOUCH); SET_INDEX_INPUT(SQUEEZE_VALUE, SQUEEZE_VALUE); SET_INDEX_INPUT(SQUEEZE_FORCE, SQUEEZE_FORCE); SET_INDEX_INPUT(TRIGGER_TOUCH, TRIGGER_TOUCH); SET_INDEX_INPUT(TRACKPAD_FORCE, TRACKPAD_FORCE); SET_INDEX_INPUT(AIM_POSE, AIM_POSE); SET_INDEX_INPUT(GRIP_POSE, GRIP_POSE); d->base.outputs[0].name = XRT_OUTPUT_NAME_INDEX_HAPTIC; d->base.update_inputs = vive_controller_device_update_index_inputs; } else if (d->variant == CONTROLLER_TRACKER_GEN1) { d->base.name = XRT_DEVICE_VIVE_TRACKER_GEN1; d->base.update_inputs = _update_tracker_inputs; } else if (d->variant == CONTROLLER_TRACKER_GEN2) { d->base.name = XRT_DEVICE_VIVE_TRACKER_GEN2; d->base.update_inputs = _update_tracker_inputs; } else { d->base.name = XRT_DEVICE_GENERIC_HMD; VIVE_ERROR(d, "Failed to assign update input function"); } if (d->controller_hid) { int ret = os_thread_helper_start(&d->controller_thread, vive_controller_run_thread, d); if (ret != 0) { VIVE_ERROR(d, "Failed to start mainboard thread!"); vive_controller_device_destroy((struct xrt_device *)d); return 0; } } VIVE_DEBUG(d, "Opened vive controller!\n"); d->base.orientation_tracking_supported = true; d->base.position_tracking_supported = false; return d; }