matrixfurry.com
The open source OpenXR runtime
1// Copyright 2023, Collabora, Ltd.
2// Copyright 2023, Jarett Millard
3// SPDX-License-Identifier: BSL-1.0
4/*!
5 * @file
6 * @brief PlayStation Sense controller prober and driver code.
7 * @author Jarett Millard <jarett.millard@gmail.com>
8 * @ingroup drv_pssense
9 */
10
11#include "xrt/xrt_prober.h"
12
13#include "os/os_threading.h"
14#include "os/os_hid.h"
15#include "os/os_time.h"
16
17#include "math/m_api.h"
18
19#include "tracking/t_imu.h"
20
21#include "util/u_var.h"
22#include "util/u_debug.h"
23#include "util/u_device.h"
24#include "util/u_logging.h"
25#include "util/u_trace_marker.h"
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"
31
32#include <stdio.h>
33
34/*!
35 * @addtogroup drv_pssense
36 * @{
37 */
38
39#define PSSENSE_TRACE(p, ...) U_LOG_XDEV_IFL_T(&p->base, p->log_level, __VA_ARGS__)
40#define PSSENSE_DEBUG(p, ...) U_LOG_XDEV_IFL_D(&p->base, p->log_level, __VA_ARGS__)
41#define PSSENSE_WARN(p, ...) U_LOG_XDEV_IFL_W(&p->base, p->log_level, __VA_ARGS__)
42#define PSSENSE_ERROR(p, ...) U_LOG_XDEV_IFL_E(&p->base, p->log_level, __VA_ARGS__)
43
44DEBUG_GET_ONCE_LOG_OPTION(pssense_log, "PSSENSE_LOG", U_LOGGING_INFO)
45
46static struct xrt_binding_input_pair simple_inputs_pssense[4] = {
47 {XRT_INPUT_SIMPLE_SELECT_CLICK, XRT_INPUT_PSSENSE_TRIGGER_VALUE},
48 {XRT_INPUT_SIMPLE_MENU_CLICK, XRT_INPUT_PSSENSE_OPTIONS_CLICK},
49 {XRT_INPUT_SIMPLE_GRIP_POSE, XRT_INPUT_PSSENSE_GRIP_POSE},
50 {XRT_INPUT_SIMPLE_AIM_POSE, XRT_INPUT_PSSENSE_AIM_POSE},
51};
52
53static struct xrt_binding_output_pair simple_outputs_pssense[1] = {
54 {XRT_OUTPUT_NAME_SIMPLE_VIBRATION, XRT_OUTPUT_NAME_PSSENSE_VIBRATION},
55};
56
57static struct xrt_binding_profile binding_profiles_pssense[1] = {
58 {
59 .name = XRT_DEVICE_SIMPLE_CONTROLLER,
60 .inputs = simple_inputs_pssense,
61 .input_count = ARRAY_SIZE(simple_inputs_pssense),
62 .outputs = simple_outputs_pssense,
63 .output_count = ARRAY_SIZE(simple_outputs_pssense),
64 },
65};
66
67/*!
68 * Indices where each input is in the input list.
69 */
70enum pssense_input_index
71{
72 PSSENSE_INDEX_PS_CLICK,
73 PSSENSE_INDEX_SHARE_CLICK,
74 PSSENSE_INDEX_OPTIONS_CLICK,
75 PSSENSE_INDEX_SQUARE_CLICK,
76 PSSENSE_INDEX_SQUARE_TOUCH,
77 PSSENSE_INDEX_TRIANGLE_CLICK,
78 PSSENSE_INDEX_TRIANGLE_TOUCH,
79 PSSENSE_INDEX_CROSS_CLICK,
80 PSSENSE_INDEX_CROSS_TOUCH,
81 PSSENSE_INDEX_CIRCLE_CLICK,
82 PSSENSE_INDEX_CIRCLE_TOUCH,
83 PSSENSE_INDEX_SQUEEZE_CLICK,
84 PSSENSE_INDEX_SQUEEZE_TOUCH,
85 PSSENSE_INDEX_SQUEEZE_PROXIMITY_FLOAT,
86 PSSENSE_INDEX_TRIGGER_CLICK,
87 PSSENSE_INDEX_TRIGGER_TOUCH,
88 PSSENSE_INDEX_TRIGGER_VALUE,
89 PSSENSE_INDEX_TRIGGER_PROXIMITY_FLOAT,
90 PSSENSE_INDEX_THUMBSTICK,
91 PSSENSE_INDEX_THUMBSTICK_CLICK,
92 PSSENSE_INDEX_THUMBSTICK_TOUCH,
93 PSSENSE_INDEX_GRIP_POSE,
94 PSSENSE_INDEX_AIM_POSE,
95};
96
97const uint8_t INPUT_REPORT_ID = 0x31;
98const uint8_t OUTPUT_REPORT_ID = 0x31;
99const uint8_t OUTPUT_REPORT_TAG = 0x10;
100const uint8_t CALIBRATION_DATA_FEATURE_REPORT_ID = 0x05;
101const uint8_t CALIBRATION_DATA_PART_ID_1 = 0;
102const uint8_t CALIBRATION_DATA_PART_ID_2 = 0x81;
103
104const uint8_t INPUT_REPORT_CRC32_SEED = 0xa1;
105const uint8_t OUTPUT_REPORT_CRC32_SEED = 0xa2;
106const uint8_t FEATURE_REPORT_CRC32_SEED = 0xa3;
107
108//! Gyro read value range is +-32768.
109const double PSSENSE_GYRO_SCALE_DEG = 180.0 / 1024;
110//! Accelerometer read value range is +-32768 and covers +-8 g.
111const double PSSENSE_ACCEL_SCALE = MATH_GRAVITY_M_S2 / 4096;
112
113//! Flag bits to enable setting vibration in an output report
114const uint8_t VIBRATE_ENABLE_BITS = 0x03;
115//! Pure 120Hz vibration
116const uint8_t VIBRATE_MODE_HIGH_120HZ = 0x00;
117//! Pure 60Hz vibration
118const uint8_t VIBRATE_MODE_LOW_60HZ = 0x20;
119//! Emulates a legacy vibration motor
120const uint8_t VIBRATE_MODE_CLASSIC_RUMBLE = 0x40;
121//! Softer rumble emulation, like an engine running
122const uint8_t VIBRATE_MODE_DIET_RUMBLE = 0x60;
123
124//! Flag bits to enable setting trigger feedback in an output report
125const uint8_t TRIGGER_FEEDBACK_ENABLE_BITS = 0x04;
126//! Clear the trigger feedback setting
127const uint8_t TRIGGER_FEEDBACK_MODE_NONE = 0x00;
128//! Constant resistance throughout the trigger movement
129const uint8_t TRIGGER_FEEDBACK_MODE_CONSTANT = 0x01;
130//! A single point of resistance at the beginning of the trigger, right before the click flag is activated
131const uint8_t TRIGGER_FEEDBACK_MODE_CATCH = 0x02;
132
133const uint8_t CHARGE_STATE_DISCHARGING = 0x00;
134const uint8_t CHARGE_STATE_CHARGING = 0x01;
135const uint8_t CHARGE_STATE_FULL = 0x02;
136const uint8_t CHARGE_STATE_ABNORMAL_VOLTAGE = 0x0A;
137const uint8_t CHARGE_STATE_ABNORMAL_TEMP = 0x0B;
138const uint8_t CHARGE_STATE_CHARGING_ERROR = 0x0F;
139
140/**
141 * 16-bit little-endian int
142 */
143struct pssense_i16_le
144{
145 uint8_t low;
146 uint8_t high;
147};
148
149/**
150 * 32-bit little-endian int
151 */
152struct pssense_i32_le
153{
154 uint8_t lowest;
155 uint8_t lower;
156 uint8_t higher;
157 uint8_t highest;
158};
159
160#define INPUT_REPORT_LENGTH 78
161/*!
162 * HID input report data packet.
163 */
164struct pssense_input_report
165{
166 uint8_t report_id;
167 uint8_t bt_header;
168 uint8_t thumbstick_x;
169 uint8_t thumbstick_y;
170 uint8_t trigger_value;
171 uint8_t trigger_proximity;
172 uint8_t squeeze_proximity;
173 uint8_t unknown1[2]; // Always 0x0001
174 uint8_t buttons[3];
175 uint8_t unknown2; // Always 0x00
176 struct pssense_i32_le seq_no;
177 struct pssense_i16_le gyro[3];
178 struct pssense_i16_le accel[3];
179 struct pssense_i32_le imu_ticks;
180 uint8_t temperature;
181 uint8_t unknown3[9];
182 uint8_t battery_state; // High bits charge level 0x00-0x0a, low bits battery state
183 uint8_t plug_state; // Flags for USB data and/or power connected
184 struct pssense_i32_le host_timestamp;
185 struct pssense_i32_le device_timestamp;
186 uint8_t unknown4[4];
187 uint8_t aes_cmac[8];
188 uint8_t unknown5;
189 uint8_t crc_failure_count;
190 uint8_t padding[7];
191 struct pssense_i32_le crc;
192};
193static_assert(sizeof(struct pssense_input_report) == INPUT_REPORT_LENGTH, "Incorrect input report struct length");
194
195#define OUTPUT_REPORT_LENGTH 78
196/**
197 * HID output report data packet.
198 */
199struct pssense_output_report
200{
201 uint8_t report_id;
202 uint8_t bt_seq_no; // High bits only; low bits are always 0
203 uint8_t tag; // Needs to be 0x10 for this report
204 uint8_t feedback_flags; // Vibrate mode and enable flags to set vibrate and trigger feedback in this report
205 uint8_t unknown;
206 uint8_t vibration_amplitude; // Vibration amplitude from 0x00-0xff. Sending 0 turns vibration off.
207 uint8_t unknown2;
208 uint8_t trigger_feedback_mode; // Constant or sticky trigger resistance
209 uint8_t ffb[10];
210 struct pssense_i32_le host_timestamp;
211 uint8_t unknown3[19];
212 uint8_t counter;
213 uint8_t haptics[32];
214 struct pssense_i32_le crc;
215};
216static_assert(sizeof(struct pssense_output_report) == OUTPUT_REPORT_LENGTH, "Incorrect output report struct length");
217
218#define FEATURE_REPORT_LENGTH 64
219#define CALIBRATION_DATA_LENGTH 116
220/**
221 * HID output report data packet.
222 */
223struct pssense_feature_report
224{
225 uint8_t report_id;
226 uint8_t part_id;
227 uint8_t data[CALIBRATION_DATA_LENGTH / 2];
228 struct pssense_i32_le crc;
229};
230static_assert(sizeof(struct pssense_feature_report) == FEATURE_REPORT_LENGTH, "Incorrect feature report struct length");
231
232/*!
233 * PlayStation Sense state parsed from a data packet.
234 */
235struct pssense_input_state
236{
237 uint64_t timestamp_ns;
238 uint32_t seq_no;
239
240 bool ps_click;
241 bool share_click;
242 bool options_click;
243 bool square_click;
244 bool square_touch;
245 bool triangle_click;
246 bool triangle_touch;
247 bool cross_click;
248 bool cross_touch;
249 bool circle_click;
250 bool circle_touch;
251 bool squeeze_click;
252 bool squeeze_touch;
253 float squeeze_proximity;
254 bool trigger_click;
255 bool trigger_touch;
256 float trigger_value;
257 float trigger_proximity;
258 bool thumbstick_click;
259 bool thumbstick_touch;
260 struct xrt_vec2 thumbstick;
261
262 uint32_t imu_ticks_last;
263 uint64_t imu_ticks_total;
264 struct xrt_vec3_i32 gyro_raw;
265 struct xrt_vec3_i32 accel_raw;
266
267 bool battery_state_valid;
268 bool battery_charging;
269 //! 0..1
270 float battery_charge_percent;
271};
272
273/*!
274 * A single PlayStation Sense Controller.
275 *
276 * @implements xrt_device
277 */
278struct pssense_device
279{
280 struct xrt_device base;
281
282 struct os_hid_device *hid;
283 struct os_thread_helper controller_thread;
284 struct os_mutex lock;
285
286 enum
287 {
288 PSSENSE_HAND_LEFT,
289 PSSENSE_HAND_RIGHT
290 } hand;
291
292 enum u_logging_level log_level;
293
294 //! Input state parsed from most recent packet
295 struct pssense_input_state state;
296 //! Pending output state to send to device
297 struct
298 {
299 uint8_t next_seq_no;
300 bool send_vibration;
301 uint8_t vibration_amplitude;
302 uint8_t vibration_mode;
303 uint64_t vibration_end_timestamp_ns;
304 uint64_t vibration_resend_timestamp_ns;
305 bool send_trigger_feedback;
306 uint8_t trigger_feedback_mode;
307 } output;
308
309 struct m_imu_3dof fusion;
310 struct xrt_pose pose;
311
312 struct
313 {
314 bool button_states;
315 bool tracking;
316 } gui;
317};
318
319static uint32_t
320pssense_i32_le_to_u32(const struct pssense_i32_le *from)
321{
322 return (uint32_t)(from->lowest | from->lower << 8 | from->higher << 16 | from->highest << 24);
323}
324
325static struct pssense_i32_le
326pssense_u32_to_i32_le(uint32_t from)
327{
328 struct pssense_i32_le ret = {
329 .lowest = (from >> 0) & 0x0ff,
330 .lower = (from >> 8) & 0x0ff,
331 .higher = (from >> 16) & 0x0ff,
332 .highest = (from >> 24) & 0x0ff,
333 };
334
335 return ret;
336}
337
338static int16_t
339pssense_i16_le_to_i16(const struct pssense_i16_le *from)
340{
341 // The cast is important, sign extend properly.
342 return (int16_t)(from->low | from->high << 8);
343}
344
345const uint32_t CRC_POLYNOMIAL = 0xedb88320;
346static uint32_t
347crc32_le(uint32_t crc, uint8_t const *p, size_t len)
348{
349 int i;
350 crc ^= 0xffffffff;
351 while (len--) {
352 crc ^= *p++;
353 for (i = 0; i < 8; i++)
354 crc = (crc >> 1) ^ ((crc & 1) ? CRC_POLYNOMIAL : 0);
355 }
356 return crc ^ 0xffffffff;
357}
358
359/*!
360 * Reads one packet from the device, handles time out, locking and checking if
361 * the thread has been told to shut down.
362 */
363static bool
364pssense_read_one_packet(struct pssense_device *pssense, uint8_t *buffer, size_t size, bool check_size)
365{
366 os_thread_helper_lock(&pssense->controller_thread);
367
368 while (os_thread_helper_is_running_locked(&pssense->controller_thread)) {
369 os_thread_helper_unlock(&pssense->controller_thread);
370
371 int ret = os_hid_read(pssense->hid, buffer, size, 1000);
372
373 if (ret == 0) {
374 PSSENSE_DEBUG(pssense, "Timeout");
375
376 // Must lock thread before check in a while.
377 os_thread_helper_lock(&pssense->controller_thread);
378 continue;
379 }
380 if (ret < 0) {
381 PSSENSE_ERROR(pssense, "Failed to read device '%i'!", ret);
382 return false;
383 }
384 // Skip this check if we haven't flushed all the compat mode packets yet, since they're shorter.
385 if (check_size && ret != (int)size) {
386 PSSENSE_ERROR(pssense, "Unexpected HID packet size %i (expected %zu)", ret, size);
387 return false;
388 }
389
390 return true;
391 }
392
393 return false;
394}
395
396static bool
397pssense_parse_packet(struct pssense_device *pssense,
398 struct pssense_input_report *data,
399 struct pssense_input_state *input)
400{
401 if (data->report_id != INPUT_REPORT_ID) {
402 PSSENSE_WARN(pssense, "Unrecognized HID report id %u", data->report_id);
403 return false;
404 }
405
406 uint32_t expected_crc = pssense_i32_le_to_u32(&data->crc);
407 uint32_t crc = crc32_le(0, &INPUT_REPORT_CRC32_SEED, 1);
408 crc = crc32_le(crc, (uint8_t *)data, sizeof(struct pssense_input_report) - 4);
409 if (crc != expected_crc) {
410 PSSENSE_WARN(pssense, "CRC mismatch; skipping input. Expected %08X but got %08X", expected_crc, crc);
411 return false;
412 }
413
414 input->timestamp_ns = os_monotonic_get_ns();
415
416 uint32_t seq_no = pssense_i32_le_to_u32(&data->seq_no);
417 if (input->seq_no != 0 && seq_no != input->seq_no + 1) {
418 PSSENSE_WARN(pssense, "Missed seq no %u. Previous was %u", seq_no, input->seq_no);
419 }
420 input->seq_no = seq_no;
421
422 input->ps_click = (data->buttons[1] & 16) != 0;
423 input->squeeze_touch = (data->buttons[2] & 8) != 0;
424 input->squeeze_proximity = data->squeeze_proximity / 255.0f;
425 input->trigger_touch = (data->buttons[1] & 128) != 0;
426 input->trigger_value = data->trigger_value / 255.0f;
427 input->trigger_proximity = data->trigger_proximity / 255.0f;
428 input->thumbstick.x = (data->thumbstick_x - 128) / 128.0f;
429 input->thumbstick.y = (data->thumbstick_y - 128) / -128.0f;
430 input->thumbstick_touch = (data->buttons[2] & 4) != 0;
431
432 if (pssense->hand == PSSENSE_HAND_LEFT) {
433 input->share_click = (data->buttons[1] & 1) != 0;
434 input->square_click = (data->buttons[0] & 1) != 0;
435 input->square_touch = (data->buttons[2] & 2) != 0;
436 input->triangle_click = (data->buttons[0] & 8) != 0;
437 input->triangle_touch = (data->buttons[2] & 1) != 0;
438 input->squeeze_click = (data->buttons[0] & 16) != 0;
439 input->trigger_click = (data->buttons[0] & 64) != 0;
440 input->thumbstick_click = (data->buttons[1] & 4) != 0;
441 } else if (pssense->hand == PSSENSE_HAND_RIGHT) {
442 input->options_click = (data->buttons[1] & 2) != 0;
443 input->cross_click = (data->buttons[0] & 2) != 0;
444 input->cross_touch = (data->buttons[2] & 2) != 0;
445 input->circle_click = (data->buttons[0] & 4) != 0;
446 input->circle_touch = (data->buttons[2] & 1) != 0;
447 input->squeeze_click = (data->buttons[0] & 32) != 0;
448 input->trigger_click = (data->buttons[0] & 128) != 0;
449 input->thumbstick_click = (data->buttons[1] & 8) != 0;
450 }
451
452 uint32_t imu_ticks = pssense_i32_le_to_u32(&data->imu_ticks);
453 int64_t imu_ticks_delta = imu_ticks - input->imu_ticks_last;
454 if (imu_ticks_delta >= 0) {
455 input->imu_ticks_total += imu_ticks_delta;
456 input->imu_ticks_last = imu_ticks;
457
458 input->gyro_raw.x = pssense_i16_le_to_i16(&data->gyro[0]);
459 input->gyro_raw.y = pssense_i16_le_to_i16(&data->gyro[1]);
460 input->gyro_raw.z = pssense_i16_le_to_i16(&data->gyro[2]);
461
462 input->accel_raw.x = pssense_i16_le_to_i16(&data->accel[0]);
463 input->accel_raw.y = pssense_i16_le_to_i16(&data->accel[1]);
464 input->accel_raw.z = pssense_i16_le_to_i16(&data->accel[2]);
465 } else {
466 PSSENSE_WARN(pssense, "Time went backwards. Check your play area for black holes.");
467 }
468
469 uint8_t battery_state = data->battery_state >> 4;
470 // Charge values go from 0..10, so add 5% and cap at 100% so we never show 0% charge
471 float battery_percent = MIN(1.0f, (data->battery_state & 0xf) * .1f + .05);
472 bool valid, charging;
473 if (battery_state == CHARGE_STATE_DISCHARGING) {
474 valid = true;
475 charging = false;
476 } else if (battery_state == CHARGE_STATE_CHARGING) {
477 valid = true;
478 charging = true;
479 } else if (battery_state == CHARGE_STATE_FULL) {
480 valid = true;
481 charging = true;
482 battery_percent = 1.0f;
483 } else if (battery_state == CHARGE_STATE_ABNORMAL_VOLTAGE) {
484 valid = false;
485 PSSENSE_WARN(pssense, "Unable to determine charge state: abnormal voltage");
486 } else if (battery_state == CHARGE_STATE_ABNORMAL_TEMP) {
487 valid = false;
488 PSSENSE_WARN(pssense, "Unable to determine charge state: abnormal temp");
489 } else if (battery_state == CHARGE_STATE_CHARGING_ERROR) {
490 valid = false;
491 PSSENSE_WARN(pssense, "Unable to determine charge state: charging error");
492 } else {
493 valid = false;
494 PSSENSE_WARN(pssense, "Unable to determine charge state: unknown reason");
495 }
496
497 input->battery_state_valid = valid;
498 if (valid) {
499 if (charging != input->battery_charging || battery_percent != input->battery_charge_percent) {
500 PSSENSE_DEBUG(pssense, "Battery at %.f%%, %s", battery_percent * 100,
501 charging ? "charging" : "discharging");
502 }
503 input->battery_charging = charging;
504 input->battery_charge_percent = battery_percent;
505 }
506
507 return true;
508}
509
510static void
511pssense_update_fusion(struct pssense_device *pssense)
512{
513 struct xrt_vec3 gyro;
514 gyro.x = DEG_TO_RAD(pssense->state.gyro_raw.x * PSSENSE_GYRO_SCALE_DEG);
515 gyro.y = DEG_TO_RAD(pssense->state.gyro_raw.y * PSSENSE_GYRO_SCALE_DEG);
516 gyro.z = DEG_TO_RAD(pssense->state.gyro_raw.z * PSSENSE_GYRO_SCALE_DEG);
517
518 struct xrt_vec3 accel;
519 accel.x = pssense->state.accel_raw.x * PSSENSE_ACCEL_SCALE;
520 accel.y = pssense->state.accel_raw.y * PSSENSE_ACCEL_SCALE;
521 accel.z = pssense->state.accel_raw.z * PSSENSE_ACCEL_SCALE;
522
523 // TODO: Apply correction from calibration data
524
525 // Each IMU tick is .33μs
526 m_imu_3dof_update(&pssense->fusion, pssense->state.imu_ticks_total * 333, &accel, &gyro);
527 pssense->pose.orientation = pssense->fusion.rot;
528}
529
530static void
531pssense_send_output_report_locked(struct pssense_device *pssense)
532{
533 uint64_t timestamp_ns = os_monotonic_get_ns();
534
535 struct pssense_output_report report = {0};
536 report.report_id = OUTPUT_REPORT_ID;
537 report.bt_seq_no = pssense->output.next_seq_no << 4;
538 report.tag = OUTPUT_REPORT_TAG;
539
540 if (timestamp_ns >= pssense->output.vibration_end_timestamp_ns) {
541 pssense->output.vibration_amplitude = 0;
542 }
543
544 if (pssense->output.send_vibration) {
545 report.feedback_flags = pssense->output.vibration_mode | VIBRATE_ENABLE_BITS;
546 report.vibration_amplitude = pssense->output.vibration_amplitude;
547 pssense->output.send_vibration = pssense->output.vibration_amplitude > 0;
548 }
549
550 if (pssense->output.send_trigger_feedback) {
551 report.feedback_flags |= TRIGGER_FEEDBACK_ENABLE_BITS;
552 report.trigger_feedback_mode = pssense->output.trigger_feedback_mode;
553 pssense->output.send_trigger_feedback = false;
554 }
555
556 pssense->output.next_seq_no = (pssense->output.next_seq_no + 1) % 16;
557
558 uint32_t crc = crc32_le(0, &OUTPUT_REPORT_CRC32_SEED, 1);
559 crc = crc32_le(crc, (uint8_t *)&report, sizeof(struct pssense_output_report) - 4);
560 report.crc = pssense_u32_to_i32_le(crc);
561
562 PSSENSE_DEBUG(pssense, "Setting vibration amplitude: %u, mode: %02X, trigger feedback mode: %02X",
563 pssense->output.vibration_amplitude, pssense->output.vibration_mode,
564 pssense->output.trigger_feedback_mode);
565 int ret = os_hid_write(pssense->hid, (uint8_t *)&report, sizeof(struct pssense_output_report));
566 if (ret == sizeof(struct pssense_output_report)) {
567 // Controller will vibrate for 5 sec unless we resend the output report. Resend every 2 sec to be safe.
568 pssense->output.vibration_resend_timestamp_ns = timestamp_ns + 2000000000;
569 if (pssense->output.vibration_resend_timestamp_ns > pssense->output.vibration_end_timestamp_ns) {
570 pssense->output.vibration_resend_timestamp_ns = pssense->output.vibration_end_timestamp_ns;
571 }
572 } else {
573 PSSENSE_WARN(pssense, "Failed to send output report: %d", ret);
574 pssense->output.vibration_resend_timestamp_ns = timestamp_ns;
575 }
576}
577
578static void *
579pssense_run_thread(void *ptr)
580{
581 U_TRACE_SET_THREAD_NAME("PS Sense");
582
583 struct pssense_device *pssense = (struct pssense_device *)ptr;
584
585 union {
586 uint8_t buffer[sizeof(struct pssense_input_report)];
587 struct pssense_input_report report;
588 } data;
589 struct pssense_input_state input_state = {0};
590
591 // The Sense controller starts in compat mode with a different HID report ID and format.
592 // We need to discard packets until we get a correct report.
593 while (pssense_read_one_packet(pssense, data.buffer, sizeof(data), false) &&
594 data.report.report_id != INPUT_REPORT_ID) {
595 PSSENSE_DEBUG(pssense, "Discarding compat mode HID report");
596 }
597
598 while (pssense_read_one_packet(pssense, data.buffer, sizeof(data), true)) {
599 if (pssense_parse_packet(pssense, &data.report, &input_state)) {
600 os_mutex_lock(&pssense->lock);
601 pssense->state = input_state;
602 pssense_update_fusion(pssense);
603 if (pssense->output.send_vibration &&
604 pssense->state.timestamp_ns >= pssense->output.vibration_resend_timestamp_ns) {
605 pssense_send_output_report_locked(pssense);
606 }
607 os_mutex_unlock(&pssense->lock);
608 }
609 }
610
611 return NULL;
612}
613
614static void
615pssense_device_destroy(struct xrt_device *xdev)
616{
617 struct pssense_device *pssense = (struct pssense_device *)xdev;
618
619 // Destroy the thread object.
620 os_thread_helper_destroy(&pssense->controller_thread);
621
622 // Now that the thread is not running we can destroy the lock.
623 os_mutex_destroy(&pssense->lock);
624
625 m_imu_3dof_close(&pssense->fusion);
626
627 // Remove the variable tracking.
628 u_var_remove_root(pssense);
629
630 if (pssense->hid != NULL) {
631 os_hid_destroy(pssense->hid);
632 pssense->hid = NULL;
633 }
634
635 free(pssense);
636}
637
638static xrt_result_t
639pssense_device_update_inputs(struct xrt_device *xdev)
640{
641 struct pssense_device *pssense = (struct pssense_device *)xdev;
642
643 // Lock the data.
644 os_mutex_lock(&pssense->lock);
645
646 for (uint32_t i = 0; i < (uint32_t)sizeof(enum pssense_input_index); i++) {
647 pssense->base.inputs[i].timestamp = (int64_t)pssense->state.timestamp_ns;
648 }
649 pssense->base.inputs[PSSENSE_INDEX_PS_CLICK].value.boolean = pssense->state.ps_click;
650 pssense->base.inputs[PSSENSE_INDEX_SHARE_CLICK].value.boolean = pssense->state.share_click;
651 pssense->base.inputs[PSSENSE_INDEX_OPTIONS_CLICK].value.boolean = pssense->state.options_click;
652 pssense->base.inputs[PSSENSE_INDEX_SQUARE_CLICK].value.boolean = pssense->state.square_click;
653 pssense->base.inputs[PSSENSE_INDEX_SQUARE_TOUCH].value.boolean = pssense->state.square_touch;
654 pssense->base.inputs[PSSENSE_INDEX_TRIANGLE_CLICK].value.boolean = pssense->state.triangle_click;
655 pssense->base.inputs[PSSENSE_INDEX_TRIANGLE_TOUCH].value.boolean = pssense->state.triangle_touch;
656 pssense->base.inputs[PSSENSE_INDEX_CROSS_CLICK].value.boolean = pssense->state.cross_click;
657 pssense->base.inputs[PSSENSE_INDEX_CROSS_TOUCH].value.boolean = pssense->state.cross_touch;
658 pssense->base.inputs[PSSENSE_INDEX_CIRCLE_CLICK].value.boolean = pssense->state.circle_click;
659 pssense->base.inputs[PSSENSE_INDEX_CIRCLE_TOUCH].value.boolean = pssense->state.circle_touch;
660 pssense->base.inputs[PSSENSE_INDEX_SQUEEZE_CLICK].value.boolean = pssense->state.squeeze_click;
661 pssense->base.inputs[PSSENSE_INDEX_SQUEEZE_TOUCH].value.boolean = pssense->state.squeeze_touch;
662 pssense->base.inputs[PSSENSE_INDEX_SQUEEZE_PROXIMITY_FLOAT].value.vec1.x = pssense->state.squeeze_proximity;
663 pssense->base.inputs[PSSENSE_INDEX_TRIGGER_CLICK].value.boolean = pssense->state.trigger_click;
664 pssense->base.inputs[PSSENSE_INDEX_TRIGGER_TOUCH].value.boolean = pssense->state.trigger_touch;
665 pssense->base.inputs[PSSENSE_INDEX_TRIGGER_VALUE].value.vec1.x = pssense->state.trigger_value;
666 pssense->base.inputs[PSSENSE_INDEX_TRIGGER_PROXIMITY_FLOAT].value.vec1.x = pssense->state.trigger_proximity;
667 pssense->base.inputs[PSSENSE_INDEX_THUMBSTICK].value.vec2 = pssense->state.thumbstick;
668 pssense->base.inputs[PSSENSE_INDEX_THUMBSTICK_CLICK].value.boolean = pssense->state.thumbstick_click;
669 pssense->base.inputs[PSSENSE_INDEX_THUMBSTICK_TOUCH].value.boolean = pssense->state.thumbstick_touch;
670
671 // Done now.
672 os_mutex_unlock(&pssense->lock);
673
674 return XRT_SUCCESS;
675}
676
677static xrt_result_t
678pssense_set_output(struct xrt_device *xdev, enum xrt_output_name name, const struct xrt_output_value *value)
679{
680 struct pssense_device *pssense = (struct pssense_device *)xdev;
681
682 bool send_vibration = false;
683 uint8_t vibration_amplitude;
684 uint8_t vibration_mode;
685 bool send_trigger_feedback = false;
686 uint8_t trigger_feedback_mode;
687 if (name == XRT_OUTPUT_NAME_PSSENSE_VIBRATION) {
688 send_vibration = true;
689 vibration_amplitude = (uint8_t)(value->vibration.amplitude * 255.0f);
690 vibration_mode = VIBRATE_MODE_CLASSIC_RUMBLE;
691 if (value->vibration.frequency != XRT_FREQUENCY_UNSPECIFIED) {
692 if (value->vibration.frequency <= 70) {
693 vibration_mode = VIBRATE_MODE_LOW_60HZ;
694 } else if (value->vibration.frequency >= 110) {
695 vibration_mode = VIBRATE_MODE_HIGH_120HZ;
696 }
697 }
698 } else if (name == XRT_OUTPUT_NAME_PSSENSE_TRIGGER_FEEDBACK) {
699 for (uint64_t i = 0; i < value->force_feedback.force_feedback_location_count; i++) {
700 if (value->force_feedback.force_feedback[i].location ==
701 XRT_FORCE_FEEDBACK_LOCATION_LEFT_INDEX) {
702 send_trigger_feedback = true;
703 if (value->force_feedback.force_feedback[i].value > 0) {
704 trigger_feedback_mode = TRIGGER_FEEDBACK_MODE_CONSTANT;
705 } else {
706 trigger_feedback_mode = TRIGGER_FEEDBACK_MODE_NONE;
707 }
708 }
709 }
710 } else {
711 U_LOG_XDEV_UNSUPPORTED_OUTPUT(&pssense->base, pssense->log_level, name);
712 return XRT_ERROR_OUTPUT_UNSUPPORTED;
713 }
714
715 os_mutex_lock(&pssense->lock);
716 if (send_vibration && (vibration_amplitude != pssense->output.vibration_amplitude ||
717 vibration_mode != pssense->output.vibration_mode)) {
718 pssense->output.send_vibration = true;
719 pssense->output.vibration_amplitude = vibration_amplitude;
720 pssense->output.vibration_mode = vibration_mode;
721 pssense->output.vibration_end_timestamp_ns = os_monotonic_get_ns() + value->vibration.duration_ns;
722 }
723 if (send_trigger_feedback && trigger_feedback_mode != pssense->output.trigger_feedback_mode) {
724 pssense->output.send_trigger_feedback = true;
725 pssense->output.trigger_feedback_mode = trigger_feedback_mode;
726 }
727 if (pssense->output.send_vibration || pssense->output.send_trigger_feedback) {
728 pssense_send_output_report_locked(pssense);
729 }
730 os_mutex_unlock(&pssense->lock);
731
732 return XRT_SUCCESS;
733}
734
735static void
736pssense_get_fusion_pose(struct pssense_device *pssense,
737 enum xrt_input_name name,
738 int64_t at_timestamp_ns,
739 struct xrt_space_relation *out_relation)
740{
741 out_relation->pose = pssense->pose;
742 out_relation->linear_velocity.x = 0.0f;
743 out_relation->linear_velocity.y = 0.0f;
744 out_relation->linear_velocity.z = 0.0f;
745
746 /*!
747 * @todo This is hack, fusion reports angvel relative to the device but
748 * it needs to be in relation to the base space. Rotating it with the
749 * device orientation is enough to get it into the right space, angular
750 * velocity is a derivative so needs a special rotation.
751 */
752 math_quat_rotate_derivative(&pssense->pose.orientation, &pssense->fusion.last.gyro,
753 &out_relation->angular_velocity);
754
755 out_relation->relation_flags = (enum xrt_space_relation_flags)(
756 XRT_SPACE_RELATION_ORIENTATION_VALID_BIT | XRT_SPACE_RELATION_ORIENTATION_TRACKED_BIT |
757 XRT_SPACE_RELATION_ANGULAR_VELOCITY_VALID_BIT | XRT_SPACE_RELATION_LINEAR_VELOCITY_VALID_BIT);
758}
759
760static xrt_result_t
761pssense_get_tracked_pose(struct xrt_device *xdev,
762 enum xrt_input_name name,
763 int64_t at_timestamp_ns,
764 struct xrt_space_relation *out_relation)
765{
766 struct pssense_device *pssense = (struct pssense_device *)xdev;
767
768 if (name != XRT_INPUT_PSSENSE_AIM_POSE && name != XRT_INPUT_PSSENSE_GRIP_POSE) {
769 U_LOG_XDEV_UNSUPPORTED_INPUT(&pssense->base, pssense->log_level, name);
770 return XRT_ERROR_INPUT_UNSUPPORTED;
771 }
772
773 struct xrt_relation_chain xrc = {0};
774 struct xrt_pose pose_correction = {0};
775
776 // Rotate the grip/aim pose up by 60 degrees around the X axis
777 struct xrt_vec3 axis = {1.0, 0, 0};
778 math_quat_from_angle_vector(DEG_TO_RAD(60), &axis, &pose_correction.orientation);
779 m_relation_chain_push_pose(&xrc, &pose_correction);
780
781 struct xrt_space_relation *rel = m_relation_chain_reserve(&xrc);
782
783 os_mutex_lock(&pssense->lock);
784 pssense_get_fusion_pose(pssense, name, at_timestamp_ns, rel);
785 os_mutex_unlock(&pssense->lock);
786
787 m_relation_chain_resolve(&xrc, out_relation);
788
789 return XRT_SUCCESS;
790}
791
792static xrt_result_t
793pssense_get_battery_status(struct xrt_device *xdev, bool *out_present, bool *out_charging, float *out_charge)
794{
795 struct pssense_device *pssense = (struct pssense_device *)xdev;
796 if (!pssense->state.battery_state_valid) {
797 *out_present = false;
798 return XRT_SUCCESS;
799 }
800
801 *out_present = true;
802 *out_charging = pssense->state.battery_charging;
803 *out_charge = pssense->state.battery_charge_percent;
804 return XRT_SUCCESS;
805}
806
807/**
808 * Retrieving the calibration data report will switch the Sense controller from compat mode into full mode.
809 */
810bool
811pssense_get_calibration_data(struct pssense_device *pssense)
812{
813 int ret;
814 uint8_t buffer[sizeof(struct pssense_feature_report)];
815 uint8_t data[CALIBRATION_DATA_LENGTH] = {0};
816 bool invalid_crc;
817 do {
818 invalid_crc = false;
819 for (int i = 0; i < 2; i++) {
820 ret = os_hid_get_feature(pssense->hid, CALIBRATION_DATA_FEATURE_REPORT_ID, buffer,
821 sizeof(buffer));
822 if (ret < 0) {
823 PSSENSE_ERROR(pssense, "Failed to retrieve calibration report: %d", ret);
824 return false;
825 }
826 if (ret != sizeof(buffer)) {
827 PSSENSE_ERROR(pssense, "Invalid byte count transferred, expected %zu got %d",
828 sizeof(buffer), ret);
829 return false;
830 }
831 struct pssense_feature_report *report = (struct pssense_feature_report *)buffer;
832 if (report->part_id == CALIBRATION_DATA_PART_ID_1) {
833 memcpy(data, report->data, sizeof(report->data));
834 } else if (report->part_id == CALIBRATION_DATA_PART_ID_2) {
835 memcpy(data + sizeof(report->data), report->data, sizeof(report->data));
836 } else {
837 PSSENSE_ERROR(pssense, "Unknown calibration data part ID %u", report->part_id);
838 return false;
839 }
840
841 uint32_t crc = crc32_le(0, &FEATURE_REPORT_CRC32_SEED, 1);
842 crc = crc32_le(crc, (uint8_t *)&buffer, sizeof(buffer) - 4);
843 uint32_t expected_crc = pssense_i32_le_to_u32(&report->crc);
844 if (crc != expected_crc) {
845 PSSENSE_WARN(pssense, "Invalid feature report CRC. Expected 0x%08X, actual 0x%08X",
846 expected_crc, crc);
847 invalid_crc = true;
848 }
849 }
850 } while (invalid_crc);
851
852 // TODO: Parse calibration data into prefiler
853
854 return true;
855}
856
857#define SET_INPUT(NAME) (pssense->base.inputs[PSSENSE_INDEX_##NAME].name = XRT_INPUT_PSSENSE_##NAME)
858
859int
860pssense_found(struct xrt_prober *xp,
861 struct xrt_prober_device **devices,
862 size_t device_count,
863 size_t index,
864 cJSON *attached_data,
865 struct xrt_device **out_xdevs)
866{
867 struct os_hid_device *hid = NULL;
868 int ret;
869
870 ret = xrt_prober_open_hid_interface(xp, devices[index], 0, &hid);
871 if (ret != 0) {
872 return -1;
873 }
874
875 unsigned char product_name[128];
876 ret = xrt_prober_get_string_descriptor( //
877 xp, //
878 devices[index], //
879 XRT_PROBER_STRING_PRODUCT, //
880 product_name, //
881 sizeof(product_name)); //
882 if (ret <= 0) {
883 U_LOG_E("Failed to get product name from Bluetooth device!");
884 return -1;
885 }
886
887 enum u_device_alloc_flags flags = U_DEVICE_ALLOC_TRACKING_NONE;
888 struct pssense_device *pssense = U_DEVICE_ALLOCATE(struct pssense_device, flags, 23, 2);
889 PSSENSE_DEBUG(pssense, "PlayStation Sense controller found");
890
891 pssense->base.name = XRT_DEVICE_PSSENSE;
892 snprintf(pssense->base.str, XRT_DEVICE_NAME_LEN, "%s", product_name);
893 pssense->base.update_inputs = pssense_device_update_inputs;
894 pssense->base.set_output = pssense_set_output;
895 pssense->base.get_tracked_pose = pssense_get_tracked_pose;
896 pssense->base.get_battery_status = pssense_get_battery_status;
897 pssense->base.destroy = pssense_device_destroy;
898 pssense->base.supported.orientation_tracking = true;
899 pssense->base.supported.battery_status = true;
900
901 pssense->base.binding_profiles = binding_profiles_pssense;
902 pssense->base.binding_profile_count = ARRAY_SIZE(binding_profiles_pssense);
903
904 m_imu_3dof_init(&pssense->fusion, M_IMU_3DOF_USE_GRAVITY_DUR_20MS);
905
906 pssense->log_level = debug_get_log_option_pssense_log();
907 pssense->hid = hid;
908
909 if (devices[index]->product_id == PSSENSE_PID_LEFT) {
910 pssense->base.device_type = XRT_DEVICE_TYPE_LEFT_HAND_CONTROLLER;
911 pssense->hand = PSSENSE_HAND_LEFT;
912 } else if (devices[index]->product_id == PSSENSE_PID_RIGHT) {
913 pssense->base.device_type = XRT_DEVICE_TYPE_RIGHT_HAND_CONTROLLER;
914 pssense->hand = PSSENSE_HAND_RIGHT;
915 } else {
916 PSSENSE_ERROR(pssense, "Unable to determine controller type");
917 pssense_device_destroy(&pssense->base);
918 return -1;
919 }
920
921 SET_INPUT(PS_CLICK);
922 SET_INPUT(SHARE_CLICK);
923 SET_INPUT(OPTIONS_CLICK);
924 SET_INPUT(SQUARE_CLICK);
925 SET_INPUT(SQUARE_TOUCH);
926 SET_INPUT(TRIANGLE_CLICK);
927 SET_INPUT(TRIANGLE_TOUCH);
928 SET_INPUT(CROSS_CLICK);
929 SET_INPUT(CROSS_TOUCH);
930 SET_INPUT(CIRCLE_CLICK);
931 SET_INPUT(CIRCLE_TOUCH);
932 SET_INPUT(SQUEEZE_CLICK);
933 SET_INPUT(SQUEEZE_TOUCH);
934 SET_INPUT(SQUEEZE_PROXIMITY_FLOAT);
935 SET_INPUT(TRIGGER_CLICK);
936 SET_INPUT(TRIGGER_TOUCH);
937 SET_INPUT(TRIGGER_VALUE);
938 SET_INPUT(TRIGGER_PROXIMITY_FLOAT);
939 SET_INPUT(THUMBSTICK);
940 SET_INPUT(THUMBSTICK_CLICK);
941 SET_INPUT(THUMBSTICK_TOUCH);
942 SET_INPUT(GRIP_POSE);
943 SET_INPUT(AIM_POSE);
944
945 pssense->base.outputs[0].name = XRT_OUTPUT_NAME_PSSENSE_VIBRATION;
946 pssense->base.outputs[1].name = XRT_OUTPUT_NAME_PSSENSE_TRIGGER_FEEDBACK;
947
948 ret = os_mutex_init(&pssense->lock);
949 if (ret != 0) {
950 PSSENSE_ERROR(pssense, "Failed to init mutex!");
951 pssense_device_destroy(&pssense->base);
952 return -1;
953 }
954
955 ret = os_thread_helper_init(&pssense->controller_thread);
956 if (ret != 0) {
957 PSSENSE_ERROR(pssense, "Failed to init threading!");
958 pssense_device_destroy(&pssense->base);
959 return -1;
960 }
961
962 ret = os_thread_helper_start(&pssense->controller_thread, pssense_run_thread, pssense);
963 if (ret != 0) {
964 PSSENSE_ERROR(pssense, "Failed to start thread!");
965 pssense_device_destroy(&pssense->base);
966 return -1;
967 }
968
969 if (!pssense_get_calibration_data(pssense)) {
970 PSSENSE_ERROR(pssense, "Failed to retrieve calibration data");
971 pssense_device_destroy(&pssense->base);
972 return -1;
973 }
974
975 u_var_add_root(pssense, pssense->base.str, false);
976 u_var_add_log_level(pssense, &pssense->log_level, "Log level");
977
978 u_var_add_gui_header(pssense, &pssense->gui.button_states, "Button States");
979 u_var_add_bool(pssense, &pssense->state.ps_click, "PS Click");
980 if (pssense->hand == PSSENSE_HAND_LEFT) {
981 u_var_add_bool(pssense, &pssense->state.share_click, "Share Click");
982 u_var_add_bool(pssense, &pssense->state.square_click, "Square Click");
983 u_var_add_bool(pssense, &pssense->state.square_touch, "Square Touch");
984 u_var_add_bool(pssense, &pssense->state.triangle_click, "Triangle Click");
985 u_var_add_bool(pssense, &pssense->state.triangle_touch, "Triangle Touch");
986 } else if (pssense->hand == PSSENSE_HAND_RIGHT) {
987 u_var_add_bool(pssense, &pssense->state.options_click, "Options Click");
988 u_var_add_bool(pssense, &pssense->state.cross_click, "Cross Click");
989 u_var_add_bool(pssense, &pssense->state.cross_touch, "Cross Touch");
990 u_var_add_bool(pssense, &pssense->state.circle_click, "Circle Click");
991 u_var_add_bool(pssense, &pssense->state.circle_touch, "Circle Touch");
992 }
993 u_var_add_bool(pssense, &pssense->state.squeeze_click, "Squeeze Click");
994 u_var_add_bool(pssense, &pssense->state.squeeze_touch, "Squeeze Touch");
995 u_var_add_ro_f32(pssense, &pssense->state.squeeze_proximity, "Squeeze Proximity");
996 u_var_add_bool(pssense, &pssense->state.trigger_click, "Trigger Click");
997 u_var_add_bool(pssense, &pssense->state.trigger_touch, "Trigger Touch");
998 u_var_add_ro_f32(pssense, &pssense->state.trigger_value, "Trigger");
999 u_var_add_ro_f32(pssense, &pssense->state.trigger_proximity, "Trigger Proximity");
1000 u_var_add_ro_f32(pssense, &pssense->state.thumbstick.x, "Thumbstick X");
1001 u_var_add_ro_f32(pssense, &pssense->state.thumbstick.y, "Thumbstick Y");
1002 u_var_add_bool(pssense, &pssense->state.thumbstick_click, "Thumbstick Click");
1003 u_var_add_bool(pssense, &pssense->state.thumbstick_touch, "Thumbstick Touch");
1004
1005 u_var_add_gui_header(pssense, &pssense->gui.tracking, "Tracking");
1006 u_var_add_ro_vec3_i32(pssense, &pssense->state.gyro_raw, "Raw Gyro");
1007 u_var_add_ro_vec3_i32(pssense, &pssense->state.accel_raw, "Raw Accel");
1008 u_var_add_pose(pssense, &pssense->pose, "Pose");
1009
1010 out_xdevs[0] = &pssense->base;
1011 return 1;
1012}
1013
1014/*!
1015 * @}
1016 */