matrixfurry.com
The open source OpenXR runtime
1// Copyright 2016, Joey Ferwerda.
2// Copyright 2019-2022, Collabora, Ltd.
3// SPDX-License-Identifier: BSL-1.0
4/*!
5 * @file
6 * @brief PSVR device implementation, imported from OpenHMD.
7 * @author Joey Ferwerda <joeyferweda@gmail.com>
8 * @author Philipp Zabel <philipp.zabel@gmail.com>
9 * @author Jakob Bornecrantz <jakob@collabora.com>
10 * @ingroup drv_psvr
11 */
12
13#include "xrt/xrt_compiler.h"
14#include "xrt/xrt_tracking.h"
15
16#include "os/os_time.h"
17#include "os/os_threading.h"
18
19#include "math/m_api.h"
20
21#include "util/u_var.h"
22#include "util/u_misc.h"
23#include "util/u_time.h"
24#include "util/u_debug.h"
25#include "util/u_device.h"
26#include "util/u_trace_marker.h"
27#include "util/u_distortion_mesh.h"
28
29#include "math/m_imu_3dof.h"
30
31#include "math/m_mathinclude.h"
32
33#include <stdio.h>
34#include <stdlib.h>
35#include <string.h>
36
37#include "psvr_device.h"
38
39
40/*
41 *
42 * Structs and defines.
43 *
44 */
45
46DEBUG_GET_ONCE_BOOL_OPTION(psvr_disco, "PSVR_DISCO", false)
47#define PSVR_DEBUG(p, ...) U_LOG_XDEV_IFL_D(&p->base, p->log_level, __VA_ARGS__)
48#define PSVR_ERROR(p, ...) U_LOG_XDEV_IFL_E(&p->base, p->log_level, __VA_ARGS__)
49
50#define FEATURE_BUFFER_SIZE 256
51
52/*!
53 * Private struct for the @ref drv_psvr device.
54 *
55 * @ingroup drv_psvr
56 * @implements xrt_device
57 */
58struct psvr_device
59{
60 struct xrt_device base;
61
62 //! Owned by the @ref oth thread.
63 hid_device *hid_sensor;
64
65 //! Owned and protected by the device_mutex.
66 hid_device *hid_control;
67 struct os_mutex device_mutex;
68
69 //! Used to read sensor packets.
70 struct os_thread_helper oth;
71
72 struct xrt_tracked_psvr *tracker;
73
74 //! Only touched from the sensor thread.
75 timepoint_ns last_sensor_time;
76
77 struct psvr_parsed_sensor last;
78
79 struct
80 {
81 uint8_t leds[9];
82 } wants;
83
84 struct
85 {
86 uint8_t leds[9];
87 } state;
88
89 struct
90 {
91 struct xrt_vec3 gyro;
92 struct xrt_vec3 accel;
93 } read;
94
95 uint16_t buttons;
96
97 bool powered_on;
98 bool in_vr_mode;
99
100 enum u_logging_level log_level;
101
102 struct
103 {
104 union {
105 uint8_t data[290];
106 struct
107 {
108 uint32_t _pad0[4];
109 struct xrt_vec3 unknown0;
110 uint32_t _zero0;
111 uint32_t _pad2_vec3_zero[4];
112 uint32_t _pad3_vec3_zero[4];
113 uint32_t _pad4_vec3_zero[4];
114 struct xrt_vec3 accel_pos_y;
115 uint32_t _pad5[1];
116 struct xrt_vec3 accel_neg_x;
117 uint32_t _pad6[1];
118 struct xrt_vec3 accel_neg_y;
119 uint32_t _pad7[1];
120 struct xrt_vec3 accel_pos_x;
121 uint32_t _pad8[1];
122 struct xrt_vec3 accel_pos_z;
123 uint32_t _pad9[1];
124 struct xrt_vec3 accel_neg_z;
125 uint32_t _pad10[1];
126 struct xrt_vec3 gyro_neg_y;
127 uint32_t _pad11[1];
128 struct xrt_vec3 gyro_pos_x;
129 uint32_t _pad12[1];
130 struct xrt_vec3 gyro_neg_z;
131 uint32_t _pad13[1];
132 };
133 };
134 int last_packet;
135 } calibration;
136
137
138 struct
139 {
140 bool last_frame;
141 bool control;
142 } gui;
143
144#if 1
145 struct m_imu_3dof fusion;
146#else
147 struct
148 {
149 struct xrt_quat rot;
150 } fusion;
151#endif
152
153 //! For compute_distortion
154 struct u_panotools_values vals;
155};
156
157
158// Alternative way to turn on all of the leds.
159XRT_MAYBE_UNUSED static const unsigned char psvr_tracking_on[12] = {
160 0x11, 0x00, 0xaa, 0x08, 0x00, 0xff, 0xff, 0xff, 0x00, 0x00, 0x00, 0x00,
161};
162
163
164#define PSVR_LED_POWER_OFF ((uint8_t)0x00)
165#define PSVR_LED_POWER_MAX ((uint8_t)0xff)
166
167#define PSVR_LED_POWER_WIRE_OFF ((uint8_t)0)
168#define PSVR_LED_POWER_WIRE_MAX ((uint8_t)100)
169
170
171enum psvr_leds
172{
173 PSVR_LED_A = (1 << 0),
174 PSVR_LED_B = (1 << 1),
175 PSVR_LED_C = (1 << 2),
176 PSVR_LED_D = (1 << 3),
177 PSVR_LED_E = (1 << 4),
178 PSVR_LED_F = (1 << 5),
179 PSVR_LED_G = (1 << 6),
180 PSVR_LED_H = (1 << 7),
181 PSVR_LED_I = (1 << 8),
182
183 PSVR_LED_FRONT = PSVR_LED_A | PSVR_LED_B | PSVR_LED_C | PSVR_LED_D | PSVR_LED_E | PSVR_LED_F | PSVR_LED_G,
184
185 PSVR_LED_BACK = PSVR_LED_H | PSVR_LED_I,
186
187 PSVR_LED_ALL = PSVR_LED_FRONT | PSVR_LED_BACK,
188};
189
190
191/*
192 *
193 * Helpers and internal functions.
194 *
195 */
196
197static inline struct psvr_device *
198psvr_device(struct xrt_device *p)
199{
200 return (struct psvr_device *)p;
201}
202
203static int
204open_hid(struct psvr_device *p, struct hid_device_info *dev_info, hid_device **out_dev)
205{
206 hid_device *dev = NULL;
207 int ret;
208
209 dev = hid_open_path(dev_info->path);
210 if (dev == NULL) {
211 PSVR_ERROR(p, "Failed to open '%s'", dev_info->path);
212 return -1;
213 }
214
215 ret = hid_set_nonblocking(dev, 1);
216 if (ret != 0) {
217 PSVR_ERROR(p, "Failed to set non-blocking on device");
218 hid_close(dev);
219 return -1;
220 }
221
222 *out_dev = dev;
223 return 0;
224}
225
226static int
227send_to_control(struct psvr_device *psvr, const uint8_t *data, size_t size)
228{
229 return hid_write(psvr->hid_control, data, size);
230}
231
232static int
233send_request_data(struct psvr_device *psvr, uint8_t id, uint8_t num)
234{
235 const uint8_t data[12] = {
236 0x81, 0x00, 0xaa, 0x08, id, num, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
237 };
238 return send_to_control(psvr, data, sizeof(data));
239}
240
241static uint8_t
242scale_led_power(uint8_t power)
243{
244 return (uint8_t)((power / 255.0f) * 100.0f);
245}
246
247
248/*
249 *
250 * Sensor functions.
251 *
252 */
253
254static void
255read_sample_and_apply_calibration(struct psvr_device *psvr,
256 struct psvr_parsed_sample *sample,
257 struct xrt_vec3 *out_accel,
258 struct xrt_vec3 *out_gyro)
259{
260 const struct xrt_vec3_i32 raw_accel = sample->accel;
261 const struct xrt_vec3_i32 raw_gyro = sample->gyro;
262
263 // Convert so that for a perfect IMU 1.0 is one G.
264 struct xrt_vec3 accel = {
265 raw_accel.x / 16384.0f,
266 raw_accel.y / 16384.0f,
267 raw_accel.z / 16384.0f,
268 };
269
270 // What unit is this?
271 struct xrt_vec3 gyro = {
272 raw_gyro.x * 0.00105f,
273 raw_gyro.y * 0.00105f,
274 raw_gyro.z * 0.00105f,
275 };
276
277 float ax = 2.0f / (psvr->calibration.accel_pos_x.x - psvr->calibration.accel_neg_x.x);
278 float ay = 2.0f / (psvr->calibration.accel_pos_y.y - psvr->calibration.accel_neg_y.y);
279 float az = 2.0f / (psvr->calibration.accel_pos_z.z - psvr->calibration.accel_neg_z.z);
280
281 float ox = (psvr->calibration.accel_pos_x.x + psvr->calibration.accel_neg_x.x) / 2.0f;
282 float oy = (psvr->calibration.accel_pos_y.y + psvr->calibration.accel_neg_y.y) / 2.0f;
283 float oz = (psvr->calibration.accel_pos_z.z + psvr->calibration.accel_neg_z.z) / 2.0f;
284
285 accel.x -= ox;
286 accel.y -= oy;
287 accel.z -= oz;
288 accel.x *= ax;
289 accel.y *= ay;
290 accel.z *= az;
291
292 // Go from Gs to m/s2 and flip the Z-axis.
293 accel.x *= (float)+MATH_GRAVITY_M_S2;
294 accel.y *= (float)+MATH_GRAVITY_M_S2;
295 accel.z *= (float)-MATH_GRAVITY_M_S2;
296
297 // Flip the Z-axis.
298 gyro.x *= +1.0;
299 gyro.y *= +1.0;
300 gyro.z *= -1.0;
301
302 *out_accel = accel;
303 *out_gyro = gyro;
304}
305
306static void
307update_fusion_locked(struct psvr_device *psvr, struct psvr_parsed_sample *sample, uint64_t timestamp_ns)
308{
309 struct xrt_vec3 mag = {0.0f, 0.0f, 0.0f};
310 (void)mag;
311
312 read_sample_and_apply_calibration(psvr, sample, &psvr->read.accel, &psvr->read.gyro);
313
314 if (psvr->tracker != NULL) {
315 struct xrt_tracking_sample sample;
316 sample.accel_m_s2 = psvr->read.accel;
317 sample.gyro_rad_secs = psvr->read.gyro;
318
319 xrt_tracked_psvr_push_imu(psvr->tracker, timestamp_ns, &sample);
320 } else {
321 m_imu_3dof_update(&psvr->fusion, timestamp_ns, &psvr->read.accel, &psvr->read.gyro);
322 }
323}
324
325static void
326update_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);
331}
332
333static uint32_t
334calc_delta_and_handle_rollover(uint32_t next, uint32_t last)
335{
336 uint32_t tick_delta = next - last;
337
338 // The 24-bit tick counter has rolled over,
339 // adjust the "negative" value to be positive.
340 if (tick_delta > 0xffffff) {
341 tick_delta += 0x1000000;
342 }
343
344 return tick_delta;
345}
346
347static timepoint_ns
348ensure_forward_progress_timestamps(struct psvr_device *psvr, timepoint_ns timestamp_ns)
349{
350 timepoint_ns t = timestamp_ns;
351
352 /*
353 * This make sure the timestamp is after the last we sent to the fusion,
354 * but it effectively drops the sample.
355 */
356 if (psvr->last_sensor_time > t) {
357 t = psvr->last_sensor_time + 1;
358 }
359
360 psvr->last_sensor_time = t;
361 return t;
362}
363
364static void
365handle_tracker_sensor_msg(struct psvr_device *psvr, unsigned char *buffer, int size)
366{
367 timepoint_ns now_ns = os_monotonic_get_ns();
368 uint32_t last_sample_tick = psvr->last.samples[1].tick;
369
370 if (!psvr_parse_sensor_packet(&psvr->last, buffer, size)) {
371 PSVR_ERROR(psvr, "couldn't decode tracker sensor message");
372 }
373
374 struct psvr_parsed_sensor *s = &psvr->last;
375
376 // Simplest is the buttons.
377 psvr->buttons = s->buttons;
378
379 uint32_t tick_delta = 500;
380
381 // Startup correction, ignore last_sample_tick if zero.
382 if (last_sample_tick > 0) {
383 tick_delta = calc_delta_and_handle_rollover(s->samples[0].tick, last_sample_tick);
384
385 // The PSVR device can buffer sensor data from previous
386 // sessions which we can get at the start of new sessions.
387 // @todo Maybe just skip the first 10 sensor packets?
388 // @todo Maybe reset sensor fusion?
389 if (tick_delta < 400 || tick_delta > 600) {
390 PSVR_DEBUG(psvr, "tick_delta = %u", tick_delta);
391 tick_delta = 500;
392 }
393 }
394
395 // New delta between the two samples.
396 uint32_t tick_delta2 = calc_delta_and_handle_rollover(s->samples[1].tick, s->samples[0].tick);
397
398 time_duration_ns inter_sample_duration_ns = tick_delta2 * PSVR_NS_PER_TICK;
399
400 // If this is larger then one second something bad is going on.
401 assert(inter_sample_duration_ns < U_TIME_1S_IN_NS);
402
403 // Move it back in time.
404 timepoint_ns timestamp_ns = (uint64_t)now_ns - (uint64_t)inter_sample_duration_ns;
405
406 // Make sure timestamps are always after a previous timestamp.
407 timestamp_ns = ensure_forward_progress_timestamps(psvr, timestamp_ns);
408
409 // Update the fusion with first sample.
410 update_fusion(psvr, &s->samples[0], timestamp_ns);
411
412 // Make sure timestamps are always after a previous timestamp.
413 timestamp_ns = ensure_forward_progress_timestamps(psvr, now_ns);
414
415 // Update the fusion with second sample.
416 update_fusion(psvr, &s->samples[1], timestamp_ns);
417}
418
419static void
420sensor_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
429static int
430sensor_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
445static void *
446sensor_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
478static void
479handle_control_status_msg(struct psvr_device *psvr, unsigned char *buffer, int size)
480{
481 struct psvr_parsed_status status = {0};
482
483 if (!psvr_parse_status_packet(&status, buffer, size)) {
484 PSVR_ERROR(psvr, "couldn't decode tracker sensor message");
485 }
486
487
488 /*
489 * Power
490 */
491
492 if (status.status & PSVR_STATUS_BIT_POWER) {
493 if (!psvr->powered_on) {
494 PSVR_DEBUG(psvr, "Device powered on! '%02x'", status.status);
495 }
496 psvr->powered_on = true;
497 } else {
498 if (psvr->powered_on) {
499 PSVR_DEBUG(psvr, "Device powered off! '%02x'", status.status);
500 }
501 psvr->powered_on = false;
502 }
503
504
505 /*
506 * VR-Mode
507 */
508
509 if (status.vr_mode == PSVR_STATUS_VR_MODE_OFF) {
510 if (psvr->in_vr_mode) {
511 PSVR_DEBUG(psvr, "Device not in vr-mode! '%02x'", status.vr_mode);
512 }
513 psvr->in_vr_mode = false;
514 } else if (status.vr_mode == PSVR_STATUS_VR_MODE_ON) {
515 if (!psvr->in_vr_mode) {
516 PSVR_DEBUG(psvr, "Device in vr-mode! '%02x'", status.vr_mode);
517 }
518 psvr->in_vr_mode = true;
519 } else {
520 PSVR_ERROR(psvr, "Unknown vr_mode status!");
521 }
522}
523
524static void
525handle_device_name_msg(struct psvr_device *psvr, unsigned char *buffer, int size)
526{
527 //! @todo Get the name here.
528}
529
530static void
531handle_calibration_msg(struct psvr_device *psvr, const unsigned char *buffer, size_t size)
532{
533 const size_t data_start = 6;
534 const size_t data_length = 58;
535 const size_t packet_length = data_start + data_length;
536
537 if (size != packet_length) {
538 PSVR_ERROR(psvr, "invalid calibration packet length");
539 return;
540 }
541
542 uint16_t which = buffer[1];
543 size_t dst = data_length * which;
544 for (size_t src = data_start; src < size; src++, dst++) {
545 psvr->calibration.data[dst] = buffer[src];
546 }
547
548 psvr->calibration.last_packet = which;
549}
550
551static void
552handle_control_0x82(struct psvr_device *psvr, unsigned char *buffer, int size)
553{
554 if (size < 4) {
555 return;
556 }
557
558 if (size < (int)sizeof(float) * 6) {
559 PSVR_DEBUG(psvr, "%02x %02x %02x %02x", buffer[0], buffer[1], buffer[2], buffer[3]);
560 }
561
562 float *f = (float *)buffer;
563 int *i = (int *)buffer;
564
565 PSVR_DEBUG(psvr,
566 "%02x %02x %02x %02x\n"
567 "%+f %+f %+f %+f %+f\n"
568 "0x%08x 0x%08x 0x%08x 0x%08x 0x%08x\n"
569 "% 10i % 10i % 10i % 10i % 10i",
570 buffer[0], buffer[1], buffer[2], buffer[3], f[1], f[2], f[3], f[4], f[5], i[1], i[2], i[3], i[4],
571 i[5], i[1], i[2], i[3], i[4], i[5]);
572}
573
574static void
575handle_control_0xA0(struct psvr_device *psvr, unsigned char *buffer, int size)
576{
577 if (size < 4) {
578 return;
579 }
580
581 PSVR_DEBUG(psvr, "%02x %02x %02x %02x", buffer[0], buffer[1], buffer[2], buffer[3]);
582}
583
584static int
585read_control_packets(struct psvr_device *psvr)
586{
587 uint8_t buffer[FEATURE_BUFFER_SIZE];
588 int size = 0;
589
590 do {
591 size = hid_read(psvr->hid_control, buffer, FEATURE_BUFFER_SIZE);
592 if (size == 0) {
593 return 0;
594 }
595 if (size < 0) {
596 return -1;
597 }
598
599 if (buffer[0] == PSVR_PKG_STATUS) {
600 handle_control_status_msg(psvr, buffer, size);
601 } else if (buffer[0] == PSVR_PKG_DEVICE_NAME) {
602 handle_device_name_msg(psvr, buffer, size);
603 } else if (buffer[0] == PSVR_PKG_CALIBRATION) {
604 handle_calibration_msg(psvr, buffer, size);
605 } else if (buffer[0] == PSVR_PKG_0x82) {
606 handle_control_0x82(psvr, buffer, size);
607 } else if (buffer[0] == PSVR_PKG_0xA0) {
608 handle_control_0xA0(psvr, buffer, size);
609 } else {
610 PSVR_ERROR(psvr, "Got report, 0x%02x", buffer[0]);
611 }
612
613 } while (true);
614}
615
616
617/*!
618 * Get the device name data and calibration data, see link below for info.
619 *
620 * https://github.com/gusmanb/PSVRFramework/wiki/Report-0x81-Device-ID-and-Calibration
621 */
622static int
623read_calibration_data(struct psvr_device *psvr)
624{
625 // Request the device name.
626 int ret = send_request_data(psvr, PSVR_GET_DATA_ID_DEVICE_NAME, 0);
627 if (ret < 0) {
628 return ret;
629 }
630
631 // Request unknown data 0x82.
632 ret = send_request_data(psvr, PSVR_GET_DATA_ID_0x82, 0);
633 if (ret < 0) {
634 return ret;
635 }
636
637 // There are 5 pages of PSVR calibration data.
638 for (int i = 0; i < 5; i++) {
639 // Request the IMU calibration data.
640 ret = send_request_data(psvr, PSVR_GET_DATA_ID_CALIBRATION, i);
641 if (ret < 0) {
642 return ret;
643 }
644 }
645
646 // We have requested 5 pages worth of data, wait for the replies.
647 for (int i = 0; i < 100; i++) {
648 os_nanosleep(1000 * 1000);
649 read_control_packets(psvr);
650
651 // If we have gotten of the packets stop wait.
652 if (psvr->calibration.last_packet == 4) {
653 break;
654 }
655 }
656
657 // Did we really get all of the data?
658 if (psvr->calibration.last_packet != 4) {
659 PSVR_ERROR(psvr, "Failed to get calibration");
660 return -1;
661 }
662
663 PSVR_DEBUG(psvr,
664 "calibration.accel_pos_x: %f %f %f\n"
665 "calibration.accel_neg_x: %f %f %f\n"
666 "calibration.accel_pos_y: %f %f %f\n"
667 "calibration.accel_neg_y: %f %f %f\n"
668 "calibration.accel_pos_z: %f %f %f\n"
669 "calibration.accel_neg_z: %f %f %f\n",
670 psvr->calibration.accel_pos_x.x, psvr->calibration.accel_pos_x.y, psvr->calibration.accel_pos_x.z,
671 psvr->calibration.accel_neg_x.x, psvr->calibration.accel_neg_x.y, psvr->calibration.accel_neg_x.z,
672 psvr->calibration.accel_pos_y.x, psvr->calibration.accel_pos_y.y, psvr->calibration.accel_pos_y.z,
673 psvr->calibration.accel_neg_y.x, psvr->calibration.accel_neg_y.y, psvr->calibration.accel_neg_y.z,
674 psvr->calibration.accel_pos_z.x, psvr->calibration.accel_pos_z.y, psvr->calibration.accel_pos_z.z,
675 psvr->calibration.accel_neg_z.x, psvr->calibration.accel_neg_z.y, psvr->calibration.accel_neg_z.z);
676
677#if 0
678 for (size_t i = 0; i < sizeof(psvr->calibration.data); i++) {
679 fprintf(stderr, "%02x ", psvr->calibration.data[i]);
680 }
681 fprintf(stderr, "\n");
682
683 int *data = (int*)&psvr->calibration.data[0];
684 for (size_t i = 0; i < (sizeof(psvr->calibration.data) / 4); i++) {
685 int v = data[i];
686 U_LOG_E("%i %f", v, *(float*)&v);
687 }
688#endif
689
690 // Jobs done!
691 // - Orc peon.
692 return 0;
693}
694
695
696/*
697 *
698 * Control sending functions.
699 *
700 */
701
702static int
703wait_for_power(struct psvr_device *psvr, bool on)
704{
705 for (int i = 0; i < 5000; i++) {
706 read_control_packets(psvr);
707
708 if (psvr->powered_on == on) {
709 return 0;
710 }
711
712 os_nanosleep(1000 * 1000);
713 }
714
715 return -1;
716}
717
718static int
719wait_for_vr_mode(struct psvr_device *psvr, bool on)
720{
721 for (int i = 0; i < 5000; i++) {
722 read_control_packets(psvr);
723
724 if (psvr->in_vr_mode == on) {
725 return 0;
726 }
727
728 os_nanosleep(1000 * 1000);
729 }
730
731 return -1;
732}
733
734static int
735control_power_and_wait(struct psvr_device *psvr, bool on)
736{
737 const char *status = on ? "on" : "off";
738 const uint8_t data[8] = {
739 0x17, 0x00, 0xaa, 0x04, on, 0x00, 0x00, 0x00,
740 };
741
742 int ret = send_to_control(psvr, data, sizeof(data));
743 if (ret < 0) {
744 PSVR_ERROR(psvr, "Failed to switch %s the headset! '%i'", status, ret);
745 }
746
747
748 ret = wait_for_power(psvr, on);
749 if (ret < 0) {
750 PSVR_ERROR(psvr, "Failed to wait for headset power %s! '%i'", status, ret);
751 return ret;
752 }
753
754 return ret;
755}
756
757static int
758control_vrmode_and_wait(struct psvr_device *psvr, bool on)
759{
760 const uint8_t data[8] = {
761 0x23, 0x00, 0xaa, 0x04, on, 0x00, 0x00, 0x00,
762 };
763 int ret;
764
765 ret = send_to_control(psvr, data, sizeof(data));
766 if (ret < 0) {
767 PSVR_ERROR(psvr, "Failed %s vr-mode the headset! '%i'", on ? "enable" : "disable", ret);
768 return ret;
769 }
770
771 ret = wait_for_vr_mode(psvr, on);
772 if (ret < 0) {
773 PSVR_ERROR(psvr, "Failed to wait for vr mode! '%i'", ret);
774 return ret;
775 }
776
777 return 0;
778}
779
780static int
781update_leds_if_changed(struct psvr_device *psvr)
782{
783 if (memcmp(psvr->wants.leds, psvr->state.leds, sizeof(psvr->state.leds)) == 0) {
784 return 0;
785 }
786
787 memcpy(psvr->state.leds, psvr->wants.leds, sizeof(psvr->state.leds));
788
789 uint8_t data[20] = {
790 0x15,
791 0x00,
792 0xaa,
793 0x10,
794 (uint8_t)PSVR_LED_ALL,
795 (uint8_t)(PSVR_LED_ALL >> 8),
796 scale_led_power(psvr->state.leds[0]),
797 scale_led_power(psvr->state.leds[1]),
798 scale_led_power(psvr->state.leds[2]),
799 scale_led_power(psvr->state.leds[3]),
800 scale_led_power(psvr->state.leds[4]),
801 scale_led_power(psvr->state.leds[5]),
802 scale_led_power(psvr->state.leds[6]),
803 scale_led_power(psvr->state.leds[7]),
804 scale_led_power(psvr->state.leds[8]),
805 0,
806 0,
807 0,
808 0,
809 0,
810 };
811
812 return send_to_control(psvr, data, sizeof(data));
813}
814
815/*!
816 * Control the leds on the headset, allowing you to turn on and off different
817 * leds with a single call.
818 *
819 * @param[in] psvr The PSVR to control leds on.
820 * @param[in] adjust The leds to adjust with @p power.
821 * @param[in] power The power level to give to @p adjust leds.
822 * @param[in] off Leds that should be turned off,
823 * @p adjust has higher priority.
824 * @ingroup drv_psvr
825 */
826static int
827control_leds(struct psvr_device *psvr, enum psvr_leds adjust, uint8_t power, enum psvr_leds off)
828{
829 // Get the leds we should control and remove any extra bits.
830 enum psvr_leds all = (enum psvr_leds)((adjust | off) & PSVR_LED_ALL);
831 if (all == 0) {
832 // Nothing todo.
833 return 0;
834 }
835
836 for (uint32_t i = 0; i < ARRAY_SIZE(psvr->wants.leds); i++) {
837 uint32_t mask = (1 << i);
838 if (adjust & mask) {
839 psvr->wants.leds[i] = power;
840 }
841 if (off & mask) {
842 psvr->wants.leds[i] = 0x00;
843 }
844 }
845
846 return update_leds_if_changed(psvr);
847}
848
849static int
850disco_leds(struct psvr_device *psvr)
851{
852 static const uint16_t leds[] = {
853 // First loop
854 PSVR_LED_A,
855 PSVR_LED_E,
856 PSVR_LED_B,
857 PSVR_LED_G,
858 PSVR_LED_D,
859 PSVR_LED_C,
860 PSVR_LED_F,
861 // Second loop
862 PSVR_LED_A,
863 PSVR_LED_E,
864 PSVR_LED_B,
865 PSVR_LED_G,
866 PSVR_LED_D,
867 PSVR_LED_C,
868 PSVR_LED_F,
869 // Blink loop
870 PSVR_LED_BACK,
871 PSVR_LED_FRONT,
872 PSVR_LED_BACK,
873 PSVR_LED_FRONT,
874 // All on after loop
875 PSVR_LED_ALL,
876 };
877
878 for (size_t i = 0; i < ARRAY_SIZE(leds); i++) {
879 int ret = control_leds(psvr, (enum psvr_leds)leds[i], PSVR_LED_POWER_MAX, PSVR_LED_ALL);
880 if (ret < 0) {
881 return ret;
882 }
883
884 // Sleep for a tenth of a second while polling for packages.
885 for (int k = 0; k < 100; k++) {
886 ret = read_control_packets(psvr);
887 if (ret < 0) {
888 return ret;
889 }
890
891 os_nanosleep(1000 * 1000);
892 }
893 }
894
895 return 0;
896}
897
898
899/*
900 *
901 * Misc functions.
902 *
903 */
904
905static void
906teardown(struct psvr_device *psvr)
907{
908 // Stop the variable tracking.
909 u_var_remove_root(psvr);
910
911 // Shutdown the sensor thread early.
912 os_thread_helper_stop_and_wait(&psvr->oth);
913
914 // Includes null check, and sets to null.
915 xrt_tracked_psvr_destroy(&psvr->tracker);
916
917 if (psvr->hid_control != NULL) {
918 // Turn off VR-mode and power down headset.
919 if (control_vrmode_and_wait(psvr, false) < 0 || control_power_and_wait(psvr, false) < 0) {
920 PSVR_ERROR(psvr, "Failed to shut down the headset!");
921 }
922
923 hid_close(psvr->hid_control);
924 psvr->hid_control = NULL;
925 }
926
927 if (psvr->hid_sensor != NULL) {
928 hid_close(psvr->hid_sensor);
929 psvr->hid_sensor = NULL;
930 }
931
932 // Destroy the fusion.
933 m_imu_3dof_close(&psvr->fusion);
934
935 os_thread_helper_destroy(&psvr->oth);
936 os_mutex_destroy(&psvr->device_mutex);
937}
938
939
940/*
941 *
942 * xrt_device functions.
943 *
944 */
945
946static xrt_result_t
947psvr_device_update_inputs(struct xrt_device *xdev)
948{
949 struct psvr_device *psvr = psvr_device(xdev);
950
951 os_mutex_lock(&psvr->device_mutex);
952
953 update_leds_if_changed(psvr);
954
955 os_mutex_unlock(&psvr->device_mutex);
956
957 return XRT_SUCCESS;
958}
959
960static xrt_result_t
961psvr_device_get_tracked_pose(struct xrt_device *xdev,
962 enum xrt_input_name name,
963 int64_t at_timestamp_ns,
964 struct xrt_space_relation *out_relation)
965{
966 struct psvr_device *psvr = psvr_device(xdev);
967
968 if (name != XRT_INPUT_GENERIC_HEAD_POSE) {
969 U_LOG_XDEV_UNSUPPORTED_INPUT(&psvr->base, psvr->log_level, name);
970 return XRT_ERROR_INPUT_UNSUPPORTED;
971 }
972
973 os_mutex_lock(&psvr->device_mutex);
974
975 // Read all packets.
976 read_control_packets(psvr);
977
978 // Clear out the relation.
979 U_ZERO(out_relation);
980
981 // We have no tracking, don't return a position.
982 if (psvr->tracker == NULL) {
983 out_relation->pose.orientation = psvr->fusion.rot;
984
985 out_relation->relation_flags = (enum xrt_space_relation_flags)(
986 XRT_SPACE_RELATION_ORIENTATION_VALID_BIT | XRT_SPACE_RELATION_ORIENTATION_TRACKED_BIT);
987 } else {
988 xrt_tracked_psvr_get_tracked_pose(psvr->tracker, at_timestamp_ns, out_relation);
989 }
990
991 os_mutex_unlock(&psvr->device_mutex);
992
993 //! @todo Move this to the tracker.
994 // Make sure that the orientation is valid.
995 math_quat_normalize(&out_relation->pose.orientation);
996
997 return XRT_SUCCESS;
998}
999
1000static void
1001psvr_device_destroy(struct xrt_device *xdev)
1002{
1003 struct psvr_device *psvr = psvr_device(xdev);
1004 teardown(psvr);
1005
1006 u_device_free(&psvr->base);
1007}
1008
1009static xrt_result_t
1010psvr_compute_distortion(struct xrt_device *xdev, uint32_t view, float u, float v, struct xrt_uv_triplet *result)
1011{
1012 struct psvr_device *psvr = psvr_device(xdev);
1013
1014 u_compute_distortion_panotools(&psvr->vals, u, v, result);
1015 return XRT_SUCCESS;
1016}
1017
1018
1019/*
1020 *
1021 * Exported functions.
1022 *
1023 */
1024
1025struct xrt_device *
1026psvr_device_create_auto_prober(struct hid_device_info *sensor_hid_info,
1027 struct hid_device_info *control_hid_info,
1028 struct xrt_tracked_psvr *tracker,
1029 enum u_logging_level log_level)
1030{
1031 enum u_device_alloc_flags flags =
1032 (enum u_device_alloc_flags)(U_DEVICE_ALLOC_HMD | U_DEVICE_ALLOC_TRACKING_NONE);
1033 struct psvr_device *psvr = U_DEVICE_ALLOCATE(struct psvr_device, flags, 1, 0);
1034 int ret;
1035
1036 psvr->log_level = log_level;
1037 psvr->base.update_inputs = psvr_device_update_inputs;
1038 psvr->base.get_tracked_pose = psvr_device_get_tracked_pose;
1039 psvr->base.get_view_poses = u_device_get_view_poses;
1040 psvr->base.compute_distortion = psvr_compute_distortion;
1041 psvr->base.destroy = psvr_device_destroy;
1042 psvr->base.inputs[0].name = XRT_INPUT_GENERIC_HEAD_POSE;
1043 psvr->base.name = XRT_DEVICE_GENERIC_HMD;
1044
1045 {
1046 struct u_panotools_values vals = {0};
1047
1048 vals.distortion_k[0] = 0.75f;
1049 vals.distortion_k[1] = -0.01f;
1050 vals.distortion_k[2] = 0.75f;
1051 vals.distortion_k[3] = 0.0f;
1052 vals.distortion_k[4] = 3.8f;
1053 vals.aberration_k[0] = 0.999f;
1054 vals.aberration_k[1] = 1.008f;
1055 vals.aberration_k[2] = 1.018f;
1056 vals.scale = 1.2f * (1980 / 2.0f);
1057 vals.viewport_size.x = (1980 / 2.0f);
1058 vals.viewport_size.y = (1080);
1059 vals.lens_center.x = vals.viewport_size.x / 2.0f;
1060 vals.lens_center.y = vals.viewport_size.y / 2.0f;
1061
1062 psvr->vals = vals;
1063
1064 struct xrt_hmd_parts *hmd = psvr->base.hmd;
1065 hmd->distortion.models = XRT_DISTORTION_MODEL_COMPUTE;
1066 hmd->distortion.preferred = XRT_DISTORTION_MODEL_COMPUTE;
1067 }
1068
1069#if 1
1070 m_imu_3dof_init(&psvr->fusion, M_IMU_3DOF_USE_GRAVITY_DUR_20MS);
1071#else
1072 psvr->fusion.rot.w = 1.0f;
1073#endif
1074
1075 snprintf(psvr->base.str, XRT_DEVICE_NAME_LEN, "PS VR Headset");
1076 snprintf(psvr->base.serial, XRT_DEVICE_NAME_LEN, "PS VR Headset");
1077
1078 // Do mutex and thread init before any call to teardown happens.
1079 os_mutex_init(&psvr->device_mutex);
1080 os_thread_helper_init(&psvr->oth);
1081
1082 ret = open_hid(psvr, sensor_hid_info, &psvr->hid_sensor);
1083 if (ret != 0) {
1084 goto cleanup;
1085 }
1086
1087 ret = open_hid(psvr, control_hid_info, &psvr->hid_control);
1088 if (ret < 0) {
1089 goto cleanup;
1090 }
1091
1092 ret = os_thread_helper_start(&psvr->oth, sensor_thread, (void *)psvr);
1093 if (ret < 0) {
1094 goto cleanup;
1095 }
1096
1097 if (control_power_and_wait(psvr, true) < 0 || control_vrmode_and_wait(psvr, true) < 0) {
1098 goto cleanup;
1099 }
1100
1101 // Device is now on and we can read calibration data now.
1102 ret = read_calibration_data(psvr);
1103 if (ret < 0) {
1104 goto cleanup;
1105 }
1106
1107 if (debug_get_bool_option_psvr_disco()) {
1108 ret = disco_leds(psvr);
1109 } else {
1110 ret = control_leds(psvr, PSVR_LED_FRONT, PSVR_LED_POWER_MAX, (enum psvr_leds)0);
1111 }
1112 if (ret < 0) {
1113 PSVR_ERROR(psvr, "Failed to control leds '%i'", ret);
1114 goto cleanup;
1115 }
1116
1117
1118 /*
1119 * Device setup.
1120 */
1121
1122 struct u_device_simple_info info;
1123 info.display.w_pixels = 1920;
1124 info.display.h_pixels = 1080;
1125 info.display.w_meters = 0.13f;
1126 info.display.h_meters = 0.07f;
1127 info.lens_horizontal_separation_meters = 0.13f / 2.0f;
1128 info.lens_vertical_position_meters = 0.07f / 2.0f;
1129 info.fov[0] = (float)(85.0 * (M_PI / 180.0));
1130 info.fov[1] = (float)(85.0 * (M_PI / 180.0));
1131
1132 if (!u_device_setup_split_side_by_side(&psvr->base, &info)) {
1133 PSVR_ERROR(psvr, "Failed to setup basic device info");
1134 goto cleanup;
1135 }
1136
1137 /*
1138 * Setup variable.
1139 */
1140
1141 // clang-format off
1142 u_var_add_root(psvr, "PS VR Headset", true);
1143 u_var_add_gui_header(psvr, &psvr->gui.last_frame, "Last data");
1144 u_var_add_ro_vec3_i32(psvr, &psvr->last.samples[0].accel, "last.samples[0].accel");
1145 u_var_add_ro_vec3_i32(psvr, &psvr->last.samples[1].accel, "last.samples[1].accel");
1146 u_var_add_ro_vec3_i32(psvr, &psvr->last.samples[0].gyro, "last.samples[0].gyro");
1147 u_var_add_ro_vec3_i32(psvr, &psvr->last.samples[1].gyro, "last.samples[1].gyro");
1148 u_var_add_ro_vec3_f32(psvr, &psvr->read.accel, "read.accel");
1149 u_var_add_ro_vec3_f32(psvr, &psvr->read.gyro, "read.gyro");
1150 u_var_add_gui_header(psvr, &psvr->gui.control, "Control");
1151 u_var_add_u8(psvr, &psvr->wants.leds[0], "Led A");
1152 u_var_add_u8(psvr, &psvr->wants.leds[1], "Led B");
1153 u_var_add_u8(psvr, &psvr->wants.leds[2], "Led C");
1154 u_var_add_u8(psvr, &psvr->wants.leds[3], "Led D");
1155 u_var_add_u8(psvr, &psvr->wants.leds[4], "Led E");
1156 u_var_add_u8(psvr, &psvr->wants.leds[5], "Led F");
1157 u_var_add_u8(psvr, &psvr->wants.leds[6], "Led G");
1158 u_var_add_u8(psvr, &psvr->wants.leds[7], "Led H");
1159 u_var_add_u8(psvr, &psvr->wants.leds[8], "Led I");
1160 u_var_add_log_level(psvr, &psvr->log_level, "Log level");
1161 // clang-format on
1162
1163 /*
1164 * Finishing touches.
1165 */
1166
1167 if (psvr->log_level <= U_LOGGING_DEBUG) {
1168 u_device_dump_config(&psvr->base, __func__, "Sony PSVR");
1169 }
1170
1171 // Did we get a tracker, use it!
1172 psvr->tracker = tracker;
1173
1174 // Use the new origin if we got a tracking system.
1175 if (psvr->tracker != NULL) {
1176 psvr->base.tracking_origin = psvr->tracker->origin;
1177 }
1178
1179 psvr->base.supported.orientation_tracking = true;
1180 psvr->base.supported.position_tracking = psvr->tracker != NULL;
1181 psvr->base.device_type = XRT_DEVICE_TYPE_HMD;
1182
1183 PSVR_DEBUG(psvr, "YES!");
1184
1185 return &psvr->base;
1186
1187
1188cleanup:
1189 PSVR_DEBUG(psvr, "NO! :(");
1190
1191 teardown(psvr);
1192 free(psvr);
1193
1194 return NULL;
1195}