Added initial support for external sensor driver

Added quaternion slerp method

configure.ac

@@ -43,6 +43,15 @@ AC_ARG_ENABLE([driver-oculus-rift],
 
 AM_CONDITIONAL([BUILD_DRIVER_OCULUS_RIFT], [test "x$driver_oculus_rift_enabled" != "xno"])
 
+# External Driver
+AC_ARG_ENABLE([driver-external],
+        [AS_HELP_STRING([--enable-driver-external],
+                [enable building of External driver (default y)])],
+        [driver_external_enabled=$enableval],
+        [driver_external_enabled='yes'])
+
+AM_CONDITIONAL([BUILD_DRIVER_EXTERNAL], [test "x$driver_external_enabled" != "xno"])
+
 # Libs required by Oculus Rift Driver
 if test "x$driver_oculus_rift_enabled" != "xno"; then
 	PKG_CHECK_MODULES([hidapi], [$hidapi] >= 0.0.5)

include/openhmd.h

@@ -42,6 +42,7 @@ typedef enum {
 	OHMD_S_OK = 0,
 	OHMD_S_UNKNOWN_ERROR = -1,
 	OHMD_S_INVALID_PARAMETER = -2,
+	OHMD_S_UNSUPPORTED = -3,
 
 	/** OHMD_S_USER_RESERVED and below can be used for user purposes, such as errors within ohmd wrappers, etc. */
 	OHMD_S_USER_RESERVED = -16384,
@@ -109,6 +110,9 @@ typedef enum {
 
 	/** float[6], get - Device specifc distortion value. */
 	OHMD_DISTORTION_K                     = 18,
+	
+	/** float[10], set - Perform sensor fusion on values from external sensors. Values are: dt (time since last update, in seconds) X, Y, Z gyro, X, Y, Z accelerometer and X, Y, Z magnetometer. */
+	OHMD_EXTERNAL_SENSOR_FUSION           = 19,
 
 } ohmd_float_value;
 

src/Makefile.am

@@ -25,4 +25,10 @@ libopenhmd_la_LDFLAGS += $(hidapi_LIBS)
 
 endif
 
+if BUILD_DRIVER_EXTERNAL
+
+libopenhmd_la_SOURCES += \
+	drv_external/external.c
+endif
+
 libopenhmd_la_LDFLAGS += $(EXTRA_LD_FLAGS)

src/drv_external/external.c

@@ -0,0 +1,207 @@
+/*
+ * OpenHMD - Free and Open Source API and drivers for immersive technology.
+ * Copyright (C) 2013 Fredrik Hultin.
+ * Copyright (C) 2013 Jakob Bornecrantz.
+ * Copyright (C) 2015 Joey Ferwerda
+ * Distributed under the Boost 1.0 licence, see LICENSE for full text.
+ */
+
+/* External Driver */
+
+typedef struct {
+	ohmd_device base;
+	fusion sensor_fusion;
+} external_priv;
+
+static void update_device(ohmd_device* device)
+{
+}
+
+static void nofusion_update(fusion* me, float dt, const vec3f* accel)
+{	
+	//avg raw accel data to smooth jitter, and normalise
+	ofq_add(&me->accel_fq, accel);
+	vec3f accel_mean;
+	ofq_get_mean(&me->accel_fq, &accel_mean);
+	vec3f acc_n = accel_mean;
+	ovec3f_normalize_me(&acc_n);
+	
+	
+	//reference vectors for axis-angle
+	vec3f xyzv[3] = { 
+		{1,0,0},
+		{0,1,0},
+		{0,0,1}
+	};
+	quatf roll, pitch;
+	
+	//pitch is rot around x, based on gravity in z and y axes
+	oquatf_init_axis(&pitch, xyzv+0, atan2f(-acc_n.z, -acc_n.y)); 
+	
+	//roll is rot around z, based on gravity in x and y axes
+	//note we need to invert the values when the device is upside down (y < 0) for proper results
+	oquatf_init_axis(&roll, xyzv+2, acc_n.y < 0 ? atan2f(-acc_n.x, -acc_n.y) : atan2f(acc_n.x, acc_n.y)); 
+	
+			
+	
+	quatf or = {0,0,0,1};
+	//order of applying is yaw-pitch-roll
+	//yaw is not possible using only accel
+	oquatf_mult_me(&or, &pitch); 
+	oquatf_mult_me(&or, &roll);
+
+	me->orient = or;
+}
+
+//shorter buffers for frame smoothing
+void nofusion_init(fusion* me)
+{
+	memset(me, 0, sizeof(fusion));
+	me->orient.w = 1.0f;
+
+	ofq_init(&me->mag_fq, 10);
+	ofq_init(&me->accel_fq, 10);
+	ofq_init(&me->ang_vel_fq, 10);
+
+	me->flags = FF_USE_GRAVITY;
+	me->grav_gain = 0.05f;
+}
+
+
+static int getf(ohmd_device* device, ohmd_float_value type, float* out)
+{
+	external_priv* priv = (external_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 (%d)", type);
+			return -1;
+			break;
+	}
+
+	return 0;
+}
+
+static int setf(ohmd_device* device, ohmd_float_value type, float* in)
+{
+	external_priv* priv = (external_priv*)device;
+
+	switch(type){
+		case OHMD_EXTERNAL_SENSOR_FUSION: {
+				ofusion_update(&priv->sensor_fusion, *in, (vec3f*)(in + 1), (vec3f*)(in + 4), (vec3f*)(in + 7));
+			}
+			break;
+
+		default:
+			ohmd_set_error(priv->base.ctx, "invalid type given to setf (%d)", type);
+			return -1;
+			break;
+	}
+
+	return 0;
+}
+
+static void close_device(ohmd_device* device)
+{
+	LOGD("closing external device");
+	free(device);
+}
+
+static ohmd_device* open_device(ohmd_driver* driver, ohmd_device_desc* desc)
+{
+	external_priv* priv = ohmd_alloc(driver->ctx, sizeof(external_priv));
+	if(!priv)
+		return NULL;
+
+	// Set default device properties
+	ohmd_set_default_device_properties(&priv->base.properties);
+
+	// Set device properties
+	//TODO: Get information from external device using set_external_properties?
+	//Using 'dummy' settings for now
+	priv->base.properties.hsize = 0.149760f;
+	priv->base.properties.vsize = 0.093600f;
+	priv->base.properties.hres = 1280;
+	priv->base.properties.vres = 800;
+	priv->base.properties.lens_sep = 0.063500;
+	priv->base.properties.lens_vpos = 0.046800;
+	priv->base.properties.fov = DEG_TO_RAD(125.5144f);
+	priv->base.properties.ratio = (1280.0f / 800.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;
+	priv->base.setf = setf;
+	priv->base.seti = seti;
+	
+	ofusion_init(&priv->sensor_fusion);
+
+	return (ohmd_device*)priv;
+}
+
+static void get_device_list(ohmd_driver* driver, ohmd_device_list* list)
+{
+	ohmd_device_desc* desc = &list->devices[list->num_devices++];
+
+	strcpy(desc->driver, "OpenHMD Generic External Driver");
+	strcpy(desc->vendor, "OpenHMD");
+	strcpy(desc->product, "External Device");
+
+	strcpy(desc->path, "(none)");
+
+	desc->driver_ptr = driver;
+}
+
+static void destroy_driver(ohmd_driver* drv)
+{
+	LOGD("shutting down external driver");
+	free(drv);
+}
+
+ohmd_driver* ohmd_create_external_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->get_device_list = get_device_list;
+	drv->open_device = open_device;
+	drv->destroy = destroy_driver;
+
+	return drv;
+}
+
+/* external specific functions */
+static void set_external_properties(ohmd_device* device, ohmd_device_properties* props)
+{
+    external_priv* priv = (external_priv*)device;
+
+	priv->base.properties.hsize = props->hsize;
+	priv->base.properties.vsize = props->vsize;
+	priv->base.properties.hres = props->hres;
+	priv->base.properties.vres = props->vres;
+	priv->base.properties.lens_sep = props->lens_sep;
+	priv->base.properties.lens_vpos = props->lens_vpos;
+	priv->base.properties.fov = DEG_TO_RAD(props->fov);
+	priv->base.properties.ratio = props->ratio;
+}

src/omath.c

@@ -122,6 +122,62 @@ void oquatf_diff(const quatf* me, const quatf* q, quatf* out_q)
 	oquatf_mult(&inv, q, out_q);
 }
 
+void oquatf_slerp (float fT, const quatf* rkP, const quatf* rkQ, bool shortestPath, quatf* out_q)
+{
+	float fCos =  oquatf_get_dot(rkP, rkQ);
+	quatf rkT;
+
+	// Do we need to invert rotation?
+	if (fCos < 0.0f && shortestPath)
+	{
+		fCos = -fCos;
+		rkT = *rkQ;
+		oquatf_inverse(&rkT);
+	}
+	else
+	{
+		rkT = *rkQ;
+	}
+
+	if (fabsf(fCos) < 1 - 0.001f)
+	{
+		// Standard case (slerp)
+		float fSin = sqrtf(1 - (fCos*fCos));
+		float fAngle = atan2f(fSin, fCos); 
+		float fInvSin = 1.0f / fSin;
+		float fCoeff0 = sin((1.0f - fT) * fAngle) * fInvSin;
+		float fCoeff1 = sin(fT * fAngle) * fInvSin;
+		
+		out_q->x = fCoeff0 * rkP->x + fCoeff1 * rkT.x;
+		out_q->y = fCoeff0 * rkP->y + fCoeff1 * rkT.y;
+		out_q->z = fCoeff0 * rkP->z + fCoeff1 * rkT.z;
+		out_q->w = fCoeff0 * rkP->w + fCoeff1 * rkT.w;
+			
+		//return fCoeff0 * rkP + fCoeff1 * rkT;
+	}
+	else
+	{
+		// There are two situations:
+		// 1. "rkP" and "rkQ" are very close (fCos ~= +1), so we can do a linear
+		//    interpolation safely.
+		// 2. "rkP" and "rkQ" are almost inverse of each other (fCos ~= -1), there
+		//    are an infinite number of possibilities interpolation. but we haven't
+		//    have method to fix this case, so just use linear interpolation here.
+		//Quaternion t = (1.0f - fT) * rkP + fT * rkT;
+		
+		out_q->x = (1.0f - fT) * rkP->x + fT * rkT.x;
+		out_q->y = (1.0f - fT) * rkP->y + fT * rkT.y;
+		out_q->z = (1.0f - fT) * rkP->z + fT * rkT.z;
+		out_q->w = (1.0f - fT) * rkP->w + fT * rkT.w;
+			
+		oquatf_normalize_me(out_q);
+		
+		// taking the complement requires renormalisation
+		//t.normalise();
+		//return t;
+	}
+}
+
 void oquatf_get_mat4x4(const quatf* me, const vec3f* point, float mat[4][4])
 {
 	mat[0][0] = 1 - 2 * me->y * me->y - 2 * me->z * me->z;

src/openhmd.c

@@ -12,7 +12,6 @@
 #include <string.h>
 #include <stdio.h>
 
-
 ohmd_context* OHMD_APIENTRY ohmd_ctx_create()
 {
 	ohmd_context* ctx = calloc(1, sizeof(ohmd_context));
@@ -25,6 +24,10 @@ ohmd_context* OHMD_APIENTRY ohmd_ctx_create()
 	ctx->drivers[ctx->num_drivers++] = ohmd_create_oculus_rift_drv(ctx);
 #endif
 
+#if DRIVER_EXTERNAL
+	ctx->drivers[ctx->num_drivers++] = ohmd_create_external_drv(ctx);
+#endif
+
 	// add dummy driver last to make it the lowest priority
 	ctx->drivers[ctx->num_drivers++] = ohmd_create_dummy_drv(ctx);
 
@@ -264,6 +267,13 @@ int OHMD_APIENTRY ohmd_device_setf(ohmd_device* device, ohmd_float_value type, f
 
 			return OHMD_S_OK;
 		}
+	case OHMD_EXTERNAL_SENSOR_FUSION:
+		{
+			if(device->setf == NULL)
+				return OHMD_S_UNSUPPORTED;
+
+			return device->setf(device, type, in);
+		}
 	default:
 		return OHMD_S_INVALID_PARAMETER;
 	}

src/openhmdi.h

@@ -76,6 +76,8 @@ struct ohmd_device {
 	vec3f position_correction;
 
 	int (*getf)(ohmd_device* device, ohmd_float_value type, float* out);
+	int (*setf)(ohmd_device* device, ohmd_float_value type, float* in);
+
 	void (*update)(ohmd_device* device);
 	void (*close)(ohmd_device* device);
 
@@ -102,6 +104,7 @@ void ohmd_calc_default_proj_matrices(ohmd_device_properties* props);
 // drivers
 ohmd_driver* ohmd_create_dummy_drv(ohmd_context* ctx);
 ohmd_driver* ohmd_create_oculus_rift_drv(ohmd_context* ctx);
+ohmd_driver* ohmd_create_external_drv(ohmd_context* ctx);
 
 #include "log.h"
 #include "platform.h"