Prechádzať zdrojové kódy

Merge pull request #87 from OpenHMD/htc-vive

Initial HTC Vive support (IMU) and basic json parsing for config
TheOnlyJoey 8 rokov pred
rodič
commit
aa74f8fbe8

+ 1 - 1
CMakeLists.txt

@@ -97,4 +97,4 @@ set(CMAKE_LIBRARY_OUTPUT_DIRECTORY ${CMAKE_CURRENT_DIR}/)
 
 #install properties
 install (TARGETS openhmd-shared openhmd-static DESTINATION lib)
-install (FILES include/openhmd.h DESTINATION include)
+install (FILES include/openhmd.h DESTINATION include/openhmd/)

+ 6 - 4
README.md

@@ -7,6 +7,7 @@ OpenHMD is released under the permissive Boost Software License (see LICENSE for
 ## Supported Devices
   * Oculus Rift DK1, DK2 and CV1 (rotation only)
   * Deepoon E2
+  * HTC Vive (rotation only)
   * Android based devices
   * External Sensor (passthrough for external sensors)
 
@@ -60,9 +61,11 @@ To avoid having to run your applications as root to access USB devices you have
 as root, run:
 
     echo 'SUBSYSTEM=="usb", ATTR{idVendor}=="2833", MODE="0666", GROUP="plugdev"' > /etc/udev/rules.d/83-hmd.rules
+    echo 'SUBSYSTEM=="usb", ATTR{idVendor}=="0bb4", MODE="0666", GROUP="plugdev"' >> /etc/udev/rules.d/83-hmd.rules
+    echo 'SUBSYSTEM=="usb", ATTR{idVendor}=="28de", MODE="0666", GROUP="plugdev"' >> /etc/udev/rules.d/83-hmd.rules
     udevadm control --reload-rules
 
-After this you have to unplug your Rift and plug it back in. You should now be able to access the Oculus Rift as a normal user.
+After this you have to unplug your device and plug it back in. You should now be able to access the HMD as a normal user.
 
 ### Compiling on Windows
 CMake has a lot of generators available for IDE's and build systems.
@@ -80,6 +83,7 @@ Using Make:
     export PREFIX=/usr/i686-w64-mingw32/ (or whatever your mingw path is)
     PKG_CONFIG_LIBDIR=$PREFIX/lib/pkgconfig ./configure --build=`gcc -dumpmachine` --host=i686-w64-mingw32 --prefix=$PREFIX
     make
+    
 the library will end up in the .lib directory, you can use microsoft's lib.exe to make a .lib file for it
 
 Using CMake:
@@ -99,6 +103,4 @@ Will be available soon.
 ## Using OpenHMD
 See the examples/ subdirectory for usage examples. The OpenGL example is not built by default, to build it use the --enable-openglexample option for the configure script. It requires SDL, glew and OpenGL.
 
-An API reference can be generated using doxygen and is also available here: http://openhmd.net/doxygen/0.1.0/openhmd_8h.html
-
-
+An API reference can be generated using doxygen and is also available here: http://openhmd.net/doxygen/0.1.0/openhmd_8h.html

+ 14 - 1
configure.ac

@@ -40,6 +40,15 @@ AC_ARG_ENABLE([driver-oculus-rift],
 
 AM_CONDITIONAL([BUILD_DRIVER_OCULUS_RIFT], [test "x$driver_oculus_rift_enabled" != "xno"])
 
+# HTC Vive driver
+AC_ARG_ENABLE([driver-htc-vive],
+        [AS_HELP_STRING([--disable-driver-htc-vive],
+                [disable building of HTC Vive driver [default=yes]])],
+        [driver_htc_vive_enabled=$enableval],
+        [driver_htc_vive_enabled='yes'])
+
+AM_CONDITIONAL([BUILD_DRIVER_HTC_VIVE], [test "x$driver_oculus_rift_enabled" != "xno"])
+
 # Deepoon Driver
 AC_ARG_ENABLE([driver-deepoon],
         [AS_HELP_STRING([--disable-driver-deepoon],
@@ -80,6 +89,10 @@ AM_CONDITIONAL([BUILD_DRIVER_ANDROID], [test "x$driver_android_enabled" != "xno"
 AS_IF([test "x$driver_oculus_rift_enabled" != "xno"],
 	[PKG_CHECK_MODULES([hidapi], [$hidapi] >= 0.0.5)])
 
+# Libs required by HTC Vive Driver
+AS_IF([test "x$driver_htc_vive_enabled" != "xno"],
+	[PKG_CHECK_MODULES([hidapi], [$hidapi] >= 0.0.5)])
+
 # Libs required by Depoon Driver
 AS_IF([test "x$driver_deepoon_enabled" != "xno"],
 	[PKG_CHECK_MODULES([hidapi], [$hidapi] >= 0.0.5)])
@@ -130,4 +143,4 @@ AC_PROG_CC_C99
 
 AC_CONFIG_HEADERS([config.h])
 AC_CONFIG_FILES([Makefile src/Makefile tests/Makefile tests/unittests/Makefile examples/Makefile examples/opengl/Makefile examples/simple/Makefile])
-AC_OUTPUT
+AC_OUTPUT 

+ 14 - 1
src/Makefile.am

@@ -13,7 +13,7 @@ libopenhmd_la_SOURCES = \
 	queue.c
 
 libopenhmd_la_LDFLAGS = -no-undefined -version-info 0:0:0
-libopenhmd_la_CPPFLAGS = -fPIC -I$(top_srcdir)/include -Wall 
+libopenhmd_la_CPPFLAGS = -fPIC -I$(top_srcdir)/include -Wall
 
 if BUILD_DRIVER_OCULUS_RIFT
 
@@ -26,6 +26,18 @@ libopenhmd_la_LDFLAGS += $(hidapi_LIBS)
 
 endif
 
+if BUILD_DRIVER_HTC_VIVE
+
+libopenhmd_la_SOURCES += \
+	drv_htc_vive/vive.c \
+	drv_htc_vive/packet.c \
+	ext_deps/mjson.c
+
+libopenhmd_la_CPPFLAGS += $(hidapi_CFLAGS) -DDRIVER_HTC_VIVE
+libopenhmd_la_LDFLAGS += $(hidapi_LIBS)
+
+endif
+
 if BUILD_DRIVER_DEEPOON
 
 libopenhmd_la_SOURCES += \
@@ -61,3 +73,4 @@ libopenhmd_la_SOURCES += \
 endif
 
 libopenhmd_la_LDFLAGS += $(EXTRA_LD_FLAGS)
+

+ 27 - 0
src/drv_htc_vive/magic.h

@@ -0,0 +1,27 @@
+static const unsigned char vive_magic_power_on[64] = {
+	0x04, 0x78, 0x29, 0x38, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x02, 0x00, 0x01,
+	0xa8, 0x0d, 0x76, 0x00, 0x40, 0xfc, 0x01, 0x05, 0xfa, 0xec, 0xd1, 0x6d, 0x00,
+	0x00, 0x6c, 0x00, 0x00, 0x00, 0x00, 0x00, 0xa8, 0x0d, 0x76, 0x00, 0x68, 0xfc,
+	0x01, 0x05, 0x2c, 0xb0, 0x2e, 0x65, 0x7a, 0x0d, 0x76, 0x00, 0x68, 0x54, 0x72,
+	0x00, 0x18, 0x54, 0x72, 0x00, 0x00, 0x6a, 0x72, 0x00, 0x00, 0x00, 0x00,
+};
+
+static const unsigned char vive_magic_power_off1[64] = {
+	0x04, 0x78, 0x29, 0x38, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x02, 0x00, 0x00,
+	0x30, 0x05, 0x77, 0x00, 0x30, 0x05, 0x77, 0x00, 0x6c, 0x4d, 0x37, 0x65, 0x40,
+	0xf9, 0x33, 0x00, 0x04, 0xf8, 0xa3, 0x04, 0x04, 0x00, 0x00, 0x00, 0x70, 0xb0,
+	0x72, 0x00, 0xf4, 0xf7, 0xa3, 0x04, 0x7c, 0xf8, 0x33, 0x00, 0x0c, 0xf8, 0xa3,
+	0x04, 0x0a, 0x6e, 0x29, 0x65, 0x24, 0xf9, 0x33, 0x00, 0x00, 0x00, 0x00,
+};
+
+static const unsigned char vive_magic_power_off2[64] = {
+	0x04, 0x78, 0x29, 0x38, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x02, 0x00, 0x00,
+	0x30, 0x05, 0x77, 0x00, 0xe4, 0xf7, 0x33, 0x00, 0xe4, 0xf7, 0x33, 0x00, 0x60,
+	0x6e, 0x72, 0x00, 0xb4, 0xf7, 0x33, 0x00, 0x04, 0x00, 0x00, 0x00, 0x70, 0xb0,
+	0x72, 0x00, 0x90, 0xf7, 0x33, 0x00, 0x7c, 0xf8, 0x33, 0x00, 0xd0, 0xf7, 0x33,
+	0x00, 0x3c, 0x68, 0x29, 0x65, 0x24, 0xf9, 0x33, 0x00, 0x00, 0x00, 0x00,
+};
+
+static const unsigned char vive_magic_enable_lighthouse[5] = {
+	0x04
+};

+ 116 - 0
src/drv_htc_vive/packet.c

@@ -0,0 +1,116 @@
+#include "vive.h"
+#include "vive_config.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 vive_decode_sensor_packet(vive_sensor_packet* pkt, const unsigned char* buffer, int size)
+{
+	if(size != 52){
+		LOGE("invalid vive sensor packet size (expected 52 but got %d)", size);
+		return false;
+	}
+
+	pkt->report_id = read8(&buffer);
+
+	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;
+}
+
+//Trim function for removing tabs and spaces from string buffers
+void trim(const char* src, char* buff, const unsigned int sizeBuff)
+{
+    if(sizeBuff < 1)
+    return;
+
+    const char* current = src;
+    unsigned int i = 0;
+    while(current != '\0' && i < sizeBuff-1)
+    {
+        if(*current != ' ' && *current != '\t')
+            buff[i++] = *current;
+        ++current;
+    }
+    buff[i] = '\0';
+}
+
+bool vive_decode_config_packet(vive_config_packet* pkt, const unsigned char* buffer, uint16_t size)
+{/*
+	if(size != 4069){
+		LOGE("invalid vive sensor packet size (expected 4069 but got %d)", size);
+		return false;
+	}*/
+
+	pkt->report_id = 17;
+	pkt->length = size;
+
+	unsigned char output[32768];
+	int output_size = 32768;
+
+	//int cmp_status = uncompress(pUncomp, &uncomp_len, pCmp, cmp_len);
+	int cmp_status = uncompress(output, (mz_ulong*)&output_size, buffer, (mz_ulong)pkt->length);
+	if (cmp_status != Z_OK){
+		LOGE("invalid vive config, could not uncompress");
+		return false;
+	}
+
+	LOGE("Decompressed from %u to %u bytes\n", (mz_uint32)pkt->length, (mz_uint32)output_size);
+
+	//printf("Debug print all the RAW JSON things!\n%s", output);
+	//pUncomp should now be the uncompressed data, lets get the json from it
+	/** DEBUG JSON PARSER CODE **/
+	trim((char*)output,(char*)output,output_size);
+	//printf("%s\n",output);
+	/*
+	FILE* dfp;
+	dfp = fopen("jsondebug.json","w");
+	json_enable_debug(3, dfp);*/
+	int status = json_read_object((char*)output, sensor_offsets, NULL);
+	printf("\n--- Converted Vive JSON Data ---\n\n");
+	printf("acc_bias = %f %f %f\n", acc_bias[0], acc_bias[1], acc_bias[2]);
+	printf("acc_scale = %f %f %f\n", acc_scale[0], acc_scale[1], acc_scale[2]);
+	printf("gyro_bias = %f %f %f\n", gyro_bias[0], gyro_bias[1], gyro_bias[2]);
+	printf("gyro_scale = %f %f %f\n", gyro_scale[0], gyro_scale[1], gyro_scale[2]);
+	printf("\n--- End of Vive JSON Data ---\n\n");
+
+	if (status != 0)
+		puts(json_error_string(status));
+	/** END OF DEBUG JSON PARSER CODE **/
+
+//	free(pCmp);
+//	free(pUncomp);
+
+	return true;
+}

+ 432 - 0
src/drv_htc_vive/vive.c

@@ -0,0 +1,432 @@
+/*
+ * 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.
+ */
+
+/* HTC Vive Driver */
+
+#define FEATURE_BUFFER_SIZE 256
+
+#define HTC_ID                   0x0bb4
+#define VIVE_HMD                 0x2c87
+
+#define VALVE_ID                 0x28de
+#define VIVE_WATCHMAN_DONGLE     0x2101
+#define VIVE_LIGHTHOUSE_FPGA_RX  0x2000
+
+#define VIVE_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 "vive.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;
+
+	vive_config_packet vive_config;
+	vec3f gyro_error;
+	filter_queue gyro_q;
+} vive_priv;
+
+void vec3f_from_vive_vec_accel(const int16_t* smp, vec3f* out_vec)
+{
+	float gravity = 9.81f;
+	float scaler = 4.0f * gravity / 32768.0f;
+
+	out_vec->x = (float)smp[0] * scaler;
+	out_vec->y = (float)smp[1] * scaler * -1;
+	out_vec->z = (float)smp[2] * scaler * -1;
+}
+
+void vec3f_from_vive_vec_gyro(const int16_t* smp, vec3f* out_vec)
+{
+	float scaler = 8.7f / 32768.0f;
+	out_vec->x = (float)smp[0] * scaler;
+	out_vec->y = (float)smp[1] * scaler * -1;
+	out_vec->z = (float)smp[2] * scaler * -1;
+}
+
+static bool process_error(vive_priv* priv)
+{
+	if(priv->gyro_q.at >= priv->gyro_q.size - 1)
+		return true;
+
+	ofq_add(&priv->gyro_q, &priv->raw_gyro);
+
+	if(priv->gyro_q.at >= priv->gyro_q.size - 1){
+		ofq_get_mean(&priv->gyro_q, &priv->gyro_error);
+		printf("gyro error: %f, %f, %f\n", priv->gyro_error.x, priv->gyro_error.y, priv->gyro_error.z);
+	}
+
+	return false;
+}
+
+vive_sensor_sample* get_next_sample(vive_sensor_packet* pkt, int last_seq)
+{
+	int diff[3];
+
+	for(int i = 0; i < 3; i++)
+	{
+		diff[i] = (int)pkt->samples[i].seq - last_seq;
+
+		if(diff[i] < -128){
+			diff[i] += 256;
+		}
+	}
+
+	int closest_diff = INT_MAX;
+	int closest_idx = -1;
+
+	for(int i = 0; i < 3; i++)
+	{
+		if(diff[i] < closest_diff && diff[i] > 0 && diff[i] < 128){
+			closest_diff = diff[i];
+			closest_idx = i;
+		}
+	}
+
+	if(closest_idx != -1)
+		return pkt->samples + closest_idx;
+
+	return NULL;
+}
+
+static void update_device(ohmd_device* device)
+{
+	vive_priv* priv = (vive_priv*)device;
+
+	int size = 0;
+	unsigned char buffer[FEATURE_BUFFER_SIZE];
+
+	while((size = hid_read(priv->imu_handle, buffer, FEATURE_BUFFER_SIZE)) > 0){
+		if(buffer[0] == VIVE_IRQ_SENSORS){
+			vive_sensor_packet pkt;
+			vive_decode_sensor_packet(&pkt, buffer, size);
+
+			vive_sensor_sample* smp = NULL;
+
+			while((smp = get_next_sample(&pkt, priv->last_seq)) != NULL)
+			{
+				if(priv->last_ticks == 0)
+					priv->last_ticks = smp->time_ticks;
+
+				uint32_t t1, t2;
+				t1 = smp->time_ticks;
+				t2 = priv->last_ticks;
+
+				float dt = (t1 - t2) / VIVE_TIME_DIV;
+
+				priv->last_ticks = smp->time_ticks;
+
+				vec3f_from_vive_vec_accel(smp->acc, &priv->raw_accel);
+				vec3f_from_vive_vec_gyro(smp->rot, &priv->raw_gyro);
+
+				if(process_error(priv)){
+					vec3f mag = {{0.0f, 0.0f, 0.0f}};
+					vec3f gyro;
+					ovec3f_subtract(&priv->raw_gyro, &priv->gyro_error, &gyro);
+
+					ofusion_update(&priv->sensor_fusion, dt, &gyro, &priv->raw_accel, &mag);
+				}
+
+				priv->last_seq = smp->seq;
+			}
+		}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)
+{
+	vive_priv* priv = (vive_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;
+	vive_priv* priv = (vive_priv*)device;
+
+	LOGD("closing HTC Vive device");
+
+	// turn the display off
+	hret = hid_send_feature_report(priv->hmd_handle, vive_magic_power_off1, sizeof(vive_magic_power_off1));
+	printf("power off magic 1: %d\n", hret);
+
+	hret = hid_send_feature_report(priv->hmd_handle, vive_magic_power_off2, sizeof(vive_magic_power_off2));
+	printf("power off magic 2: %d\n", hret);
+
+	hid_close(priv->hmd_handle);
+	hid_close(priv->imu_handle);
+
+	free(device);
+}
+
+#if 0
+static void dump_indexed_string(hid_device* device, int index)
+{
+	wchar_t wbuffer[512] = {0};
+	char buffer[1024] = {0};
+
+	int hret = hid_get_indexed_string(device, index, wbuffer, 511);
+
+	if(hret == 0){
+		wcstombs(buffer, wbuffer, sizeof(buffer));
+		printf("indexed string 0x%02x: '%s'\n", index, buffer);
+	}
+}
+#endif
+
+static void dump_info_string(int (*fun)(hid_device*, wchar_t*, size_t), const char* what, hid_device* device)
+{
+	wchar_t wbuffer[512] = {0};
+	char buffer[1024] = {0};
+
+	int hret = fun(device, wbuffer, 511);
+
+	if(hret == 0){
+		wcstombs(buffer, wbuffer, sizeof(buffer));
+		printf("%s: '%s'\n", what, buffer);
+	}
+}
+
+#if 0
+static void dumpbin(const char* label, const unsigned char* data, int length)
+{
+	printf("%s:\n", label);
+	for(int i = 0; i < length; i++){
+		printf("%02x ", data[i]);
+		if((i % 16) == 15)
+			printf("\n");
+	}
+	printf("\n");
+}
+#endif
+
+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)
+{
+	vive_priv* priv = ohmd_alloc(driver->ctx, sizeof(vive_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(HTC_ID, VIVE_HMD, 0, 1, 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;
+	}
+
+	// Open the lighthouse device
+	priv->imu_handle = open_device_idx(VALVE_ID, VIVE_LIGHTHOUSE_FPGA_RX, 0, 2, idx);
+
+	if(!priv->imu_handle)
+		goto cleanup;
+
+	if(hid_set_nonblocking(priv->imu_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, vive_magic_power_on, sizeof(vive_magic_power_on));
+	printf("power on magic: %d\n", hret);
+
+	// enable lighthouse
+	//hret = hid_send_feature_report(priv->hmd_handle, vive_magic_enable_lighthouse, sizeof(vive_magic_enable_lighthouse));
+	//printf("enable lighthouse magic: %d\n", hret);
+
+	unsigned char buffer[128];
+	int bytes;
+
+	printf("Getting feature report 16 to 39\n");
+	buffer[0] = 16;
+	bytes = hid_get_feature_report(priv->imu_handle, buffer, sizeof(buffer));
+	printf("got %i bytes\n", bytes);
+	for (int i = 0; i < bytes; i++) {
+		printf("%02hx ", buffer[i]);
+	}
+	printf("\n\n");
+
+	unsigned char* packet_buffer = malloc(4096);
+
+	int offset = 0;
+	while (buffer[1] != 0) {
+		buffer[0] = 17;
+		bytes = hid_get_feature_report(priv->imu_handle, buffer, sizeof(buffer));
+
+ 		memcpy((uint8_t*)packet_buffer + offset, buffer+2, buffer[1]);
+ 		offset += buffer[1];
+	}
+	packet_buffer[offset] = '\0';
+	//printf("Result: %s\n", packet_buffer);
+	vive_decode_config_packet(&priv->vive_config, packet_buffer, offset);
+
+	// 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 = 2160;
+	priv->base.properties.vres = 1200;
+	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 = (2160.0f / 1200.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);
+
+	ofq_init(&priv->gyro_q, 128);
+
+	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(HTC_ID, VIVE_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 HTC Vive Driver");
+		strcpy(desc->vendor, "HTC/Valve");
+		strcpy(desc->product, "HTC Vive");
+
+		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 Vive driver");
+	free(drv);
+}
+
+ohmd_driver* ohmd_create_htc_vive_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;
+}

+ 41 - 0
src/drv_htc_vive/vive.h

@@ -0,0 +1,41 @@
+#ifndef VIVE_H
+#define VIVE_H
+
+#include <stdint.h>
+#include <stdbool.h>
+
+#include "../openhmdi.h"
+#include "magic.h"
+
+typedef enum
+{
+	VIVE_CONFIG_DATA = 17,
+	VIVE_IRQ_SENSORS = 32,
+} vive_irq_cmd;
+
+typedef struct
+{
+	int16_t acc[3];
+	int16_t rot[3];
+	uint32_t time_ticks;
+	uint8_t seq;
+} vive_sensor_sample;
+
+typedef struct
+{
+	uint8_t report_id;
+	vive_sensor_sample samples[3];
+} vive_sensor_packet;
+
+typedef struct
+{
+	uint8_t report_id;
+	uint16_t length;
+	unsigned char config_data[99999];
+} vive_config_packet;
+
+void vec3f_from_vive_vec(const int16_t* smp, vec3f* out_vec);
+bool vive_decode_sensor_packet(vive_sensor_packet* pkt, const unsigned char* buffer, int size);
+bool vive_decode_config_packet(vive_config_packet* pkt, const unsigned char* buffer, uint16_t size);
+
+#endif

+ 41 - 0
src/drv_htc_vive/vive_config.h

@@ -0,0 +1,41 @@
+/* Suppress the warnings for this include, since we don't care about them for external dependencies
+ * Requires at least GCC 4.6 or higher
+*/
+#pragma GCC diagnostic push
+#pragma GCC diagnostic ignored "-Wstrict-aliasing"
+#pragma GCC diagnostic ignored "-Wswitch"
+#include "../ext_deps/miniz.c"
+#include "../ext_deps/mjson.h"
+#pragma GCC diagnostic pop
+
+//Temporary, will get 'structed up'
+static double acc_bias[3];
+static double acc_scale[3];
+static double gyro_bias[3];
+static double gyro_scale[3];
+//end of temporary
+
+//Vive config packet
+const struct json_attr_t sensor_offsets[] = {
+    {"acc_bias", t_array,   .addr.array.element_type = t_real,
+                            .addr.array.arr.reals.store = acc_bias,
+                            .addr.array.maxlen = 3},
+    {"acc_scale", t_array,  .addr.array.element_type = t_real,
+                            .addr.array.arr.reals.store = acc_scale,
+                            .addr.array.maxlen = 3},
+    {"device", t_object},
+    {"device_class", t_ignore},
+    {"device_pid", t_ignore},
+    {"device_serial_number", t_ignore},
+    {"device_vid", t_ignore},
+    {"display_edid", t_ignore},
+    {"display_gc", t_ignore},
+    {"display_mc", t_ignore},
+    {"gyro_bias", t_array,  .addr.array.element_type = t_real,
+                            .addr.array.arr.reals.store = gyro_bias,
+                            .addr.array.maxlen = 3},
+    {"gyro_scale", t_array, .addr.array.element_type = t_real,
+                            .addr.array.arr.reals.store = gyro_scale,
+                            .addr.array.maxlen = 3},
+    {NULL},
+};

Rozdielové dáta súboru neboli zobrazené, pretože súbor je príliš veľký
+ 4916 - 0
src/ext_deps/miniz.c


+ 779 - 0
src/ext_deps/mjson.c

@@ -0,0 +1,779 @@
+/****************************************************************************
+
+NAME
+   mjson.c - parse JSON into fixed-extent data structures
+
+DESCRIPTION
+   This module parses a large subset of JSON (JavaScript Object
+Notation).  Unlike more general JSON parsers, it doesn't use malloc(3)
+and doesn't support polymorphism; you need to give it a set of
+template structures describing the expected shape of the incoming
+JSON, and it will error out if that shape is not matched.  When the
+parse succeeds, attribute values will be extracted into static
+locations specified in the template structures.
+
+   The "shape" of a JSON object in the type signature of its
+attributes (and attribute values, and so on recursively down through
+all nestings of objects and arrays).  This parser is indifferent to
+the order of attributes at any level, but you have to tell it in
+advance what the type of each attribute value will be and where the
+parsed value will be stored. The template structures may supply
+default values to be used when an expected attribute is omitted.
+
+   The preceding paragraph told one fib.  A single attribute may
+actually have a span of multiple specifications with different
+syntactically distinguishable types (e.g. string vs. real vs. integer
+vs. boolean, but not signed integer vs. unsigned integer).  The parser
+will match the right spec against the actual data.
+
+   The dialect this parses has some limitations.  First, it cannot
+recognize the JSON "null" value. Second, all elements of an array must
+be of the same type. Third, characters may not be array elements (this
+restriction could be lifted)
+
+   There are separate entry points for beginning a parse of either
+JSON object or a JSON array. JSON "float" quantities are actually
+stored as doubles.
+
+   This parser processes object arrays in one of two different ways,
+defending on whether the array subtype is declared as object or
+structobject.
+
+   Object arrays take one base address per object subfield, and are
+mapped into parallel C arrays (one per subfield).  Strings are not
+supported in this kind of array, as they don't have a "natural" size
+to use as an offset multiplier.
+
+   Structobjects arrays are a way to parse a list of objects to a set
+of modifications to a corresponding array of C structs.  The trick is
+that the array object initialization has to specify both the C struct
+array's base address and the stride length (the size of the C struct).
+If you initialize the offset fields with the correct offsetof calls,
+everything will work. Strings are supported but all string storage
+has to be inline in the struct.
+
+PERMISSIONS
+   This file is Copyright (c) 2014 by Eric S. Raymond
+   BSD terms apply: see the file COPYING in the distribution root for details.
+
+***************************************************************************/
+/* The strptime prototype is not provided unless explicitly requested.
+ * We also need to set the value high enough to signal inclusion of
+ * newer features (like clock_gettime).  See the POSIX spec for more info:
+ * http://pubs.opengroup.org/onlinepubs/9699919799/functions/V2_chap02.html#tag_15_02_01_02 */
+#define _XOPEN_SOURCE 600
+
+#include <stdio.h>
+#include <string.h>
+#include <stdlib.h>
+#include <stdbool.h>
+#include <stdarg.h>
+#include <ctype.h>
+#include <errno.h>
+#include <time.h>
+#include <math.h>	/* for HUGE_VAL */
+
+#include "mjson.h"
+
+#define DEBUG_ENABLE 1
+#ifdef DEBUG_ENABLE
+static int debuglevel = 0;
+static FILE *debugfp;
+
+void json_enable_debug(int level, FILE * fp)
+/* control the level and destination of debug trace messages */
+{
+    debuglevel = level;
+    debugfp = fp;
+}
+
+static void json_trace(int errlevel, const char *fmt, ...)
+/* assemble command in printf(3) style */
+{
+    if (errlevel <= debuglevel) {
+	char buf[BUFSIZ];
+	va_list ap;
+
+	(void)strncpy(buf, "json: ", BUFSIZ-1);
+	buf[BUFSIZ-1] = '\0';
+	va_start(ap, fmt);
+	(void)vsnprintf(buf + strlen(buf), sizeof(buf) - strlen(buf), fmt,
+			ap);
+	va_end(ap);
+
+	(void)fputs(buf, debugfp);
+    }
+}
+
+# define json_debug_trace(args) (void) json_trace args
+#else
+# define json_debug_trace(args) /*@i1@*/do { } while (0)
+#endif /* DEBUG_ENABLE */
+
+/*@-immediatetrans -dependenttrans -usereleased -compdef@*/
+static /*@null@*/ char *json_target_address(const struct json_attr_t *cursor,
+					     /*@null@*/
+					     const struct json_array_t
+					     *parent, int offset)
+{
+    char *targetaddr = NULL;
+    if (parent == NULL || parent->element_type != t_structobject) {
+	/* ordinary case - use the address in the cursor structure */
+	switch (cursor->type) {
+	case t_ignore:
+	    targetaddr = NULL;
+	    break;
+	case t_integer:
+	    targetaddr = (char *)&cursor->addr.integer[offset];
+	    break;
+	case t_uinteger:
+	    targetaddr = (char *)&cursor->addr.uinteger[offset];
+	    break;
+	case t_time:
+	case t_real:
+	    targetaddr = (char *)&cursor->addr.real[offset];
+	    break;
+	case t_string:
+	    targetaddr = cursor->addr.string;
+	    break;
+	case t_boolean:
+	    targetaddr = (char *)&cursor->addr.boolean[offset];
+	    break;
+	case t_character:
+	    targetaddr = (char *)&cursor->addr.character[offset];
+	    break;
+	default:
+	    targetaddr = NULL;
+	    break;
+	}
+    } else
+	/* tricky case - hacking a member in an array of structures */
+	targetaddr =
+	    parent->arr.objects.base + (offset * parent->arr.objects.stride) +
+	    cursor->addr.offset;
+    json_debug_trace((1, "Target address for %s (offset %d) is %p\n",
+		      cursor->attribute, offset, targetaddr));
+    return targetaddr;
+}
+
+#ifdef TIME_ENABLE
+static double iso8601_to_unix( /*@in@*/ char *isotime)
+/* ISO8601 UTC to Unix UTC */
+{
+    char *dp = NULL;
+    double usec;
+    struct tm tm;
+
+   /*@i1@*/ dp = strptime(isotime, "%Y-%m-%dT%H:%M:%S", &tm);
+    if (dp == NULL)
+	return (double)HUGE_VAL;
+    if (*dp == '.')
+	usec = strtod(dp, NULL);
+    else
+	usec = 0;
+    return (double)timegm(&tm) + usec;
+}
+#endif /* TIME_ENABLE */
+
+/*@-immediatetrans -dependenttrans +usereleased +compdef@*/
+
+static int json_internal_read_object(const char *cp,
+				     const struct json_attr_t *attrs,
+				     /*@null@*/
+				     const struct json_array_t *parent,
+				     int offset,
+				     /*@null@*/ const char **end)
+{
+    /*@ -nullstate -nullderef -mustfreefresh -nullpass -usedef @*/
+    enum
+    { init, await_attr, in_attr, await_value, in_val_string,
+	in_escape, in_val_token, post_val, post_array
+    } state = 0;
+#ifdef DEBUG_ENABLE
+    char *statenames[] = {
+	"init", "await_attr", "in_attr", "await_value", "in_val_string",
+	"in_escape", "in_val_token", "post_val", "post_array",
+    };
+#endif /* DEBUG_ENABLE */
+    char attrbuf[JSON_ATTR_MAX + 1], *pattr = NULL;
+    char valbuf[JSON_VAL_MAX + 1], *pval = NULL;
+    bool value_quoted = false;
+    char uescape[5];		/* enough space for 4 hex digits and a NUL */
+    const struct json_attr_t *cursor;
+    int substatus, n, maxlen = 0;
+    unsigned int u;
+    const struct json_enum_t *mp;
+    char *lptr;
+
+#ifdef S_SPLINT_S
+    /* prevents gripes about buffers not being completely defined */
+    memset(valbuf, '\0', sizeof(valbuf));
+    memset(attrbuf, '\0', sizeof(attrbuf));
+#endif /* S_SPLINT_S */
+
+    if (end != NULL)
+	*end = NULL;		/* give it a well-defined value on parse failure */
+
+    /* stuff fields with defaults in case they're omitted in the JSON input */
+    for (cursor = attrs; cursor->attribute != NULL; cursor++)
+	if (!cursor->nodefault) {
+	    lptr = json_target_address(cursor, parent, offset);
+	    if (lptr != NULL)
+		switch (cursor->type) {
+		case t_integer:
+		    memcpy(lptr, &cursor->dflt.integer, sizeof(int));
+		    break;
+		case t_uinteger:
+		    memcpy(lptr, &cursor->dflt.uinteger, sizeof(unsigned int));
+		    break;
+		case t_time:
+		case t_real:
+		    memcpy(lptr, &cursor->dflt.real, sizeof(double));
+		    break;
+		case t_string:
+		    if (parent != NULL
+			&& parent->element_type != t_structobject
+			&& offset > 0)
+			return JSON_ERR_NOPARSTR;
+		    lptr[0] = '\0';
+		    break;
+		case t_boolean:
+		    memcpy(lptr, &cursor->dflt.boolean, sizeof(bool));
+		    break;
+		case t_character:
+		    lptr[0] = cursor->dflt.character;
+		    break;
+		case t_object:	/* silences a compiler warning */
+		case t_structobject:
+		case t_array:
+		case t_check:
+		case t_ignore:
+		    break;
+		}
+	}
+
+    json_debug_trace((1, "JSON parse of '%s' begins.\n", cp));
+
+    /* parse input JSON */
+    for (; *cp != '\0'; cp++) {
+	json_debug_trace((2, "State %-14s, looking at '%c' (%p)\n",
+			  statenames[state], *cp, cp));
+	switch (state) {
+	case init:
+	    if (isspace((unsigned char) *cp))
+		continue;
+	    else if (*cp == '{')
+		state = await_attr;
+	    else {
+		json_debug_trace((1,
+				  "Non-WS when expecting object start.\n"));
+		if (end != NULL)
+		    *end = cp;
+		return JSON_ERR_OBSTART;
+	    }
+	    break;
+	case await_attr:
+	    if (isspace((unsigned char) *cp))
+		continue;
+	    else if (*cp == '"') {
+		state = in_attr;
+		pattr = attrbuf;
+		if (end != NULL)
+		    *end = cp;
+	    } else if (*cp == '}')
+		break;
+	    else {
+		json_debug_trace((1, "Non-WS when expecting attribute.\n"));
+		if (end != NULL)
+		    *end = cp;
+		return JSON_ERR_ATTRSTART;
+	    }
+	    break;
+	case in_attr:
+	    if (pattr == NULL)
+		/* don't update end here, leave at attribute start */
+		return JSON_ERR_NULLPTR;
+	    if (*cp == '"') {
+		*pattr++ = '\0';
+		json_debug_trace((1, "Collected attribute name %s\n",
+				  attrbuf));
+		for (cursor = attrs; cursor->attribute != NULL; cursor++) {
+		    json_debug_trace((2, "Checking against %s\n",
+				      cursor->attribute));
+		    if (strcmp(cursor->attribute, attrbuf) == 0)
+			break;
+		}
+		if (cursor->attribute == NULL) {
+		    json_debug_trace((1,
+				      "Unknown attribute name '%s' (attributes begin with '%s').\n",
+				      attrbuf, attrs->attribute));
+		    /* don't update end here, leave at attribute start */
+		    return JSON_ERR_BADATTR;
+		}
+		state = await_value;
+		if (cursor->type == t_string)
+		    maxlen = (int)cursor->len - 1;
+		else if (cursor->type == t_check)
+		    maxlen = (int)strlen(cursor->dflt.check);
+		else if (cursor->type == t_time || cursor->type == t_ignore)
+		    maxlen = JSON_VAL_MAX;
+		else if (cursor->map != NULL)
+		    maxlen = (int)sizeof(valbuf) - 1;
+		pval = valbuf;
+	    } else if (pattr >= attrbuf + JSON_ATTR_MAX - 1) {
+		json_debug_trace((1, "Attribute name too long.\n"));
+		/* don't update end here, leave at attribute start */
+		return JSON_ERR_ATTRLEN;
+	    } else
+		*pattr++ = *cp;
+	    break;
+	case await_value:
+	    if (isspace((unsigned char) *cp) || *cp == ':')
+		continue;
+	    else if (*cp == '[') {
+		if (cursor->type != t_array) {
+		    json_debug_trace((1,
+				      "Saw [ when not expecting array.\n"));
+		    if (end != NULL)
+			*end = cp;
+		    return JSON_ERR_NOARRAY;
+		}
+		substatus = json_read_array(cp, &cursor->addr.array, &cp);
+		if (substatus != 0)
+		    return substatus;
+		state = post_array;
+	    } else if (cursor->type == t_array) {
+		json_debug_trace((1,
+				  "Array element was specified, but no [.\n"));
+		if (end != NULL)
+		    *end = cp;
+		return JSON_ERR_NOBRAK;
+	    } else if (*cp == '"') {
+		value_quoted = true;
+		state = in_val_string;
+		pval = valbuf;
+	    } else {
+		value_quoted = false;
+		state = in_val_token;
+		pval = valbuf;
+		*pval++ = *cp;
+	    }
+	    break;
+	case in_val_string:
+	    if (pval == NULL)
+		/* don't update end here, leave at value start */
+		return JSON_ERR_NULLPTR;
+	    if (*cp == '\\')
+		state = in_escape;
+	    else if (*cp == '"') {
+		*pval++ = '\0';
+		json_debug_trace((1, "Collected string value %s\n", valbuf));
+		state = post_val;
+	    } else if (pval > valbuf + JSON_VAL_MAX - 1
+		       || pval > valbuf + maxlen) {
+		json_debug_trace((1, "String value too long.\n"));
+		/* don't update end here, leave at value start */
+		return JSON_ERR_STRLONG;	/*  */
+	    } else
+		*pval++ = *cp;
+	    break;
+	case in_escape:
+	    if (pval == NULL)
+		/* don't update end here, leave at value start */
+		return JSON_ERR_NULLPTR;
+	    switch (*cp) {
+	    case 'b':
+		*pval++ = '\b';
+		break;
+	    case 'f':
+		*pval++ = '\f';
+		break;
+	    case 'n':
+		*pval++ = '\n';
+		break;
+	    case 'r':
+		*pval++ = '\r';
+		break;
+	    case 't':
+		*pval++ = '\t';
+		break;
+	    case 'u':
+		for (n = 0; n < 4 && cp[n] != '\0'; n++)
+		    uescape[n] = *cp++;
+		--cp;
+		(void)sscanf(uescape, "%04x", &u);
+		*pval++ = (char)u;	/* will truncate values above 0xff */
+		break;
+	    default:		/* handles double quote and solidus */
+		*pval++ = *cp;
+		break;
+	    }
+	    state = in_val_string;
+	    break;
+	case in_val_token:
+	    if (pval == NULL)
+		/* don't update end here, leave at value start */
+		return JSON_ERR_NULLPTR;
+	    if (isspace((unsigned char) *cp) || *cp == ',' || *cp == '}') {
+		*pval = '\0';
+		json_debug_trace((1, "Collected token value %s.\n", valbuf));
+		state = post_val;
+		if (*cp == '}' || *cp == ',')
+		    --cp;
+	    } else if (pval > valbuf + JSON_VAL_MAX - 1) {
+		json_debug_trace((1, "Token value too long.\n"));
+		/* don't update end here, leave at value start */
+		return JSON_ERR_TOKLONG;
+	    } else
+		*pval++ = *cp;
+	    break;
+	case post_val:
+	    /*
+	     * We know that cursor points at the first spec matching
+	     * the current attribute.  We don't know that it's *the*
+	     * correct spec; our dialect allows there to be any number
+	     * of adjacent ones with the same attrname but different
+	     * types.  Here's where we try to seek forward for a
+	     * matching type/attr pair if we're not looking at one.
+	     */
+	    for (;;) {
+		int seeking = cursor->type;
+		if (value_quoted && (cursor->type == t_string || cursor->type == t_time))
+		    break;
+		if ((strcmp(valbuf, "true")==0 || strcmp(valbuf, "false")==0)
+			&& seeking == t_boolean)
+		    break;
+		if (isdigit((unsigned char) valbuf[0])) {
+		    bool decimal = strchr(valbuf, '.') != NULL;
+		    if (decimal && seeking == t_real)
+			break;
+		    if (!decimal && (seeking == t_integer || seeking == t_uinteger))
+			break;
+		}
+		if (cursor[1].attribute==NULL)	/* out of possiblities */
+		    break;
+		if (strcmp(cursor[1].attribute, attrbuf)!=0)
+		    break;
+		++cursor;
+	    }
+	    if (value_quoted
+		&& (cursor->type != t_string && cursor->type != t_character
+		    && cursor->type != t_check && cursor->type != t_time
+		    && cursor->type != t_ignore && cursor->map == 0)) {
+		json_debug_trace((1,
+				  "Saw quoted value when expecting non-string.\n"));
+		return JSON_ERR_QNONSTRING;
+	    }
+	    if (!value_quoted
+		&& (cursor->type == t_string || cursor->type == t_check
+		    || cursor->type == t_time || cursor->map != 0)) {
+		json_debug_trace((1,
+				  "Didn't see quoted value when expecting string.\n"));
+		return JSON_ERR_NONQSTRING;
+	    }
+	    if (cursor->map != 0) {
+		for (mp = cursor->map; mp->name != NULL; mp++)
+		    if (strcmp(mp->name, valbuf) == 0) {
+			goto foundit;
+		    }
+		json_debug_trace((1, "Invalid enumerated value string %s.\n",
+				  valbuf));
+		return JSON_ERR_BADENUM;
+	      foundit:
+		(void)snprintf(valbuf, sizeof(valbuf), "%d", mp->value);
+	    }
+	    lptr = json_target_address(cursor, parent, offset);
+	    if (lptr != NULL)
+		switch (cursor->type) {
+		case t_integer:
+		    {
+			int tmp = atoi(valbuf);
+			memcpy(lptr, &tmp, sizeof(int));
+		    }
+		    break;
+		case t_uinteger:
+		    {
+			unsigned int tmp = (unsigned int)atoi(valbuf);
+			memcpy(lptr, &tmp, sizeof(unsigned int));
+		    }
+		    break;
+		case t_time:
+#ifdef TIME_ENABLE
+		    {
+			double tmp = iso8601_to_unix(valbuf);
+			memcpy(lptr, &tmp, sizeof(double));
+		    }
+#endif /* TIME_ENABLE */
+		    break;
+		case t_real:
+		    {
+			double tmp = atof(valbuf);
+			memcpy(lptr, &tmp, sizeof(double));
+		    }
+		    break;
+		case t_string:
+		    if (parent != NULL
+			&& parent->element_type != t_structobject
+			&& offset > 0)
+			return JSON_ERR_NOPARSTR;
+		    (void)strncpy(lptr, valbuf, cursor->len);
+		    valbuf[sizeof(valbuf)-1] = '\0';
+		    break;
+		case t_boolean:
+		    {
+			bool tmp = (strcmp(valbuf, "true") == 0);
+			memcpy(lptr, &tmp, sizeof(bool));
+		    }
+		    break;
+		case t_character:
+		    if (strlen(valbuf) > 1)
+			/* don't update end here, leave at value start */
+			return JSON_ERR_STRLONG;
+		    else
+			lptr[0] = valbuf[0];
+		    break;
+		case t_ignore:	/* silences a compiler warning */
+		case t_object:	/* silences a compiler warning */
+		case t_structobject:
+		case t_array:
+		    break;
+		case t_check:
+		    if (strcmp(cursor->dflt.check, valbuf) != 0) {
+			json_debug_trace((1,
+					  "Required attribute value %s not present.\n",
+					  cursor->dflt.check));
+			/* don't update end here, leave at start of attribute */
+			return JSON_ERR_CHECKFAIL;
+		    }
+		    break;
+		}
+	    /*@fallthrough@*/
+	case post_array:
+	    if (isspace((unsigned char) *cp))
+		continue;
+	    else if (*cp == ',')
+		state = await_attr;
+	    else if (*cp == '}') {
+		++cp;
+		goto good_parse;
+	    } else {
+		json_debug_trace((1, "Garbage while expecting comma or }\n"));
+		if (end != NULL)
+		    *end = cp;
+		return JSON_ERR_BADTRAIL;
+	    }
+	    break;
+	}
+    }
+
+  good_parse:
+    /* in case there's another object following, consume trailing WS */
+    while (isspace((unsigned char) *cp))
+	++cp;
+    if (end != NULL)
+	*end = cp;
+    json_debug_trace((1, "JSON parse ends.\n"));
+    return 0;
+    /*@ +nullstate +nullderef +mustfreefresh +nullpass +usedef @*/
+}
+
+int json_read_array(const char *cp, const struct json_array_t *arr,
+		    const char **end)
+{
+    /*@-nullstate -onlytrans@*/
+    int substatus, offset, arrcount;
+    char *tp;
+
+    if (end != NULL)
+	*end = NULL;		/* give it a well-defined value on parse failure */
+
+    json_debug_trace((1, "Entered json_read_array()\n"));
+
+    while (isspace((unsigned char) *cp))
+	cp++;
+    if (*cp != '[') {
+	json_debug_trace((1, "Didn't find expected array start\n"));
+	return JSON_ERR_ARRAYSTART;
+    } else
+	cp++;
+
+    tp = arr->arr.strings.store;
+    arrcount = 0;
+
+    /* Check for empty array */
+    while (isspace((unsigned char) *cp))
+	cp++;
+    if (*cp == ']')
+	goto breakout;
+
+    for (offset = 0; offset < arr->maxlen; offset++) {
+	char *ep = NULL;
+	json_debug_trace((1, "Looking at %s\n", cp));
+	switch (arr->element_type) {
+	case t_string:
+	    if (isspace((unsigned char) *cp))
+		cp++;
+	    if (*cp != '"')
+		return JSON_ERR_BADSTRING;
+	    else
+		++cp;
+	    arr->arr.strings.ptrs[offset] = tp;
+	    for (; tp - arr->arr.strings.store < arr->arr.strings.storelen;
+		 tp++)
+		if (*cp == '"') {
+		    ++cp;
+		    *tp++ = '\0';
+		    goto stringend;
+		} else if (*cp == '\0') {
+		    json_debug_trace((1,
+				      "Bad string syntax in string list.\n"));
+		    return JSON_ERR_BADSTRING;
+		} else {
+		    *tp = *cp++;
+		}
+	    json_debug_trace((1, "Bad string syntax in string list.\n"));
+	    return JSON_ERR_BADSTRING;
+	  stringend:
+	    break;
+	case t_object:
+	case t_structobject:
+	    substatus =
+		json_internal_read_object(cp, arr->arr.objects.subtype, arr,
+					  offset, &cp);
+	    if (substatus != 0) {
+		if (end != NULL)
+		    end = &cp;
+		return substatus;
+	    }
+	    break;
+	case t_integer:
+	    arr->arr.integers.store[offset] = (int)strtol(cp, &ep, 0);
+	    if (ep == cp)
+		return JSON_ERR_BADNUM;
+	    else
+		cp = ep;
+	    break;
+	case t_uinteger:
+	    arr->arr.uintegers.store[offset] = (unsigned int)strtoul(cp, &ep, 0);
+	    if (ep == cp)
+		return JSON_ERR_BADNUM;
+	    else
+		cp = ep;
+	    break;
+#ifdef TIME_ENABLE
+	case t_time:
+	    if (*cp != '"')
+		return JSON_ERR_BADSTRING;
+	    else
+		++cp;
+	    arr->arr.reals.store[offset] = iso8601_to_unix((char *)cp);
+	    if (arr->arr.reals.store[offset] >= HUGE_VAL)
+		return JSON_ERR_BADNUM;
+	    while (*cp && *cp != '"')
+		cp++;
+	    if (*cp != '"')
+		return JSON_ERR_BADSTRING;
+	    else
+		++cp; 
+	    break;
+#endif /* TIME_ENABLE */
+	case t_real:
+	    arr->arr.reals.store[offset] = strtod(cp, &ep);
+	    if (ep == cp)
+		return JSON_ERR_BADNUM;
+	    else
+		cp = ep;
+	    break;
+	case t_boolean:
+	    if (strncmp(cp, "true", 4) == 0) {
+		arr->arr.booleans.store[offset] = true;
+		cp += 4;
+	    }
+	    else if (strncmp(cp, "false", 5) == 0) {
+		arr->arr.booleans.store[offset] = false;
+		cp += 5;
+	    }
+	    break;
+	case t_character:
+	case t_array:
+	case t_check:
+	case t_ignore:
+	    json_debug_trace((1, "Invalid array subtype.\n"));
+	    return JSON_ERR_SUBTYPE;
+	}
+	arrcount++;
+	if (isspace((unsigned char) *cp))
+	    cp++;
+	if (*cp == ']') {
+	    json_debug_trace((1, "End of array found.\n"));
+	    goto breakout;
+	} else if (*cp == ',')
+	    cp++;
+	else {
+	    json_debug_trace((1, "Bad trailing syntax on array.\n"));
+	    return JSON_ERR_BADSUBTRAIL;
+	}
+    }
+    json_debug_trace((1, "Too many elements in array.\n"));
+    if (end != NULL)
+	*end = cp;
+    return JSON_ERR_SUBTOOLONG;
+  breakout:
+    if (arr->count != NULL)
+	*(arr->count) = arrcount;
+    if (end != NULL)
+	*end = cp;
+    /*@ -nullderef @*/
+    json_debug_trace((1, "leaving json_read_array() with %d elements\n",
+		      arrcount));
+    /*@ +nullderef @*/
+    return 0;
+    /*@+nullstate +onlytrans@*/
+}
+
+int json_read_object(const char *cp, const struct json_attr_t *attrs,
+		     /*@null@*/ const char **end)
+{
+    int st;
+
+    json_debug_trace((1, "json_read_object() sees '%s'\n", cp));
+    st = json_internal_read_object(cp, attrs, NULL, 0, end);
+    return st;
+}
+
+const /*@observer@*/ char *json_error_string(int err)
+{
+    const char *errors[] = {
+	"unknown error while parsing JSON",
+	"non-whitespace when expecting object start",
+	"non-whitespace when expecting attribute start",
+	"unknown attribute name",
+	"attribute name too long",
+	"saw [ when not expecting array",
+	"array element specified, but no [",
+	"string value too long",
+	"token value too long",
+	"garbage while expecting comma or } or ]",
+	"didn't find expected array start",
+	"error while parsing object array",
+	"too many array elements",
+	"garbage while expecting array comma",
+	"unsupported array element type",
+	"error while string parsing",
+	"check attribute not matched",
+	"can't support strings in parallel arrays",
+	"invalid enumerated value",
+	"saw quoted value when expecting nonstring",
+	"didn't see quoted value when expecting string",
+	"other data conversion error",
+	"unexpected null value or attribute pointer",
+    };
+
+    if (err <= 0 || err >= (int)(sizeof(errors) / sizeof(errors[0])))
+	return errors[0];
+    else
+	return errors[err];
+}
+
+/* end */
+

+ 141 - 0
src/ext_deps/mjson.h

@@ -0,0 +1,141 @@
+/* Structures for JSON parsing using only fixed-extent memory
+ *
+ * This file is Copyright (c) 2014 by Eric S. Raymond.
+ * BSD terms apply: see the file COPYING in the distribution root for details.
+ */
+
+#include <stdbool.h>
+#include <ctype.h>
+#include <stdio.h>
+#include <sys/types.h>
+
+#define NITEMS(x) (int)(sizeof(x)/sizeof(x[0]))
+
+typedef enum {t_integer, t_uinteger, t_real,
+	      t_string, t_boolean, t_character,
+	      t_time,
+	      t_object, t_structobject, t_array,
+	      t_check, t_ignore} 
+    json_type;
+
+struct json_enum_t {
+    char	*name;
+    int		value;
+};
+
+struct json_array_t {
+    json_type element_type;
+    union {
+	struct {
+	    const struct json_attr_t *subtype;
+	    char *base;
+	    size_t stride;
+	} objects;
+	struct {
+	    char **ptrs;
+	    char *store;
+	    int storelen;
+	} strings;
+	struct {
+	    int *store;
+	} integers;
+	struct {
+	    unsigned int *store;
+	} uintegers;
+	struct {
+	    double *store;
+	} reals;
+	struct {
+	    bool *store;
+	} booleans;
+    } arr;
+    int *count, maxlen;
+};
+
+struct json_attr_t {
+    char *attribute;
+    json_type type;
+    union {
+	int *integer;
+	unsigned int *uinteger;
+	double *real;
+	char *string;
+	bool *boolean;
+	char *character;
+	struct json_array_t array;
+	size_t offset;
+    } addr;
+    union {
+	int integer;
+	unsigned int uinteger;
+	double real;
+	bool boolean;
+	char character;
+	char *check;
+    } dflt;
+    size_t len;
+    const struct json_enum_t *map;
+    bool nodefault;
+};
+
+#define JSON_ATTR_MAX	31	/* max chars in JSON attribute name */
+#define JSON_VAL_MAX	512	/* max chars in JSON value part */
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+int json_read_object(const char *, const struct json_attr_t *,
+		     /*@null@*/const char **);
+int json_read_array(const char *, const struct json_array_t *,
+		    /*@null@*/const char **);
+const /*@observer@*/char *json_error_string(int);
+
+void json_enable_debug(int, FILE *);
+#ifdef __cplusplus
+}
+#endif
+
+#define JSON_ERR_OBSTART	1	/* non-WS when expecting object start */
+#define JSON_ERR_ATTRSTART	2	/* non-WS when expecting attrib start */
+#define JSON_ERR_BADATTR	3	/* unknown attribute name */
+#define JSON_ERR_ATTRLEN	4	/* attribute name too long */
+#define JSON_ERR_NOARRAY	5	/* saw [ when not expecting array */
+#define JSON_ERR_NOBRAK 	6	/* array element specified, but no [ */
+#define JSON_ERR_STRLONG	7	/* string value too long */
+#define JSON_ERR_TOKLONG	8	/* token value too long */
+#define JSON_ERR_BADTRAIL	9	/* garbage while expecting comma or } or ] */
+#define JSON_ERR_ARRAYSTART	10	/* didn't find expected array start */
+#define JSON_ERR_OBJARR 	11	/* error while parsing object array */
+#define JSON_ERR_SUBTOOLONG	12	/* too many array elements */
+#define JSON_ERR_BADSUBTRAIL	13	/* garbage while expecting array comma */
+#define JSON_ERR_SUBTYPE	14	/* unsupported array element type */
+#define JSON_ERR_BADSTRING	15	/* error while string parsing */
+#define JSON_ERR_CHECKFAIL	16	/* check attribute not matched */
+#define JSON_ERR_NOPARSTR	17	/* can't support strings in parallel arrays */
+#define JSON_ERR_BADENUM	18	/* invalid enumerated value */
+#define JSON_ERR_QNONSTRING	19	/* saw quoted value when expecting nonstring */
+#define JSON_ERR_NONQSTRING	19	/* didn't see quoted value when expecting string */
+#define JSON_ERR_MISC		20	/* other data conversion error */
+#define JSON_ERR_BADNUM		21	/* error while parsing a numerical argument */
+#define JSON_ERR_NULLPTR	22	/* unexpected null value or attribute pointer */
+
+/*
+ * Use the following macros to declare template initializers for structobject
+ * arrays.  Writing the equivalents out by hand is error-prone.
+ *
+ * STRUCTOBJECT takes a structure name s, and a fieldname f in s.
+ *
+ * STRUCTARRAY takes the name of a structure array, a pointer to a an
+ * initializer defining the subobject type, and the address of an integer to
+ * store the length in.
+ */
+#define STRUCTOBJECT(s, f)	.addr.offset = offsetof(s, f)
+#define STRUCTARRAY(a, e, n) \
+	.addr.array.element_type = t_structobject, \
+	.addr.array.arr.objects.subtype = e, \
+	.addr.array.arr.objects.base = (char*)a, \
+	.addr.array.arr.objects.stride = sizeof(a[0]), \
+	.addr.array.count = n, \
+	.addr.array.maxlen = (int)(sizeof(a)/sizeof(a[0]))
+
+/* json.h ends here */

+ 6 - 0
src/omath.c

@@ -28,6 +28,12 @@ void ovec3f_normalize_me(vec3f* me)
 	me->z /= len;
 }
 
+void ovec3f_subtract(const vec3f* a, const vec3f* b, vec3f* out)
+{
+	for(int i = 0; i < 3; i++)
+		out->arr[i] = a->arr[i] - b->arr[i];
+}
+
 float ovec3f_get_dot(const vec3f* me, const vec3f* vec)
 {
 	return me->x * vec->x + me->y * vec->y + me->z * vec->z;

+ 1 - 0
src/omath.h

@@ -34,6 +34,7 @@ void ovec3f_normalize_me(vec3f* me);
 float ovec3f_get_length(const vec3f* me);
 float ovec3f_get_angle(const vec3f* me, const vec3f* vec); 
 float ovec3f_get_dot(const vec3f* me, const vec3f* vec);
+void ovec3f_subtract(const vec3f* a, const vec3f* b, vec3f* out);
 
 
 // quaternion

+ 4 - 0
src/openhmd.c

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

+ 1 - 0
src/openhmdi.h

@@ -140,6 +140,7 @@ void ohmd_set_universal_aberration_k(ohmd_device_properties* props, float r, flo
 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_psvr_drv(ohmd_context* ctx);
 ohmd_driver* ohmd_create_external_drv(ohmd_context* ctx);
 ohmd_driver* ohmd_create_android_drv(ohmd_context* ctx);