PSVR IMU now functional, currently uses 1 sample, quite basic

examples/opengl/gl.h

@@ -10,7 +10,7 @@
 #ifndef GL_H
 #define GL_H
 
-#include <SDL.h>
+#include <SDL/SDL.h>
 
 #include <GL/glew.h>
 #include <GL/gl.h>

src/drv_psvr/packet.c

@@ -0,0 +1,69 @@
+#include "psvr.h"
+
+inline static uint8_t read8(const unsigned char** buffer)
+{
+	uint8_t ret = **buffer;
+	*buffer += 1;
+	return ret;
+}
+
+inline static int16_t read16(const unsigned char** buffer)
+{
+	int16_t ret = **buffer | (*(*buffer + 1) << 8);
+	*buffer += 2;
+	return ret;
+}
+
+inline static uint32_t read32(const unsigned char** buffer)
+{
+	uint32_t ret = **buffer | (*(*buffer + 1) << 8) | (*(*buffer + 2) << 16) | (*(*buffer + 3) << 24);
+	*buffer += 4;
+	return ret;
+}
+
+bool psvr_decode_sensor_packet(psvr_sensor_packet* pkt, const unsigned char* buffer, int size)
+{
+	if(size != 64){
+		LOGE("invalid psvr sensor packet size (expected 64 but got %d)", size);
+		return false;
+	}
+	int16_t accel[3];
+	int16_t gyro[3];
+
+	buffer += 2;
+	uint16_t volume = read16(&buffer); //volume
+	buffer += 12; //uknown, skip 12
+	uint32_t tick = read32(&buffer); //TICK
+	// acceleration
+	for(int i = 0; i < 3; i++){
+		pkt->samples[0].gyro[i] = read16(&buffer);
+	}
+
+	// rotation
+	for(int i = 0; i < 3; i++){
+		pkt->samples[0].accel[i] = read16(&buffer);
+	}
+/*
+	printf("JOEDEBUG - Tick = %u\n", tick);
+	printf("JOEDEBUG - Accel = %d %d %d\n", accel[0], accel[1], accel[2]);
+	printf("JOEDEBUG - Gyro = %d %d %d\n", gyro[0], gyro[1], gyro[2]);*/
+	return true;
+
+/*
+	for(int j = 0; j < 3; j++){
+		// acceleration
+		for(int i = 0; i < 3; i++){
+			pkt->samples[j].acc[i] = read16(&buffer);
+		}
+
+		// rotation
+		for(int i = 0; i < 3; i++){
+			pkt->samples[j].rot[i] = read16(&buffer);
+		}
+
+		pkt->samples[j].time_ticks = read32(&buffer);
+		pkt->samples[j].seq = read8(&buffer);
+	}
+
+	return true;*/
+}

src/drv_psvr/psvr.c

@@ -0,0 +1,299 @@
+/*
+ * OpenHMD - Free and Open Source API and drivers for immersive technology.
+ * Copyright (C) 2013 Fredrik Hultin.
+ * Copyright (C) 2013 Jakob Bornecrantz.
+ * Distributed under the Boost 1.0 licence, see LICENSE for full text.
+ */
+
+/* Sony PSVR Driver */
+
+#define FEATURE_BUFFER_SIZE 256
+
+#define TICK_LEN (1.0f / 1000000.0f) // 1000 Hz ticks
+
+#define SONY_ID                  0x054c
+#define PSVR_HMD                 0x09af
+
+#define PSVR_TIME_DIV 48000000.0f
+
+#include <string.h>
+#include <wchar.h>
+#include <hidapi.h>
+#include <assert.h>
+#include <limits.h>
+#include <stdint.h>
+#include <stdbool.h>
+
+#include "psvr.h"
+#include "openhmdi.h"
+
+typedef struct {
+	ohmd_device base;
+
+	hid_device* hmd_handle;
+	hid_device* imu_handle;
+	fusion sensor_fusion;
+	vec3f raw_accel, raw_gyro;
+	uint32_t last_ticks;
+	uint8_t last_seq;
+	psvr_sensor_packet sensor;
+
+} psvr_priv;
+
+void vec3f_from_psvr_vec(const int16_t* smp, vec3f* out_vec)
+{
+	out_vec->x = (float)smp[1] * 0.001f;
+	out_vec->y = (float)smp[0] * 0.001f;
+	out_vec->z = (float)smp[2] * 0.001f;
+}
+
+static void handle_tracker_sensor_msg(psvr_priv* priv, unsigned char* buffer, int size)
+{
+	uint32_t last_sample_tick = priv->sensor.tick;
+
+	if(!psvr_decode_sensor_packet(&priv->sensor, buffer, size)){
+		LOGE("couldn't decode tracker sensor message");
+	}
+
+	psvr_sensor_packet* s = &priv->sensor;
+
+	uint32_t tick_delta = 1000;
+	if(last_sample_tick > 0) //startup correction
+		tick_delta = s->tick - last_sample_tick;
+
+	float dt = tick_delta * TICK_LEN;
+	vec3f mag = {{0.0f, 0.0f, 0.0f}};
+
+	for(int i = 0; i < 1; i++){ //just use 1 sample since we don't have sample order for this frame
+		vec3f_from_psvr_vec(s->samples[i].accel, &priv->raw_accel);
+		vec3f_from_psvr_vec(s->samples[i].gyro, &priv->raw_gyro);
+
+		ofusion_update(&priv->sensor_fusion, dt, &priv->raw_gyro, &priv->raw_accel, &mag);
+
+		// reset dt to tick_len for the last samples if there were more than one sample
+		dt = TICK_LEN;
+	}
+}
+
+static void update_device(ohmd_device* device)
+{
+	psvr_priv* priv = (psvr_priv*)device;
+
+	int size = 0;
+	unsigned char buffer[FEATURE_BUFFER_SIZE];
+
+	while(true){
+		int size = hid_read(priv->hmd_handle, buffer, FEATURE_BUFFER_SIZE);
+		if(size < 0){
+			LOGE("error reading from device");
+			return;
+		} else if(size == 0) {
+			return; // No more messages, return.
+		}
+
+		// currently the only message type the hardware supports (I think)
+		if(buffer[0] == 0){
+			handle_tracker_sensor_msg(priv, buffer, size);
+		}else{
+			LOGE("unknown message type: %u", buffer[0]);
+		}
+	}
+
+	if(size < 0){
+		LOGE("error reading from device");
+	}
+}
+
+static int getf(ohmd_device* device, ohmd_float_value type, float* out)
+{
+	psvr_priv* priv = (psvr_priv*)device;
+
+	switch(type){
+	case OHMD_ROTATION_QUAT:
+		*(quatf*)out = priv->sensor_fusion.orient;
+		break;
+
+	case OHMD_POSITION_VECTOR:
+		out[0] = out[1] = out[2] = 0;
+		break;
+
+	case OHMD_DISTORTION_K:
+		// TODO this should be set to the equivalent of no distortion
+		memset(out, 0, sizeof(float) * 6);
+		break;
+
+	default:
+		ohmd_set_error(priv->base.ctx, "invalid type given to getf (%ud)", type);
+		return -1;
+		break;
+	}
+
+	return 0;
+}
+
+static void close_device(ohmd_device* device)
+{
+	//int hret = 0;
+	psvr_priv* priv = (psvr_priv*)device;
+
+	LOGD("closing HTC Psvr device");
+
+	// turn the display off
+	//hret = hid_send_feature_report(priv->hmd_handle, psvr_magic_power_off1, sizeof(psvr_magic_power_off1));
+	//printf("power off magic 1: %d\n", hret);
+
+	//hret = hid_send_feature_report(priv->hmd_handle, psvr_magic_power_off2, sizeof(psvr_magic_power_off2));
+	//printf("power off magic 2: %d\n", hret);
+
+	hid_close(priv->hmd_handle);
+	hid_close(priv->imu_handle);
+
+	free(device);
+}
+
+static hid_device* open_device_idx(int manufacturer, int product, int iface, int iface_tot, int device_index)
+{
+	struct hid_device_info* devs = hid_enumerate(manufacturer, product);
+	struct hid_device_info* cur_dev = devs;
+
+	int idx = 0;
+	int iface_cur = 0;
+	hid_device* ret = NULL;
+
+	while (cur_dev) {
+		printf("%04x:%04x %s\n", manufacturer, product, cur_dev->path);
+
+		if(idx == device_index && iface == iface_cur){
+			ret = hid_open_path(cur_dev->path);
+			printf("opening\n");
+		}
+
+		cur_dev = cur_dev->next;
+
+		iface_cur++;
+
+		if(iface_cur >= iface_tot){
+			idx++;
+			iface_cur = 0;
+		}
+	}
+
+	hid_free_enumeration(devs);
+
+	return ret;
+}
+
+static ohmd_device* open_device(ohmd_driver* driver, ohmd_device_desc* desc)
+{
+	psvr_priv* priv = ohmd_alloc(driver->ctx, sizeof(psvr_priv));
+
+	if(!priv)
+		return NULL;
+
+	//int hret = 0;
+
+	priv->base.ctx = driver->ctx;
+
+	int idx = atoi(desc->path);
+
+	// Open the HMD device
+	priv->hmd_handle = open_device_idx(SONY_ID, PSVR_HMD, 0, 0, idx);
+
+	if(!priv->hmd_handle)
+		goto cleanup;
+
+	if(hid_set_nonblocking(priv->hmd_handle, 1) == -1){
+		ohmd_set_error(driver->ctx, "failed to set non-blocking on device");
+		goto cleanup;
+	}
+/*
+	dump_info_string(hid_get_manufacturer_string, "manufacturer", priv->hmd_handle);
+	dump_info_string(hid_get_product_string , "product", priv->hmd_handle);
+	dump_info_string(hid_get_serial_number_string, "serial number", priv->hmd_handle);
+*/
+	// turn the display on
+	//hret = hid_send_feature_report(priv->hmd_handle, psvr_magic_power_on, sizeof(psvr_magic_power_on));
+	//printf("power on magic: %d\n", hret);
+
+	// enable lighthouse
+	//hret = hid_send_feature_report(priv->hmd_handle, psvr_magic_enable_lighthouse, sizeof(psvr_magic_enable_lighthouse));
+	//printf("enable lighthouse magic: %d\n", hret);
+
+	// Set default device properties
+	ohmd_set_default_device_properties(&priv->base.properties);
+
+	// Set device properties TODO: Get from device
+	priv->base.properties.hsize = 0.122822f;
+	priv->base.properties.vsize = 0.068234f;
+	priv->base.properties.hres = 1920;
+	priv->base.properties.vres = 1080;
+	priv->base.properties.lens_sep = 0.063500;
+	priv->base.properties.lens_vpos = 0.049694;
+	priv->base.properties.fov = DEG_TO_RAD(111.435f); //TODO: Confirm exact mesurements
+	priv->base.properties.ratio = (1920.0f / 1080.0f) / 2.0f;
+
+	// calculate projection eye projection matrices from the device properties
+	ohmd_calc_default_proj_matrices(&priv->base.properties);
+
+	// set up device callbacks
+	priv->base.update = update_device;
+	priv->base.close = close_device;
+	priv->base.getf = getf;
+
+	ofusion_init(&priv->sensor_fusion);
+
+	return (ohmd_device*)priv;
+
+cleanup:
+	if(priv)
+		free(priv);
+
+	return NULL;
+}
+
+static void get_device_list(ohmd_driver* driver, ohmd_device_list* list)
+{
+	struct hid_device_info* devs = hid_enumerate(SONY_ID, PSVR_HMD);
+	struct hid_device_info* cur_dev = devs;
+
+	int idx = 0;
+	while (cur_dev) {
+		ohmd_device_desc* desc = &list->devices[list->num_devices++];
+
+		strcpy(desc->driver, "OpenHMD Sony PSVR Driver");
+		strcpy(desc->vendor, "Sony");
+		strcpy(desc->product, "PSVR");
+
+		desc->revision = 0;
+
+		snprintf(desc->path, OHMD_STR_SIZE, "%d", idx);
+
+		desc->driver_ptr = driver;
+
+		cur_dev = cur_dev->next;
+		idx++;
+	}
+
+	hid_free_enumeration(devs);
+}
+
+static void destroy_driver(ohmd_driver* drv)
+{
+	LOGD("shutting down HTC PSVR driver");
+	free(drv);
+}
+
+ohmd_driver* ohmd_create_psvr_drv(ohmd_context* ctx)
+{
+	ohmd_driver* drv = ohmd_alloc(ctx, sizeof(ohmd_driver));
+
+	if(!drv)
+		return NULL;
+
+	drv->get_device_list = get_device_list;
+	drv->open_device = open_device;
+	drv->destroy = destroy_driver;
+	drv->ctx = ctx;
+
+	return drv;
+}

src/drv_psvr/psvr.h

@@ -0,0 +1,32 @@
+#ifndef PSVR_H
+#define PSVR_H
+
+#include <stdint.h>
+#include <stdbool.h>
+
+#include "../openhmdi.h"
+
+typedef enum
+{
+	PSVR_IRQ_SENSORS = 0
+} psvr_irq_cmd;
+
+typedef struct
+{
+	int16_t accel[3];
+	int16_t gyro[3];
+	uint32_t tick;
+	uint8_t seq;
+} psvr_sensor_sample;
+
+typedef struct
+{
+	uint8_t report_id;
+	uint32_t tick;
+	psvr_sensor_sample samples[1];
+} psvr_sensor_packet;
+
+void vec3f_from_psvr_vec(const int16_t* smp, vec3f* out_vec);
+bool psvr_decode_sensor_packet(psvr_sensor_packet* pkt, const unsigned char* buffer, int size);
+
+#endif

src/openhmd.c

@@ -31,6 +31,10 @@ ohmd_context* OHMD_APIENTRY ohmd_ctx_create(void)
 	ctx->drivers[ctx->num_drivers++] = ohmd_create_deepoon_drv(ctx);
 #endif
 
+#if DRIVER_PSVR
+	ctx->drivers[ctx->num_drivers++] = ohmd_create_psvr_drv(ctx);
+#endif
+
 #if DRIVER_EXTERNAL
 	ctx->drivers[ctx->num_drivers++] = ohmd_create_external_drv(ctx);
 #endif

src/openhmdi.h

@@ -127,6 +127,7 @@ void ohmd_calc_default_proj_matrices(ohmd_device_properties* props);
 ohmd_driver* ohmd_create_dummy_drv(ohmd_context* ctx);
 ohmd_driver* ohmd_create_oculus_rift_drv(ohmd_context* ctx);
 ohmd_driver* ohmd_create_deepoon_drv(ohmd_context* ctx);
+ohmd_driver* ohmd_create_psvr_drv(ohmd_context* ctx);
 ohmd_driver* ohmd_create_external_drv(ohmd_context* ctx);
 ohmd_driver* ohmd_create_android_drv(ohmd_context* ctx);