From 8efb55403c7513e0df27a8b8d52eb14c3cbb00eb Mon Sep 17 00:00:00 2001 From: Jakob Bornecrantz Date: Sun, 6 Nov 2022 03:15:47 +0000 Subject: [PATCH] d/psvr: Switch to reading sensors from its own thread Tested-by: number-g --- src/xrt/drivers/psvr/psvr_device.c | 141 +++++++++++++++++++++-------- 1 file changed, 103 insertions(+), 38 deletions(-) diff --git a/src/xrt/drivers/psvr/psvr_device.c b/src/xrt/drivers/psvr/psvr_device.c index 7d1b76378..9d10affe6 100644 --- a/src/xrt/drivers/psvr/psvr_device.c +++ b/src/xrt/drivers/psvr/psvr_device.c @@ -1,5 +1,5 @@ // Copyright 2016, Joey Ferwerda. -// Copyright 2019-2021, Collabora, Ltd. +// Copyright 2019-2022, Collabora, Ltd. // SPDX-License-Identifier: BSL-1.0 /*! * @file @@ -23,6 +23,7 @@ #include "util/u_time.h" #include "util/u_debug.h" #include "util/u_device.h" +#include "util/u_trace_marker.h" #include "util/u_distortion_mesh.h" #include "math/m_imu_3dof.h" @@ -58,12 +59,19 @@ struct psvr_device { struct xrt_device base; + //! Owned by the @ref oth thread. hid_device *hid_sensor; + + //! Owned and protected by the device_mutex. hid_device *hid_control; struct os_mutex device_mutex; + //! Used to read sensor packets. + struct os_thread_helper oth; + struct xrt_tracked_psvr *tracker; + //! Only touched from the sensor thread. timepoint_ns last_sensor_time; struct psvr_parsed_sensor last; @@ -230,19 +238,19 @@ send_request_data(struct psvr_device *psvr, uint8_t id, uint8_t num) return send_to_control(psvr, data, sizeof(data)); } - -/* - * - * Packet reading code. - * - */ - static uint8_t scale_led_power(uint8_t power) { return (uint8_t)((power / 255.0f) * 100.0f); } + +/* + * + * Sensor functions. + * + */ + static void read_sample_and_apply_calibration(struct psvr_device *psvr, struct psvr_parsed_sample *sample, @@ -296,7 +304,7 @@ read_sample_and_apply_calibration(struct psvr_device *psvr, } static void -update_fusion(struct psvr_device *psvr, struct psvr_parsed_sample *sample, uint64_t timestamp_ns) +update_fusion_locked(struct psvr_device *psvr, struct psvr_parsed_sample *sample, uint64_t timestamp_ns) { struct xrt_vec3 mag = {0.0f, 0.0f, 0.0f}; (void)mag; @@ -314,6 +322,14 @@ update_fusion(struct psvr_device *psvr, struct psvr_parsed_sample *sample, uint6 } } +static void +update_fusion(struct psvr_device *psvr, struct psvr_parsed_sample *sample, uint64_t timestamp_ns) +{ + os_mutex_lock(&psvr->device_mutex); + update_fusion_locked(psvr, sample, timestamp_ns); + os_mutex_unlock(&psvr->device_mutex); +} + static uint32_t calc_delta_and_handle_rollover(uint32_t next, uint32_t last) { @@ -400,6 +416,65 @@ handle_tracker_sensor_msg(struct psvr_device *psvr, unsigned char *buffer, int s update_fusion(psvr, &s->samples[1], timestamp_ns); } +static void +sensor_clear_queue(struct psvr_device *psvr) +{ + uint8_t buffer[FEATURE_BUFFER_SIZE]; + + while (hid_read(psvr->hid_sensor, buffer, FEATURE_BUFFER_SIZE) > 0) { + // Just drop the packets. + } +} + +static int +sensor_read_one_packet(struct psvr_device *psvr) +{ + uint8_t buffer[FEATURE_BUFFER_SIZE]; + + // Try for one second to get packates. + int size = hid_read_timeout(psvr->hid_sensor, buffer, FEATURE_BUFFER_SIZE, 1000); + if (size <= 0) { + return size; + } + + handle_tracker_sensor_msg(psvr, buffer, size); + + return 0; +} + +static void * +sensor_thread(void *ptr) +{ + U_TRACE_SET_THREAD_NAME("PS VR"); + + struct psvr_device *psvr = (struct psvr_device *)ptr; + int ret = 0; + + // Empty the queue. + sensor_clear_queue(psvr); + + os_thread_helper_lock(&psvr->oth); + + while (os_thread_helper_is_running_locked(&psvr->oth) && ret >= 0) { + os_thread_helper_unlock(&psvr->oth); + + ret = sensor_read_one_packet(psvr); + + os_thread_helper_lock(&psvr->oth); + } + + os_thread_helper_unlock(&psvr->oth); + + return NULL; +} + + +/* + * + * Control device handling. + * + */ + static void handle_control_status_msg(struct psvr_device *psvr, unsigned char *buffer, int size) { @@ -506,25 +581,6 @@ handle_control_0xA0(struct psvr_device *psvr, unsigned char *buffer, int size) PSVR_DEBUG(psvr, "%02x %02x %02x %02x", buffer[0], buffer[1], buffer[2], buffer[3]); } -static int -read_sensor_packets(struct psvr_device *psvr) -{ - uint8_t buffer[FEATURE_BUFFER_SIZE]; - int size = 0; - - do { - size = hid_read(psvr->hid_sensor, buffer, FEATURE_BUFFER_SIZE); - if (size == 0) { - return 0; - } - if (size < 0) { - return -1; - } - - handle_tracker_sensor_msg(psvr, buffer, size); - } while (true); -} - static int read_control_packets(struct psvr_device *psvr) { @@ -647,7 +703,6 @@ static int wait_for_power(struct psvr_device *psvr, bool on) { for (int i = 0; i < 5000; i++) { - read_sensor_packets(psvr); read_control_packets(psvr); if (psvr->powered_on == on) { @@ -664,7 +719,6 @@ static int wait_for_vr_mode(struct psvr_device *psvr, bool on) { for (int i = 0; i < 5000; i++) { - read_sensor_packets(psvr); read_control_packets(psvr); if (psvr->in_vr_mode == on) { @@ -690,6 +744,7 @@ control_power_and_wait(struct psvr_device *psvr, bool on) PSVR_ERROR(psvr, "Failed to switch %s the headset! '%i'", status, ret); } + ret = wait_for_power(psvr, on); if (ret < 0) { PSVR_ERROR(psvr, "Failed to wait for headset power %s! '%i'", status, ret); @@ -828,11 +883,6 @@ disco_leds(struct psvr_device *psvr) // Sleep for a tenth of a second while polling for packages. for (int k = 0; k < 100; k++) { - ret = read_sensor_packets(psvr); - if (ret < 0) { - return ret; - } - ret = read_control_packets(psvr); if (ret < 0) { return ret; @@ -845,12 +895,22 @@ disco_leds(struct psvr_device *psvr) return 0; } + +/* + * + * Misc functions. + * + */ + static void teardown(struct psvr_device *psvr) { // Stop the variable tracking. u_var_remove_root(psvr); + // Shutdown the sensor thread early. + os_thread_helper_stop_and_wait(&psvr->oth); + // Includes null check, and sets to null. xrt_tracked_psvr_destroy(&psvr->tracker); @@ -872,6 +932,7 @@ teardown(struct psvr_device *psvr) // Destroy the fusion. m_imu_3dof_close(&psvr->fusion); + os_thread_helper_destroy(&psvr->oth); os_mutex_destroy(&psvr->device_mutex); } @@ -889,7 +950,6 @@ psvr_device_update_inputs(struct xrt_device *xdev) os_mutex_lock(&psvr->device_mutex); - read_sensor_packets(psvr); update_leds_if_changed(psvr); os_mutex_unlock(&psvr->device_mutex); @@ -911,7 +971,6 @@ psvr_device_get_tracked_pose(struct xrt_device *xdev, os_mutex_lock(&psvr->device_mutex); // Read all packets. - read_sensor_packets(psvr); read_control_packets(psvr); // Clear out the relation. @@ -1024,8 +1083,9 @@ psvr_device_create_auto_prober(struct hid_device_info *sensor_hid_info, snprintf(psvr->base.str, XRT_DEVICE_NAME_LEN, "PS VR Headset"); snprintf(psvr->base.serial, XRT_DEVICE_NAME_LEN, "PS VR Headset"); - // Do mutex init before any call to teardown happens. + // Do mutex and thread init before any call to teardown happens. os_mutex_init(&psvr->device_mutex); + os_thread_helper_init(&psvr->oth); ret = open_hid(psvr, sensor_hid_info, &psvr->hid_sensor); if (ret != 0) { @@ -1037,6 +1097,11 @@ psvr_device_create_auto_prober(struct hid_device_info *sensor_hid_info, goto cleanup; } + ret = os_thread_helper_start(&psvr->oth, sensor_thread, (void *)psvr); + if (ret < 0) { + goto cleanup; + } + if (control_power_and_wait(psvr, true) < 0 || control_vrmode_and_wait(psvr, true) < 0) { goto cleanup; }