mirrors/monado
1
0
Fork 0
mirror of https://gitlab.freedesktop.org/monado/monado.git synced 2024-12-28 02:26:16 +00:00
Code Issues Actions Packages Projects Releases Wiki Activity

d/psvr: Switch to reading sensors from its own thread

Browse source 
Tested-by: number-g <g@imagination.eu.org>
This commit is contained in:
Jakob Bornecrantz 2022-11-06 03:15:47 +00:00
parent 503979c723
commit 8efb55403c
1 changed files with 103 additions and 38 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

141
src/xrt/drivers/psvr/psvr_device.c
View file

@ -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;
}