monado/src/xrt/drivers/psvr/psvr_device.c

// Copyright 2016, Joey Ferwerda.
// Copyright 2019-2022, Collabora, Ltd.
// 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"
#include "xrt/xrt_tracking.h"

#include "os/os_time.h"
#include "os/os_threading.h"

#include "math/m_api.h"

#include "util/u_var.h"
#include "util/u_misc.h"
#include "util/u_time.h"
#include "util/u_debug.h"
#include "util/u_device.h"
#include "util/u_trace_marker.h"
#include "util/u_distortion_mesh.h"

#include "math/m_imu_3dof.h"

#include "math/m_mathinclude.h"

#include <stdio.h>
#include <stdlib.h>
#include <string.h>

#include "psvr_device.h"


/*
 *
 * Structs and defines.
 *
 */

DEBUG_GET_ONCE_BOOL_OPTION(psvr_disco, "PSVR_DISCO", false)
#define PSVR_DEBUG(p, ...) U_LOG_XDEV_IFL_D(&p->base, p->log_level, __VA_ARGS__)
#define PSVR_ERROR(p, ...) U_LOG_XDEV_IFL_E(&p->base, p->log_level, __VA_ARGS__)

#define FEATURE_BUFFER_SIZE 256

/*!
 * Private struct for the @ref drv_psvr device.
 *
 * @ingroup drv_psvr
 * @implements xrt_device
 */
struct psvr_device
{
	struct xrt_device base;

	//! Owned by the @ref oth thread.
	hid_device *hid_sensor;

	//! Owned and protected by the device_mutex.
	hid_device *hid_control;
	struct os_mutex device_mutex;

	//! Used to read sensor packets.
	struct os_thread_helper oth;

	struct xrt_tracked_psvr *tracker;

	//! Only touched from the sensor thread.
	timepoint_ns last_sensor_time;

	struct psvr_parsed_sensor last;

	struct
	{
		uint8_t leds[9];
	} wants;

	struct
	{
		uint8_t leds[9];
	} state;

	struct
	{
		struct xrt_vec3 gyro;
		struct xrt_vec3 accel;
	} read;

	uint16_t buttons;

	bool powered_on;
	bool in_vr_mode;

	enum u_logging_level log_level;

	struct
	{
		union {
			uint8_t data[290];
			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];
			};
		};
		int last_packet;
	} calibration;


	struct
	{
		bool last_frame;
		bool control;
	} gui;

#if 1
	struct m_imu_3dof fusion;
#else
	struct
	{
		struct xrt_quat rot;
	} fusion;
#endif

	//! For compute_distortion
	struct u_panotools_values vals;
};


// 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,
};


#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)


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->hid_control, data, size);
}

static int
send_request_data(struct psvr_device *psvr, uint8_t id, uint8_t num)
{
	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));
}

static uint8_t
scale_led_power(uint8_t power)
{
	return (uint8_t)((power / 255.0f) * 100.0f);
}


/*
 *
 * Sensor functions.
 *
 */

static void
read_sample_and_apply_calibration(struct psvr_device *psvr,
                                  struct psvr_parsed_sample *sample,
                                  struct xrt_vec3 *out_accel,
                                  struct xrt_vec3 *out_gyro)
{
	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 = {
	    raw_accel.x / 16384.0f,
	    raw_accel.y / 16384.0f,
	    raw_accel.z / 16384.0f,
	};

	// What unit is this?
	struct xrt_vec3 gyro = {
	    raw_gyro.x * 0.00105f,
	    raw_gyro.y * 0.00105f,
	    raw_gyro.z * 0.00105f,
	};

	float ax = 2.0f / (psvr->calibration.accel_pos_x.x - psvr->calibration.accel_neg_x.x);
	float ay = 2.0f / (psvr->calibration.accel_pos_y.y - psvr->calibration.accel_neg_y.y);
	float az = 2.0f / (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.0f;
	float oy = (psvr->calibration.accel_pos_y.y + psvr->calibration.accel_neg_y.y) / 2.0f;
	float oz = (psvr->calibration.accel_pos_z.z + psvr->calibration.accel_neg_z.z) / 2.0f;

	accel.x -= ox;
	accel.y -= oy;
	accel.z -= oz;
	accel.x *= ax;
	accel.y *= ay;
	accel.z *= az;

	// Go from Gs to m/s2 and flip the Z-axis.
	accel.x *= (float)+MATH_GRAVITY_M_S2;
	accel.y *= (float)+MATH_GRAVITY_M_S2;
	accel.z *= (float)-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;
}

static void
update_fusion_locked(struct psvr_device *psvr, struct psvr_parsed_sample *sample, uint64_t timestamp_ns)
{
	struct xrt_vec3 mag = {0.0f, 0.0f, 0.0f};
	(void)mag;

	read_sample_and_apply_calibration(psvr, sample, &psvr->read.accel, &psvr->read.gyro);

	if (psvr->tracker != NULL) {
		struct xrt_tracking_sample sample;
		sample.accel_m_s2 = psvr->read.accel;
		sample.gyro_rad_secs = psvr->read.gyro;

		xrt_tracked_psvr_push_imu(psvr->tracker, timestamp_ns, &sample);
	} else {
		m_imu_3dof_update(&psvr->fusion, timestamp_ns, &psvr->read.accel, &psvr->read.gyro);
	}
}

static void
update_fusion(struct psvr_device *psvr, struct psvr_parsed_sample *sample, uint64_t timestamp_ns)
{
	os_mutex_lock(&psvr->device_mutex);
	update_fusion_locked(psvr, sample, timestamp_ns);
	os_mutex_unlock(&psvr->device_mutex);
}

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 timepoint_ns
ensure_forward_progress_timestamps(struct psvr_device *psvr, timepoint_ns timestamp_ns)
{
	timepoint_ns t = timestamp_ns;

	/*
	 * This make sure the timestamp is after the last we sent to the fusion,
	 * but it effectively drops the sample.
	 */
	if (psvr->last_sensor_time > t) {
		t = psvr->last_sensor_time + 1;
	}

	psvr->last_sensor_time = t;
	return t;
}

static void
handle_tracker_sensor_msg(struct psvr_device *psvr, unsigned char *buffer, int size)
{
	timepoint_ns now_ns = os_monotonic_get_ns();
	uint32_t last_sample_tick = psvr->last.samples[1].tick;

	if (!psvr_parse_sensor_packet(&psvr->last, buffer, size)) {
		PSVR_ERROR(psvr, "couldn't decode tracker sensor message");
	}

	struct psvr_parsed_sensor *s = &psvr->last;

	// 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;
		}
	}

	// New delta between the two samples.
	uint32_t tick_delta2 = calc_delta_and_handle_rollover(s->samples[1].tick, s->samples[0].tick);

	time_duration_ns inter_sample_duration_ns = tick_delta2 * PSVR_NS_PER_TICK;

	// If this is larger then one second something bad is going on.
	assert(inter_sample_duration_ns < U_TIME_1S_IN_NS);

	// Move it back in time.
	timepoint_ns timestamp_ns = (uint64_t)now_ns - (uint64_t)inter_sample_duration_ns;

	// Make sure timestamps are always after a previous timestamp.
	timestamp_ns = ensure_forward_progress_timestamps(psvr, timestamp_ns);

	// Update the fusion with first sample.
	update_fusion(psvr, &s->samples[0], timestamp_ns);

	// Make sure timestamps are always after a previous timestamp.
	timestamp_ns = ensure_forward_progress_timestamps(psvr, now_ns);

	// Update the fusion with second sample.
	update_fusion(psvr, &s->samples[1], timestamp_ns);
}

static void
sensor_clear_queue(struct psvr_device *psvr)
{
	uint8_t buffer[FEATURE_BUFFER_SIZE];

	while (hid_read(psvr->hid_sensor, buffer, FEATURE_BUFFER_SIZE) > 0) {
		// Just drop the packets.
	}
}

static int
sensor_read_one_packet(struct psvr_device *psvr)
{
	uint8_t buffer[FEATURE_BUFFER_SIZE];

	// Try for one second to get packates.
	int size = hid_read_timeout(psvr->hid_sensor, buffer, FEATURE_BUFFER_SIZE, 1000);
	if (size <= 0) {
		return size;
	}

	handle_tracker_sensor_msg(psvr, buffer, size);

	return 0;
}

static void *
sensor_thread(void *ptr)
{
	U_TRACE_SET_THREAD_NAME("PS VR");

	struct psvr_device *psvr = (struct psvr_device *)ptr;
	int ret = 0;

	// Empty the queue.
	sensor_clear_queue(psvr);

	os_thread_helper_lock(&psvr->oth);

	while (os_thread_helper_is_running_locked(&psvr->oth) && ret >= 0) {
		os_thread_helper_unlock(&psvr->oth);

		ret = sensor_read_one_packet(psvr);

		os_thread_helper_lock(&psvr->oth);
	}

	os_thread_helper_unlock(&psvr->oth);

	return NULL;
}


/*
 *
 * Control device handling.
 *
 */

static void
handle_control_status_msg(struct psvr_device *psvr, unsigned char *buffer, int size)
{
	struct psvr_parsed_status status = {0};

	if (!psvr_parse_status_packet(&status, buffer, size)) {
		PSVR_ERROR(psvr, "couldn't decode tracker sensor message");
	}


	/*
	 * Power
	 */

	if (status.status & PSVR_STATUS_BIT_POWER) {
		if (!psvr->powered_on) {
			PSVR_DEBUG(psvr, "Device powered on! '%02x'", status.status);
		}
		psvr->powered_on = true;
	} else {
		if (psvr->powered_on) {
			PSVR_DEBUG(psvr, "Device powered off! '%02x'", status.status);
		}
		psvr->powered_on = false;
	}


	/*
	 * VR-Mode
	 */

	if (status.vr_mode == PSVR_STATUS_VR_MODE_OFF) {
		if (psvr->in_vr_mode) {
			PSVR_DEBUG(psvr, "Device not in vr-mode! '%02x'", status.vr_mode);
		}
		psvr->in_vr_mode = false;
	} else if (status.vr_mode == PSVR_STATUS_VR_MODE_ON) {
		if (!psvr->in_vr_mode) {
			PSVR_DEBUG(psvr, "Device in vr-mode! '%02x'", status.vr_mode);
		}
		psvr->in_vr_mode = true;
	} else {
		PSVR_ERROR(psvr, "Unknown vr_mode status!");
	}
}

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, const unsigned char *buffer, 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;
	}

	uint16_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;
}

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]);
}

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_control_packets(struct psvr_device *psvr)
{
	uint8_t buffer[FEATURE_BUFFER_SIZE];
	int size = 0;

	do {
		size = hid_read(psvr->hid_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);
		} 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);
		} else if (buffer[0] == PSVR_PKG_0x82) {
			handle_control_0x82(psvr, buffer, size);
		} else if (buffer[0] == PSVR_PKG_0xA0) {
			handle_control_0xA0(psvr, buffer, size);
		} else {
			PSVR_ERROR(psvr, "Got report, 0x%02x", buffer[0]);
		}

	} while (true);
}


/*!
 * 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
 */
static int
read_calibration_data(struct psvr_device *psvr)
{
	// Request the device name.
	int ret = send_request_data(psvr, PSVR_GET_DATA_ID_DEVICE_NAME, 0);
	if (ret < 0) {
		return ret;
	}

	// Request unknown data 0x82.
	ret = send_request_data(psvr, PSVR_GET_DATA_ID_0x82, 0);
	if (ret < 0) {
		return ret;
	}

	// There are 5 pages of PSVR calibration data.
	for (int i = 0; i < 5; i++) {
		// Request the IMU calibration data.
		ret = send_request_data(psvr, PSVR_GET_DATA_ID_CALIBRATION, i);
		if (ret < 0) {
			return ret;
		}
	}

	// We have requested 5 pages worth of data, wait for the replies.
	for (int i = 0; i < 100; i++) {
		os_nanosleep(1000 * 1000);
		read_control_packets(psvr);

		// If we have gotten of the packets stop wait.
		if (psvr->calibration.last_packet == 4) {
			break;
		}
	}

	// Did we really get all of the data?
	if (psvr->calibration.last_packet != 4) {
		PSVR_ERROR(psvr, "Failed to get calibration");
		return -1;
	}

	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);

#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];
		U_LOG_E("%i %f", v, *(float*)&v);
	}
#endif

	// Jobs done!
	//  - Orc peon.
	return 0;
}


/*
 *
 * Control sending functions.
 *
 */

static int
wait_for_power(struct psvr_device *psvr, bool on)
{
	for (int i = 0; i < 5000; i++) {
		read_control_packets(psvr);

		if (psvr->powered_on == on) {
			return 0;
		}

		os_nanosleep(1000 * 1000);
	}

	return -1;
}

static int
wait_for_vr_mode(struct psvr_device *psvr, bool on)
{
	for (int i = 0; i < 5000; i++) {
		read_control_packets(psvr);

		if (psvr->in_vr_mode == on) {
			return 0;
		}

		os_nanosleep(1000 * 1000);
	}

	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;
}

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));
}

/*!
 * 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.
	enum psvr_leds all = (enum psvr_leds)((adjust | off) & PSVR_LED_ALL);
	if (all == 0) {
		// Nothing todo.
		return 0;
	}

	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;
		}
	}

	return update_leds_if_changed(psvr);
}

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++) {
		int ret = control_leds(psvr, (enum psvr_leds)leds[i], PSVR_LED_POWER_MAX, PSVR_LED_ALL);
		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_control_packets(psvr);
			if (ret < 0) {
				return ret;
			}

			os_nanosleep(1000 * 1000);
		}
	}

	return 0;
}


/*
 *
 * Misc functions.
 *
 */

static void
teardown(struct psvr_device *psvr)
{
	// Stop the variable tracking.
	u_var_remove_root(psvr);

	// Shutdown the sensor thread early.
	os_thread_helper_stop_and_wait(&psvr->oth);

	// Includes null check, and sets to null.
	xrt_tracked_psvr_destroy(&psvr->tracker);

	if (psvr->hid_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->hid_control);
		psvr->hid_control = NULL;
	}

	if (psvr->hid_sensor != NULL) {
		hid_close(psvr->hid_sensor);
		psvr->hid_sensor = NULL;
	}

	// Destroy the fusion.
	m_imu_3dof_close(&psvr->fusion);

	os_thread_helper_destroy(&psvr->oth);
	os_mutex_destroy(&psvr->device_mutex);
}


/*
 *
 * xrt_device functions.
 *
 */

static void
psvr_device_update_inputs(struct xrt_device *xdev)
{
	struct psvr_device *psvr = psvr_device(xdev);

	os_mutex_lock(&psvr->device_mutex);

	update_leds_if_changed(psvr);

	os_mutex_unlock(&psvr->device_mutex);
}

static void
psvr_device_get_tracked_pose(struct xrt_device *xdev,
                             enum xrt_input_name name,
                             uint64_t at_timestamp_ns,
                             struct xrt_space_relation *out_relation)
{
	struct psvr_device *psvr = psvr_device(xdev);

	if (name != XRT_INPUT_GENERIC_HEAD_POSE) {
		PSVR_ERROR(psvr, "unknown input name");
		return;
	}

	os_mutex_lock(&psvr->device_mutex);

	// Read all packets.
	read_control_packets(psvr);

	// Clear out the relation.
	U_ZERO(out_relation);

	// We have no tracking, don't return a position.
	if (psvr->tracker == NULL) {
		out_relation->pose.orientation = psvr->fusion.rot;

		out_relation->relation_flags = (enum xrt_space_relation_flags)(
		    XRT_SPACE_RELATION_ORIENTATION_VALID_BIT | XRT_SPACE_RELATION_ORIENTATION_TRACKED_BIT);
	} else {
		xrt_tracked_psvr_get_tracked_pose(psvr->tracker, at_timestamp_ns, out_relation);
	}

	os_mutex_unlock(&psvr->device_mutex);

	//! @todo Move this to the tracker.
	// Make sure that the orientation is valid.
	math_quat_normalize(&out_relation->pose.orientation);
}

static void
psvr_device_get_view_poses(struct xrt_device *xdev,
                           const struct xrt_vec3 *default_eye_relation,
                           uint64_t at_timestamp_ns,
                           uint32_t view_count,
                           struct xrt_space_relation *out_head_relation,
                           struct xrt_fov *out_fovs,
                           struct xrt_pose *out_poses)
{
	u_device_get_view_poses(xdev, default_eye_relation, at_timestamp_ns, view_count, out_head_relation, out_fovs,
	                        out_poses);
}

static void
psvr_device_destroy(struct xrt_device *xdev)
{
	struct psvr_device *psvr = psvr_device(xdev);
	teardown(psvr);

	u_device_free(&psvr->base);
}

static bool
psvr_compute_distortion(struct xrt_device *xdev, uint32_t view, float u, float v, struct xrt_uv_triplet *result)
{
	struct psvr_device *psvr = psvr_device(xdev);

	return u_compute_distortion_panotools(&psvr->vals, u, v, result);
}


/*
 *
 * Exported functions.
 *
 */

struct xrt_device *
psvr_device_create_auto_prober(struct hid_device_info *sensor_hid_info,
                               struct hid_device_info *control_hid_info,
                               struct xrt_tracked_psvr *tracker,
                               enum u_logging_level log_level)
{
	enum u_device_alloc_flags flags =
	    (enum u_device_alloc_flags)(U_DEVICE_ALLOC_HMD | U_DEVICE_ALLOC_TRACKING_NONE);
	struct psvr_device *psvr = U_DEVICE_ALLOCATE(struct psvr_device, flags, 1, 0);
	int ret;

	psvr->log_level = log_level;
	psvr->base.update_inputs = psvr_device_update_inputs;
	psvr->base.get_tracked_pose = psvr_device_get_tracked_pose;
	psvr->base.get_view_poses = psvr_device_get_view_poses;
	psvr->base.compute_distortion = psvr_compute_distortion;
	psvr->base.destroy = psvr_device_destroy;
	psvr->base.inputs[0].name = XRT_INPUT_GENERIC_HEAD_POSE;
	psvr->base.name = XRT_DEVICE_GENERIC_HMD;

	{
		struct u_panotools_values vals = {0};

		vals.distortion_k[0] = 0.75f;
		vals.distortion_k[1] = -0.01f;
		vals.distortion_k[2] = 0.75f;
		vals.distortion_k[3] = 0.0f;
		vals.distortion_k[4] = 3.8f;
		vals.aberration_k[0] = 0.999f;
		vals.aberration_k[1] = 1.008f;
		vals.aberration_k[2] = 1.018f;
		vals.scale = 1.2f * (1980 / 2.0f);
		vals.viewport_size.x = (1980 / 2.0f);
		vals.viewport_size.y = (1080);
		vals.lens_center.x = vals.viewport_size.x / 2.0f;
		vals.lens_center.y = vals.viewport_size.y / 2.0f;

		psvr->vals = vals;

		struct xrt_hmd_parts *hmd = psvr->base.hmd;
		hmd->distortion.models = XRT_DISTORTION_MODEL_COMPUTE;
		hmd->distortion.preferred = XRT_DISTORTION_MODEL_COMPUTE;
	}

#if 1
	m_imu_3dof_init(&psvr->fusion, M_IMU_3DOF_USE_GRAVITY_DUR_20MS);
#else
	psvr->fusion.rot.w = 1.0f;
#endif

	snprintf(psvr->base.str, XRT_DEVICE_NAME_LEN, "PS VR Headset");
	snprintf(psvr->base.serial, XRT_DEVICE_NAME_LEN, "PS VR Headset");

	// Do mutex and thread init before any call to teardown happens.
	os_mutex_init(&psvr->device_mutex);
	os_thread_helper_init(&psvr->oth);

	ret = open_hid(psvr, sensor_hid_info, &psvr->hid_sensor);
	if (ret != 0) {
		goto cleanup;
	}

	ret = open_hid(psvr, control_hid_info, &psvr->hid_control);
	if (ret < 0) {
		goto cleanup;
	}

	ret = os_thread_helper_start(&psvr->oth, sensor_thread, (void *)psvr);
	if (ret < 0) {
		goto cleanup;
	}

	if (control_power_and_wait(psvr, true) < 0 || control_vrmode_and_wait(psvr, true) < 0) {
		goto cleanup;
	}

	// Device is now on and we can read calibration data now.
	ret = read_calibration_data(psvr);
	if (ret < 0) {
		goto cleanup;
	}

	if (debug_get_bool_option_psvr_disco()) {
		ret = disco_leds(psvr);
	} else {
		ret = control_leds(psvr, PSVR_LED_FRONT, PSVR_LED_POWER_MAX, (enum psvr_leds)0);
	}
	if (ret < 0) {
		PSVR_ERROR(psvr, "Failed to control leds '%i'", ret);
		goto cleanup;
	}


	/*
	 * Device setup.
	 */

	struct u_device_simple_info info;
	info.display.w_pixels = 1920;
	info.display.h_pixels = 1080;
	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;
	info.fov[0] = (float)(85.0 * (M_PI / 180.0));
	info.fov[1] = (float)(85.0 * (M_PI / 180.0));

	if (!u_device_setup_split_side_by_side(&psvr->base, &info)) {
		PSVR_ERROR(psvr, "Failed to setup basic device info");
		goto cleanup;
	}

	/*
	 * Setup variable.
	 */

	// clang-format off
	u_var_add_root(psvr, "PS VR Headset", true);
	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");
	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_log_level(psvr, &psvr->log_level, "Log level");
	// clang-format on

	/*
	 * Finishing touches.
	 */

	if (psvr->log_level <= U_LOGGING_DEBUG) {
		u_device_dump_config(&psvr->base, __func__, "Sony PSVR");
	}

	// Did we get a tracker, use it!
	psvr->tracker = tracker;

	// Use the new origin if we got a tracking system.
	if (psvr->tracker != NULL) {
		psvr->base.tracking_origin = psvr->tracker->origin;
	}

	psvr->base.orientation_tracking_supported = true;
	psvr->base.position_tracking_supported = psvr->tracker != NULL;
	psvr->base.device_type = XRT_DEVICE_TYPE_HMD;

	PSVR_DEBUG(psvr, "YES!");

	return &psvr->base;


cleanup:
	PSVR_DEBUG(psvr, "NO! :(");

	teardown(psvr);
	free(psvr);

	return NULL;
}