Add initial Windows Mixed Reality support (#121)

* Added HoloLens Sensors IMU driver

* HoloLens: add config store readout and fix up device properties a bit

Update screen size and resolution from specs, not sure about lens separation
and vertical position. Read the configuration store to determine the HMD model
from the stored name. Samsung Odyssey has a different display than the others.

* Rename HoloLens driver to WMR to avoid confusion

This driver is for Windows Mixed Reality VR headsets, not for the Microsoft
HoloLens AR headset. These just happen to identify themselves as "Microsoft
HoloLens Sensors" USB devices.

CMakeLists.txt

@@ -29,6 +29,7 @@ set(openhmd_source_files
 
 OPTION(OPENHMD_DRIVER_OCULUS_RIFT "Oculus Rift DK1 and DK2" ON)
 OPTION(OPENHMD_DRIVER_DEEPOON "Deepoon E2" ON)
+OPTION(OPENHMD_DRIVER_WMR "Windows Mixed Reality" ON)
 OPTION(OPENHMD_DRIVER_PSVR "Sony PSVR" ON)
 OPTION(OPENHMD_DRIVER_HTC_VIVE "HTC Vive" ON)
 OPTION(OPENHMD_DRIVER_NOLO "NOLO VR CV1" ON)
@@ -62,6 +63,18 @@ if(OPENHMD_DRIVER_DEEPOON)
 	set(LIBS ${LIBS} ${HIDAPI_LIBRARIES})
 endif(OPENHMD_DRIVER_DEEPOON)
 
+if(OPENHMD_DRIVER_WMR)
+	set(openhmd_source_files ${openhmd_source_files}
+	${CMAKE_CURRENT_LIST_DIR}/src/drv_wmr/wmr.c
+	${CMAKE_CURRENT_LIST_DIR}/src/drv_wmr/packet.c
+	)
+	add_definitions(-DDRIVER_HOLOLENS)
+
+	find_package(HIDAPI REQUIRED)
+	include_directories(${HIDAPI_INCLUDE_DIRS})
+	set(LIBS ${LIBS} ${HIDAPI_LIBRARIES})
+endif(OPENHMD_DRIVER_WMR)
+
 if(OPENHMD_DRIVER_PSVR)
 	set(openhmd_source_files ${openhmd_source_files}
 	${CMAKE_CURRENT_LIST_DIR}/src/drv_psvr/psvr.c

configure.ac

@@ -62,6 +62,15 @@ AC_ARG_ENABLE([driver-deepoon],
 
 AM_CONDITIONAL([BUILD_DRIVER_DEEPOON], [test "x$driver_deepoon_enabled" != "xno"])
 
+# Windows Mixed Reality Driver
+AC_ARG_ENABLE([driver-wmr],
+        [AS_HELP_STRING([--disable-driver-wmr],
+                [disable building of Windows Mixed Reality driver [default=yes]])],
+        [driver_wmr_enabled=$enableval],
+        [driver_wmr_enabled='yes'])
+
+AM_CONDITIONAL([BUILD_DRIVER_WMR], [test "x$driver_wmr_enabled" != "xno"])
+
 # Sony PSVR Driver
 AC_ARG_ENABLE([driver-psvr],
         [AS_HELP_STRING([--disable-driver-psvr],

src/Makefile.am

@@ -49,6 +49,17 @@ libopenhmd_la_LDFLAGS += $(hidapi_LIBS)
 
 endif
 
+if BUILD_DRIVER_WMR
+
+libopenhmd_la_SOURCES += \
+	drv_wmr/wmr.c \
+	drv_wmr/packet.c
+
+libopenhmd_la_CPPFLAGS += $(hidapi_CFLAGS) -DDRIVER_WMR
+libopenhmd_la_LDFLAGS += $(hidapi_LIBS)
+
+endif
+
 if BUILD_DRIVER_PSVR
 
 libopenhmd_la_SOURCES += \

src/drv_wmr/packet.c

@@ -0,0 +1,68 @@
+#include "wmr.h"
+
+#ifdef _MSC_VER
+#define inline __inline
+#endif
+
+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;
+}
+
+inline static uint64_t read64(const unsigned char** buffer)
+{
+	uint64_t ret = (uint64_t)**buffer |
+		       ((uint64_t)*(*buffer + 1) << 8) |
+		       ((uint64_t)*(*buffer + 2) << 16) |
+		       ((uint64_t)*(*buffer + 3) << 24) |
+		       ((uint64_t)*(*buffer + 4) << 32) |
+		       ((uint64_t)*(*buffer + 5) << 40) |
+		       ((uint64_t)*(*buffer + 6) << 48) |
+		       ((uint64_t)*(*buffer + 7) << 56);
+	*buffer += 8;
+	return ret;
+}
+
+bool hololens_sensors_decode_packet(hololens_sensors_packet* pkt, const unsigned char* buffer, int size)
+{
+	if(size != 497){
+		LOGE("invalid hololens sensor packet size (expected 497 but got %d)", size);
+		return false;
+	}
+
+	pkt->id = read8(&buffer);
+	for(int i = 0; i < 4; i++)
+		pkt->temperature[i] = read16(&buffer);
+	for(int i = 0; i < 4; i++)
+		pkt->gyro_timestamp[i] = read64(&buffer);
+	for(int i = 0; i < 3; i++){
+		for (int j = 0; j < 32; j++)
+			pkt->gyro[i][j] = read16(&buffer);
+	}
+	for(int i = 0; i < 4; i++)
+		pkt->accel_timestamp[i] = read64(&buffer);
+	for(int i = 0; i < 3; i++){
+		for (int j = 0; j < 4; j++)
+			pkt->accel[i][j] = read32(&buffer);
+	}
+	for(int i = 0; i < 4; i++)
+		pkt->video_timestamp[i] = read64(&buffer);
+
+	return true;
+}

src/drv_wmr/wmr.c

@@ -0,0 +1,414 @@
+/*
+ * OpenHMD - Free and Open Source API and drivers for immersive technology.
+ * Copyright (C) 2018 Philipp Zabel.
+ * Distributed under the Boost 1.0 licence, see LICENSE for full text.
+ */
+
+/* Windows Mixed Reality Driver */
+
+#define FEATURE_BUFFER_SIZE 497
+
+#define TICK_LEN (1.0f / 10000000.0f) // 1000 Hz ticks
+
+#define MICROSOFT_VID        0x045e
+#define HOLOLENS_SENSORS_PID 0x0659
+
+#include <string.h>
+#include <wchar.h>
+#include <hidapi.h>
+#include <assert.h>
+#include <limits.h>
+#include <stdint.h>
+#include <stdbool.h>
+
+#include "wmr.h"
+
+typedef struct {
+	ohmd_device base;
+
+	hid_device* hmd_imu;
+	fusion sensor_fusion;
+	vec3f raw_accel, raw_gyro;
+	uint32_t last_ticks;
+	uint8_t last_seq;
+	hololens_sensors_packet sensor;
+
+} wmr_priv;
+
+static void vec3f_from_hololens_gyro(const int16_t smp[3][32], int i, vec3f* out_vec)
+{
+	out_vec->x = (float)(smp[1][8*i+0] +
+			     smp[1][8*i+1] +
+			     smp[1][8*i+2] +
+			     smp[1][8*i+3] +
+			     smp[1][8*i+4] +
+			     smp[1][8*i+5] +
+			     smp[1][8*i+6] +
+			     smp[1][8*i+7]) * 0.001f * -0.125f;
+	out_vec->y = (float)(smp[0][8*i+0] +
+			     smp[0][8*i+1] +
+			     smp[0][8*i+2] +
+			     smp[0][8*i+3] +
+			     smp[0][8*i+4] +
+			     smp[0][8*i+5] +
+			     smp[0][8*i+6] +
+			     smp[0][8*i+7]) * 0.001f * -0.125f;
+	out_vec->z = (float)(smp[2][8*i+0] +
+			     smp[2][8*i+1] +
+			     smp[2][8*i+2] +
+			     smp[2][8*i+3] +
+			     smp[2][8*i+4] +
+			     smp[2][8*i+5] +
+			     smp[2][8*i+6] +
+			     smp[2][8*i+7]) * 0.001f * -0.125f;
+}
+
+static void vec3f_from_hololens_accel(const int32_t smp[3][4], int i, vec3f* out_vec)
+{
+	out_vec->x = (float)smp[1][i] * 0.001f * -1.0f;
+	out_vec->y = (float)smp[0][i] * 0.001f * -1.0f;
+	out_vec->z = (float)smp[2][i] * 0.001f * -1.0f;
+}
+
+static void handle_tracker_sensor_msg(wmr_priv* priv, unsigned char* buffer, int size)
+{
+	uint64_t last_sample_tick = priv->sensor.gyro_timestamp[3];
+
+	if(!hololens_sensors_decode_packet(&priv->sensor, buffer, size)){
+		LOGE("couldn't decode tracker sensor message");
+	}
+
+	hololens_sensors_packet* s = &priv->sensor;
+
+
+	vec3f mag = {{0.0f, 0.0f, 0.0f}};
+
+	for(int i = 0; i < 4; i++){
+		uint64_t tick_delta = 1000;
+		if(last_sample_tick > 0) //startup correction
+			tick_delta = s->gyro_timestamp[i] - last_sample_tick;
+
+		float dt = tick_delta * TICK_LEN;
+
+		vec3f_from_hololens_gyro(s->gyro, i, &priv->raw_gyro);
+		vec3f_from_hololens_accel(s->accel, i, &priv->raw_accel);
+
+		ofusion_update(&priv->sensor_fusion, dt, &priv->raw_gyro, &priv->raw_accel, &mag);
+
+		last_sample_tick = s->gyro_timestamp[i];
+	}
+}
+
+static void update_device(ohmd_device* device)
+{
+	wmr_priv* priv = (wmr_priv*)device;
+
+	int size = 0;
+	unsigned char buffer[FEATURE_BUFFER_SIZE];
+
+	while(true){
+		int size = hid_read(priv->hmd_imu, 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] == HOLOLENS_IRQ_SENSORS){
+			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)
+{
+	wmr_priv* priv = (wmr_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)
+{
+	wmr_priv* priv = (wmr_priv*)device;
+
+	LOGD("closing Microsoft HoloLens Sensors device");
+
+	hid_close(priv->hmd_imu);
+
+	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(findEndPoint(cur_dev->path, device_index) > 0 && 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 int config_command_sync(hid_device* hmd_imu, unsigned char type,
+			       unsigned char* buf, int len)
+{
+	unsigned char cmd[64] = { 0x02, type };
+
+	hid_write(hmd_imu, cmd, sizeof(cmd));
+	do {
+		int size = hid_read(hmd_imu, buf, len);
+		if (size == -1)
+			return -1;
+		if (buf[0] == 0x02)
+			return size;
+	} while (buf[0] == 0x01);
+
+	return -1;
+}
+
+int read_config_part(wmr_priv *priv, unsigned char type,
+		     unsigned char *data, int len)
+{
+	unsigned char buf[33];
+	int offset = 0;
+	int size;
+
+	size = config_command_sync(priv->hmd_imu, 0x0b, buf, sizeof(buf));
+	if (size != 33 || buf[0] != 0x02) {
+		printf("Failed to issue command 0b: %02x %02x %02x\n",
+		       buf[0], buf[1], buf[2]);
+		return -1;
+	}
+	size = config_command_sync(priv->hmd_imu, type, buf, sizeof(buf));
+	if (size != 33 || buf[0] != 0x02) {
+		printf("Failed to issue command %02x: %02x %02x %02x\n", type,
+		       buf[0], buf[1], buf[2]);
+		return -1;
+	}
+	for (;;) {
+		size = config_command_sync(priv->hmd_imu, 0x08, buf, sizeof(buf));
+		if (size != 33 || (buf[1] != 0x01 && buf[1] != 0x02)) {
+			printf("Failed to issue command 08: %02x %02x %02x\n",
+			       buf[0], buf[1], buf[2]);
+			return -1;
+		}
+		if (buf[1] != 0x01)
+			break;
+		if (buf[2] > len || offset + buf[2] > len) {
+			return -1;
+		}
+		memcpy(data + offset, buf + 3, buf[2]);
+		offset += buf[2];
+	}
+
+	return offset;
+}
+
+unsigned char *read_config(wmr_priv *priv)
+{
+	unsigned char meta[66];
+	unsigned char *data;
+	int size, data_size;
+
+	size = read_config_part(priv, 0x06, meta, sizeof(meta));
+	if (size == -1)
+		return NULL;
+
+	/*
+	 * No idea what the other 64 bytes of metadata are, but the first two
+	 * seem to be little endian size of the data store.
+	 */
+	data_size = meta[0] | (meta[1] << 8);
+	data = calloc(1, data_size);
+	if (!data)
+                return NULL;
+
+	size = read_config_part(priv, 0x04, data, data_size);
+	if (size == -1) {
+		free(data);
+		return NULL;
+	}
+
+	printf("Read %d-byte config data\n", data_size);
+
+	return data;
+}
+
+static ohmd_device* open_device(ohmd_driver* driver, ohmd_device_desc* desc)
+{
+	wmr_priv* priv = ohmd_alloc(driver->ctx, sizeof(wmr_priv));
+	unsigned char *config;
+	bool samsung = false;
+
+	if(!priv)
+		return NULL;
+
+	priv->base.ctx = driver->ctx;
+
+	// Open the HMD device
+	priv->hmd_imu = open_device_idx(MICROSOFT_VID, HOLOLENS_SENSORS_PID, 0, 0, 2);
+
+	if(!priv->hmd_imu)
+		goto cleanup;
+
+	config = read_config(priv);
+	if (config) {
+		printf("Model name: %.64s\n", config + 0x1c3);
+		if (strncmp(config + 0x1c3,
+			    "Samsung Windows Mixed Reality 800ZAA", 64) == 0) {
+			samsung = true;
+		}
+		free(config);
+	}
+
+	if(hid_set_nonblocking(priv->hmd_imu, 1) == -1){
+		ohmd_set_error(driver->ctx, "failed to set non-blocking on device");
+		goto cleanup;
+	}
+
+	// turn the IMU on
+	hid_write(priv->hmd_imu, hololens_sensors_imu_on, sizeof(hololens_sensors_imu_on));
+
+	// Set default device properties
+	ohmd_set_default_device_properties(&priv->base.properties);
+
+	// Set device properties
+	if (samsung) {
+		// Samsung Odyssey has two 3.5" 1440x1600 OLED displays.
+		priv->base.properties.hsize = 0.118942f;
+		priv->base.properties.vsize = 0.066079f;
+		priv->base.properties.hres = 2880;
+		priv->base.properties.vres = 1600;
+		priv->base.properties.lens_sep = 0.063f; /* FIXME */
+		priv->base.properties.lens_vpos = 0.03304f; /* FIXME */
+		priv->base.properties.fov = DEG_TO_RAD(110.0f);
+		priv->base.properties.ratio = 0.9f;
+	} else {
+		// Most Windows Mixed Reality Headsets have two 2.89" 1440x1440 LCDs
+		priv->base.properties.hsize = 0.103812f;
+		priv->base.properties.vsize = 0.051905f;
+		priv->base.properties.hres = 2880;
+		priv->base.properties.vres = 1440;
+		priv->base.properties.lens_sep = 0.063f; /* FIXME */
+		priv->base.properties.lens_vpos = 0.025953f; /* FIXME */
+		priv->base.properties.fov = DEG_TO_RAD(95.0f);
+		priv->base.properties.ratio = 1.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(MICROSOFT_VID, HOLOLENS_SENSORS_PID);
+	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 Windows Mixed Reality Driver");
+		strcpy(desc->vendor, "Microsoft");
+		strcpy(desc->product, "HoloLens Sensors");
+
+		desc->revision = 0;
+
+		snprintf(desc->path, OHMD_STR_SIZE, "%d", idx);
+
+		desc->driver_ptr = driver;
+
+		desc->device_class = OHMD_DEVICE_CLASS_HMD;
+		desc->device_flags = OHMD_DEVICE_FLAGS_ROTATIONAL_TRACKING;
+
+		cur_dev = cur_dev->next;
+		idx++;
+	}
+
+	hid_free_enumeration(devs);
+}
+
+static void destroy_driver(ohmd_driver* drv)
+{
+	LOGD("shutting down Windows Mixed Reality driver");
+	free(drv);
+}
+
+ohmd_driver* ohmd_create_wmr_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_wmr/wmr.h

@@ -0,0 +1,32 @@
+#ifndef WMR_H
+#define WMR_H
+
+#include <stdint.h>
+#include <stdbool.h>
+
+#include "../openhmdi.h"
+
+typedef enum
+{
+	HOLOLENS_IRQ_SENSORS = 1,
+	HOLOLENS_IRQ_CONTROL = 2
+} hololens_sensors_irq_cmd;
+
+typedef struct
+{
+        uint8_t id;
+        uint16_t temperature[4];
+        uint64_t gyro_timestamp[4];
+        int16_t gyro[3][32];
+        uint64_t accel_timestamp[4];
+        int32_t accel[3][4];
+        uint64_t video_timestamp[4];
+} hololens_sensors_packet;
+
+static const unsigned char hololens_sensors_imu_on[64] = {
+	0x02, 0x07
+};
+
+bool hololens_sensors_decode_packet(hololens_sensors_packet* pkt, const unsigned char* buffer, int size);
+
+#endif

src/openhmd.c

@@ -38,6 +38,10 @@ ohmd_context* OHMD_APIENTRY ohmd_ctx_create(void)
 	ctx->drivers[ctx->num_drivers++] = ohmd_create_htc_vive_drv(ctx);
 #endif
 
+#if DRIVER_WMR
+	ctx->drivers[ctx->num_drivers++] = ohmd_create_wmr_drv(ctx);
+#endif
+
 #if DRIVER_PSVR
 	ctx->drivers[ctx->num_drivers++] = ohmd_create_psvr_drv(ctx);
 #endif

src/openhmdi.h

@@ -144,6 +144,7 @@ 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_htc_vive_drv(ohmd_context* ctx);
+ohmd_driver* ohmd_create_wmr_drv(ohmd_context* ctx);
 ohmd_driver* ohmd_create_psvr_drv(ohmd_context* ctx);
 ohmd_driver* ohmd_create_nolo_drv(ohmd_context* ctx);
 ohmd_driver* ohmd_create_external_drv(ohmd_context* ctx);