The open source OpenXR runtime
d/pssense: Basic 3DoF pose tracking from IMU data
+277
-32
+274
-32
src/xrt/drivers/pssense/pssense_driver.c
+274
-32
src/xrt/drivers/pssense/pssense_driver.c
···
1
1
// Copyright 2023, Collabora, Ltd.
2
+
// Copyright 2023, Jarett Millard
2
3
// SPDX-License-Identifier: BSL-1.0
3
4
/*!
4
5
* @file
···
24
25
#include "util/u_trace_marker.h"
25
26
26
27
#include "pssense_interface.h"
28
+
#include "math/m_mathinclude.h"
29
+
#include "math/m_space.h"
30
+
#include "math/m_imu_3dof.h"
27
31
28
32
#include <stdio.h>
29
33
···
37
41
#define PSSENSE_WARN(p, ...) U_LOG_XDEV_IFL_W(&p->base, p->log_level, __VA_ARGS__)
38
42
#define PSSENSE_ERROR(p, ...) U_LOG_XDEV_IFL_E(&p->base, p->log_level, __VA_ARGS__)
39
43
40
-
DEBUG_GET_ONCE_LOG_OPTION(pssense_log, "PSSENSE_LOG", U_LOGGING_WARN)
44
+
DEBUG_GET_ONCE_LOG_OPTION(pssense_log, "PSSENSE_LOG", U_LOGGING_INFO)
45
+
46
+
#define DEG_TO_RAD(DEG) (DEG * M_PI / 180.)
47
+
48
+
static struct xrt_binding_input_pair simple_inputs_pssense[4] = {
49
+
{XRT_INPUT_SIMPLE_SELECT_CLICK, XRT_INPUT_PSSENSE_TRIGGER_VALUE},
50
+
{XRT_INPUT_SIMPLE_MENU_CLICK, XRT_INPUT_PSSENSE_OPTIONS_CLICK},
51
+
{XRT_INPUT_SIMPLE_GRIP_POSE, XRT_INPUT_PSSENSE_GRIP_POSE},
52
+
{XRT_INPUT_SIMPLE_AIM_POSE, XRT_INPUT_PSSENSE_AIM_POSE},
53
+
};
54
+
55
+
static struct xrt_binding_profile binding_profiles_pssense[1] = {
56
+
{
57
+
.name = XRT_DEVICE_SIMPLE_CONTROLLER,
58
+
.inputs = simple_inputs_pssense,
59
+
.input_count = ARRAY_SIZE(simple_inputs_pssense),
60
+
.outputs = NULL,
61
+
.output_count = 0,
62
+
},
63
+
};
41
64
42
65
/*!
43
66
* Indices where each input is in the input list.
···
64
87
PSSENSE_INDEX_TRIGGER_PROXIMITY,
65
88
PSSENSE_INDEX_THUMBSTICK,
66
89
PSSENSE_INDEX_THUMBSTICK_CLICK,
67
-
PSSENSE_INDEX_THUMBSTICK_TOUCH
90
+
PSSENSE_INDEX_THUMBSTICK_TOUCH,
91
+
PSSENSE_INDEX_GRIP_POSE,
92
+
PSSENSE_INDEX_AIM_POSE,
93
+
};
94
+
95
+
const uint8_t HID_PACKET_REPORT_ID = 0x31;
96
+
const uint8_t CALIBRATION_DATA_FEATURE_REPORT_ID = 0x05;
97
+
const uint8_t CALIBRATION_DATA_PART_ID_1 = 0;
98
+
const uint8_t CALIBRATION_DATA_PART_ID_2 = 0x81;
99
+
100
+
/**
101
+
* Gyro read value range is +-32768.
102
+
*/
103
+
const double PSSENSE_GYRO_SCALE_DEG = 180.0 / 1024;
104
+
/**
105
+
* Accelerometer read value range is +-32768 and covers +-8 g.
106
+
*/
107
+
const double PSSENSE_ACCEL_SCALE = MATH_GRAVITY_M_S2 / 4096;
108
+
109
+
/**
110
+
* 16-bit little-endian int
111
+
*/
112
+
struct pssense_i16_le
113
+
{
114
+
uint8_t low;
115
+
uint8_t high;
116
+
};
117
+
118
+
/**
119
+
* 32-bit little-endian int
120
+
*/
121
+
struct pssense_i32_le
122
+
{
123
+
uint8_t lowest;
124
+
uint8_t lower;
125
+
uint8_t higher;
126
+
uint8_t highest;
68
127
};
69
128
70
129
/*!
···
72
131
*/
73
132
struct pssense_data_packet
74
133
{
75
-
uint8_t header;
134
+
uint8_t report_id;
135
+
uint8_t bt_header;
76
136
uint8_t thumbstick_x;
77
137
uint8_t thumbstick_y;
78
138
uint8_t trigger_value;
79
139
uint8_t trigger_proximity;
80
140
uint8_t squeeze_proximity;
81
-
uint8_t reserved;
82
-
uint8_t seq_no;
141
+
uint8_t unknown1[2]; // Always 0x0001
83
142
uint8_t buttons[3];
143
+
uint8_t unknown2; // Always 0x00
144
+
struct pssense_i32_le seq_no;
145
+
struct pssense_i16_le gyro[3];
146
+
struct pssense_i16_le accel[3];
147
+
uint8_t unknown3[3];
148
+
uint8_t unknown4; // Increments occasionally
149
+
uint8_t battery_level; // Range appears to be 0x00-0x0e
150
+
uint8_t unknown5[10];
151
+
uint8_t charging_state; // 0x00 when unplugged, 0x20 when charging
152
+
uint8_t unknown6[29];
153
+
uint8_t crc[4];
84
154
};
85
155
86
156
/*!
···
88
158
*/
89
159
struct pssense_input_state
90
160
{
91
-
uint64_t timestamp;
92
-
uint8_t seq_no;
161
+
uint64_t timestamp_ns;
162
+
uint32_t seq_no;
93
163
94
164
bool ps_click;
95
165
bool share_click;
···
112
182
bool thumbstick_click;
113
183
bool thumbstick_touch;
114
184
struct xrt_vec2 thumbstick;
185
+
186
+
struct xrt_vec3_i32 gyro_raw;
187
+
struct xrt_vec3_i32 accel_raw;
115
188
};
116
189
117
190
/*!
···
138
211
//! Input state parsed from most recent packet
139
212
struct pssense_input_state state;
140
213
214
+
struct m_imu_3dof fusion;
215
+
struct xrt_pose pose;
216
+
141
217
struct
142
218
{
143
219
bool button_states;
220
+
bool tracking;
144
221
} gui;
145
222
};
146
223
224
+
static uint32_t
225
+
pssense_i32_le_to_u32(const struct pssense_i32_le *from)
226
+
{
227
+
return (uint32_t)(from->lowest | from->lower << 8 | from->higher << 16 | from->highest << 24);
228
+
}
229
+
230
+
static int16_t
231
+
pssense_i16_le_to_i16(const struct pssense_i16_le *from)
232
+
{
233
+
// The cast is important, sign extend properly.
234
+
return (int16_t)(from->low | from->high << 8);
235
+
}
236
+
147
237
/*!
148
238
* Reads one packet from the device, handles time out, locking and checking if
149
239
* the thread has been told to shut down.
150
240
*/
151
241
static bool
152
-
pssense_read_one_packet(struct pssense_device *pssense, uint8_t *buffer, size_t size)
242
+
pssense_read_one_packet(struct pssense_device *pssense, uint8_t *buffer, size_t size, bool check_size)
153
243
{
154
244
os_thread_helper_lock(&pssense->controller_thread);
155
245
···
169
259
PSSENSE_ERROR(pssense, "Failed to read device '%i'!", ret);
170
260
return false;
171
261
}
172
-
if (ret != (int)size) {
262
+
// Skip this check if we haven't flushed all the compat mode packets yet, since they're shorter.
263
+
if (check_size && ret != (int)size) {
173
264
PSSENSE_ERROR(pssense, "Unexpected HID packet size %i (expected %zu)", ret, size);
174
265
return false;
175
266
}
···
180
271
return false;
181
272
}
182
273
183
-
static void
274
+
static bool
184
275
pssense_parse_packet(struct pssense_device *pssense,
185
276
struct pssense_data_packet *data,
186
277
struct pssense_input_state *input)
187
278
{
188
-
input->timestamp = os_monotonic_get_ns();
189
-
input->seq_no = data->seq_no;
279
+
if (data->report_id != HID_PACKET_REPORT_ID) {
280
+
PSSENSE_WARN(pssense, "Unrecognized HID report id %u", data->report_id);
281
+
return false;
282
+
}
283
+
284
+
input->timestamp_ns = os_monotonic_get_ns();
285
+
286
+
uint32_t seq_no = pssense_i32_le_to_u32(&data->seq_no);
287
+
if (input->seq_no != 0 && seq_no != input->seq_no + 1) {
288
+
PSSENSE_WARN(pssense, "Missed seq no %u. Previous was %u", seq_no, input->seq_no);
289
+
}
290
+
input->seq_no = seq_no;
190
291
191
292
input->ps_click = (data->buttons[1] & 16) != 0;
192
293
input->squeeze_touch = (data->buttons[2] & 8) != 0;
···
217
318
input->trigger_click = (data->buttons[0] & 128) != 0;
218
319
input->thumbstick_click = (data->buttons[1] & 8) != 0;
219
320
}
321
+
322
+
input->gyro_raw.x = pssense_i16_le_to_i16(&data->gyro[0]);
323
+
input->gyro_raw.y = pssense_i16_le_to_i16(&data->gyro[1]);
324
+
input->gyro_raw.z = pssense_i16_le_to_i16(&data->gyro[2]);
325
+
326
+
input->accel_raw.x = pssense_i16_le_to_i16(&data->accel[0]);
327
+
input->accel_raw.y = pssense_i16_le_to_i16(&data->accel[1]);
328
+
input->accel_raw.z = pssense_i16_le_to_i16(&data->accel[2]);
329
+
330
+
return true;
331
+
}
332
+
333
+
static void
334
+
pssense_update_fusion(struct pssense_device *pssense)
335
+
{
336
+
struct xrt_vec3 gyro;
337
+
gyro.x = DEG_TO_RAD(pssense->state.gyro_raw.x * PSSENSE_GYRO_SCALE_DEG);
338
+
gyro.y = DEG_TO_RAD(pssense->state.gyro_raw.y * PSSENSE_GYRO_SCALE_DEG);
339
+
gyro.z = DEG_TO_RAD(pssense->state.gyro_raw.z * PSSENSE_GYRO_SCALE_DEG);
340
+
341
+
struct xrt_vec3 accel;
342
+
accel.x = pssense->state.accel_raw.x * PSSENSE_ACCEL_SCALE;
343
+
accel.y = pssense->state.accel_raw.y * PSSENSE_ACCEL_SCALE;
344
+
accel.z = pssense->state.accel_raw.z * PSSENSE_ACCEL_SCALE;
345
+
346
+
// TODO: Apply correction from calibration data
347
+
348
+
m_imu_3dof_update(&pssense->fusion, pssense->state.timestamp_ns, &accel, &gyro);
349
+
pssense->pose.orientation = pssense->fusion.rot;
220
350
}
221
351
222
352
static void *
···
228
358
229
359
union {
230
360
uint8_t buffer[sizeof(struct pssense_data_packet)];
231
-
struct pssense_data_packet input;
361
+
struct pssense_data_packet packet;
232
362
} data;
233
-
struct pssense_input_state input = {0};
363
+
struct pssense_input_state input_state = {0};
234
364
235
-
while (os_hid_read(pssense->hid, data.buffer, sizeof(data), 0) > 0) {
236
-
// Empty queue first
365
+
// The Sense controller starts in compat mode with a different HID report ID and format.
366
+
// We need to discard packets until we get a correct report.
367
+
while (pssense_read_one_packet(pssense, data.buffer, sizeof(data), false) &&
368
+
data.packet.report_id != HID_PACKET_REPORT_ID) {
369
+
PSSENSE_DEBUG(pssense, "Discarding compat mode HID report");
237
370
}
238
371
239
-
// Now wait for a package to sync up, it's discarded but that's okay.
240
-
if (!pssense_read_one_packet(pssense, data.buffer, sizeof(data))) {
241
-
return NULL;
242
-
}
243
-
244
-
while (pssense_read_one_packet(pssense, data.buffer, sizeof(data))) {
245
-
pssense_parse_packet(pssense, (struct pssense_data_packet *)data.buffer, &input);
246
-
os_mutex_lock(&pssense->lock);
247
-
pssense->state = input;
248
-
os_mutex_unlock(&pssense->lock);
372
+
while (pssense_read_one_packet(pssense, data.buffer, sizeof(data), true)) {
373
+
if (pssense_parse_packet(pssense, &data.packet, &input_state)) {
374
+
os_mutex_lock(&pssense->lock);
375
+
pssense->state = input_state;
376
+
pssense_update_fusion(pssense);
377
+
os_mutex_unlock(&pssense->lock);
378
+
}
249
379
}
250
380
251
381
return NULL;
···
262
392
// Now that the thread is not running we can destroy the lock.
263
393
os_mutex_destroy(&pssense->lock);
264
394
395
+
m_imu_3dof_close(&pssense->fusion);
396
+
265
397
// Remove the variable tracking.
266
398
u_var_remove_root(pssense);
267
399
···
282
414
os_mutex_lock(&pssense->lock);
283
415
284
416
for (uint i = 0; i < (uint)sizeof(enum pssense_input_index); i++) {
285
-
pssense->base.inputs[i].timestamp = (int64_t)pssense->state.timestamp;
417
+
pssense->base.inputs[i].timestamp = (int64_t)pssense->state.timestamp_ns;
286
418
}
287
419
pssense->base.inputs[PSSENSE_INDEX_PS_CLICK].value.boolean = pssense->state.ps_click;
288
420
pssense->base.inputs[PSSENSE_INDEX_SHARE_CLICK].value.boolean = pssense->state.share_click;
···
310
442
os_mutex_unlock(&pssense->lock);
311
443
}
312
444
445
+
static void
446
+
pssense_get_fusion_pose(struct pssense_device *pssense,
447
+
enum xrt_input_name name,
448
+
uint64_t at_timestamp_ns,
449
+
struct xrt_space_relation *out_relation)
450
+
{
451
+
out_relation->pose = pssense->pose;
452
+
out_relation->linear_velocity.x = 0.0f;
453
+
out_relation->linear_velocity.y = 0.0f;
454
+
out_relation->linear_velocity.z = 0.0f;
455
+
456
+
/*!
457
+
* @todo This is hack, fusion reports angvel relative to the device but
458
+
* it needs to be in relation to the base space. Rotating it with the
459
+
* device orientation is enough to get it into the right space, angular
460
+
* velocity is a derivative so needs a special rotation.
461
+
*/
462
+
math_quat_rotate_derivative(&pssense->pose.orientation, &pssense->fusion.last.gyro,
463
+
&out_relation->angular_velocity);
464
+
465
+
out_relation->relation_flags = (enum xrt_space_relation_flags)(
466
+
XRT_SPACE_RELATION_ORIENTATION_VALID_BIT | XRT_SPACE_RELATION_ORIENTATION_TRACKED_BIT |
467
+
XRT_SPACE_RELATION_ANGULAR_VELOCITY_VALID_BIT | XRT_SPACE_RELATION_LINEAR_VELOCITY_VALID_BIT);
468
+
}
469
+
470
+
static void
471
+
pssense_get_tracked_pose(struct xrt_device *xdev,
472
+
enum xrt_input_name name,
473
+
uint64_t at_timestamp_ns,
474
+
struct xrt_space_relation *out_relation)
475
+
{
476
+
struct pssense_device *pssense = (struct pssense_device *)xdev;
477
+
478
+
if (name != XRT_INPUT_PSSENSE_AIM_POSE && name != XRT_INPUT_PSSENSE_GRIP_POSE) {
479
+
PSSENSE_ERROR(pssense, "Unknown pose name requested %u", name);
480
+
return;
481
+
}
482
+
483
+
struct xrt_relation_chain xrc = {0};
484
+
struct xrt_pose pose_correction = {0};
485
+
486
+
// Rotate the grip/aim pose up by 60 degrees around the X axis
487
+
struct xrt_vec3 axis = {1.0, 0, 0};
488
+
math_quat_from_angle_vector(DEG_TO_RAD(60), &axis, &pose_correction.orientation);
489
+
m_relation_chain_push_pose(&xrc, &pose_correction);
490
+
491
+
struct xrt_space_relation *rel = m_relation_chain_reserve(&xrc);
492
+
493
+
os_mutex_lock(&pssense->lock);
494
+
pssense_get_fusion_pose(pssense, name, at_timestamp_ns, rel);
495
+
os_mutex_unlock(&pssense->lock);
496
+
497
+
m_relation_chain_resolve(&xrc, out_relation);
498
+
}
499
+
500
+
/**
501
+
* Retrieving the calibration data report will switch the Sense controller from compat mode into full mode.
502
+
*/
503
+
bool
504
+
pssense_get_calibration_data(struct pssense_device *pssense)
505
+
{
506
+
int ret;
507
+
uint8_t buffer[64];
508
+
uint8_t data[(sizeof(buffer) - 2) * 2];
509
+
for (int i = 0; i < 2; i++) {
510
+
ret = os_hid_get_feature(pssense->hid, CALIBRATION_DATA_FEATURE_REPORT_ID, buffer, sizeof(buffer));
511
+
if (ret < 0) {
512
+
PSSENSE_ERROR(pssense, "Failed to retrieve calibration report: %d", ret);
513
+
return false;
514
+
}
515
+
if (ret != sizeof(buffer)) {
516
+
PSSENSE_ERROR(pssense, "Invalid byte count transferred, expected %zu got %d\n", sizeof(buffer),
517
+
ret);
518
+
return false;
519
+
}
520
+
if (buffer[1] == CALIBRATION_DATA_PART_ID_1) {
521
+
memcpy(data, buffer + 2, sizeof(buffer) - 2);
522
+
} else if (buffer[1] == CALIBRATION_DATA_PART_ID_2) {
523
+
memcpy(data + sizeof(buffer) - 2, buffer + 2, sizeof(buffer) - 2);
524
+
} else {
525
+
PSSENSE_ERROR(pssense, "Unknown calibration data part ID %u", buffer[1]);
526
+
return false;
527
+
}
528
+
}
529
+
530
+
// TODO: Parse calibration data into prefiler
531
+
532
+
return true;
533
+
}
534
+
313
535
#define SET_INPUT(NAME) (pssense->base.inputs[PSSENSE_INDEX_##NAME].name = XRT_INPUT_PSSENSE_##NAME)
314
536
315
537
int
···
335
557
XRT_PROBER_STRING_PRODUCT, //
336
558
product_name, //
337
559
sizeof(product_name)); //
338
-
if (ret != 0) {
560
+
if (ret <= 0) {
339
561
U_LOG_E("Failed to get product name from Bluetooth device!");
340
562
return -1;
341
563
}
342
564
343
565
enum u_device_alloc_flags flags = U_DEVICE_ALLOC_TRACKING_NONE;
344
-
struct pssense_device *pssense = U_DEVICE_ALLOCATE(struct pssense_device, flags, 13, 1);
345
-
566
+
struct pssense_device *pssense = U_DEVICE_ALLOCATE(struct pssense_device, flags, 23, 0);
346
567
PSSENSE_DEBUG(pssense, "PlayStation Sense controller found");
347
-
pssense->base.destroy = pssense_device_destroy;
348
-
pssense->base.update_inputs = pssense_device_update_inputs;
568
+
349
569
pssense->base.name = XRT_DEVICE_PSSENSE;
350
570
snprintf(pssense->base.str, XRT_DEVICE_NAME_LEN, "%s", product_name);
571
+
pssense->base.update_inputs = pssense_device_update_inputs;
572
+
pssense->base.get_tracked_pose = pssense_get_tracked_pose;
573
+
pssense->base.destroy = pssense_device_destroy;
574
+
pssense->base.orientation_tracking_supported = true;
575
+
576
+
pssense->base.binding_profiles = binding_profiles_pssense;
577
+
pssense->base.binding_profile_count = ARRAY_SIZE(binding_profiles_pssense);
578
+
579
+
m_imu_3dof_init(&pssense->fusion, M_IMU_3DOF_USE_GRAVITY_DUR_20MS);
351
580
352
581
pssense->log_level = debug_get_log_option_pssense_log();
353
582
pssense->hid = hid;
···
385
614
SET_INPUT(THUMBSTICK);
386
615
SET_INPUT(THUMBSTICK_CLICK);
387
616
SET_INPUT(THUMBSTICK_TOUCH);
617
+
SET_INPUT(GRIP_POSE);
618
+
SET_INPUT(AIM_POSE);
388
619
389
620
ret = os_mutex_init(&pssense->lock);
390
621
if (ret != 0) {
···
407
638
return -1;
408
639
}
409
640
641
+
if (!pssense_get_calibration_data(pssense)) {
642
+
PSSENSE_ERROR(pssense, "Failed to retrieve calibration data");
643
+
pssense_device_destroy(&pssense->base);
644
+
return -1;
645
+
}
646
+
410
647
u_var_add_root(pssense, pssense->base.str, false);
411
648
u_var_add_log_level(pssense, &pssense->log_level, "Log level");
412
649
···
436
673
u_var_add_ro_f32(pssense, &pssense->state.thumbstick.y, "Thumbstick Y");
437
674
u_var_add_bool(pssense, &pssense->state.thumbstick_click, "Thumbstick Click");
438
675
u_var_add_bool(pssense, &pssense->state.thumbstick_touch, "Thumbstick Touch");
676
+
677
+
u_var_add_gui_header(pssense, &pssense->gui.tracking, "Tracking");
678
+
u_var_add_ro_vec3_i32(pssense, &pssense->state.gyro_raw, "Raw Gyro");
679
+
u_var_add_ro_vec3_i32(pssense, &pssense->state.accel_raw, "Raw Accel");
680
+
u_var_add_pose(pssense, &pssense->pose, "Pose");
439
681
440
682
out_xdevs[0] = &pssense->base;
441
683
return 1;
+1
src/xrt/drivers/pssense/pssense_interface.h
+1
src/xrt/drivers/pssense/pssense_interface.h
+2
src/xrt/include/xrt/xrt_defines.h
+2
src/xrt/include/xrt/xrt_defines.h
···
946
946
XRT_INPUT_PSSENSE_THUMBSTICK = XRT_INPUT_NAME(0x0312, VEC2_MINUS_ONE_TO_ONE),
947
947
XRT_INPUT_PSSENSE_THUMBSTICK_CLICK = XRT_INPUT_NAME(0x0313, BOOLEAN),
948
948
XRT_INPUT_PSSENSE_THUMBSTICK_TOUCH = XRT_INPUT_NAME(0x0314, BOOLEAN),
949
+
XRT_INPUT_PSSENSE_GRIP_POSE = XRT_INPUT_NAME(0x0315, POSE),
950
+
XRT_INPUT_PSSENSE_AIM_POSE = XRT_INPUT_NAME(0x0316, POSE),
949
951
// clang-format on
950
952
};
951
953