2019-04-20 01:23:50 +00:00
|
|
|
// Copyright 2016, Joey Ferwerda.
|
2020-05-30 11:44:22 +00:00
|
|
|
// Copyright 2019-2020, Collabora, Ltd.
|
2019-04-20 01:23:50 +00:00
|
|
|
// SPDX-License-Identifier: BSL-1.0
|
|
|
|
/*!
|
|
|
|
* @file
|
|
|
|
* @brief PSVR device implementation, imported from OpenHMD.
|
|
|
|
* @author Joey Ferwerda <joeyferweda@gmail.com>
|
|
|
|
* @author Philipp Zabel <philipp.zabel@gmail.com>
|
|
|
|
* @author Jakob Bornecrantz <jakob@collabora.com>
|
|
|
|
* @ingroup drv_psvr
|
|
|
|
*/
|
|
|
|
|
|
|
|
#include "xrt/xrt_compiler.h"
|
2019-09-09 10:42:54 +00:00
|
|
|
#include "xrt/xrt_tracking.h"
|
2019-04-20 01:23:50 +00:00
|
|
|
|
2019-09-27 10:44:54 +00:00
|
|
|
#include "os/os_time.h"
|
|
|
|
|
2019-09-11 15:42:59 +00:00
|
|
|
#include "math/m_api.h"
|
|
|
|
|
2019-08-31 11:47:20 +00:00
|
|
|
#include "util/u_var.h"
|
2019-04-20 01:23:50 +00:00
|
|
|
#include "util/u_misc.h"
|
|
|
|
#include "util/u_time.h"
|
|
|
|
#include "util/u_debug.h"
|
|
|
|
#include "util/u_device.h"
|
2019-10-23 22:28:46 +00:00
|
|
|
#include "util/u_distortion_mesh.h"
|
2019-04-20 01:23:50 +00:00
|
|
|
|
2020-04-17 18:28:03 +00:00
|
|
|
#include "math/m_imu_3dof.h"
|
2019-10-22 18:51:05 +00:00
|
|
|
|
2019-04-20 01:23:50 +00:00
|
|
|
#include <math.h>
|
|
|
|
#include <stdio.h>
|
|
|
|
#include <stdlib.h>
|
|
|
|
#include <string.h>
|
|
|
|
#include <unistd.h>
|
|
|
|
|
|
|
|
#include "psvr_device.h"
|
|
|
|
|
|
|
|
|
|
|
|
/*
|
|
|
|
*
|
|
|
|
* Structs and defines.
|
|
|
|
*
|
|
|
|
*/
|
|
|
|
|
|
|
|
DEBUG_GET_ONCE_BOOL_OPTION(psvr_disco, "PSVR_DISCO", false)
|
|
|
|
|
|
|
|
#define FEATURE_BUFFER_SIZE 256
|
|
|
|
|
|
|
|
/*!
|
|
|
|
* Private struct for the @ref drv_psvr device.
|
|
|
|
*
|
|
|
|
* @ingroup drv_psvr
|
|
|
|
*/
|
|
|
|
struct psvr_device
|
|
|
|
{
|
|
|
|
struct xrt_device base;
|
|
|
|
|
|
|
|
hid_device *hmd_handle;
|
|
|
|
hid_device *hmd_control;
|
|
|
|
|
2019-09-09 10:42:54 +00:00
|
|
|
struct xrt_tracked_psvr *tracker;
|
|
|
|
|
2019-11-12 17:11:56 +00:00
|
|
|
timepoint_ns last_sensor_time;
|
|
|
|
|
2019-09-11 15:06:58 +00:00
|
|
|
struct psvr_parsed_sensor last;
|
2019-04-20 01:23:50 +00:00
|
|
|
|
2019-08-31 11:47:20 +00:00
|
|
|
struct
|
|
|
|
{
|
|
|
|
uint8_t leds[9];
|
|
|
|
} wants;
|
|
|
|
|
|
|
|
struct
|
|
|
|
{
|
|
|
|
uint8_t leds[9];
|
|
|
|
} state;
|
|
|
|
|
2019-04-20 01:23:50 +00:00
|
|
|
struct
|
|
|
|
{
|
|
|
|
struct xrt_vec3 gyro;
|
|
|
|
struct xrt_vec3 accel;
|
2019-09-11 15:06:58 +00:00
|
|
|
} read;
|
2019-04-20 01:23:50 +00:00
|
|
|
|
|
|
|
uint16_t buttons;
|
|
|
|
|
|
|
|
bool powered_on;
|
|
|
|
bool in_vr_mode;
|
|
|
|
|
|
|
|
bool print_spew;
|
|
|
|
bool print_debug;
|
2019-09-11 15:05:10 +00:00
|
|
|
|
2019-10-25 09:54:39 +00:00
|
|
|
struct
|
|
|
|
{
|
|
|
|
union {
|
|
|
|
uint8_t data[290];
|
2019-10-26 16:16:06 +00:00
|
|
|
struct
|
|
|
|
{
|
|
|
|
uint32_t _pad0[4];
|
|
|
|
struct xrt_vec3 unknown0;
|
|
|
|
uint32_t _zero0;
|
|
|
|
uint32_t _pad2_vec3_zero[4];
|
|
|
|
uint32_t _pad3_vec3_zero[4];
|
|
|
|
uint32_t _pad4_vec3_zero[4];
|
|
|
|
struct xrt_vec3 accel_pos_y;
|
|
|
|
uint32_t _pad5[1];
|
|
|
|
struct xrt_vec3 accel_neg_x;
|
|
|
|
uint32_t _pad6[1];
|
|
|
|
struct xrt_vec3 accel_neg_y;
|
|
|
|
uint32_t _pad7[1];
|
|
|
|
struct xrt_vec3 accel_pos_x;
|
|
|
|
uint32_t _pad8[1];
|
|
|
|
struct xrt_vec3 accel_pos_z;
|
|
|
|
uint32_t _pad9[1];
|
|
|
|
struct xrt_vec3 accel_neg_z;
|
|
|
|
uint32_t _pad10[1];
|
|
|
|
struct xrt_vec3 gyro_neg_y;
|
|
|
|
uint32_t _pad11[1];
|
|
|
|
struct xrt_vec3 gyro_pos_x;
|
|
|
|
uint32_t _pad12[1];
|
|
|
|
struct xrt_vec3 gyro_neg_z;
|
|
|
|
uint32_t _pad13[1];
|
|
|
|
};
|
2019-10-25 09:54:39 +00:00
|
|
|
};
|
|
|
|
int last_packet;
|
|
|
|
} calibration;
|
|
|
|
|
2019-09-11 15:05:10 +00:00
|
|
|
struct
|
|
|
|
{
|
|
|
|
bool last_frame;
|
|
|
|
bool control;
|
|
|
|
} gui;
|
2019-09-11 15:42:59 +00:00
|
|
|
|
2020-04-17 18:28:03 +00:00
|
|
|
#if 1
|
|
|
|
struct m_imu_3dof fusion;
|
2019-10-22 18:51:05 +00:00
|
|
|
#else
|
2019-09-11 15:42:59 +00:00
|
|
|
struct
|
|
|
|
{
|
|
|
|
struct xrt_quat rot;
|
|
|
|
} fusion;
|
2019-10-22 18:51:05 +00:00
|
|
|
#endif
|
2019-04-20 01:23:50 +00:00
|
|
|
};
|
|
|
|
|
|
|
|
|
|
|
|
// Alternative way to turn on all of the leds.
|
|
|
|
XRT_MAYBE_UNUSED static const unsigned char psvr_tracking_on[12] = {
|
|
|
|
0x11, 0x00, 0xaa, 0x08, 0x00, 0xff, 0xff, 0xff, 0x00, 0x00, 0x00, 0x00,
|
|
|
|
};
|
|
|
|
|
|
|
|
|
2019-08-31 11:47:20 +00:00
|
|
|
#define PSVR_LED_POWER_OFF ((uint8_t)0x00)
|
|
|
|
#define PSVR_LED_POWER_MAX ((uint8_t)0xff)
|
|
|
|
|
|
|
|
#define PSVR_LED_POWER_WIRE_OFF ((uint8_t)0)
|
|
|
|
#define PSVR_LED_POWER_WIRE_MAX ((uint8_t)100)
|
|
|
|
|
2019-04-20 01:23:50 +00:00
|
|
|
|
|
|
|
enum psvr_leds
|
|
|
|
{
|
|
|
|
PSVR_LED_A = (1 << 0),
|
|
|
|
PSVR_LED_B = (1 << 1),
|
|
|
|
PSVR_LED_C = (1 << 2),
|
|
|
|
PSVR_LED_D = (1 << 3),
|
|
|
|
PSVR_LED_E = (1 << 4),
|
|
|
|
PSVR_LED_F = (1 << 5),
|
|
|
|
PSVR_LED_G = (1 << 6),
|
|
|
|
PSVR_LED_H = (1 << 7),
|
|
|
|
PSVR_LED_I = (1 << 8),
|
|
|
|
|
|
|
|
PSVR_LED_FRONT = PSVR_LED_A | PSVR_LED_B | PSVR_LED_C | PSVR_LED_D |
|
|
|
|
PSVR_LED_E | PSVR_LED_F | PSVR_LED_G,
|
|
|
|
|
|
|
|
PSVR_LED_BACK = PSVR_LED_H | PSVR_LED_I,
|
|
|
|
|
|
|
|
PSVR_LED_ALL = PSVR_LED_FRONT | PSVR_LED_BACK,
|
|
|
|
};
|
|
|
|
|
|
|
|
|
|
|
|
/*
|
|
|
|
*
|
|
|
|
* Helpers and internal functions.
|
|
|
|
*
|
|
|
|
*/
|
|
|
|
|
|
|
|
static inline struct psvr_device *
|
|
|
|
psvr_device(struct xrt_device *p)
|
|
|
|
{
|
|
|
|
return (struct psvr_device *)p;
|
|
|
|
}
|
|
|
|
|
|
|
|
static int
|
|
|
|
open_hid(struct psvr_device *p,
|
|
|
|
struct hid_device_info *dev_info,
|
|
|
|
hid_device **out_dev)
|
|
|
|
{
|
|
|
|
hid_device *dev = NULL;
|
|
|
|
int ret;
|
|
|
|
|
|
|
|
dev = hid_open_path(dev_info->path);
|
|
|
|
if (dev == NULL) {
|
|
|
|
PSVR_ERROR(p, "Failed to open '%s'", dev_info->path);
|
|
|
|
return -1;
|
|
|
|
}
|
|
|
|
|
|
|
|
ret = hid_set_nonblocking(dev, 1);
|
|
|
|
if (ret != 0) {
|
|
|
|
PSVR_ERROR(p, "Failed to set non-blocking on device");
|
|
|
|
hid_close(dev);
|
|
|
|
return -1;
|
|
|
|
}
|
|
|
|
|
|
|
|
*out_dev = dev;
|
|
|
|
return 0;
|
|
|
|
}
|
|
|
|
|
|
|
|
static int
|
|
|
|
send_to_control(struct psvr_device *psvr, const uint8_t *data, size_t size)
|
|
|
|
{
|
|
|
|
return hid_write(psvr->hmd_control, data, size);
|
|
|
|
}
|
|
|
|
|
2019-10-25 09:54:39 +00:00
|
|
|
static int
|
2019-11-14 17:37:57 +00:00
|
|
|
send_request_data(struct psvr_device *psvr, uint8_t id, uint8_t num)
|
2019-10-25 09:54:39 +00:00
|
|
|
{
|
|
|
|
const uint8_t data[12] = {
|
|
|
|
0x81, 0x00, 0xaa, 0x08, id, num, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
|
|
|
|
};
|
|
|
|
return send_to_control(psvr, data, sizeof(data));
|
|
|
|
}
|
|
|
|
|
2019-04-20 01:23:50 +00:00
|
|
|
|
|
|
|
/*
|
|
|
|
*
|
|
|
|
* Packet reading code.
|
|
|
|
*
|
|
|
|
*/
|
|
|
|
|
2019-08-31 11:47:20 +00:00
|
|
|
static uint8_t
|
|
|
|
scale_led_power(uint8_t power)
|
|
|
|
{
|
|
|
|
return (uint8_t)((power / 255.0f) * 100.0f);
|
|
|
|
}
|
|
|
|
|
2019-04-20 01:23:50 +00:00
|
|
|
static void
|
2019-10-26 15:54:44 +00:00
|
|
|
read_sample_and_apply_calibration(struct psvr_device *psvr,
|
|
|
|
struct psvr_parsed_sample *sample,
|
|
|
|
struct xrt_vec3 *out_accel,
|
|
|
|
struct xrt_vec3 *out_gyro)
|
2019-04-20 01:23:50 +00:00
|
|
|
{
|
2019-10-26 15:54:44 +00:00
|
|
|
const struct xrt_vec3_i32 raw_accel = sample->accel;
|
|
|
|
const struct xrt_vec3_i32 raw_gyro = sample->gyro;
|
|
|
|
|
|
|
|
// Convert so that for a perfect IMU 1.0 is one G.
|
|
|
|
struct xrt_vec3 accel = {
|
2019-11-14 17:37:57 +00:00
|
|
|
raw_accel.x / 16384.0f,
|
|
|
|
raw_accel.y / 16384.0f,
|
|
|
|
raw_accel.z / 16384.0f,
|
2019-10-26 15:54:44 +00:00
|
|
|
};
|
2019-04-20 01:23:50 +00:00
|
|
|
|
2019-10-26 15:54:44 +00:00
|
|
|
// What unit is this?
|
|
|
|
struct xrt_vec3 gyro = {
|
2019-11-14 17:37:57 +00:00
|
|
|
raw_gyro.x * 0.00105f,
|
|
|
|
raw_gyro.y * 0.00105f,
|
|
|
|
raw_gyro.z * 0.00105f,
|
2019-10-26 15:54:44 +00:00
|
|
|
};
|
|
|
|
|
2019-10-26 16:16:06 +00:00
|
|
|
float ax = 2.0 / (psvr->calibration.accel_pos_x.x -
|
|
|
|
psvr->calibration.accel_neg_x.x);
|
|
|
|
float ay = 2.0 / (psvr->calibration.accel_pos_y.y -
|
|
|
|
psvr->calibration.accel_neg_y.y);
|
|
|
|
float az = 2.0 / (psvr->calibration.accel_pos_z.z -
|
|
|
|
psvr->calibration.accel_neg_z.z);
|
|
|
|
|
|
|
|
float ox = (psvr->calibration.accel_pos_x.x +
|
|
|
|
psvr->calibration.accel_neg_x.x) /
|
|
|
|
2.0;
|
|
|
|
float oy = (psvr->calibration.accel_pos_y.y +
|
|
|
|
psvr->calibration.accel_neg_y.y) /
|
|
|
|
2.0;
|
|
|
|
float oz = (psvr->calibration.accel_pos_z.z +
|
|
|
|
psvr->calibration.accel_neg_z.z) /
|
|
|
|
2.0;
|
|
|
|
|
|
|
|
accel.x -= ox;
|
|
|
|
accel.y -= oy;
|
|
|
|
accel.z -= oz;
|
|
|
|
accel.x *= ax;
|
|
|
|
accel.y *= ay;
|
|
|
|
accel.z *= az;
|
|
|
|
|
2019-10-26 15:54:44 +00:00
|
|
|
// Go from Gs to m/s2 and flip the Z-axis.
|
|
|
|
accel.x *= +MATH_GRAVITY_M_S2;
|
|
|
|
accel.y *= +MATH_GRAVITY_M_S2;
|
|
|
|
accel.z *= -MATH_GRAVITY_M_S2;
|
|
|
|
|
|
|
|
// Flip the Z-axis.
|
|
|
|
gyro.x *= +1.0;
|
|
|
|
gyro.y *= +1.0;
|
|
|
|
gyro.z *= -1.0;
|
|
|
|
|
|
|
|
*out_accel = accel;
|
|
|
|
*out_gyro = gyro;
|
2019-04-20 01:23:50 +00:00
|
|
|
}
|
|
|
|
|
|
|
|
static void
|
|
|
|
update_fusion(struct psvr_device *psvr,
|
2019-09-11 15:06:58 +00:00
|
|
|
struct psvr_parsed_sample *sample,
|
2019-11-12 17:11:56 +00:00
|
|
|
uint32_t tick_delta,
|
|
|
|
timepoint_ns timestamp_ns)
|
2019-04-20 01:23:50 +00:00
|
|
|
{
|
|
|
|
struct xrt_vec3 mag = {0.0f, 0.0f, 0.0f};
|
|
|
|
(void)mag;
|
|
|
|
|
2019-10-26 15:54:44 +00:00
|
|
|
read_sample_and_apply_calibration(psvr, sample, &psvr->read.accel,
|
|
|
|
&psvr->read.gyro);
|
2019-04-20 01:23:50 +00:00
|
|
|
|
2019-09-09 10:42:54 +00:00
|
|
|
if (psvr->tracker != NULL) {
|
|
|
|
struct xrt_tracking_sample sample;
|
|
|
|
sample.accel_m_s2 = psvr->read.accel;
|
|
|
|
sample.gyro_rad_secs = psvr->read.gyro;
|
2019-09-11 15:42:59 +00:00
|
|
|
|
2019-11-12 17:11:56 +00:00
|
|
|
xrt_tracked_psvr_push_imu(psvr->tracker, timestamp_ns, &sample);
|
2019-09-09 10:42:54 +00:00
|
|
|
} else {
|
2019-09-21 18:08:11 +00:00
|
|
|
float delta_secs = tick_delta / PSVR_TICKS_PER_SECOND;
|
|
|
|
|
2020-04-17 18:28:03 +00:00
|
|
|
#if 1
|
|
|
|
m_imu_3dof_update(&psvr->fusion, delta_secs, &psvr->read.accel,
|
|
|
|
&psvr->read.gyro);
|
2019-10-22 18:51:05 +00:00
|
|
|
#else
|
2019-09-09 10:42:54 +00:00
|
|
|
math_quat_integrate_velocity(&psvr->fusion.rot,
|
2019-09-21 18:08:11 +00:00
|
|
|
&psvr->read.gyro, delta_secs,
|
2019-09-09 10:42:54 +00:00
|
|
|
&psvr->fusion.rot);
|
2019-10-22 18:51:05 +00:00
|
|
|
#endif
|
2019-09-09 10:42:54 +00:00
|
|
|
}
|
2019-04-20 01:23:50 +00:00
|
|
|
}
|
|
|
|
|
|
|
|
static uint32_t
|
|
|
|
calc_delta_and_handle_rollover(uint32_t next, uint32_t last)
|
|
|
|
{
|
|
|
|
uint32_t tick_delta = next - last;
|
|
|
|
|
|
|
|
// The 24-bit tick counter has rolled over,
|
|
|
|
// adjust the "negative" value to be positive.
|
|
|
|
if (tick_delta > 0xffffff) {
|
|
|
|
tick_delta += 0x1000000;
|
|
|
|
}
|
|
|
|
|
|
|
|
return tick_delta;
|
|
|
|
}
|
|
|
|
|
|
|
|
static void
|
|
|
|
handle_tracker_sensor_msg(struct psvr_device *psvr,
|
|
|
|
unsigned char *buffer,
|
|
|
|
int size)
|
|
|
|
{
|
2020-04-16 12:23:12 +00:00
|
|
|
timepoint_ns now = os_monotonic_get_ns();
|
2019-09-11 15:06:58 +00:00
|
|
|
uint32_t last_sample_tick = psvr->last.samples[1].tick;
|
2019-04-20 01:23:50 +00:00
|
|
|
|
2019-09-11 15:06:58 +00:00
|
|
|
if (!psvr_parse_sensor_packet(&psvr->last, buffer, size)) {
|
2019-04-20 01:23:50 +00:00
|
|
|
PSVR_ERROR(psvr, "couldn't decode tracker sensor message");
|
|
|
|
}
|
|
|
|
|
2019-09-11 15:06:58 +00:00
|
|
|
struct psvr_parsed_sensor *s = &psvr->last;
|
2019-04-20 01:23:50 +00:00
|
|
|
|
|
|
|
// Simplest is the buttons.
|
|
|
|
psvr->buttons = s->buttons;
|
|
|
|
|
|
|
|
uint32_t tick_delta = 500;
|
|
|
|
|
|
|
|
// Startup correction, ignore last_sample_tick if zero.
|
|
|
|
if (last_sample_tick > 0) {
|
|
|
|
tick_delta = calc_delta_and_handle_rollover(s->samples[0].tick,
|
|
|
|
last_sample_tick);
|
|
|
|
|
|
|
|
// The PSVR device can buffer sensor data from previous
|
|
|
|
// sessions which we can get at the start of new sessions.
|
|
|
|
// @todo Maybe just skip the first 10 sensor packets?
|
|
|
|
// @todo Maybe reset sensor fusion?
|
|
|
|
if (tick_delta < 400 || tick_delta > 600) {
|
|
|
|
PSVR_DEBUG(psvr, "tick_delta = %u", tick_delta);
|
|
|
|
tick_delta = 500;
|
|
|
|
}
|
|
|
|
}
|
2019-11-12 17:11:56 +00:00
|
|
|
// New delta between the two samples.
|
|
|
|
uint32_t tick_delta2 = calc_delta_and_handle_rollover(
|
|
|
|
s->samples[1].tick, s->samples[0].tick);
|
2019-04-20 01:23:50 +00:00
|
|
|
|
2019-11-12 17:11:56 +00:00
|
|
|
time_duration_ns inter_sample_duration_ns =
|
|
|
|
tick_delta2 * PSVR_NS_PER_TICK;
|
2019-04-20 01:23:50 +00:00
|
|
|
// Update the fusion with first sample.
|
2019-11-12 17:11:56 +00:00
|
|
|
update_fusion(psvr, &s->samples[0], tick_delta,
|
|
|
|
now - inter_sample_duration_ns);
|
2019-04-20 01:23:50 +00:00
|
|
|
|
|
|
|
// Update the fusion with second sample.
|
2019-11-12 17:11:56 +00:00
|
|
|
update_fusion(psvr, &s->samples[1], tick_delta2, now);
|
|
|
|
psvr->last_sensor_time = now;
|
2019-04-20 01:23:50 +00:00
|
|
|
}
|
|
|
|
|
|
|
|
static void
|
|
|
|
handle_control_status_msg(struct psvr_device *psvr,
|
|
|
|
unsigned char *buffer,
|
|
|
|
int size)
|
|
|
|
{
|
2020-05-19 22:34:00 +00:00
|
|
|
struct psvr_parsed_status status = {0};
|
2019-04-20 01:23:50 +00:00
|
|
|
|
2019-09-11 15:06:58 +00:00
|
|
|
if (!psvr_parse_status_packet(&status, buffer, size)) {
|
2019-04-20 01:23:50 +00:00
|
|
|
PSVR_ERROR(psvr, "couldn't decode tracker sensor message");
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
/*
|
|
|
|
* Power
|
|
|
|
*/
|
|
|
|
|
2019-09-11 15:06:58 +00:00
|
|
|
if (status.status & PSVR_STATUS_BIT_POWER) {
|
2019-04-20 01:23:50 +00:00
|
|
|
if (!psvr->powered_on) {
|
|
|
|
PSVR_DEBUG(psvr, "Device powered on! '%02x'",
|
2019-09-11 15:06:58 +00:00
|
|
|
status.status);
|
2019-04-20 01:23:50 +00:00
|
|
|
}
|
|
|
|
psvr->powered_on = true;
|
|
|
|
} else {
|
|
|
|
if (psvr->powered_on) {
|
|
|
|
PSVR_DEBUG(psvr, "Device powered off! '%02x'",
|
2019-09-11 15:06:58 +00:00
|
|
|
status.status);
|
2019-04-20 01:23:50 +00:00
|
|
|
}
|
|
|
|
psvr->powered_on = false;
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
/*
|
|
|
|
* VR-Mode
|
|
|
|
*/
|
|
|
|
|
2019-09-11 15:06:58 +00:00
|
|
|
if (status.vr_mode == PSVR_STATUS_VR_MODE_OFF) {
|
2019-04-20 01:23:50 +00:00
|
|
|
if (psvr->in_vr_mode) {
|
|
|
|
PSVR_DEBUG(psvr, "Device not in vr-mode! '%02x'",
|
2019-09-11 15:06:58 +00:00
|
|
|
status.vr_mode);
|
2019-04-20 01:23:50 +00:00
|
|
|
}
|
|
|
|
psvr->in_vr_mode = false;
|
2019-09-11 15:06:58 +00:00
|
|
|
} else if (status.vr_mode == PSVR_STATUS_VR_MODE_ON) {
|
2019-04-20 01:23:50 +00:00
|
|
|
if (!psvr->in_vr_mode) {
|
|
|
|
PSVR_DEBUG(psvr, "Device in vr-mode! '%02x'",
|
2019-09-11 15:06:58 +00:00
|
|
|
status.vr_mode);
|
2019-04-20 01:23:50 +00:00
|
|
|
}
|
|
|
|
psvr->in_vr_mode = true;
|
|
|
|
} else {
|
|
|
|
PSVR_ERROR(psvr, "Unknown vr_mode status!");
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
2019-10-25 09:54:39 +00:00
|
|
|
static void
|
|
|
|
handle_device_name_msg(struct psvr_device *psvr,
|
|
|
|
unsigned char *buffer,
|
|
|
|
int size)
|
|
|
|
{
|
|
|
|
//! @todo Get the name here.
|
|
|
|
}
|
|
|
|
|
|
|
|
static void
|
|
|
|
handle_calibration_msg(struct psvr_device *psvr,
|
2019-12-03 16:27:44 +00:00
|
|
|
const unsigned char *buffer,
|
2019-10-25 09:54:39 +00:00
|
|
|
size_t size)
|
|
|
|
{
|
|
|
|
const size_t data_start = 6;
|
|
|
|
const size_t data_length = 58;
|
|
|
|
const size_t packet_length = data_start + data_length;
|
|
|
|
|
|
|
|
if (size != packet_length) {
|
|
|
|
PSVR_ERROR(psvr, "invalid calibration packet length");
|
|
|
|
return;
|
|
|
|
}
|
|
|
|
|
|
|
|
size_t which = buffer[1];
|
|
|
|
size_t dst = data_length * which;
|
|
|
|
for (size_t src = data_start; src < size; src++, dst++) {
|
|
|
|
psvr->calibration.data[dst] = buffer[src];
|
|
|
|
}
|
|
|
|
|
|
|
|
psvr->calibration.last_packet = which;
|
|
|
|
}
|
|
|
|
|
2019-11-15 14:07:39 +00:00
|
|
|
static void
|
|
|
|
handle_control_0x82(struct psvr_device *psvr, unsigned char *buffer, int size)
|
|
|
|
{
|
|
|
|
if (size < 4) {
|
|
|
|
return;
|
|
|
|
}
|
|
|
|
|
|
|
|
if (size < (int)sizeof(float) * 6) {
|
|
|
|
PSVR_DEBUG(psvr, "%02x %02x %02x %02x", buffer[0], buffer[1],
|
|
|
|
buffer[2], buffer[3]);
|
|
|
|
}
|
|
|
|
|
|
|
|
float *f = (float *)buffer;
|
|
|
|
int *i = (int *)buffer;
|
|
|
|
|
|
|
|
PSVR_DEBUG(psvr,
|
|
|
|
"%02x %02x %02x %02x\n"
|
|
|
|
"%+f %+f %+f %+f %+f\n"
|
|
|
|
"0x%08x 0x%08x 0x%08x 0x%08x 0x%08x\n"
|
|
|
|
"% 10i % 10i % 10i % 10i % 10i",
|
|
|
|
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], i[5], i[1], i[2], i[3],
|
|
|
|
i[4], i[5]);
|
|
|
|
}
|
|
|
|
|
2019-04-20 01:23:50 +00:00
|
|
|
static void
|
|
|
|
handle_control_0xA0(struct psvr_device *psvr, unsigned char *buffer, int size)
|
|
|
|
{
|
|
|
|
if (size < 4) {
|
|
|
|
return;
|
|
|
|
}
|
|
|
|
|
|
|
|
PSVR_DEBUG(psvr, "%02x %02x %02x %02x", buffer[0], buffer[1], buffer[2],
|
|
|
|
buffer[3]);
|
|
|
|
}
|
|
|
|
|
|
|
|
static int
|
|
|
|
read_handle_packets(struct psvr_device *psvr)
|
|
|
|
{
|
|
|
|
uint8_t buffer[FEATURE_BUFFER_SIZE];
|
|
|
|
int size = 0;
|
|
|
|
|
|
|
|
do {
|
|
|
|
size = hid_read(psvr->hmd_handle, buffer, FEATURE_BUFFER_SIZE);
|
|
|
|
if (size == 0) {
|
|
|
|
return 0;
|
|
|
|
}
|
|
|
|
if (size < 0) {
|
|
|
|
return -1;
|
|
|
|
}
|
|
|
|
|
|
|
|
handle_tracker_sensor_msg(psvr, buffer, size);
|
|
|
|
} while (true);
|
|
|
|
}
|
|
|
|
|
|
|
|
static int
|
|
|
|
read_control_packets(struct psvr_device *psvr)
|
|
|
|
{
|
|
|
|
uint8_t buffer[FEATURE_BUFFER_SIZE];
|
|
|
|
int size = 0;
|
|
|
|
|
|
|
|
do {
|
|
|
|
size = hid_read(psvr->hmd_control, buffer, FEATURE_BUFFER_SIZE);
|
|
|
|
if (size == 0) {
|
|
|
|
return 0;
|
|
|
|
}
|
|
|
|
if (size < 0) {
|
|
|
|
return -1;
|
|
|
|
}
|
|
|
|
|
|
|
|
if (buffer[0] == PSVR_PKG_STATUS) {
|
|
|
|
handle_control_status_msg(psvr, buffer, size);
|
2019-10-25 09:54:39 +00:00
|
|
|
} else if (buffer[0] == PSVR_PKG_DEVICE_NAME) {
|
|
|
|
handle_device_name_msg(psvr, buffer, size);
|
|
|
|
} else if (buffer[0] == PSVR_PKG_CALIBRATION) {
|
|
|
|
handle_calibration_msg(psvr, buffer, size);
|
2019-11-15 14:07:39 +00:00
|
|
|
} else if (buffer[0] == PSVR_PKG_0x82) {
|
|
|
|
handle_control_0x82(psvr, buffer, size);
|
2019-04-20 01:23:50 +00:00
|
|
|
} else if (buffer[0] == PSVR_PKG_0xA0) {
|
|
|
|
handle_control_0xA0(psvr, buffer, size);
|
|
|
|
} else {
|
2019-11-15 14:07:39 +00:00
|
|
|
PSVR_ERROR(psvr, "Got report, 0x%02x", buffer[0]);
|
2019-04-20 01:23:50 +00:00
|
|
|
}
|
|
|
|
|
|
|
|
} while (true);
|
|
|
|
}
|
|
|
|
|
2019-10-25 11:32:40 +00:00
|
|
|
|
|
|
|
/*!
|
|
|
|
* Get the device name data and calibration data, see link below for info.
|
|
|
|
*
|
|
|
|
* https://github.com/gusmanb/PSVRFramework/wiki/Report-0x81-Device-ID-and-Calibration
|
|
|
|
*/
|
2019-10-25 09:54:39 +00:00
|
|
|
static int
|
|
|
|
read_calibration_data(struct psvr_device *psvr)
|
|
|
|
{
|
|
|
|
// Request the device name.
|
2019-10-25 11:27:54 +00:00
|
|
|
int ret = send_request_data(psvr, PSVR_GET_DATA_ID_DEVICE_NAME, 0);
|
2019-10-25 09:54:39 +00:00
|
|
|
if (ret < 0) {
|
|
|
|
return ret;
|
|
|
|
}
|
|
|
|
|
2019-11-15 14:07:39 +00:00
|
|
|
// Request unknown data 0x82.
|
|
|
|
ret = send_request_data(psvr, PSVR_GET_DATA_ID_0x82, 0);
|
|
|
|
if (ret < 0) {
|
|
|
|
return ret;
|
|
|
|
}
|
|
|
|
|
2019-10-25 11:27:54 +00:00
|
|
|
// There are 5 pages of PSVR calibration data.
|
2019-10-25 09:54:39 +00:00
|
|
|
for (int i = 0; i < 5; i++) {
|
|
|
|
// Request the IMU calibration data.
|
2019-10-25 11:27:54 +00:00
|
|
|
ret = send_request_data(psvr, PSVR_GET_DATA_ID_CALIBRATION, i);
|
2019-10-25 09:54:39 +00:00
|
|
|
if (ret < 0) {
|
|
|
|
return ret;
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
2019-10-25 11:27:54 +00:00
|
|
|
// We have requested 5 pages worth of data, wait for the replies.
|
2019-10-25 09:54:39 +00:00
|
|
|
for (int i = 0; i < 100; i++) {
|
|
|
|
os_nanosleep(1000 * 1000);
|
|
|
|
read_control_packets(psvr);
|
|
|
|
|
2019-10-25 11:27:54 +00:00
|
|
|
// If we have gotten of the packets stop wait.
|
2019-10-25 09:54:39 +00:00
|
|
|
if (psvr->calibration.last_packet == 4) {
|
|
|
|
break;
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
2019-10-25 11:27:54 +00:00
|
|
|
// Did we really get all of the data?
|
2019-10-25 09:54:39 +00:00
|
|
|
if (psvr->calibration.last_packet != 4) {
|
|
|
|
PSVR_ERROR(psvr, "Failed to get calibration");
|
|
|
|
return -1;
|
|
|
|
}
|
|
|
|
|
2019-10-26 16:16:06 +00:00
|
|
|
PSVR_DEBUG(
|
|
|
|
psvr,
|
|
|
|
"calibration.accel_pos_x: %f %f %f\n"
|
|
|
|
"calibration.accel_neg_x: %f %f %f\n"
|
|
|
|
"calibration.accel_pos_y: %f %f %f\n"
|
|
|
|
"calibration.accel_neg_y: %f %f %f\n"
|
|
|
|
"calibration.accel_pos_z: %f %f %f\n"
|
|
|
|
"calibration.accel_neg_z: %f %f %f\n",
|
|
|
|
psvr->calibration.accel_pos_x.x, psvr->calibration.accel_pos_x.y,
|
|
|
|
psvr->calibration.accel_pos_x.z, psvr->calibration.accel_neg_x.x,
|
|
|
|
psvr->calibration.accel_neg_x.y, psvr->calibration.accel_neg_x.z,
|
|
|
|
psvr->calibration.accel_pos_y.x, psvr->calibration.accel_pos_y.y,
|
|
|
|
psvr->calibration.accel_pos_y.z, psvr->calibration.accel_neg_y.x,
|
|
|
|
psvr->calibration.accel_neg_y.y, psvr->calibration.accel_neg_y.z,
|
|
|
|
psvr->calibration.accel_pos_z.x, psvr->calibration.accel_pos_z.y,
|
|
|
|
psvr->calibration.accel_pos_z.z, psvr->calibration.accel_neg_z.x,
|
|
|
|
psvr->calibration.accel_neg_z.y, psvr->calibration.accel_neg_z.z);
|
|
|
|
|
2019-10-25 09:54:39 +00:00
|
|
|
#if 0
|
|
|
|
for (size_t i = 0; i < sizeof(psvr->calibration.data); i++) {
|
|
|
|
fprintf(stderr, "%02x ", psvr->calibration.data[i]);
|
|
|
|
}
|
|
|
|
fprintf(stderr, "\n");
|
|
|
|
|
|
|
|
int *data = (int*)&psvr->calibration.data[0];
|
|
|
|
for (size_t i = 0; i < (sizeof(psvr->calibration.data) / 4); i++) {
|
|
|
|
int v = data[i];
|
|
|
|
fprintf(stderr, "%i %f\n", v, *(float*)&v);
|
|
|
|
}
|
|
|
|
#endif
|
|
|
|
|
2019-10-25 11:27:54 +00:00
|
|
|
// Jobs done!
|
|
|
|
// - Orc peon.
|
2019-10-25 09:54:39 +00:00
|
|
|
return 0;
|
|
|
|
}
|
|
|
|
|
2019-04-20 01:23:50 +00:00
|
|
|
|
|
|
|
/*
|
|
|
|
*
|
|
|
|
* Control sending functions.
|
|
|
|
*
|
|
|
|
*/
|
|
|
|
|
|
|
|
static int
|
|
|
|
wait_for_power(struct psvr_device *psvr, bool on)
|
|
|
|
{
|
|
|
|
for (int i = 0; i < 5000; i++) {
|
|
|
|
read_handle_packets(psvr);
|
|
|
|
read_control_packets(psvr);
|
|
|
|
|
|
|
|
if (psvr->powered_on == on) {
|
|
|
|
return 0;
|
|
|
|
}
|
|
|
|
|
2019-09-27 10:44:54 +00:00
|
|
|
os_nanosleep(1000 * 1000);
|
2019-04-20 01:23:50 +00:00
|
|
|
}
|
|
|
|
|
|
|
|
return -1;
|
|
|
|
}
|
|
|
|
|
|
|
|
static int
|
|
|
|
wait_for_vr_mode(struct psvr_device *psvr, bool on)
|
|
|
|
{
|
|
|
|
for (int i = 0; i < 5000; i++) {
|
|
|
|
read_handle_packets(psvr);
|
|
|
|
read_control_packets(psvr);
|
|
|
|
|
|
|
|
if (psvr->in_vr_mode == on) {
|
|
|
|
return 0;
|
|
|
|
}
|
|
|
|
|
2019-09-27 10:44:54 +00:00
|
|
|
os_nanosleep(1000 * 1000);
|
2019-04-20 01:23:50 +00:00
|
|
|
}
|
|
|
|
|
|
|
|
return -1;
|
|
|
|
}
|
|
|
|
|
|
|
|
static int
|
|
|
|
control_power_and_wait(struct psvr_device *psvr, bool on)
|
|
|
|
{
|
|
|
|
const char *status = on ? "on" : "off";
|
|
|
|
const uint8_t data[8] = {
|
|
|
|
0x17, 0x00, 0xaa, 0x04, on, 0x00, 0x00, 0x00,
|
|
|
|
};
|
|
|
|
|
|
|
|
int ret = send_to_control(psvr, data, sizeof(data));
|
|
|
|
if (ret < 0) {
|
|
|
|
PSVR_ERROR(psvr, "Failed to switch %s the headset! '%i'",
|
|
|
|
status, ret);
|
|
|
|
}
|
|
|
|
|
|
|
|
ret = wait_for_power(psvr, on);
|
|
|
|
if (ret < 0) {
|
|
|
|
PSVR_ERROR(psvr, "Failed to wait for headset power %s! '%i'",
|
|
|
|
status, ret);
|
|
|
|
return ret;
|
|
|
|
}
|
|
|
|
|
|
|
|
return ret;
|
|
|
|
}
|
|
|
|
|
|
|
|
static int
|
|
|
|
control_vrmode_and_wait(struct psvr_device *psvr, bool on)
|
|
|
|
{
|
|
|
|
const uint8_t data[8] = {
|
|
|
|
0x23, 0x00, 0xaa, 0x04, on, 0x00, 0x00, 0x00,
|
|
|
|
};
|
|
|
|
int ret;
|
|
|
|
|
|
|
|
ret = send_to_control(psvr, data, sizeof(data));
|
|
|
|
if (ret < 0) {
|
|
|
|
PSVR_ERROR(psvr, "Failed %s vr-mode the headset! '%i'",
|
|
|
|
on ? "enable" : "disable", ret);
|
|
|
|
return ret;
|
|
|
|
}
|
|
|
|
|
|
|
|
ret = wait_for_vr_mode(psvr, on);
|
|
|
|
if (ret < 0) {
|
|
|
|
PSVR_ERROR(psvr, "Failed to wait for vr mode! '%i'", ret);
|
|
|
|
return ret;
|
|
|
|
}
|
|
|
|
|
|
|
|
return 0;
|
|
|
|
}
|
|
|
|
|
2019-08-31 11:47:20 +00:00
|
|
|
static int
|
|
|
|
update_leds_if_changed(struct psvr_device *psvr)
|
|
|
|
{
|
|
|
|
if (memcmp(psvr->wants.leds, psvr->state.leds,
|
|
|
|
sizeof(psvr->state.leds)) == 0) {
|
|
|
|
return 0;
|
|
|
|
}
|
|
|
|
|
|
|
|
memcpy(psvr->state.leds, psvr->wants.leds, sizeof(psvr->state.leds));
|
|
|
|
|
|
|
|
uint8_t data[20] = {
|
|
|
|
0x15,
|
|
|
|
0x00,
|
|
|
|
0xaa,
|
|
|
|
0x10,
|
|
|
|
(uint8_t)PSVR_LED_ALL,
|
|
|
|
(uint8_t)(PSVR_LED_ALL >> 8),
|
|
|
|
scale_led_power(psvr->state.leds[0]),
|
|
|
|
scale_led_power(psvr->state.leds[1]),
|
|
|
|
scale_led_power(psvr->state.leds[2]),
|
|
|
|
scale_led_power(psvr->state.leds[3]),
|
|
|
|
scale_led_power(psvr->state.leds[4]),
|
|
|
|
scale_led_power(psvr->state.leds[5]),
|
|
|
|
scale_led_power(psvr->state.leds[6]),
|
|
|
|
scale_led_power(psvr->state.leds[7]),
|
|
|
|
scale_led_power(psvr->state.leds[8]),
|
|
|
|
0,
|
|
|
|
0,
|
|
|
|
0,
|
|
|
|
0,
|
|
|
|
0,
|
|
|
|
};
|
|
|
|
|
|
|
|
return send_to_control(psvr, data, sizeof(data));
|
|
|
|
}
|
2019-04-20 01:23:50 +00:00
|
|
|
|
|
|
|
/*!
|
|
|
|
* Control the leds on the headset, allowing you to turn on and off different
|
|
|
|
* leds with a single call.
|
|
|
|
*
|
|
|
|
* @param[in] psvr The PSVR to control leds on.
|
|
|
|
* @param[in] adjust The leds to adjust with @p power.
|
|
|
|
* @param[in] power The power level to give to @p adjust leds.
|
|
|
|
* @param[in] off Leds that should be turned off,
|
|
|
|
* @p adjust has higher priority.
|
|
|
|
* @ingroup drv_psvr
|
|
|
|
*/
|
|
|
|
static int
|
|
|
|
control_leds(struct psvr_device *psvr,
|
|
|
|
enum psvr_leds adjust,
|
|
|
|
uint8_t power,
|
|
|
|
enum psvr_leds off)
|
|
|
|
{
|
|
|
|
// Get the leds we should control and remove any extra bits.
|
2019-05-06 16:57:23 +00:00
|
|
|
enum psvr_leds all = (enum psvr_leds)((adjust | off) & PSVR_LED_ALL);
|
2019-04-20 01:23:50 +00:00
|
|
|
if (all == 0) {
|
|
|
|
// Nothing todo.
|
|
|
|
return 0;
|
|
|
|
}
|
|
|
|
|
2019-08-31 11:47:20 +00:00
|
|
|
for (uint32_t i = 0; i < ARRAY_SIZE(psvr->wants.leds); i++) {
|
|
|
|
uint32_t mask = (1 << i);
|
|
|
|
if (adjust & mask) {
|
|
|
|
psvr->wants.leds[i] = power;
|
|
|
|
}
|
|
|
|
if (off & mask) {
|
|
|
|
psvr->wants.leds[i] = 0x00;
|
|
|
|
}
|
2019-04-20 01:23:50 +00:00
|
|
|
}
|
|
|
|
|
2019-08-31 11:47:20 +00:00
|
|
|
return update_leds_if_changed(psvr);
|
2019-04-20 01:23:50 +00:00
|
|
|
}
|
|
|
|
|
|
|
|
static int
|
|
|
|
disco_leds(struct psvr_device *psvr)
|
|
|
|
{
|
|
|
|
static const uint16_t leds[] = {
|
|
|
|
// First loop
|
|
|
|
PSVR_LED_A,
|
|
|
|
PSVR_LED_E,
|
|
|
|
PSVR_LED_B,
|
|
|
|
PSVR_LED_G,
|
|
|
|
PSVR_LED_D,
|
|
|
|
PSVR_LED_C,
|
|
|
|
PSVR_LED_F,
|
|
|
|
// Second loop
|
|
|
|
PSVR_LED_A,
|
|
|
|
PSVR_LED_E,
|
|
|
|
PSVR_LED_B,
|
|
|
|
PSVR_LED_G,
|
|
|
|
PSVR_LED_D,
|
|
|
|
PSVR_LED_C,
|
|
|
|
PSVR_LED_F,
|
|
|
|
// Blink loop
|
|
|
|
PSVR_LED_BACK,
|
|
|
|
PSVR_LED_FRONT,
|
|
|
|
PSVR_LED_BACK,
|
|
|
|
PSVR_LED_FRONT,
|
|
|
|
// All on after loop
|
|
|
|
PSVR_LED_ALL,
|
|
|
|
};
|
|
|
|
|
|
|
|
for (size_t i = 0; i < ARRAY_SIZE(leds); i++) {
|
2019-05-06 16:57:23 +00:00
|
|
|
int ret = control_leds(psvr, (enum psvr_leds)leds[i],
|
|
|
|
PSVR_LED_POWER_MAX, PSVR_LED_ALL);
|
2019-04-20 01:23:50 +00:00
|
|
|
if (ret < 0) {
|
|
|
|
return ret;
|
|
|
|
}
|
|
|
|
|
|
|
|
// Sleep for a tenth of a second while polling for packages.
|
|
|
|
for (int k = 0; k < 100; k++) {
|
|
|
|
ret = read_handle_packets(psvr);
|
|
|
|
if (ret < 0) {
|
|
|
|
return ret;
|
|
|
|
}
|
|
|
|
|
|
|
|
ret = read_control_packets(psvr);
|
|
|
|
if (ret < 0) {
|
|
|
|
return ret;
|
|
|
|
}
|
|
|
|
|
2019-09-27 10:44:54 +00:00
|
|
|
os_nanosleep(1000 * 1000);
|
2019-04-20 01:23:50 +00:00
|
|
|
}
|
|
|
|
}
|
|
|
|
|
|
|
|
return 0;
|
|
|
|
}
|
|
|
|
|
|
|
|
static void
|
|
|
|
teardown(struct psvr_device *psvr)
|
|
|
|
{
|
2019-09-09 10:42:54 +00:00
|
|
|
// Stop the variable tracking.
|
|
|
|
u_var_remove_root(psvr);
|
|
|
|
|
|
|
|
// Includes null check, and sets to null.
|
|
|
|
xrt_tracked_psvr_destroy(&psvr->tracker);
|
|
|
|
|
2019-04-20 01:23:50 +00:00
|
|
|
if (psvr->hmd_control != NULL) {
|
|
|
|
// Turn off VR-mode and power down headset.
|
|
|
|
if (control_vrmode_and_wait(psvr, false) < 0 ||
|
|
|
|
control_power_and_wait(psvr, false) < 0) {
|
|
|
|
PSVR_ERROR(psvr, "Failed to shut down the headset!");
|
|
|
|
}
|
|
|
|
|
|
|
|
hid_close(psvr->hmd_control);
|
|
|
|
psvr->hmd_control = NULL;
|
|
|
|
}
|
|
|
|
|
|
|
|
if (psvr->hmd_handle != NULL) {
|
|
|
|
hid_close(psvr->hmd_handle);
|
|
|
|
psvr->hmd_handle = NULL;
|
|
|
|
}
|
2020-04-17 18:28:03 +00:00
|
|
|
|
|
|
|
// Destroy the fusion.
|
|
|
|
m_imu_3dof_close(&psvr->fusion);
|
2019-04-20 01:23:50 +00:00
|
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
/*
|
|
|
|
*
|
|
|
|
* xrt_device functions.
|
|
|
|
*
|
|
|
|
*/
|
|
|
|
|
2019-05-09 10:54:48 +00:00
|
|
|
static void
|
2020-04-16 12:23:12 +00:00
|
|
|
psvr_device_update_inputs(struct xrt_device *xdev)
|
2019-05-09 10:54:48 +00:00
|
|
|
{
|
2019-08-31 11:47:20 +00:00
|
|
|
struct psvr_device *psvr = psvr_device(xdev);
|
|
|
|
|
2019-09-11 15:06:55 +00:00
|
|
|
read_handle_packets(psvr);
|
2019-08-31 11:47:20 +00:00
|
|
|
update_leds_if_changed(psvr);
|
2019-05-09 10:54:48 +00:00
|
|
|
}
|
|
|
|
|
2019-04-20 01:23:50 +00:00
|
|
|
static void
|
|
|
|
psvr_device_get_tracked_pose(struct xrt_device *xdev,
|
2019-05-09 10:54:48 +00:00
|
|
|
enum xrt_input_name name,
|
2020-04-16 12:23:12 +00:00
|
|
|
uint64_t at_timestamp_ns,
|
|
|
|
uint64_t *out_relation_timestamp_ns,
|
2019-04-20 01:23:50 +00:00
|
|
|
struct xrt_space_relation *out_relation)
|
|
|
|
{
|
|
|
|
struct psvr_device *psvr = psvr_device(xdev);
|
|
|
|
|
2019-10-24 20:51:07 +00:00
|
|
|
if (name != XRT_INPUT_GENERIC_HEAD_POSE) {
|
2019-05-09 10:54:48 +00:00
|
|
|
PSVR_ERROR(psvr, "unknown input name");
|
|
|
|
return;
|
|
|
|
}
|
|
|
|
|
2019-04-20 01:23:50 +00:00
|
|
|
// Read all packets.
|
|
|
|
read_handle_packets(psvr);
|
|
|
|
read_control_packets(psvr);
|
|
|
|
|
|
|
|
// Clear out the relation.
|
2019-06-18 19:04:01 +00:00
|
|
|
U_ZERO(out_relation);
|
2019-04-20 01:23:50 +00:00
|
|
|
|
2019-09-09 10:42:54 +00:00
|
|
|
// We have no tracking, don't return a position.
|
|
|
|
if (psvr->tracker == NULL) {
|
|
|
|
out_relation->pose.orientation = psvr->fusion.rot;
|
2019-10-22 18:51:05 +00:00
|
|
|
|
2019-09-09 10:42:54 +00:00
|
|
|
out_relation->relation_flags = (enum xrt_space_relation_flags)(
|
|
|
|
XRT_SPACE_RELATION_ORIENTATION_VALID_BIT |
|
|
|
|
XRT_SPACE_RELATION_ORIENTATION_TRACKED_BIT);
|
2020-04-16 12:23:12 +00:00
|
|
|
|
2020-05-30 11:44:22 +00:00
|
|
|
|
2020-04-16 12:23:12 +00:00
|
|
|
*out_relation_timestamp_ns = os_monotonic_get_ns();
|
2019-09-09 10:42:54 +00:00
|
|
|
} else {
|
2020-04-16 12:23:12 +00:00
|
|
|
xrt_tracked_psvr_get_tracked_pose(
|
|
|
|
psvr->tracker, at_timestamp_ns, out_relation);
|
2020-05-30 11:44:22 +00:00
|
|
|
|
2020-04-16 12:23:12 +00:00
|
|
|
*out_relation_timestamp_ns = at_timestamp_ns;
|
2019-09-09 10:42:54 +00:00
|
|
|
}
|
2020-05-30 11:44:22 +00:00
|
|
|
|
|
|
|
//! @todo Move this to the tracker.
|
|
|
|
// Make sure that the orientation is valid.
|
|
|
|
math_quat_normalize(&out_relation->pose.orientation);
|
2019-04-20 01:23:50 +00:00
|
|
|
}
|
|
|
|
|
|
|
|
static void
|
|
|
|
psvr_device_get_view_pose(struct xrt_device *xdev,
|
|
|
|
struct xrt_vec3 *eye_relation,
|
|
|
|
uint32_t view_index,
|
|
|
|
struct xrt_pose *out_pose)
|
|
|
|
{
|
|
|
|
struct xrt_pose pose = {{0.0f, 0.0f, 0.0f, 1.0f}, {0.0f, 0.0f, 0.0f}};
|
|
|
|
bool adjust = view_index == 0;
|
|
|
|
|
|
|
|
pose.position.x = eye_relation->x / 2.0f;
|
|
|
|
pose.position.y = eye_relation->y / 2.0f;
|
|
|
|
pose.position.z = eye_relation->z / 2.0f;
|
|
|
|
|
|
|
|
// Adjust for left/right while also making sure there aren't any -0.f.
|
|
|
|
if (pose.position.x > 0.0f && adjust) {
|
|
|
|
pose.position.x = -pose.position.x;
|
|
|
|
}
|
|
|
|
if (pose.position.y > 0.0f && adjust) {
|
|
|
|
pose.position.y = -pose.position.y;
|
|
|
|
}
|
|
|
|
if (pose.position.z > 0.0f && adjust) {
|
|
|
|
pose.position.z = -pose.position.z;
|
|
|
|
}
|
|
|
|
|
|
|
|
*out_pose = pose;
|
|
|
|
}
|
|
|
|
|
|
|
|
static void
|
|
|
|
psvr_device_destroy(struct xrt_device *xdev)
|
|
|
|
{
|
|
|
|
struct psvr_device *psvr = psvr_device(xdev);
|
|
|
|
teardown(psvr);
|
2019-10-23 22:28:46 +00:00
|
|
|
|
|
|
|
u_device_free(&psvr->base);
|
2019-04-20 01:23:50 +00:00
|
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
/*
|
|
|
|
*
|
|
|
|
* Exported functions.
|
|
|
|
*
|
|
|
|
*/
|
|
|
|
|
|
|
|
struct xrt_device *
|
|
|
|
psvr_device_create(struct hid_device_info *hmd_handle_info,
|
|
|
|
struct hid_device_info *hmd_control_info,
|
2019-09-09 10:42:54 +00:00
|
|
|
struct xrt_prober *xp,
|
2019-04-20 01:23:50 +00:00
|
|
|
bool print_spew,
|
|
|
|
bool print_debug)
|
|
|
|
{
|
2019-05-06 16:57:23 +00:00
|
|
|
enum u_device_alloc_flags flags = (enum u_device_alloc_flags)(
|
|
|
|
U_DEVICE_ALLOC_HMD | U_DEVICE_ALLOC_TRACKING_NONE);
|
2019-05-09 10:54:48 +00:00
|
|
|
struct psvr_device *psvr =
|
2019-06-07 12:22:59 +00:00
|
|
|
U_DEVICE_ALLOCATE(struct psvr_device, flags, 1, 0);
|
2019-04-20 01:23:50 +00:00
|
|
|
int ret;
|
|
|
|
|
|
|
|
psvr->print_spew = print_spew;
|
|
|
|
psvr->print_debug = print_debug;
|
2019-05-09 10:54:48 +00:00
|
|
|
psvr->base.update_inputs = psvr_device_update_inputs;
|
2019-04-20 01:23:50 +00:00
|
|
|
psvr->base.get_tracked_pose = psvr_device_get_tracked_pose;
|
|
|
|
psvr->base.get_view_pose = psvr_device_get_view_pose;
|
2019-05-09 10:54:48 +00:00
|
|
|
psvr->base.destroy = psvr_device_destroy;
|
2019-10-24 20:51:07 +00:00
|
|
|
psvr->base.inputs[0].name = XRT_INPUT_GENERIC_HEAD_POSE;
|
2019-09-04 11:43:57 +00:00
|
|
|
psvr->base.name = XRT_DEVICE_GENERIC_HMD;
|
2019-10-23 20:57:31 +00:00
|
|
|
|
2019-10-23 22:28:46 +00:00
|
|
|
{
|
|
|
|
struct u_panotools_values vals = {0};
|
|
|
|
|
2019-10-24 01:56:02 +00:00
|
|
|
vals.distortion_k[0] = 0.75;
|
|
|
|
vals.distortion_k[1] = -0.01;
|
|
|
|
vals.distortion_k[2] = 0.75;
|
2019-10-23 22:28:46 +00:00
|
|
|
vals.distortion_k[3] = 0.0;
|
2019-10-24 01:56:02 +00:00
|
|
|
vals.distortion_k[4] = 3.8;
|
2019-10-23 22:28:46 +00:00
|
|
|
vals.aberration_k[0] = 0.999;
|
|
|
|
vals.aberration_k[1] = 1.008;
|
|
|
|
vals.aberration_k[2] = 1.018;
|
2019-11-05 12:08:32 +00:00
|
|
|
vals.scale = 1.2 * (1980 / 2.0f);
|
|
|
|
vals.viewport_size.x = (1980 / 2.0f);
|
|
|
|
vals.viewport_size.y = (1080);
|
|
|
|
vals.lens_center.x = vals.viewport_size.x / 2.0;
|
|
|
|
vals.lens_center.y = vals.viewport_size.y / 2.0;
|
2019-10-23 22:28:46 +00:00
|
|
|
|
|
|
|
u_distortion_mesh_from_panotools(&vals, &vals, psvr->base.hmd);
|
|
|
|
}
|
|
|
|
|
2020-04-17 18:28:03 +00:00
|
|
|
#if 1
|
|
|
|
m_imu_3dof_init(&psvr->fusion, M_IMU_3DOF_USE_GRAVITY_DUR_20MS);
|
2019-10-22 18:51:05 +00:00
|
|
|
#else
|
2019-09-11 15:42:59 +00:00
|
|
|
psvr->fusion.rot.w = 1.0f;
|
2019-10-22 18:51:05 +00:00
|
|
|
#endif
|
2019-04-20 01:23:50 +00:00
|
|
|
|
2019-09-04 11:43:57 +00:00
|
|
|
snprintf(psvr->base.str, XRT_DEVICE_NAME_LEN, "PS VR Headset");
|
2019-06-21 20:55:43 +00:00
|
|
|
|
2019-04-20 01:23:50 +00:00
|
|
|
ret = open_hid(psvr, hmd_handle_info, &psvr->hmd_handle);
|
|
|
|
if (ret != 0) {
|
|
|
|
goto cleanup;
|
|
|
|
}
|
|
|
|
|
|
|
|
ret = open_hid(psvr, hmd_control_info, &psvr->hmd_control);
|
|
|
|
if (ret < 0) {
|
|
|
|
goto cleanup;
|
|
|
|
}
|
|
|
|
|
|
|
|
if (control_power_and_wait(psvr, true) < 0 ||
|
|
|
|
control_vrmode_and_wait(psvr, true) < 0) {
|
|
|
|
goto cleanup;
|
|
|
|
}
|
|
|
|
|
2019-10-25 09:54:39 +00:00
|
|
|
// Device is now on and we can read calibration data now.
|
|
|
|
ret = read_calibration_data(psvr);
|
|
|
|
if (ret < 0) {
|
|
|
|
goto cleanup;
|
|
|
|
}
|
|
|
|
|
2019-04-20 01:23:50 +00:00
|
|
|
if (debug_get_bool_option_psvr_disco()) {
|
|
|
|
ret = disco_leds(psvr);
|
|
|
|
} else {
|
2019-05-06 16:57:23 +00:00
|
|
|
ret = control_leds(psvr, PSVR_LED_ALL, PSVR_LED_POWER_MAX,
|
|
|
|
(enum psvr_leds)0);
|
2019-04-20 01:23:50 +00:00
|
|
|
}
|
|
|
|
if (ret < 0) {
|
|
|
|
PSVR_ERROR(psvr, "Failed to control leds '%i'", ret);
|
|
|
|
goto cleanup;
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
/*
|
|
|
|
* Device setup.
|
|
|
|
*/
|
|
|
|
|
|
|
|
struct u_device_simple_info info;
|
2019-10-08 10:18:40 +00:00
|
|
|
info.display.w_pixels = 1920;
|
2019-04-20 01:23:50 +00:00
|
|
|
info.display.h_pixels = 1080;
|
2019-11-05 12:08:32 +00:00
|
|
|
info.display.w_meters = 0.13f;
|
|
|
|
info.display.h_meters = 0.07f;
|
|
|
|
info.lens_horizontal_separation_meters = 0.13f / 2.0f;
|
|
|
|
info.lens_vertical_position_meters = 0.07f / 2.0f;
|
2019-10-24 01:56:02 +00:00
|
|
|
info.views[0].fov = 85.0f * (M_PI / 180.0f);
|
|
|
|
info.views[1].fov = 85.0f * (M_PI / 180.0f);
|
2019-04-20 01:23:50 +00:00
|
|
|
|
|
|
|
if (!u_device_setup_split_side_by_side(&psvr->base, &info)) {
|
|
|
|
PSVR_ERROR(psvr, "Failed to setup basic device info");
|
|
|
|
goto cleanup;
|
|
|
|
}
|
|
|
|
|
2019-08-31 11:47:20 +00:00
|
|
|
/*
|
|
|
|
* Setup variable.
|
|
|
|
*/
|
|
|
|
|
2019-09-11 15:05:10 +00:00
|
|
|
// clang-format off
|
2019-08-31 11:47:20 +00:00
|
|
|
u_var_add_root(psvr, "PS VR Headset", true);
|
2019-09-11 15:05:10 +00:00
|
|
|
u_var_add_gui_header(psvr, &psvr->gui.last_frame, "Last data");
|
|
|
|
u_var_add_ro_vec3_i32(psvr, &psvr->last.samples[0].accel, "last.samples[0].accel");
|
|
|
|
u_var_add_ro_vec3_i32(psvr, &psvr->last.samples[1].accel, "last.samples[1].accel");
|
|
|
|
u_var_add_ro_vec3_i32(psvr, &psvr->last.samples[0].gyro, "last.samples[0].gyro");
|
|
|
|
u_var_add_ro_vec3_i32(psvr, &psvr->last.samples[1].gyro, "last.samples[1].gyro");
|
|
|
|
u_var_add_ro_vec3_f32(psvr, &psvr->read.accel, "read.accel");
|
|
|
|
u_var_add_ro_vec3_f32(psvr, &psvr->read.gyro, "read.gyro");
|
|
|
|
u_var_add_gui_header(psvr, &psvr->gui.control, "Control");
|
2019-08-31 11:47:20 +00:00
|
|
|
u_var_add_u8(psvr, &psvr->wants.leds[0], "Led A");
|
|
|
|
u_var_add_u8(psvr, &psvr->wants.leds[1], "Led B");
|
|
|
|
u_var_add_u8(psvr, &psvr->wants.leds[2], "Led C");
|
|
|
|
u_var_add_u8(psvr, &psvr->wants.leds[3], "Led D");
|
|
|
|
u_var_add_u8(psvr, &psvr->wants.leds[4], "Led E");
|
|
|
|
u_var_add_u8(psvr, &psvr->wants.leds[5], "Led F");
|
|
|
|
u_var_add_u8(psvr, &psvr->wants.leds[6], "Led G");
|
|
|
|
u_var_add_u8(psvr, &psvr->wants.leds[7], "Led H");
|
|
|
|
u_var_add_u8(psvr, &psvr->wants.leds[8], "Led I");
|
|
|
|
u_var_add_bool(psvr, &psvr->print_debug, "Debug");
|
|
|
|
u_var_add_bool(psvr, &psvr->print_spew, "Spew");
|
2019-09-11 15:05:10 +00:00
|
|
|
// clang-format on
|
2019-04-20 01:23:50 +00:00
|
|
|
|
|
|
|
/*
|
|
|
|
* Finishing touches.
|
|
|
|
*/
|
|
|
|
|
|
|
|
if (psvr->print_debug) {
|
|
|
|
u_device_dump_config(&psvr->base, __func__, "Sony PSVR");
|
|
|
|
}
|
|
|
|
|
2019-09-09 10:42:54 +00:00
|
|
|
// If there is a tracking factory use it.
|
|
|
|
if (xp->tracking != NULL) {
|
|
|
|
xp->tracking->create_tracked_psvr(xp->tracking, &psvr->base,
|
|
|
|
&psvr->tracker);
|
|
|
|
}
|
|
|
|
|
|
|
|
// Use the new origin if we got a tracking system.
|
|
|
|
if (psvr->tracker != NULL) {
|
|
|
|
psvr->base.tracking_origin = psvr->tracker->origin;
|
|
|
|
}
|
|
|
|
|
2019-04-20 01:23:50 +00:00
|
|
|
PSVR_DEBUG(psvr, "YES!");
|
|
|
|
|
|
|
|
return &psvr->base;
|
|
|
|
|
|
|
|
|
|
|
|
cleanup:
|
|
|
|
PSVR_DEBUG(psvr, "NO! :(");
|
|
|
|
|
|
|
|
teardown(psvr);
|
|
|
|
free(psvr);
|
|
|
|
|
|
|
|
return NULL;
|
|
|
|
}
|