The open source OpenXR runtime
d/psvr: Switch to reading sensors from its own thread
Tested-by: number-g <g@imagination.eu.org>
+103
-38
+103
-38
src/xrt/drivers/psvr/psvr_device.c
+103
-38
src/xrt/drivers/psvr/psvr_device.c
···
1
1
// Copyright 2016, Joey Ferwerda.
2
-
// Copyright 2019-2021, Collabora, Ltd.
2
+
// Copyright 2019-2022, Collabora, Ltd.
3
3
// SPDX-License-Identifier: BSL-1.0
4
4
/*!
5
5
* @file
···
23
23
#include "util/u_time.h"
24
24
#include "util/u_debug.h"
25
25
#include "util/u_device.h"
26
+
#include "util/u_trace_marker.h"
26
27
#include "util/u_distortion_mesh.h"
27
28
28
29
#include "math/m_imu_3dof.h"
···
58
59
{
59
60
struct xrt_device base;
60
61
62
+
//! Owned by the @ref oth thread.
61
63
hid_device *hid_sensor;
64
+
65
+
//! Owned and protected by the device_mutex.
62
66
hid_device *hid_control;
63
67
struct os_mutex device_mutex;
64
68
69
+
//! Used to read sensor packets.
70
+
struct os_thread_helper oth;
71
+
65
72
struct xrt_tracked_psvr *tracker;
66
73
74
+
//! Only touched from the sensor thread.
67
75
timepoint_ns last_sensor_time;
68
76
69
77
struct psvr_parsed_sensor last;
···
230
238
return send_to_control(psvr, data, sizeof(data));
231
239
}
232
240
233
-
234
-
/*
235
-
*
236
-
* Packet reading code.
237
-
*
238
-
*/
239
-
240
241
static uint8_t
241
242
scale_led_power(uint8_t power)
242
243
{
243
244
return (uint8_t)((power / 255.0f) * 100.0f);
244
245
}
245
246
247
+
248
+
/*
249
+
*
250
+
* Sensor functions.
251
+
*
252
+
*/
253
+
246
254
static void
247
255
read_sample_and_apply_calibration(struct psvr_device *psvr,
248
256
struct psvr_parsed_sample *sample,
···
296
304
}
297
305
298
306
static void
299
-
update_fusion(struct psvr_device *psvr, struct psvr_parsed_sample *sample, uint64_t timestamp_ns)
307
+
update_fusion_locked(struct psvr_device *psvr, struct psvr_parsed_sample *sample, uint64_t timestamp_ns)
300
308
{
301
309
struct xrt_vec3 mag = {0.0f, 0.0f, 0.0f};
302
310
(void)mag;
···
312
320
} else {
313
321
m_imu_3dof_update(&psvr->fusion, timestamp_ns, &psvr->read.accel, &psvr->read.gyro);
314
322
}
323
+
}
324
+
325
+
static void
326
+
update_fusion(struct psvr_device *psvr, struct psvr_parsed_sample *sample, uint64_t timestamp_ns)
327
+
{
328
+
os_mutex_lock(&psvr->device_mutex);
329
+
update_fusion_locked(psvr, sample, timestamp_ns);
330
+
os_mutex_unlock(&psvr->device_mutex);
315
331
}
316
332
317
333
static uint32_t
···
401
417
}
402
418
403
419
static void
420
+
sensor_clear_queue(struct psvr_device *psvr)
421
+
{
422
+
uint8_t buffer[FEATURE_BUFFER_SIZE];
423
+
424
+
while (hid_read(psvr->hid_sensor, buffer, FEATURE_BUFFER_SIZE) > 0) {
425
+
// Just drop the packets.
426
+
}
427
+
}
428
+
429
+
static int
430
+
sensor_read_one_packet(struct psvr_device *psvr)
431
+
{
432
+
uint8_t buffer[FEATURE_BUFFER_SIZE];
433
+
434
+
// Try for one second to get packates.
435
+
int size = hid_read_timeout(psvr->hid_sensor, buffer, FEATURE_BUFFER_SIZE, 1000);
436
+
if (size <= 0) {
437
+
return size;
438
+
}
439
+
440
+
handle_tracker_sensor_msg(psvr, buffer, size);
441
+
442
+
return 0;
443
+
}
444
+
445
+
static void *
446
+
sensor_thread(void *ptr)
447
+
{
448
+
U_TRACE_SET_THREAD_NAME("PS VR");
449
+
450
+
struct psvr_device *psvr = (struct psvr_device *)ptr;
451
+
int ret = 0;
452
+
453
+
// Empty the queue.
454
+
sensor_clear_queue(psvr);
455
+
456
+
os_thread_helper_lock(&psvr->oth);
457
+
458
+
while (os_thread_helper_is_running_locked(&psvr->oth) && ret >= 0) {
459
+
os_thread_helper_unlock(&psvr->oth);
460
+
461
+
ret = sensor_read_one_packet(psvr);
462
+
463
+
os_thread_helper_lock(&psvr->oth);
464
+
}
465
+
466
+
os_thread_helper_unlock(&psvr->oth);
467
+
468
+
return NULL;
469
+
}
470
+
471
+
472
+
/*
473
+
*
474
+
* Control device handling.
475
+
*
476
+
*/
477
+
478
+
static void
404
479
handle_control_status_msg(struct psvr_device *psvr, unsigned char *buffer, int size)
405
480
{
406
481
struct psvr_parsed_status status = {0};
···
507
582
}
508
583
509
584
static int
510
-
read_sensor_packets(struct psvr_device *psvr)
511
-
{
512
-
uint8_t buffer[FEATURE_BUFFER_SIZE];
513
-
int size = 0;
514
-
515
-
do {
516
-
size = hid_read(psvr->hid_sensor, buffer, FEATURE_BUFFER_SIZE);
517
-
if (size == 0) {
518
-
return 0;
519
-
}
520
-
if (size < 0) {
521
-
return -1;
522
-
}
523
-
524
-
handle_tracker_sensor_msg(psvr, buffer, size);
525
-
} while (true);
526
-
}
527
-
528
-
static int
529
585
read_control_packets(struct psvr_device *psvr)
530
586
{
531
587
uint8_t buffer[FEATURE_BUFFER_SIZE];
···
647
703
wait_for_power(struct psvr_device *psvr, bool on)
648
704
{
649
705
for (int i = 0; i < 5000; i++) {
650
-
read_sensor_packets(psvr);
651
706
read_control_packets(psvr);
652
707
653
708
if (psvr->powered_on == on) {
···
664
719
wait_for_vr_mode(struct psvr_device *psvr, bool on)
665
720
{
666
721
for (int i = 0; i < 5000; i++) {
667
-
read_sensor_packets(psvr);
668
722
read_control_packets(psvr);
669
723
670
724
if (psvr->in_vr_mode == on) {
···
689
743
if (ret < 0) {
690
744
PSVR_ERROR(psvr, "Failed to switch %s the headset! '%i'", status, ret);
691
745
}
746
+
692
747
693
748
ret = wait_for_power(psvr, on);
694
749
if (ret < 0) {
···
828
883
829
884
// Sleep for a tenth of a second while polling for packages.
830
885
for (int k = 0; k < 100; k++) {
831
-
ret = read_sensor_packets(psvr);
832
-
if (ret < 0) {
833
-
return ret;
834
-
}
835
-
836
886
ret = read_control_packets(psvr);
837
887
if (ret < 0) {
838
888
return ret;
···
845
895
return 0;
846
896
}
847
897
898
+
899
+
/*
900
+
*
901
+
* Misc functions.
902
+
*
903
+
*/
904
+
848
905
static void
849
906
teardown(struct psvr_device *psvr)
850
907
{
851
908
// Stop the variable tracking.
852
909
u_var_remove_root(psvr);
853
910
911
+
// Shutdown the sensor thread early.
912
+
os_thread_helper_stop_and_wait(&psvr->oth);
913
+
854
914
// Includes null check, and sets to null.
855
915
xrt_tracked_psvr_destroy(&psvr->tracker);
856
916
···
872
932
// Destroy the fusion.
873
933
m_imu_3dof_close(&psvr->fusion);
874
934
935
+
os_thread_helper_destroy(&psvr->oth);
875
936
os_mutex_destroy(&psvr->device_mutex);
876
937
}
877
938
···
889
950
890
951
os_mutex_lock(&psvr->device_mutex);
891
952
892
-
read_sensor_packets(psvr);
893
953
update_leds_if_changed(psvr);
894
954
895
955
os_mutex_unlock(&psvr->device_mutex);
···
911
971
os_mutex_lock(&psvr->device_mutex);
912
972
913
973
// Read all packets.
914
-
read_sensor_packets(psvr);
915
974
read_control_packets(psvr);
916
975
917
976
// Clear out the relation.
···
1024
1083
snprintf(psvr->base.str, XRT_DEVICE_NAME_LEN, "PS VR Headset");
1025
1084
snprintf(psvr->base.serial, XRT_DEVICE_NAME_LEN, "PS VR Headset");
1026
1085
1027
-
// Do mutex init before any call to teardown happens.
1086
+
// Do mutex and thread init before any call to teardown happens.
1028
1087
os_mutex_init(&psvr->device_mutex);
1088
+
os_thread_helper_init(&psvr->oth);
1029
1089
1030
1090
ret = open_hid(psvr, sensor_hid_info, &psvr->hid_sensor);
1031
1091
if (ret != 0) {
···
1033
1093
}
1034
1094
1035
1095
ret = open_hid(psvr, control_hid_info, &psvr->hid_control);
1096
+
if (ret < 0) {
1097
+
goto cleanup;
1098
+
}
1099
+
1100
+
ret = os_thread_helper_start(&psvr->oth, sensor_thread, (void *)psvr);
1036
1101
if (ret < 0) {
1037
1102
goto cleanup;
1038
1103
}