Merge pull request #1 from cnlohr/master

Pulling new changes

15 files changed, 1320 insertions, 326 deletions

diff --git a/src/ootx_decoder.c b/src/ootx_decoder.c
index f7a7938..b7327d5 100644
--- a/src/ootx_decoder.c
+++ b/src/ootx_decoder.c
@@ -44,7 +44,7 @@ void ootx_free_decoder_context(ootx_decoder_context *ctx) {
 }
 
 uint8_t ootx_decode_bit(uint32_t length) {
-	uint8_t t = (length - 2750) / 500; //why 2750?
+	uint8_t t = (uint8_t)((length - 2750) / 500); //why 2750?
 //	return ((t & 0x02)>0)?0xFF:0x00; //easier if we need to bitshift right
 	return ((t & 0x02)>>1);
 }
@@ -182,8 +182,13 @@ union iFloat {
 	float f;
 };
 
+
+struct __attribute__((__packed__)) unaligned_u16_t {
+	uint16_t v;
+};
+
 float _half_to_float(uint8_t* data) {
-	uint16_t x = *(uint16_t*)data;
+	uint16_t x = ((struct unaligned_u16_t*)data)->v;
 	union iFloat fnum;
 	fnum.f = 0;
 
diff --git a/src/poser_octavioradii.c b/src/poser_octavioradii.c
index 0d8674c..5805e1a 100644
--- a/src/poser_octavioradii.c
+++ b/src/poser_octavioradii.c
@@ -252,7 +252,7 @@ static void normalizeAndMultiplyVector(FLT *vectorToNormalize, size_t count, FLT
 }
 
 
-static RefineEstimateUsingGradientDescentRadii(FLT *estimateOut, SensorAngles *angles, FLT *initialEstimate, size_t numRadii, PointPair *pairs, size_t numPairs, FILE *logFile)
+static void RefineEstimateUsingGradientDescentRadii(FLT *estimateOut, SensorAngles *angles, FLT *initialEstimate, size_t numRadii, PointPair *pairs, size_t numPairs, FILE *logFile)
 {
 	int i = 0;
 	FLT lastMatchFitness = calculateFitness(angles, initialEstimate, pairs, numPairs);
@@ -521,9 +521,9 @@ static void QuickPose(SurviveObject *so)
 
 		if (sensorCount > 4)
 		{
-			FLT pos[3];
+			Point pos;
 			FLT orient[4];
-			SolveForLighthouseRadii(pos, orient, to);
+			SolveForLighthouseRadii(&pos, orient, to);
 		}
 
 		
@@ -591,7 +591,7 @@ int PoserOctavioRadii( SurviveObject * so, PoserData * pd )
 				}
 				else
 				{
-					dd->hitCount[i][l->lh][axis] *= 0.5;
+					dd->hitCount[i][l->lh][axis] = (int)((double)dd->hitCount[i][l->lh][axis] * 0.5);
 				}
 			}
 			//if (0 == l->lh)
diff --git a/src/poser_turveytori.c b/src/poser_turveytori.c
index 80e8d89..94d572e 100644
--- a/src/poser_turveytori.c
+++ b/src/poser_turveytori.c
@@ -1,4 +1,5 @@
 #include <survive.h>
+#include "survive_config.h"
 #include <stdio.h>
 #include <stdlib.h>
 #include <string.h>
@@ -15,6 +16,8 @@
 #endif
 
 
+static int ttDebug = 0;
+
 #define PointToFlts(x) ((FLT*)(x))
 
 typedef struct
@@ -85,7 +88,8 @@ typedef struct
 	int lastAxis[NUM_LIGHTHOUSES];
 
 	Point lastLhPos[NUM_LIGHTHOUSES];
-	FLT lastLhRotAxisAngle[NUM_LIGHTHOUSES][4];
+//	FLT lastLhRotAxisAngle[NUM_LIGHTHOUSES][4];
+	FLT lastLhRotQuat[NUM_LIGHTHOUSES][4];
 } ToriData;
 
 
@@ -412,7 +416,7 @@ FLT getPointFitness(Point pointIn, PointsAndAngle *pna, size_t pnaCount, int deu
 		}
 
 		fitnesses[i] = FLT_FABS(fitness);
-		if (deubgPrint)
+		if (0)
 		{
 			printf("  [%d, %d](%f)\n", pna[i].ai, pna[i].bi, fitness);
 		}
@@ -585,7 +589,7 @@ static Point RefineEstimateUsingModifiedGradientDescent1(Point initialEstimate,
 			break;
 		}
 	}
-	printf(" i=%3d ", i);
+	if (ttDebug) printf(" i=%3d ", i);
 
 	return lastPoint;
 }
@@ -772,6 +776,20 @@ FLT RotationEstimateFitnessAxisAngleOriginal(Point lhPoint, FLT *quaternion, Tra
 // just an x or y axis to make our estimate better.  TODO: bring that data to this fn.
 FLT RotationEstimateFitnessQuaternion(Point lhPoint, FLT *quaternion, TrackedObject *obj)
 {
+
+// TODO: ************************************************************************************************** THIS LIES!!!! NEED TO DO THIS IN QUATERNIONS!!!!!!!!!!!!!!!!!
+	{
+		FLT axisAngle[4];
+
+		axisanglefromquat(&(axisAngle[3]), axisAngle, quaternion);
+
+		FLT throwaway = RotationEstimateFitnessAxisAngle(lhPoint, axisAngle, obj);
+
+		return throwaway;
+	}
+
+
+
 	FLT fitness = 0;
 	for (size_t i = 0; i < obj->numSensors; i++)
 	{
@@ -910,7 +928,7 @@ static void WhereIsTheTrackedObjectAxisAngle(FLT *posOut, FLT *rotation, Point l
 	
 	rotatearoundaxis(posOut, posOut, rotation, rotation[3]);
 
-	printf("{% 04.4f, % 04.4f, % 04.4f}  ", posOut[0], posOut[1], posOut[2]);
+	if (ttDebug) printf("{% 04.4f, % 04.4f, % 04.4f}  ", posOut[0], posOut[1], posOut[2]);
 }
 
 static void RefineRotationEstimateAxisAngle(FLT *rotOut, Point lhPoint, FLT *initialEstimate, TrackedObject *obj)
@@ -1011,25 +1029,63 @@ static void RefineRotationEstimateAxisAngle(FLT *rotOut, Point lhPoint, FLT *ini
 			break;
 		}
 	}
-	printf(" Ri=%d ", i);
+	if (ttDebug) printf(" Ri=%d ", i);
 }
-static void WhereIsTheTrackedObjectQuaternion(FLT *rotation, Point lhPoint)
+//static void WhereIsTheTrackedObjectQuaternion(FLT *rotation, Point lhPoint)
+//{
+//	FLT reverseRotation[4] = { rotation[0], rotation[1], rotation[2], -rotation[3] };
+//	FLT objPoint[3] = { lhPoint.x, lhPoint.y, lhPoint.z };
+//
+//	//rotatearoundaxis(objPoint, objPoint, reverseRotation, reverseRotation[3]);
+//	quatrotatevector(objPoint, rotation, objPoint);
+//	if (ttDebug) printf("(%f, %f, %f)\n", objPoint[0], objPoint[1], objPoint[2]);
+//}
+static void WhereIsTheTrackedObjectQuaternion(FLT *posOut, FLT *rotation, Point lhPoint)
 {
-	FLT reverseRotation[4] = {rotation[0], rotation[1], rotation[2], -rotation[3]};
-	FLT objPoint[3] = {lhPoint.x, lhPoint.y, lhPoint.z};
-	
+	posOut[0] = -lhPoint.x;
+	posOut[1] = -lhPoint.y;
+	posOut[2] = -lhPoint.z;
+
+	FLT inverseRotation[4];
+
+	quatgetreciprocal(inverseRotation, rotation);
+
+	//FLT objPoint[3] = { lhPoint.x, lhPoint.y, lhPoint.z };
+
 	//rotatearoundaxis(objPoint, objPoint, reverseRotation, reverseRotation[3]);
-	quatrotatevector(objPoint, rotation, objPoint);
-	printf("(%f, %f, %f)\n", objPoint[0], objPoint[1], objPoint[2]);
+	quatrotatevector(posOut, inverseRotation, posOut);
+//	if (ttDebug) printf("(%f, %f, %f)\n", objPoint[0], objPoint[1], objPoint[2]);
 }
 
+//static void WhereIsTheTrackedObjectAxisAngle(FLT *posOut, FLT *rotation, Point lhPoint)
+//{
+//	posOut[0] = -lhPoint.x;
+//	posOut[1] = -lhPoint.y;
+//	posOut[2] = -lhPoint.z;
+//
+//	rotatearoundaxis(posOut, posOut, rotation, rotation[3]);
+//
+//	if (ttDebug) printf("{% 04.4f, % 04.4f, % 04.4f}  ", posOut[0], posOut[1], posOut[2]);
+//}
 
 
 static void RefineRotationEstimateQuaternion(FLT *rotOut, Point lhPoint, FLT *initialEstimate, TrackedObject *obj)
 {
 	int i = 0;
+
 	FLT lastMatchFitness = RotationEstimateFitnessQuaternion(lhPoint, initialEstimate, obj);
 
+	//{
+	//	FLT axisAngle[4];
+
+	//	axisanglefromquat(&(axisAngle[3]), axisAngle, initialEstimate);
+
+	//	FLT throwaway = RotationEstimateFitnessAxisAngle(lhPoint, axisAngle, obj);
+
+	//	int a = throwaway;
+	//}
+
+
 	quatcopy(rotOut, initialEstimate);
 
 	// The values below are somewhat magic, and definitely tunable
@@ -1105,9 +1161,9 @@ static void RefineRotationEstimateQuaternion(FLT *rotOut, Point lhPoint, FLT *in
 //#ifdef TORI_DEBUG
 			//printf("+  %8.8f, (%8.8f, %8.8f, %8.8f) %f\n", newMatchFitness, point4[0], point4[1], point4[2], point4[3]);
 //#endif
-			g *= 1.02;
-			printf("+");
-			WhereIsTheTrackedObjectQuaternion(rotOut, lhPoint);
+			g *= 1.04;
+			 //printf("+");
+			//WhereIsTheTrackedObjectQuaternion(rotOut, lhPoint);
 		}
 		else
 		{
@@ -1115,12 +1171,13 @@ static void RefineRotationEstimateQuaternion(FLT *rotOut, Point lhPoint, FLT *in
 			//printf("-         , %f\n", point4[3]);
 //#endif
 			g *= 0.7;
-			printf("-");
+			//printf("-");
+			//printf("%3f", lastMatchFitness);
 		}
 
 
 	}
-	printf("Ri=%3d  Fitness=%3f ", i, lastMatchFitness);
+	if (ttDebug) printf("Ri=%3d  Fitness=%3f ", i, lastMatchFitness);
 }
 
 
@@ -1129,9 +1186,9 @@ void SolveForRotation(FLT rotOut[4], TrackedObject *obj, Point lh)
 
 	// Step 1, create initial quaternion for guess.  
 	// This should have the lighthouse directly facing the tracked object.
-	Point trackedObjRelativeToLh = { .x = -lh.x,.y = -lh.y,.z = -lh.z };
+	//Point trackedObjRelativeToLh = { .x = -lh.x,.y = -lh.y,.z = -lh.z };
 	FLT theta = atan2(-lh.x, -lh.y);
-	FLT zAxis[4] = { 0, 0, 1 , theta-LINMATHPI/2};
+	FLT zAxis[4] = { 0, 0, 1 , theta - LINMATHPI / 2 };
 	FLT quat1[4];
 	quatfromaxisangle(quat1, zAxis, theta);
 
@@ -1143,6 +1200,7 @@ void SolveForRotation(FLT rotOut[4], TrackedObject *obj, Point lh)
 	RefineRotationEstimateAxisAngle(rotOut, lh, zAxis, obj);
 
 
+	// TODO:  Need to use the quaternion version here!!!
 	//// Step 2, optimize the quaternion to match the data.
 	//RefineRotationEstimateQuaternion(rotOut, lh, quat1, obj);
 
@@ -1150,6 +1208,32 @@ void SolveForRotation(FLT rotOut[4], TrackedObject *obj, Point lh)
 
 }
 
+void SolveForRotationQuat(FLT rotOut[4], TrackedObject *obj, Point lh)
+{
+
+	// Step 1, create initial quaternion for guess.  
+	// This should have the lighthouse directly facing the tracked object.
+	Point trackedObjRelativeToLh = { .x = -lh.x,.y = -lh.y,.z = -lh.z };
+	FLT theta = atan2(-lh.x, -lh.y);
+	FLT zAxis[4] = { 0, 0, 1 , theta - LINMATHPI / 2 };
+	FLT quat1[4];
+	quatfromaxisangle(quat1, zAxis, theta);
+
+	//quatfrom2vectors(0,0)
+	// not correcting for phi, but that's less important.
+
+
+	// Step 2, optimize the axis/ angle to match the data.
+	//RefineRotationEstimateAxisAngle(rotOut, lh, zAxis, obj);
+
+
+	//// Step 2, optimize the quaternion to match the data.
+	RefineRotationEstimateQuaternion(rotOut, lh, quat1, obj);
+
+	//WhereIsTheTrackedObjectQuaternion(rotOut, lh);
+
+}
+
 
 static Point SolveForLighthouse(FLT posOut[3], FLT quatOut[4], TrackedObject *obj, SurviveObject *so, char doLogOutput,const int lh,const int setLhCalibration)
 {
@@ -1280,21 +1364,29 @@ static Point SolveForLighthouse(FLT posOut[3], FLT quatOut[4], TrackedObject *ob
 	FLT fitGd = getPointFitness(refinedEstimateGd, pna, pnaCount, 0);
 
 	FLT distance = FLT_SQRT(SQUARED(refinedEstimateGd.x) + SQUARED(refinedEstimateGd.y) + SQUARED(refinedEstimateGd.z));
-	printf(" la(% 04.4f) SnsrCnt(%2d) LhPos:(% 04.4f, % 04.4f, % 04.4f) Dist: % 08.8f ", largestAngle, (int)obj->numSensors, refinedEstimateGd.x, refinedEstimateGd.y, refinedEstimateGd.z, distance);
+	if (ttDebug) printf(" la(% 04.4f) SnsrCnt(%2d) LhPos:(% 04.4f, % 04.4f, % 04.4f) Dist: % 08.8f ", largestAngle, (int)obj->numSensors, refinedEstimateGd.x, refinedEstimateGd.y, refinedEstimateGd.z, distance);
 	//printf("Distance is %f,   Fitness is %f\n", distance, fitGd);
 
 	FLT rot[4]; // this is axis/ angle rotation, not a quaternion!
+	FLT rotQuat[4]; // this is a quaternion!
 
 	// if we've already guessed at the rotation of the lighthouse,
 	// then let's use that as a starting guess, because it's probably
 	// going to make convergence happen much faster.
-	if (toriData->lastLhRotAxisAngle[lh][0] != 0)
-	{
-		rot[0] = toriData->lastLhRotAxisAngle[lh][0];
-		rot[1] = toriData->lastLhRotAxisAngle[lh][1];
-		rot[2] = toriData->lastLhRotAxisAngle[lh][2];
-		rot[3] = toriData->lastLhRotAxisAngle[lh][3];
-	}
+	//if (toriData->lastLhRotAxisAngle[lh][0] != 0)
+	//{
+	//	rot[0] = toriData->lastLhRotAxisAngle[lh][0];
+	//	rot[1] = toriData->lastLhRotAxisAngle[lh][1];
+	//	rot[2] = toriData->lastLhRotAxisAngle[lh][2];
+	//	rot[3] = toriData->lastLhRotAxisAngle[lh][3];
+	//}
+	//if (toriData->lastLhRotQuat[lh][0] != 0)
+	//{
+	//	rotQuat[0] = toriData->lastLhRotQuat[lh][0];
+	//	rotQuat[1] = toriData->lastLhRotQuat[lh][1];
+	//	rotQuat[2] = toriData->lastLhRotQuat[lh][2];
+	//	rotQuat[3] = toriData->lastLhRotQuat[lh][3];
+	//}
 
 	// Given the relative position of the lighthouse
 	// to the tracked object, in the tracked object's coordinate
@@ -1302,22 +1394,30 @@ static Point SolveForLighthouse(FLT posOut[3], FLT quatOut[4], TrackedObject *ob
 	// tracked object's coordinate system.
 	// TODO: I believe this could be radically improved
 	// using an SVD.  
+	SolveForRotationQuat(rotQuat, obj, refinedEstimateGd);
 	SolveForRotation(rot, obj, refinedEstimateGd);
 	FLT objPos[3];
+	//FLT objPos2[3];
 
-	{
-		toriData->lastLhRotAxisAngle[lh][0] = rot[0];
-		toriData->lastLhRotAxisAngle[lh][1] = rot[1];
-		toriData->lastLhRotAxisAngle[lh][2] = rot[2];
-		toriData->lastLhRotAxisAngle[lh][3] = rot[3];
-	}
+	//{
+	//	toriData->lastLhRotQuat[lh][0] = rotQuat[0];
+	//	toriData->lastLhRotQuat[lh][1] = rotQuat[1];
+	//	toriData->lastLhRotQuat[lh][2] = rotQuat[2];
+	//	toriData->lastLhRotQuat[lh][3] = rotQuat[3];
+	//}
 
-	WhereIsTheTrackedObjectAxisAngle(objPos, rot, refinedEstimateGd);
 
 
-	FLT rotQuat[4];
+	FLT rotQuat2[4];
+	FLT rot2[4];
 
-	quatfromaxisangle(rotQuat, rot, rot[3]);
+	quatfromaxisangle(rotQuat2, rot, rot[3]);
+	axisanglefromquat(&(rot2[3]), rot2, rotQuat);
+
+
+//	WhereIsTheTrackedObjectAxisAngle(objPos, rot, refinedEstimateGd); // this is the original axis angle one
+	WhereIsTheTrackedObjectAxisAngle(objPos, rot2, refinedEstimateGd); // this one is axis angle, but using data derived by quaternions.
+	//	WhereIsTheTrackedObjectQuaternion(objPos, rotQuat, refinedEstimateGd);  <--------------This is hte one we need to use, might need to be fixed.
 
 	//{
 		//FLT tmpPos[3] = {refinedEstimateGd.x, refinedEstimateGd.y, refinedEstimateGd.z};
@@ -1405,7 +1505,16 @@ static Point SolveForLighthouse(FLT posOut[3], FLT quatOut[4], TrackedObject *ob
 	so->FromLHPose[lh].Rot[2] = so->OutPose.Rot[2];
 	so->FromLHPose[lh].Rot[3] = so->OutPose.Rot[3];
 
-	printf(" <% 04.4f, % 04.4f, % 04.4f >  ", wcPos[0], wcPos[1], wcPos[2]);
+	if (ttDebug) printf(" <% 04.4f, % 04.4f, % 04.4f >  ", wcPos[0], wcPos[1], wcPos[2]);
+
+	posOut[0] = wcPos[0];
+	posOut[1] = wcPos[1];
+	posOut[2] = wcPos[2];
+
+	quatOut[0] = so->OutPose.Rot[0];
+	quatOut[1] = so->OutPose.Rot[1];
+	quatOut[2] = so->OutPose.Rot[2];
+	quatOut[3] = so->OutPose.Rot[3];
 
 	if (logFile)
 	{
@@ -1418,6 +1527,7 @@ static Point SolveForLighthouse(FLT posOut[3], FLT quatOut[4], TrackedObject *ob
 	toriData->lastLhPos[lh].y = refinedEstimateGd.y;
 	toriData->lastLhPos[lh].z = refinedEstimateGd.z;
 
+
 	return refinedEstimateGd;
 }
 
@@ -1528,7 +1638,14 @@ static void QuickPose(SurviveObject *so, int lh)
 
 
 			SolveForLighthouse(pos, quat, to, so, 0, lh, 0);
-			printf("!\n");
+
+			//printf("P&O: [% 08.8f,% 08.8f,% 08.8f] [% 08.8f,% 08.8f,% 08.8f,% 08.8f]\n", pos[0], pos[1], pos[2], quat[0], quat[1], quat[2], quat[3]);
+			if (so->ctx->rawposeproc)
+			{
+				so->ctx->rawposeproc(so, lh, pos, quat);
+			}
+
+			if (ttDebug) printf("!\n");
 		}
 
 		
@@ -1547,6 +1664,14 @@ int PoserTurveyTori( SurviveObject * so, PoserData * poserData )
 	SurviveContext * ctx = so->ctx;
 	ToriData * td = so->PoserData;
 
+	static int firstRun = 1;
+
+	if (firstRun)
+	{
+		ttDebug = config_read_uint32(ctx->global_config_values, "TurveyToriDebug", 0);
+
+		firstRun = 0;
+	}
 
 	if (!td)
 	{
@@ -1657,8 +1782,8 @@ int PoserTurveyTori( SurviveObject * so, PoserData * poserData )
 					FLT norm[3] = { so->sensor_normals[i * 3 + 0] , so->sensor_normals[i * 3 + 1] , so->sensor_normals[i * 3 + 2] };
 					FLT point[3] = { so->sensor_locations[i * 3 + 0] , so->sensor_locations[i * 3 + 1] , so->sensor_locations[i * 3 + 2] };
 
-					//quatrotatevector(norm, downQuat, norm);
-					//quatrotatevector(point, downQuat, point);
+					quatrotatevector(norm, downQuat, norm);
+					quatrotatevector(point, downQuat, point);
 
 					//rotatearoundaxis(norm, norm, axis, angle);
 					//rotatearoundaxis(point, point, axis, angle);
@@ -1693,8 +1818,8 @@ int PoserTurveyTori( SurviveObject * so, PoserData * poserData )
 					FLT norm[3] = { so->sensor_normals[i * 3 + 0] , so->sensor_normals[i * 3 + 1] , so->sensor_normals[i * 3 + 2] };
 					FLT point[3] = { so->sensor_locations[i * 3 + 0] , so->sensor_locations[i * 3 + 1] , so->sensor_locations[i * 3 + 2] };
 
-					//quatrotatevector(norm, downQuat, norm);
-					//quatrotatevector(point, downQuat, point);
+					quatrotatevector(norm, downQuat, norm);
+					quatrotatevector(point, downQuat, point);
 
 					//rotatearoundaxis(norm, norm, axis, angle);
 					//rotatearoundaxis(point, point, axis, angle);
@@ -1720,9 +1845,9 @@ int PoserTurveyTori( SurviveObject * so, PoserData * poserData )
 		}
 
 
-		// This code block rotates the lighthouse fixes to accound for any time the tracked object
+		// This code block rotates the lighthouse fixes to account for any time the tracked object
 		// is oriented other than +z = up
-		// This REALLY DOESN'T WORK!!!
+		 //This REALLY DOESN'T WORK!!!
 		//{
 		//	for (int lh = 0; lh < 2; lh++)
 		//	{
@@ -1732,6 +1857,22 @@ int PoserTurveyTori( SurviveObject * so, PoserData * poserData )
 		//	}
 		//}
 
+		for (int i=0; i < ctx->activeLighthouses; i++)
+		{ 
+			printf("Lighthouse Pose: [%1.1x][% 08.8f,% 08.8f,% 08.8f] [% 08.8f,% 08.8f,% 08.8f,% 08.8f]\n",
+				i,
+				ctx->bsd[i].Pose.Pos[0],
+				ctx->bsd[i].Pose.Pos[1],
+				ctx->bsd[i].Pose.Pos[2],
+				ctx->bsd[i].Pose.Rot[0],
+				ctx->bsd[i].Pose.Rot[1],
+				ctx->bsd[i].Pose.Rot[2],
+				ctx->bsd[i].Pose.Rot[3]);
+		}
+		config_set_lighthouse(ctx->lh_config, &ctx->bsd[0], 0);
+		config_set_lighthouse(ctx->lh_config, &ctx->bsd[1], 1);
+
+		config_save(ctx, "config.json");
 
 		free(to);
 		//printf( "Full scene data.\n" );
diff --git a/src/survive.c b/src/survive.c
index f637bd8..76bf8e4 100755
--- a/src/survive.c
+++ b/src/survive.c
@@ -8,6 +8,7 @@
 #include <string.h>
 
 #include "survive_config.h"
+#include "os_generic.h"
 
 #ifdef __APPLE__
 #define z_const const
@@ -40,8 +41,69 @@ static void survivenote( struct SurviveContext * ctx, const char * fault )
 	fprintf( stderr, "Info: %s\n", fault );
 }
 
+static void *button_servicer(void * context)
+{
+	SurviveContext *ctx = (SurviveContext*)context;
+
+	while (1)
+	{
+		OGLockSema(ctx->buttonQueue.buttonservicesem);
+
+		if (ctx->isClosing)
+		{
+			// we're shutting down.  Close.
+			return NULL;
+		}
+
+		ButtonQueueEntry *entry = &(ctx->buttonQueue.entry[ctx->buttonQueue.nextReadIndex]);
+		if (entry->isPopulated == 0)
+		{
+			// should never happen.  indicates failure of code pushing stuff onto
+			// the buttonQueue
+			// if it does happen, it will kill all future button input
+			printf("ERROR: Unpopulated ButtonQueueEntry! NextReadIndex=%d\n", ctx->buttonQueue.nextReadIndex);
+			return NULL;
+		}
+
+		//printf("ButtonEntry: eventType:%x, buttonId:%d, axis1:%d, axis1Val:%8.8x, axis2:%d, axis2Val:%8.8x\n",
+		//	entry->eventType,
+		//	entry->buttonId,
+		//	entry->axis1Id,
+		//	entry->axis1Val,
+		//	entry->axis2Id,
+		//	entry->axis2Val);
+
+		button_process_func butt_func = ctx->buttonproc;
+		if (butt_func)
+		{
+			butt_func(entry->so,
+				entry->eventType,
+				entry->buttonId,
+				entry->axis1Id,
+				entry->axis1Val,
+				entry->axis2Id,
+				entry->axis2Val);
+		}
+
+		ctx->buttonQueue.nextReadIndex++;
+		if (ctx->buttonQueue.nextReadIndex >= BUTTON_QUEUE_MAX_LEN)
+		{
+			ctx->buttonQueue.nextReadIndex = 0;
+		}
+	};
+	return NULL;
+}
 
-SurviveContext * survive_init( int headless )
+void survive_verify_FLT_size(uint32_t user_size) {
+  if(sizeof(FLT) != user_size) {
+    fprintf(stderr, "FLT type incompatible; the shared library libsurvive has FLT size %lu vs user program %u\n", sizeof(FLT), user_size);
+    fprintf(stderr, "Add '#define FLT %s' before including survive.h or recompile the shared library with the appropriate flag. \n",
+	    sizeof(FLT) == sizeof(double) ? "double" : "float");
+    exit(-1);
+  }
+}
+
+SurviveContext * survive_init_internal( int headless )
 {
 #ifdef RUNTIME_SYMNUM
 	if( !did_runtime_symnum )
@@ -66,6 +128,8 @@ SurviveContext * survive_init( int headless )
 	int i = 0;
 	SurviveContext * ctx = calloc( 1, sizeof( SurviveContext ) );
 
+	ctx->isClosing = 0;
+
 	ctx->global_config_values = malloc( sizeof(config_group) );
 	ctx->lh_config = malloc( sizeof(config_group) * NUM_LIGHTHOUSES);
 
@@ -75,6 +139,10 @@ SurviveContext * survive_init( int headless )
 
 	config_read(ctx, "config.json");
 
+	ctx->activeLighthouses = config_read_uint32(ctx->global_config_values, "LighthouseCount", 2);
+	config_read_lighthouse(ctx->lh_config, &(ctx->bsd[0]), 0);
+	config_read_lighthouse(ctx->lh_config, &(ctx->bsd[1]), 1);
+
 	ctx->faultfunction = survivefault;
 	ctx->notefunction = survivenote;
 
@@ -92,7 +160,8 @@ SurviveContext * survive_init( int headless )
 	}
 
 	i = 0;
-	const char * PreferredPoser = config_read_str( ctx->global_config_values, "DefaultPoser", "PoserDummy" );
+	//const char * PreferredPoser = config_read_str(ctx->global_config_values, "DefaultPoser", "PoserDummy");
+	const char * PreferredPoser = config_read_str(ctx->global_config_values, "DefaultPoser", "PoserTurveyTori");
 	PoserCB PreferredPoserCB = 0;
 	const char * FirstPoser = 0;
 	printf( "Available posers:\n" );
@@ -115,6 +184,19 @@ SurviveContext * survive_init( int headless )
 		ctx->objs[i]->PoserFn = PreferredPoserCB;
 	}
 
+	// saving the config extra to make sure that the user has a config file they can change.
+	config_save(ctx, "config.json");
+
+	// initialize the button queue
+	memset(&(ctx->buttonQueue), 0, sizeof(ctx->buttonQueue));
+
+	ctx->buttonQueue.buttonservicesem = OGCreateSema();	
+
+	// start the thread to process button data
+	ctx->buttonservicethread = OGCreateThread(button_servicer, ctx);
+	survive_install_button_fn(ctx, NULL);
+	survive_install_raw_pose_fn(ctx, NULL);
+
 	return ctx;
 }
 
@@ -159,6 +241,21 @@ void survive_install_angle_fn( SurviveContext * ctx,  angle_process_func fbp )
 		ctx->angleproc = survive_default_angle_process;
 }
 
+void survive_install_button_fn(SurviveContext * ctx, button_process_func fbp)
+{
+	if (fbp)
+		ctx->buttonproc = fbp;
+	else
+		ctx->buttonproc = survive_default_button_process;
+}
+
+void survive_install_raw_pose_fn(SurviveContext * ctx, raw_pose_func fbp)
+{
+	if (fbp)
+		ctx->rawposeproc = fbp;
+	else
+		ctx->rawposeproc = survive_default_raw_pose_process;
+}
 int survive_add_object( SurviveContext * ctx, SurviveObject * obj )
 {
 	int oldct = ctx->objs_ct;
@@ -193,10 +290,27 @@ int survive_send_magic( SurviveContext * ctx, int magic_code, void * data, int d
 	return 0;
 }
 
+int survive_haptic(SurviveObject * so, uint8_t reserved, uint16_t pulseHigh, uint16_t pulseLow, uint16_t repeatCount)
+{
+	if (NULL == so || NULL == so->haptic)
+	{
+		return -404;
+	}
+
+	return so->haptic(so, reserved, pulseHigh, pulseLow, repeatCount);
+}
+
+
 void survive_close( SurviveContext * ctx )
 {
 	const char * DriverName;
 	int r = 0;
+
+	ctx->isClosing = 1;
+
+	// unlock/ post to button service semaphore so the thread can kill itself
+	OGUnlockSema(ctx->buttonQueue.buttonservicesem);
+
 	while( ( DriverName = GetDriverNameMatching( "DriverUnreg", r++ ) ) )
 	{
 		DeviceDriver dd = GetDriver( DriverName );
diff --git a/src/survive_cal.c b/src/survive_cal.c
index ae92bad..f3a682a 100755
--- a/src/survive_cal.c
+++ b/src/survive_cal.c
@@ -15,6 +15,7 @@
 #include <sys/stat.h>
 #include <sys/types.h>
 #include <linmath.h>
+#include <assert.h>
 
 #include "survive_config.h"
 
@@ -124,8 +125,11 @@ void survive_cal_install( struct SurviveContext * ctx )
 
 	cd->numPoseObjects = 0;
 
-	const char * RequiredTrackersForCal = config_read_str( ctx->global_config_values, "RequiredTrackersForCal", "HMD,WM0,WM1" );
-	const uint32_t AllowAllTrackersForCal = config_read_uint32( ctx->global_config_values, "AllowAllTrackersForCal", 0 );
+	// setting the required trackers for calibration to be permissive to make it easier for a newbie to start-- 
+	// basically, libsurvive will detect whatever they have plugged in and start using that.  
+//	const char * RequiredTrackersForCal = config_read_str(ctx->global_config_values, "RequiredTrackersForCal", "HMD,WM0,WM1");	
+	const char * RequiredTrackersForCal = config_read_str(ctx->global_config_values, "RequiredTrackersForCal", ""); 
+	const uint32_t AllowAllTrackersForCal = config_read_uint32( ctx->global_config_values, "AllowAllTrackersForCal", 1 );
 	size_t requiredTrackersFound = 0;
 
 	for (int j=0; j < ctx->objs_ct; j++)
@@ -169,7 +173,8 @@ void survive_cal_install( struct SurviveContext * ctx )
 	}
 
 	const char * DriverName;
-	const char * PreferredPoser = config_read_str( ctx->global_config_values, "ConfigPoser", "PoserCharlesSlow" );
+//	const char * PreferredPoser = config_read_str(ctx->global_config_values, "ConfigPoser", "PoserCharlesSlow");
+	const char * PreferredPoser = config_read_str(ctx->global_config_values, "ConfigPoser", "PoserTurveyTori");
 	PoserCB PreferredPoserCB = 0;
 	const char * FirstPoser = 0;
 	printf( "Available posers:\n" );
@@ -218,7 +223,7 @@ void survive_cal_light( struct SurviveObject * so, int sensor_id, int acode, int
 			int i;
 			for( i = 0; i < NUM_LIGHTHOUSES; i++ )
 				if( ctx->bsd[i].OOTXSet == 0 ) break;
-			if( i == NUM_LIGHTHOUSES ) cd->stage = 2;  //TODO: Make this configuratble to allow single lighthouse.
+			if( i == ctx->activeLighthouses ) cd->stage = 2;  //TODO: Make this configuratble to allow single lighthouse.
 		}
 		break;
 	case 3: //Look for light sync lengths.
@@ -309,7 +314,7 @@ void survive_cal_angle( struct SurviveObject * so, int sensor_id, int acode, uin
 			cd->found_common = 1;
 			for( i = 0; i < cd->numPoseObjects; i++ )
 			//for( i = 0; i < MAX_SENSORS_TO_CAL/SENSORS_PER_OBJECT; i++ )
-			for( j = 0; j < NUM_LIGHTHOUSES; j++ )
+			for( j = 0; j < ctx->activeLighthouses; j++ )
 			{
 				int sensors_visible = 0;
 				for( k = 0; k < SENSORS_PER_OBJECT; k++ )
diff --git a/src/survive_cal.h b/src/survive_cal.h
index ae644d1..f5ef6dc 100644
--- a/src/survive_cal.h
+++ b/src/survive_cal.h
@@ -51,14 +51,14 @@ struct SurviveCalData
 	ootx_decoder_context ootx_decoders[NUM_LIGHTHOUSES];
 
 	//For statistics-gathering phase. (Stage 2/3)
-	FLT all_lengths[MAX_SENSORS_TO_CAL][NUM_LIGHTHOUSES][2][DRPTS];
-	FLT all_angles[MAX_SENSORS_TO_CAL][NUM_LIGHTHOUSES][2][DRPTS];
+	FLT all_lengths[MAX_SENSORS_TO_CAL][NUM_LIGHTHOUSES][2][DRPTS + 1];
+	FLT all_angles[MAX_SENSORS_TO_CAL][NUM_LIGHTHOUSES][2][DRPTS + 1];
 	int16_t all_counts[MAX_SENSORS_TO_CAL][NUM_LIGHTHOUSES][2];
 	int16_t peak_counts;
 	int8_t found_common;
 	int8_t times_found_common;
 
-	FLT all_sync_times[MAX_SENSORS_TO_CAL][NUM_LIGHTHOUSES][DRPTS];
+	FLT all_sync_times[MAX_SENSORS_TO_CAL][NUM_LIGHTHOUSES][DRPTS + 1];
 	int16_t all_sync_counts[MAX_SENSORS_TO_CAL][NUM_LIGHTHOUSES];
 
 	//For camfind (4+)
diff --git a/src/survive_config.c b/src/survive_config.c
index 3a83902..46c6658 100644
--- a/src/survive_config.c
+++ b/src/survive_config.c
@@ -86,6 +86,42 @@ void config_init() {
 }
 */
 
+void config_read_lighthouse(config_group* lh_config, BaseStationData* bsd, uint8_t idx) {
+	config_group *cg = lh_config + idx;
+	uint8_t found = 0;
+	for (int i = 0; i < NUM_LIGHTHOUSES; i++)
+	{
+		uint32_t tmpIdx = 0xffffffff;
+		cg = lh_config + idx;
+
+		tmpIdx = config_read_uint32(cg, "index", 0xffffffff);
+
+		if (tmpIdx == idx && i == idx) // assumes that lighthouses are stored in the config in order.
+		{
+			found = 1;
+			break;
+		}
+	}
+
+//	assert(found); // throw an assertion if we didn't find it...  Is this good?  not necessarily?
+	if (!found)
+	{
+		return;
+	}
+
+
+	FLT defaults[7] = { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 };
+
+	bsd->BaseStationID = config_read_uint32(cg, "id", 0);
+	config_read_float_array(cg, "pose", &bsd->Pose.Pos[0], defaults, 7);
+	config_read_float_array(cg, "fcalphase", bsd->fcalphase, defaults, 2);
+	config_read_float_array(cg, "fcaltilt", bsd->fcaltilt, defaults, 2);
+	config_read_float_array(cg, "fcalcurve", bsd->fcalcurve, defaults, 2);
+	config_read_float_array(cg, "fcalgibpha", bsd->fcalgibpha, defaults, 2);
+	config_read_float_array(cg, "fcalgibmag", bsd->fcalgibmag, defaults, 2);
+}
+
+
 void config_set_lighthouse(config_group* lh_config, BaseStationData* bsd, uint8_t idx) {
 	config_group *cg = lh_config+idx;
 	config_set_uint32(cg,"index", idx);
@@ -143,18 +179,28 @@ FLT config_read_float(config_group *cg, const char *tag, const FLT def) {
 	return config_set_float(cg, tag, def);
 }
 
-uint16_t config_read_float_array(config_group *cg, const char *tag, const FLT** values, const FLT* def, uint8_t count) {
+// TODO: Do something better than this:
+#define CFG_MIN(x,y) ((x) < (y)? (x): (y))
+
+
+uint16_t config_read_float_array(config_group *cg, const char *tag, FLT* values, const FLT* def, uint8_t count) {
 	config_entry *cv = find_config_entry(cg, tag);
 
 	if (cv != NULL) {
-		*values = (FLT*)cv->data;
+		for (unsigned int i=0; i < CFG_MIN(count, cv->elements); i++)
+		{
+			values[i] = ((double*)cv->data)[i];
+		}
 		return cv->elements;
 	}
 
 	if (def == NULL) return 0;
 
 	config_set_float_a(cg, tag, def, count);
-	*values = def;
+	for (int i = 0; i < count; i++)
+	{
+		values[i] = def[i];
+	}
 	return count;
 }
 
diff --git a/src/survive_config.h b/src/survive_config.h
index 83db624..e2686e9 100644
--- a/src/survive_config.h
+++ b/src/survive_config.h
@@ -43,6 +43,7 @@ void destroy_config_group(config_group* cg);
 
 //void config_write_lighthouse(struct BaseStationData* bsd, uint8_t length);
 void config_set_lighthouse(config_group* lh_config, BaseStationData* bsd, uint8_t idx);
+void config_read_lighthouse(config_group* lh_config, BaseStationData* bsd, uint8_t idx);
 
 void config_read(SurviveContext* sctx, const char* path);
 void config_save(SurviveContext* sctx, const char* path);
@@ -52,7 +53,7 @@ const uint32_t config_set_uint32(config_group *cg, const char *tag, const uint32
 const char* config_set_str(config_group *cg, const char *tag, const char* value);
 
 FLT config_read_float(config_group *cg, const char *tag, const FLT def);
-uint16_t config_read_float_array(config_group *cg, const char *tag, const FLT** values, const FLT* def, uint8_t count);
+uint16_t config_read_float_array(config_group *cg, const char *tag, FLT* values, const FLT* def, uint8_t count);
 uint32_t config_read_uint32(config_group *cg, const char *tag, const uint32_t def);
 const char* config_read_str(config_group *cg, const char *tag, const char *def);
 
diff --git a/src/survive_data.c b/src/survive_data.c
index 1b80269..0427659 100644
--- a/src/survive_data.c
+++ b/src/survive_data.c
@@ -419,8 +419,8 @@ void handle_lightcap2_sweep(SurviveObject * so, LightcapElement * le )
 	}
 
 	if (lcd->per_sweep.activeLighthouse < 0) {
-		fprintf(stderr, "WARNING: No active lighthouse!\n");
-		fprintf(stderr, "            %2d %8d %d %d\n", le->sensor_id, le->length,lcd->per_sweep.lh_acode[0],lcd->per_sweep.lh_acode[1]);
+		//fprintf(stderr, "WARNING: No active lighthouse!\n");
+		//fprintf(stderr, "            %2d %8d %d %d\n", le->sensor_id, le->length,lcd->per_sweep.lh_acode[0],lcd->per_sweep.lh_acode[1]);
 		return;
 	}
 
diff --git a/src/survive_default_devices.c b/src/survive_default_devices.c
new file mode 100644
index 0000000..6615f1e
--- /dev/null
+++ b/src/survive_default_devices.c
@@ -0,0 +1,199 @@
+#include "survive_default_devices.h"
+#include <jsmn.h>
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+
+#include "json_helpers.h"
+
+static SurviveObject *
+survive_create_device(SurviveContext *ctx, const char *driver_name,
+					  void *driver, const char *device_name, haptic_func fn) {
+	SurviveObject *device = calloc(1, sizeof(SurviveObject));
+
+	device->ctx = ctx;
+	device->driver = driver;
+	memcpy(device->codename, device_name, strlen(device_name));
+	memcpy(device->drivername, driver_name, strlen(driver_name));
+
+	device->timebase_hz = 48000000;
+	device->pulsedist_max_ticks = 500000;
+	device->pulselength_min_sync = 2200;
+	device->pulse_in_clear_time = 35000;
+	device->pulse_max_for_sweep = 1800;
+	device->pulse_synctime_offset = 20000;
+	device->pulse_synctime_slack = 5000;
+	device->timecenter_ticks = device->timebase_hz / 240;
+
+	device->haptic = fn;
+
+	return device;
+}
+
+SurviveObject *survive_create_hmd(SurviveContext *ctx, const char *driver_name,
+								  void *driver) {
+	return survive_create_device(ctx, driver_name, driver, "HMD", 0);
+}
+
+SurviveObject *survive_create_wm0(SurviveContext *ctx, const char *driver_name,
+								  void *driver, haptic_func fn) {
+	return survive_create_device(ctx, driver_name, driver, "WM0", fn);
+}
+SurviveObject *survive_create_wm1(SurviveContext *ctx, const char *driver_name,
+								  void *driver, haptic_func fn) {
+	return survive_create_device(ctx, driver_name, driver, "WM1", fn);
+}
+SurviveObject *survive_create_tr0(SurviveContext *ctx, const char *driver_name,
+								  void *driver) {
+	return survive_create_device(ctx, driver_name, driver, "TR0", 0);
+}
+SurviveObject *survive_create_ww0(SurviveContext *ctx, const char *driver_name,
+								  void *driver) {
+	return survive_create_device(ctx, driver_name, driver, "WW0", 0);
+}
+
+static int jsoneq(const char *json, jsmntok_t *tok, const char *s) {
+	if (tok->type == JSMN_STRING && (int)strlen(s) == tok->end - tok->start &&
+		strncmp(json + tok->start, s, tok->end - tok->start) == 0) {
+		return 0;
+	}
+	return -1;
+}
+static int ParsePoints(SurviveContext *ctx, SurviveObject *so, char *ct0conf,
+					   FLT **floats_out, jsmntok_t *t, int i) {
+	int k;
+	int pts = t[i + 1].size;
+	jsmntok_t *tk;
+
+	so->nr_locations = 0;
+	*floats_out = malloc(sizeof(**floats_out) * 32 * 3);
+
+	for (k = 0; k < pts; k++) {
+		tk = &t[i + 2 + k * 4];
+
+		int m;
+		for (m = 0; m < 3; m++) {
+			char ctt[128];
+
+			tk++;
+			int elemlen = tk->end - tk->start;
+
+			if (tk->type != 4 || elemlen > sizeof(ctt) - 1) {
+				SV_ERROR("Parse error in JSON\n");
+				return 1;
+			}
+
+			memcpy(ctt, ct0conf + tk->start, elemlen);
+			ctt[elemlen] = 0;
+			FLT f = atof(ctt);
+			int id = so->nr_locations * 3 + m;
+			(*floats_out)[id] = f;
+		}
+		so->nr_locations++;
+	}
+	return 0;
+}
+
+int survive_load_htc_config_format(char *ct0conf, int len, SurviveObject *so) {
+	if (len == 0)
+		return -1;
+
+	SurviveContext *ctx = so->ctx;
+	// From JSMN example.
+	jsmn_parser p;
+	jsmntok_t t[4096];
+	jsmn_init(&p);
+	int i;
+	int r = jsmn_parse(&p, ct0conf, len, t, sizeof(t) / sizeof(t[0]));
+	if (r < 0) {
+		SV_INFO("Failed to parse JSON in HMD configuration: %d\n", r);
+		return -1;
+	}
+	if (r < 1 || t[0].type != JSMN_OBJECT) {
+		SV_INFO("Object expected in HMD configuration\n");
+		return -2;
+	}
+
+	for (i = 1; i < r; i++) {
+		jsmntok_t *tk = &t[i];
+
+		char ctxo[100];
+		int ilen = tk->end - tk->start;
+		if (ilen > 99)
+			ilen = 99;
+		memcpy(ctxo, ct0conf + tk->start, ilen);
+		ctxo[ilen] = 0;
+
+		//				printf( "%d / %d / %d / %d %s %d\n", tk->type, tk->start,
+		//tk->end, tk->size, ctxo, jsoneq(ct0conf, &t[i], "modelPoints") );
+		//				printf( "%.*s\n", ilen, ct0conf + tk->start );
+
+		if (jsoneq(ct0conf, tk, "modelPoints") == 0) {
+			if (ParsePoints(ctx, so, ct0conf, &so->sensor_locations, t, i)) {
+				break;
+			}
+		}
+		if (jsoneq(ct0conf, tk, "modelNormals") == 0) {
+			if (ParsePoints(ctx, so, ct0conf, &so->sensor_normals, t, i)) {
+				break;
+			}
+		}
+
+		if (jsoneq(ct0conf, tk, "acc_bias") == 0) {
+			int32_t count = (tk + 1)->size;
+			FLT *values = NULL;
+			if (parse_float_array(ct0conf, tk + 2, &values, count) > 0) {
+				so->acc_bias = values;
+				so->acc_bias[0] *= .125; // XXX Wat?  Observed by CNL.  Biasing
+										 // by more than this seems to hose
+										 // things.
+				so->acc_bias[1] *= .125;
+				so->acc_bias[2] *= .125;
+			}
+		}
+		if (jsoneq(ct0conf, tk, "acc_scale") == 0) {
+			int32_t count = (tk + 1)->size;
+			FLT *values = NULL;
+			if (parse_float_array(ct0conf, tk + 2, &values, count) > 0) {
+				so->acc_scale = values;
+			}
+		}
+
+		if (jsoneq(ct0conf, tk, "gyro_bias") == 0) {
+			int32_t count = (tk + 1)->size;
+			FLT *values = NULL;
+			if (parse_float_array(ct0conf, tk + 2, &values, count) > 0) {
+				so->gyro_bias = values;
+			}
+		}
+		if (jsoneq(ct0conf, tk, "gyro_scale") == 0) {
+			int32_t count = (tk + 1)->size;
+			FLT *values = NULL;
+			if (parse_float_array(ct0conf, tk + 2, &values, count) > 0) {
+				so->gyro_scale = values;
+			}
+		}
+	}
+
+	char fname[64];
+
+	sprintf(fname, "calinfo/%s_points.csv", so->codename);
+	FILE *f = fopen(fname, "w");
+	int j;
+	for (j = 0; j < so->nr_locations; j++) {
+		fprintf(f, "%f %f %f\n", so->sensor_locations[j * 3 + 0],
+				so->sensor_locations[j * 3 + 1],
+				so->sensor_locations[j * 3 + 2]);
+	}
+	fclose(f);
+
+	sprintf(fname, "calinfo/%s_normals.csv", so->codename);
+	f = fopen(fname, "w");
+	for (j = 0; j < so->nr_locations; j++) {
+		fprintf(f, "%f %f %f\n", so->sensor_normals[j * 3 + 0],
+				so->sensor_normals[j * 3 + 1], so->sensor_normals[j * 3 + 2]);
+	}
+	fclose(f);
+
+	return 0;
+}
diff --git a/src/survive_default_devices.h b/src/survive_default_devices.h
new file mode 100644
index 0000000..1fcca72
--- /dev/null
+++ b/src/survive_default_devices.h
@@ -0,0 +1,19 @@
+#ifndef _SURVIVE_DEFAULT_DEVICES_H
+#define _SURVIVE_DEFAULT_DEVICES_H
+
+#include <survive.h>
+
+SurviveObject *survive_create_hmd(SurviveContext *ctx, const char *driver_name,
+								  void *driver);
+SurviveObject *survive_create_wm0(SurviveContext *ctx, const char *driver_name,
+								  void *driver, haptic_func cb);
+SurviveObject *survive_create_wm1(SurviveContext *ctx, const char *driver_name,
+								  void *driver, haptic_func cb);
+SurviveObject *survive_create_tr0(SurviveContext *ctx, const char *driver_name,
+								  void *driver);
+SurviveObject *survive_create_ww0(SurviveContext *ctx, const char *driver_name,
+								  void *driver);
+
+int survive_load_htc_config_format(char *ct0conf, int length,
+								   SurviveObject *so);
+#endif
diff --git a/src/survive_internal.h b/src/survive_internal.h
index e1a733d..86b119f 100644
--- a/src/survive_internal.h
+++ b/src/survive_internal.h
@@ -17,6 +17,7 @@ void * GetDriver( const char * name );
 const char * GetDriverNameMatching( const char * prefix, int place );
 void   ListDrivers();
 
+
 #endif
 
 
diff --git a/src/survive_playback.c b/src/survive_playback.c
new file mode 100644
index 0000000..4b25b4c
--- /dev/null
+++ b/src/survive_playback.c
@@ -0,0 +1,242 @@
+// All MIT/x11 Licensed Code in this file may be relicensed freely under the GPL
+// or LGPL licenses.
+
+#include <stdio.h>
+#include <stdlib.h>
+#include <survive.h>
+
+#include <string.h>
+#include <sys/time.h>
+
+#include "survive_config.h"
+#include "survive_default_devices.h"
+
+#include "redist/os_generic.h"
+
+struct SurvivePlaybackData {
+	SurviveContext *ctx;
+	const char *playback_dir;
+	FILE *playback_file;
+	int lineno;
+
+	FLT time_factor;
+	double next_time_us;
+};
+typedef struct SurvivePlaybackData SurvivePlaybackData;
+
+double timestamp_in_us() {
+	static double start_time_us = 0;
+	if (start_time_us == 0.)
+		start_time_us = OGGetAbsoluteTime();
+	return OGGetAbsoluteTime() - start_time_us;
+}
+
+static int parse_and_run_imu(const char *line, SurvivePlaybackData *driver) {
+	char dev[10];
+	int timecode = 0;
+	FLT accelgyro[6];
+	int mask;
+	int id;
+
+	int rr =
+		sscanf(line, "I %s %d %d " FLT_format " " FLT_format " " FLT_format
+					 " " FLT_format " " FLT_format " " FLT_format "%d",
+			   dev, &mask, &timecode, &accelgyro[0], &accelgyro[1],
+			   &accelgyro[2], &accelgyro[3], &accelgyro[4], &accelgyro[5], &id);
+
+	if (rr != 10) {
+		fprintf(stderr, "Warning:  On line %d, only %d values read: '%s'\n",
+				driver->lineno, rr, line);
+		return -1;
+	}
+
+	SurviveObject *so = survive_get_so_by_name(driver->ctx, dev);
+	if (!so) {
+		fprintf(stderr, "Could not find device named %s from lineno %d\n", dev,
+				driver->lineno);
+		return -1;
+	}
+
+	driver->ctx->imuproc(so, mask, accelgyro, timecode, id);
+	return 0;
+}
+
+static int parse_and_run_lightcode(const char *line,
+								   SurvivePlaybackData *driver) {
+	char lhn[10];
+	char axn[10];
+	char dev[10];
+	uint32_t timecode = 0;
+	int sensor = 0;
+	int acode = 0;
+	int timeinsweep = 0;
+	uint32_t pulselength = 0;
+	uint32_t lh = 0;
+
+	int rr =
+		sscanf(line, "%8s %8s %8s %u %d %d %d %u %u\n", lhn, axn, dev,
+			   &timecode, &sensor, &acode, &timeinsweep, &pulselength, &lh);
+
+	if (rr != 9) {
+		fprintf(stderr, "Warning:  On line %d, only %d values read: '%s'\n",
+				driver->lineno, rr, line);
+		return -1;
+	}
+
+	SurviveObject *so = survive_get_so_by_name(driver->ctx, dev);
+	if (!so) {
+		fprintf(stderr, "Could not find device named %s from lineno %d\n", dev,
+				driver->lineno);
+		return -1;
+	}
+
+	driver->ctx->lightproc(so, sensor, acode, timeinsweep, timecode,
+						   pulselength, lh);
+	return 0;
+}
+
+static int playback_poll(struct SurviveContext *ctx, void *_driver) {
+	SurvivePlaybackData *driver = _driver;
+	FILE *f = driver->playback_file;
+
+	if (f && !feof(f) && !ferror(f)) {
+		int i;
+		driver->lineno++;
+		char *line;
+
+		if (driver->next_time_us == 0) {
+			char *buffer;
+			size_t n = 0;
+			ssize_t r = getdelim(&line, &n, ' ', f);
+			if (r <= 0)
+				return 0;
+
+			if (sscanf(line, "%lf", &driver->next_time_us) != 1) {
+				free(line);
+				return 0;
+			}
+			free(line);
+			line = 0;
+		}
+
+		if (driver->next_time_us * driver->time_factor > timestamp_in_us())
+			return 0;
+		driver->next_time_us = 0;
+
+		char *buffer;
+		size_t n = 0;
+		ssize_t r = getline(&line, &n, f);
+		if (r <= 0)
+			return 0;
+
+		if ((line[0] != 'R' && line[0] != 'L' && line[0] != 'I') ||
+			line[1] != ' ')
+			return 0;
+
+		switch (line[0]) {
+		case 'L':
+		case 'R':
+			parse_and_run_lightcode(line, driver);
+			break;
+		case 'I':
+			parse_and_run_imu(line, driver);
+			break;
+		}
+
+		free(line);
+	} else {
+		if (f) {
+			fclose(driver->playback_file);
+		}
+		driver->playback_file = 0;
+		return -1;
+	}
+
+	return 0;
+}
+
+static int playback_close(struct SurviveContext *ctx, void *_driver) {
+	SurvivePlaybackData *driver = _driver;
+	if (driver->playback_file)
+		fclose(driver->playback_file);
+	driver->playback_file = 0;
+	return 0;
+}
+
+static int LoadConfig(SurvivePlaybackData *sv, SurviveObject *so) {
+	SurviveContext *ctx = sv->ctx;
+	char *ct0conf = 0;
+
+	char fname[100];
+	sprintf(fname, "%s/%s_config.json", sv->playback_dir, so->codename);
+	FILE *f = fopen(fname, "r");
+
+	if (f == 0 || feof(f) || ferror(f))
+		return 1;
+
+	fseek(f, 0, SEEK_END);
+	int len = ftell(f);
+	fseek(f, 0, SEEK_SET); // same as rewind(f);
+
+	ct0conf = malloc(len + 1);
+	int read = fread(ct0conf, len, 1, f);
+	fclose(f);
+	ct0conf[len] = 0;
+
+	printf("Loading config: %d\n", len);
+	return survive_load_htc_config_format(ct0conf, len, so);
+}
+
+int DriverRegPlayback(SurviveContext *ctx) {
+	const char *playback_dir =
+		config_read_str(ctx->global_config_values, "PlaybackDir", "");
+
+	if (strlen(playback_dir) == 0) {
+		return 0;
+	}
+
+	SurvivePlaybackData *sp = calloc(1, sizeof(SurvivePlaybackData));
+	sp->ctx = ctx;
+	sp->playback_dir = playback_dir;
+	sp->time_factor =
+		config_read_float(ctx->global_config_values, "PlaybackFactor", 1.);
+
+	printf("%s\n", playback_dir);
+
+	char playback_file[100];
+	sprintf(playback_file, "%s/events", playback_dir);
+	sp->playback_file = fopen(playback_file, "r");
+	if (sp->playback_file == 0) {
+		fprintf(stderr, "Could not open playback events file %s",
+				playback_file);
+		return -1;
+	}
+	SurviveObject *hmd = survive_create_hmd(ctx, "Playback", sp);
+	SurviveObject *wm0 = survive_create_wm0(ctx, "Playback", sp, 0);
+	SurviveObject *wm1 = survive_create_wm1(ctx, "Playback", sp, 0);
+	SurviveObject *tr0 = survive_create_tr0(ctx, "Playback", sp);
+	SurviveObject *ww0 = survive_create_ww0(ctx, "Playback", sp);
+
+	if (!LoadConfig(sp, hmd)) {
+		survive_add_object(ctx, hmd);
+	}
+	if (!LoadConfig(sp, wm0)) {
+		survive_add_object(ctx, wm0);
+	}
+	if (!LoadConfig(sp, wm1)) {
+		survive_add_object(ctx, wm1);
+	}
+	if (!LoadConfig(sp, tr0)) {
+		survive_add_object(ctx, tr0);
+	}
+	if (!LoadConfig(sp, ww0)) {
+		survive_add_object(ctx, ww0);
+	}
+
+	survive_add_driver(ctx, sp, playback_poll, playback_close, 0);
+	return 0;
+fail_gracefully:
+	return -1;
+}
+
+REGISTER_LINKTIME(DriverRegPlayback);
diff --git a/src/survive_process.c b/src/survive_process.c
index 3af2da9..0f19007 100644
--- a/src/survive_process.c
+++ b/src/survive_process.c
@@ -63,6 +63,50 @@ void survive_default_angle_process( SurviveObject * so, int sensor_id, int acode
 	}
 }	
 
+void survive_default_button_process(SurviveObject * so, uint8_t eventType, uint8_t buttonId, uint8_t axis1Id, uint16_t axis1Val, uint8_t axis2Id, uint16_t axis2Val)
+{
+	// do nothing.
+	//printf("ButtonEntry: eventType:%x, buttonId:%d, axis1:%d, axis1Val:%8.8x, axis2:%d, axis2Val:%8.8x\n",
+	//	eventType,
+	//	buttonId,
+	//	axis1Id,
+	//	axis1Val,
+	//	axis2Id,
+	//	axis2Val);
+	//if (buttonId == 24 && eventType == 1) // trigger engage
+	//{
+	//	for (int j = 0; j < 6; j++)
+	//	{
+	//		for (int i = 0; i < 0x5; i++)
+	//		{
+	//			survive_haptic(so, 0, 0xf401, 0xb5a2, 0x0100);
+	//			//survive_haptic(so, 0, 0xf401, 0xb5a2, 0x0100);
+	//			OGUSleep(1000);
+	//		}
+	//		OGUSleep(20000);
+	//	}
+	//}
+	//if (buttonId == 2 && eventType == 1) // trigger engage
+	//{
+	//	for (int j = 0; j < 6; j++)
+	//	{
+	//		for (int i = 0; i < 0x1; i++)
+	//		{
+	//			survive_haptic(so, 0, 0xf401, 0x05a2, 0xf100);
+	//			//survive_haptic(so, 0, 0xf401, 0xb5a2, 0x0100);
+	//			OGUSleep(5000);
+	//		}
+	//		OGUSleep(20000);
+	//	}
+	//}
+}
+
+void survive_default_raw_pose_process(SurviveObject * so, uint8_t lighthouse, FLT *pos, FLT *quat)
+{
+	// print the pose;
+	//printf("Pose: [%1.1x][%s][% 08.8f,% 08.8f,% 08.8f] [% 08.8f,% 08.8f,% 08.8f,% 08.8f]\n", lighthouse, so->codename, pos[0], pos[1], pos[2], quat[0], quat[1], quat[2], quat[3]);
+
+}
 
 void survive_default_imu_process( SurviveObject * so, int mask, FLT * accelgyromag, uint32_t timecode, int id )
 {
diff --git a/src/survive_vive.c b/src/survive_vive.c
index 9a3cb03..b3d990a 100755
--- a/src/survive_vive.c
+++ b/src/survive_vive.c
@@ -23,6 +23,8 @@
 #endif
 
 #include "json_helpers.h"
+#include "survive_default_devices.h"
+#include "survive_config.h"
 
 #ifdef HIDAPI
 #if defined(WINDOWS) || defined(WIN32) || defined (_WIN32)
@@ -51,6 +53,11 @@ const short vidpids[] = {
 	0x28de, 0x2000, 1, //Valve HMD lighthouse(B) (only used on HIDAPI, for lightcap)
 	0x28de, 0x2022, 1, //HTC Tracker (only used on HIDAPI, for lightcap)
 	0x28de, 0x2012, 1, //Valve Watchman, USB connected (only used on HIDAPI, for lightcap)
+
+	0x28de, 0x2000, 2, //Valve HMD lighthouse(B) (only used on HIDAPI, for lightcap)
+	0x28de, 0x2022, 2, //HTC Tracker (only used on HIDAPI, for lightcap)
+	0x28de, 0x2012, 2, //Valve Watchman, USB connected (only used on HIDAPI, for lightcap)
+
 #endif
 }; //length MAX_USB_INTERFACES*2
 
@@ -65,6 +72,10 @@ const char * devnames[] = {
 	"HMD Lightcap",
 	"Tracker 0 Lightcap",
 	"Wired Watchman 1 Lightcap",
+
+	"HMD Buttons",
+	"Tracker 0 Buttons",
+	"Wired Watchman 1 Buttons",
 #endif
 }; //length MAX_USB_INTERFACES
 
@@ -80,7 +91,12 @@ const char * devnames[] = {
 #define USB_DEV_HMD_IMU_LHB 6
 #define USB_DEV_TRACKER0_LIGHTCAP 7
 #define USB_DEV_W_WATCHMAN1_LIGHTCAP 8
-#define MAX_USB_DEVS		9
+
+#define USB_DEV_HMD_BUTTONS 9
+#define USB_DEV_TRACKER0_BUTTONS 10
+#define USB_DEV_W_WATCHMAN1_BUTTONS 11
+
+#define MAX_USB_DEVS		12
 #else
 #define MAX_USB_DEVS		6
 #endif
@@ -94,7 +110,10 @@ const char * devnames[] = {
 #define USB_IF_LIGHTCAP		6
 #define USB_IF_TRACKER0_LIGHTCAP		7
 #define USB_IF_W_WATCHMAN1_LIGHTCAP		8
-#define MAX_INTERFACES		9
+#define USB_IF_HMD_BUTTONS		9
+#define USB_IF_TRACKER0_BUTTONS		10
+#define USB_IF_W_WATCHMAN1_BUTTONS		11
+#define MAX_INTERFACES		12
 
 typedef struct SurviveUSBInterface SurviveUSBInterface;
 typedef struct SurviveViveData SurviveViveData;
@@ -292,8 +311,8 @@ static inline int getupdate_feature_report(libusb_device_handle* dev, uint16_t i
 	int ret = libusb_control_transfer(dev, LIBUSB_REQUEST_TYPE_CLASS | LIBUSB_RECIPIENT_INTERFACE | LIBUSB_ENDPOINT_IN,
 		0x01, 0x300 | data[0], interface, data, datalen, 1000 );
 	if( ret == -9 ) return -9;
-    if (ret < 0)
-        return -1;
+	if (ret < 0)
+		return -1;
 	return ret;
 }
 
@@ -427,7 +446,6 @@ int survive_usb_init( SurviveViveData * sv, SurviveObject * hmd, SurviveObject *
 
 		if( d == 0 )
 		{
-			printf( "!!%p  %d %04x %04x %d\n", devnames[i], i, vid, pid, which );
 			SV_INFO( "Did not find device %s (%04x:%04x.%d)", devnames[i], vid, pid, which );
 			sv->udev[i] = 0;
 			continue;
@@ -487,10 +505,16 @@ int survive_usb_init( SurviveViveData * sv, SurviveObject * hmd, SurviveObject *
 	// This is a HACK!  But it works.  Need to investigate further
 	sv->uiface[USB_DEV_TRACKER0_LIGHTCAP].actual_len = 64;
 	if( sv->udev[USB_DEV_TRACKER0_LIGHTCAP] && AttachInterface( sv, tr0, USB_IF_TRACKER0_LIGHTCAP, sv->udev[USB_DEV_TRACKER0_LIGHTCAP], 0x82, survive_data_cb, "Tracker 1 Lightcap")) { return -13; }
+
 	if( sv->udev[USB_DEV_W_WATCHMAN1_LIGHTCAP] && AttachInterface( sv, ww0, USB_IF_W_WATCHMAN1_LIGHTCAP, sv->udev[USB_DEV_W_WATCHMAN1_LIGHTCAP], 0x82, survive_data_cb, "Wired Watchman 1 Lightcap")) { return -13; }
+
+
+	if (sv->udev[USB_DEV_TRACKER0_BUTTONS] && AttachInterface(sv, tr0, USB_IF_TRACKER0_BUTTONS, sv->udev[USB_DEV_TRACKER0_BUTTONS], 0x83, survive_data_cb, "Tracker 1 Buttons")) { return -13; }
+	if (sv->udev[USB_DEV_W_WATCHMAN1_BUTTONS] && AttachInterface(sv, ww0, USB_IF_W_WATCHMAN1_BUTTONS, sv->udev[USB_DEV_W_WATCHMAN1_BUTTONS], 0x83, survive_data_cb, "Wired Watchman 1 BUTTONS")) { return -13; }
+
 #else
 	if( sv->udev[USB_DEV_HMD_IMU_LH] && AttachInterface( sv, hmd, USB_IF_LIGHTCAP, sv->udev[USB_DEV_HMD_IMU_LH], 0x82, survive_data_cb, "Lightcap")) { return -12; }
-	if( sv->udev[USB_DEV_TRACKER0] && AttachInterface( sv, ww0, USB_IF_TRACKER0_LIGHTCAP, sv->udev[USB_DEV_TRACKER0], 0x82, survive_data_cb, "Tracker 0 Lightcap")) { return -13; }
+	if( sv->udev[USB_DEV_TRACKER0] && AttachInterface( sv, tr0, USB_IF_TRACKER0_LIGHTCAP, sv->udev[USB_DEV_TRACKER0], 0x82, survive_data_cb, "Tracker 0 Lightcap")) { return -13; }
 	if( sv->udev[USB_DEV_W_WATCHMAN1] && AttachInterface( sv, ww0, USB_IF_W_WATCHMAN1_LIGHTCAP, sv->udev[USB_DEV_W_WATCHMAN1], 0x82, survive_data_cb, "Wired Watchman 1 Lightcap")) { return -13; }
 #endif
 	SV_INFO( "All enumerated devices attached." );
@@ -596,18 +620,38 @@ int survive_vive_send_magic(SurviveContext * ctx, void * drv, int magic_code, vo
 
 //#endif
 
-#if 0
-		for( int i = 0; i < 256; i++ )
+#if 0		
+		for (int j=0; j < 40; j++)
 		{
-			static uint8_t vive_controller_haptic_pulse[64] = { 0xff, 0x8f, 0xff, 0, 0, 0, 0, 0, 0, 0 };
-			//r = update_feature_report( sv->udev[USB_DEV_WATCHMAN1], 0, vive_controller_haptic_pulse, sizeof( vive_controller_haptic_pulse ) );
-			r = update_feature_report( sv->udev[USB_DEV_W_WATCHMAN1_LIGHTCAP], 0, vive_controller_haptic_pulse, sizeof( vive_controller_haptic_pulse ) );
-			SV_INFO( "UCR: %d", r );
-			if( r != sizeof( vive_controller_haptic_pulse ) ) return 5;
-			OGUSleep( 1000 );
+			for (int i = 0; i < 0x1; i++)
+			{
+				//uint8_t vive_controller_haptic_pulse[64] = { 0xff, 0x8f, 0x7, 0 /*right*/, 0xFF /*period on*/, 0xFF/*period on*/, 0xFF/*period off*/, 0xFF/*period off*/, 0xFF /* repeat Count */, 0xFF /* repeat count */ };
+				//uint8_t vive_controller_haptic_pulse[64] = { 0xff, 0x8f, 0x07, 0x00, 0xf4, 0x01, 0xb5, 0xa2, 0x01, 0x00 }; // data taken from Nairol's captures
+				uint8_t vive_controller_haptic_pulse[64] = { 0xff, 0x8f, 0x07, 0x00, 0xf4, 0x01, 0xb5, 0xa2, 0x01* j, 0x00 }; 
+				r = update_feature_report( sv->udev[USB_DEV_WATCHMAN1], 0, vive_controller_haptic_pulse, sizeof( vive_controller_haptic_pulse ) );
+				r = getupdate_feature_report(sv->udev[USB_DEV_WATCHMAN1], 0, vive_controller_haptic_pulse, sizeof(vive_controller_haptic_pulse));
+				SV_INFO("UCR: %d", r);
+				if (r != sizeof(vive_controller_haptic_pulse)) printf("HAPTIC FAILED **************************\n"); // return 5;
+				OGUSleep(5000);
+			}
+
+			OGUSleep(20000);
 		}
+
+
 #endif
 
+#if 0
+		//// working code to turn off a wireless controller:
+		//{
+		//	uint8_t vive_controller_off[64] = { 0xff, 0x9f, 0x04, 'o', 'f', 'f', '!' };
+		//	//r = update_feature_report( sv->udev[USB_DEV_WATCHMAN1], 0, vive_controller_haptic_pulse, sizeof( vive_controller_haptic_pulse ) );
+		//	r = update_feature_report(sv->udev[USB_DEV_WATCHMAN1], 0, vive_controller_off, sizeof(vive_controller_off));
+		//	SV_INFO("UCR: %d", r);
+		//	if (r != sizeof(vive_controller_off)) printf("OFF FAILED **************************\n"); // return 5;
+		//	OGUSleep(1000);
+		//}
+#endif
 		//if (sv->udev[USB_DEV_TRACKER0])
 		//{
 		//	static uint8_t vive_magic_power_on[64] = {  0x04, 0x78, 0x29, 0x38 };
@@ -650,6 +694,54 @@ int survive_vive_send_magic(SurviveContext * ctx, void * drv, int magic_code, vo
 	return 0;
 }
 
+int survive_vive_send_haptic(SurviveObject * so, uint8_t reserved, uint16_t pulseHigh, uint16_t pulseLow, uint16_t repeatCount)
+{
+	SurviveViveData *sv = (SurviveViveData*)so->driver;
+
+	if (NULL == sv)
+	{
+		return -500;
+	}
+
+	int r;
+	uint8_t vive_controller_haptic_pulse[64] = { 
+		0xff, 0x8f, 0x07, 0x00, 
+		pulseHigh & 0xff00 >> 8,  pulseHigh & 0xff,
+		pulseLow & 0xff00 >> 8,  pulseLow & 0xff,
+		repeatCount & 0xff00 >> 8,  repeatCount & 0xff,
+	};
+
+	r = update_feature_report(sv->udev[USB_DEV_WATCHMAN1], 0, vive_controller_haptic_pulse, sizeof(vive_controller_haptic_pulse));
+	r = getupdate_feature_report(sv->udev[USB_DEV_WATCHMAN1], 0, vive_controller_haptic_pulse, sizeof(vive_controller_haptic_pulse));
+	//SV_INFO("UCR: %d", r);
+	if (r != sizeof(vive_controller_haptic_pulse)) 
+	{
+		printf("HAPTIC FAILED **************************\n"); 
+		return -1;
+	}
+
+	return 0;
+
+	//for (int j = 0; j < 40; j++)
+	//{
+	//	for (int i = 0; i < 0x1; i++)
+	//	{
+	//		//uint8_t vive_controller_haptic_pulse[64] = { 0xff, 0x8f, 0x7, 0 /*right*/, 0xFF /*period on*/, 0xFF/*period on*/, 0xFF/*period off*/, 0xFF/*period off*/, 0xFF /* repeat Count */, 0xFF /* repeat count */ };
+	//		//uint8_t vive_controller_haptic_pulse[64] = { 0xff, 0x8f, 0x07, 0x00, 0xf4, 0x01, 0xb5, 0xa2, 0x01, 0x00 }; // data taken from Nairol's captures
+	//		uint8_t vive_controller_haptic_pulse[64] = { 0xff, 0x8f, 0x07, 0x00, 0xf4, 0x01, 0xb5, 0xa2, 0x01 * j, 0x00 };
+	//		r = update_feature_report(sv->udev[USB_DEV_WATCHMAN1], 0, vive_controller_haptic_pulse, sizeof(vive_controller_haptic_pulse));
+	//		r = getupdate_feature_report(sv->udev[USB_DEV_WATCHMAN1], 0, vive_controller_haptic_pulse, sizeof(vive_controller_haptic_pulse));
+	//		if (r != sizeof(vive_controller_haptic_pulse)) printf("HAPTIC FAILED **************************\n"); // return 5;
+	//		OGUSleep(5000);
+	//	}
+
+	//	OGUSleep(20000);
+	//}
+
+	////OGUSleep(5000);
+	//return 0;
+}
+
 void survive_vive_usb_close( SurviveViveData * sv )
 {
 	int i;
@@ -811,8 +903,21 @@ int survive_get_config( char ** config, SurviveViveData * sv, int devno, int ifa
 ///////////////////////////////////////////////////////////////////////////////
 
 #define POP1  (*(readdata++))
-#define POP2  (*(((uint16_t*)((readdata+=2)-2))))
-#define POP4  (*(((uint32_t*)((readdata+=4)-4))))
+
+struct __attribute__((__packed__)) unaligned_16_t {
+    int16_t v;
+};
+struct __attribute__((__packed__)) unaligned_32_t {
+    int32_t v;
+};
+struct __attribute__((__packed__)) unaligned_u16_t {
+    uint16_t v;
+};
+struct __attribute__((__packed__)) unaligned_u32_t {
+    uint32_t v;
+};
+#define POP2  ((((( struct unaligned_u16_t*)((readdata+=2)-2))))->v)
+#define POP4  ((((( struct unaligned_u32_t*)((readdata+=4)-4))))->v)
 
 void calibrate_acc(SurviveObject* so, FLT* agm) {
 	if (so->acc_bias != NULL) {
@@ -842,6 +947,171 @@ void calibrate_gyro(SurviveObject* so, FLT* agm) {
 	}
 }
 
+typedef struct 
+{
+	// could use a bitfield here, but since this data is short-lived,
+	// the space savings probably isn't worth the processing overhead.
+	uint8_t pressedButtonsValid;
+	uint8_t triggerOfBatteryValid;
+	uint8_t batteryChargeValid;
+	uint8_t hardwareIdValid;
+	uint8_t touchpadHorizontalValid;
+	uint8_t touchpadVerticalValid;
+	uint8_t triggerHighResValid;
+
+	uint32_t pressedButtons;
+	uint16_t triggerOrBattery;
+	uint8_t  batteryCharge;
+	uint32_t hardwareId;
+	uint16_t touchpadHorizontal;
+	uint16_t touchpadVertical;
+	uint16_t triggerHighRes;
+} buttonEvent;
+
+void incrementAndPostButtonQueue(SurviveContext *ctx)
+{
+	ButtonQueueEntry *entry = &(ctx->buttonQueue.entry[ctx->buttonQueue.nextWriteIndex]);
+
+	if ((ctx->buttonQueue.nextWriteIndex+1)% BUTTON_QUEUE_MAX_LEN == ctx->buttonQueue.nextReadIndex)
+	{
+		// There's not enough space to write this entry.  Clear it out and move along
+		//printf("Button Buffer Full\n");
+		memset(entry, 0, sizeof(ButtonQueueEntry));
+		return;
+	}
+	entry->isPopulated = 1;
+	ctx->buttonQueue.nextWriteIndex++;
+	// if we've exceeded the size of the buffer, loop around to the beginning.
+	if (ctx->buttonQueue.nextWriteIndex >= BUTTON_QUEUE_MAX_LEN)
+	{
+		ctx->buttonQueue.nextWriteIndex = 0;
+	}
+	OGUnlockSema(ctx->buttonQueue.buttonservicesem);
+
+	// clear out any old data in the entry so we always start with a clean slate.
+	entry = &(ctx->buttonQueue.entry[ctx->buttonQueue.nextWriteIndex]);
+	memset(entry, 0, sizeof(ButtonQueueEntry));
+}
+
+// important!  This must be the only place that we're posting to the buttonEntryQueue
+// if that ever needs to be changed, you will have to add locking so that only one
+// thread is posting at a time.
+void registerButtonEvent(SurviveObject *so, buttonEvent *event)
+{
+	ButtonQueueEntry *entry = &(so->ctx->buttonQueue.entry[so->ctx->buttonQueue.nextWriteIndex]);
+
+	memset(entry, 0, sizeof(ButtonQueueEntry));
+
+	entry->so = so;
+	if (event->pressedButtonsValid)
+	{
+		//printf("trigger %8.8x\n", event->triggerHighRes);
+		for (int a=0; a < 16; a++)
+		{
+			if (((event->pressedButtons) & (1<<a)) != ((so->buttonmask) & (1<<a)))
+			{
+				// Hey, the button did something
+				if (event->pressedButtons & (1 << a))
+				{
+					// it went down
+					entry->eventType = BUTTON_EVENT_BUTTON_DOWN;
+				}
+				else
+				{
+					// it went up
+					entry->eventType = BUTTON_EVENT_BUTTON_UP;
+				}
+				entry->buttonId = a;
+				if (entry->buttonId == 0)
+				{
+					// this fixes 2 issues.  First, is the a button id of 0 indicates no button pressed.
+					// second is that the trigger shows up as button 0 coming from the wireless controller,
+					// but we infer it from the position on the wired controller.  On the wired, we treat it
+					// as buttonId 24 (look further down in this function)
+					entry->buttonId = 24;
+				}
+				incrementAndPostButtonQueue(so->ctx);
+				entry = &(so->ctx->buttonQueue.entry[so->ctx->buttonQueue.nextWriteIndex]);
+			}
+		}
+		// if the trigger button is depressed & it wasn't before
+		if ((((event->pressedButtons) & (0xff000000)) == 0xff000000) &&
+			((so->buttonmask) & (0xff000000)) != 0xff000000)
+		{
+			entry->eventType = BUTTON_EVENT_BUTTON_DOWN;
+			entry->buttonId = 24;
+			incrementAndPostButtonQueue(so->ctx);
+			entry = &(so->ctx->buttonQueue.entry[so->ctx->buttonQueue.nextWriteIndex]);
+		}
+		// if the trigger button isn't depressed but it was before
+		else if ((((event->pressedButtons) & (0xff000000)) != 0xff000000) &&
+			((so->buttonmask) & (0xff000000)) == 0xff000000)
+		{
+			entry->eventType = BUTTON_EVENT_BUTTON_UP;
+			entry->buttonId = 24;
+			incrementAndPostButtonQueue(so->ctx);
+			entry = &(so->ctx->buttonQueue.entry[so->ctx->buttonQueue.nextWriteIndex]);
+		}
+	}
+	if (event->triggerHighResValid)
+	{
+		if (so->axis1 != event->triggerHighRes)
+		{
+			entry->eventType = BUTTON_EVENT_AXIS_CHANGED;
+			entry->axis1Id = 1;
+			entry->axis1Val = event->triggerHighRes;
+			incrementAndPostButtonQueue(so->ctx);
+			entry = &(so->ctx->buttonQueue.entry[so->ctx->buttonQueue.nextWriteIndex]);
+
+		}
+	}
+	if ((event->touchpadHorizontalValid) && (event->touchpadVerticalValid))
+	{
+		if ((so->axis2 != event->touchpadHorizontal) ||
+			(so->axis3 != event->touchpadVertical))
+		{
+			entry->eventType = BUTTON_EVENT_AXIS_CHANGED;
+			entry->axis1Id = 2;
+			entry->axis1Val = event->touchpadHorizontal;
+			entry->axis2Id = 3;
+			entry->axis2Val = event->touchpadVertical;
+			incrementAndPostButtonQueue(so->ctx);
+			entry = &(so->ctx->buttonQueue.entry[so->ctx->buttonQueue.nextWriteIndex]);
+
+		}
+	}
+
+	if (event->pressedButtonsValid)
+	{
+		so->buttonmask = event->pressedButtons;
+	}
+	if (event->batteryChargeValid)
+	{
+		so->charge = event->batteryCharge;
+	}
+	if (event->touchpadHorizontalValid)
+	{
+		so->axis2 = event->touchpadHorizontal;
+	}
+	if (event->touchpadVerticalValid)
+	{
+		so->axis3 = event->touchpadVertical;
+	}
+	if (event->triggerHighResValid)
+	{
+		so->axis1 = event->triggerHighRes;
+	}
+}
+
+uint8_t isPopulated;  //probably can remove this given the semaphore in the parent struct.   helps with debugging
+uint8_t eventType;
+uint8_t buttonId;
+uint8_t axis1Id;
+uint16_t axis1Val;
+uint8_t axis2Id;
+uint16_t axis2Val;
+SurviveObject *so;
+
 
 static void handle_watchman( SurviveObject * w, uint8_t * readdata )
 {
@@ -862,36 +1132,55 @@ static void handle_watchman( SurviveObject * w, uint8_t * readdata )
 	uint8_t qty = POP1;
 	uint8_t time2 = POP1;
 	uint8_t type = POP1;
+
+
 	qty-=2;
 	int propset = 0;
 	int doimu = 0;
 
 	if( (type & 0xf0) == 0xf0 )
 	{
+		buttonEvent bEvent;
+		memset(&bEvent, 0, sizeof(bEvent));
+
 		propset |= 4;
 		//printf( "%02x %02x %02x %02x\n", qty, type, time1, time2 );
 		type &= ~0x10;
 
 		if( type & 0x01 )
 		{
+			
 			qty-=1;
-			w->buttonmask = POP1;
+			bEvent.pressedButtonsValid = 1;
+			bEvent.pressedButtons = POP1;
+
+			//printf("buttonmask is %d\n", w->buttonmask);
 			type &= ~0x01;
 		}
 		if( type & 0x04 ) 
 		{
 			qty-=1;
-			w->axis1 = ( POP1 ) * 128; 
+			bEvent.triggerHighResValid = 1;
+			bEvent.triggerHighRes = ( POP1 ) * 128;
 			type &= ~0x04;
 		}
 		if( type & 0x02 )
 		{
 			qty-=4;
-			w->axis2 = POP2;
-			w->axis3 = POP2;
+			bEvent.touchpadHorizontalValid = 1;
+			bEvent.touchpadVerticalValid = 1;
+
+			bEvent.touchpadHorizontal = POP2;
+			bEvent.touchpadVertical = POP2;
 			type &= ~0x02;
 		}
 
+		if (bEvent.pressedButtonsValid || bEvent.triggerHighResValid || bEvent.touchpadHorizontalValid)
+		{
+			registerButtonEvent(w, &bEvent);
+		}
+
+
 		//XXX TODO: Is this correct?  It looks SO WACKY
 		type &= 0x7f;
 		if( type == 0x68 ) doimu = 1;
@@ -1156,7 +1445,7 @@ void survive_data_cb( SurviveUSBInterface * si )
 		//printf( "%d -> ", size );
 		for( i = 0; i < 3; i++ )
 		{
-			int16_t * acceldata = (int16_t*)readdata;
+            struct unaligned_16_t * acceldata = (struct unaligned_16_t *)readdata;
 			readdata += 12;
 			uint32_t timecode = POP4;
 			uint8_t code = POP1;
@@ -1168,8 +1457,8 @@ void survive_data_cb( SurviveUSBInterface * si )
 				obj->oldcode = code;
 
 				//XXX XXX BIG TODO!!! Actually recal gyro data.
-				FLT agm[9] = { acceldata[0], acceldata[1], acceldata[2],
-								acceldata[3], acceldata[4], acceldata[5],
+				FLT agm[9] = { acceldata[0].v, acceldata[1].v, acceldata[2].v,
+								acceldata[3].v, acceldata[4].v, acceldata[5].v,
 								0,0,0 };
 
 				agm[0]*=(float)(1./8192.0);
@@ -1192,7 +1481,10 @@ void survive_data_cb( SurviveUSBInterface * si )
 				ctx->imuproc( obj, 3, agm, timecode, code );
 			}
 		}
-
+		if (id != 32)
+		{
+			int a=0; // set breakpoint here
+		}
 		//DONE OK.
 		break;
 	}
@@ -1211,7 +1503,7 @@ void survive_data_cb( SurviveUSBInterface * si )
 		}
 		else if( id == 38 )
 		{
-			w->ison = 0;
+			w->ison = 0; // turning off
 		}
 		else
 		{
@@ -1250,7 +1542,94 @@ void survive_data_cb( SurviveUSBInterface * si )
 			handle_lightcap( obj, &le );
 		}		
 		break;
+
+		if (id != 33)
+		{
+			int a = 0; // breakpoint here
+		}
+	}
+	case USB_IF_TRACKER0_BUTTONS:
+	case USB_IF_W_WATCHMAN1_BUTTONS:
+	{
+		if (1 == id)
+		{
+			//0x00	uint8	1	reportID	HID report identifier(= 1)
+			//0x02	uint16	2	reportType(? )	0x0B04: Ping(every second) / 0x3C01 : User input
+			//0x04	uint32	4	reportCount	Counter that increases with every report
+			//0x08	uint32	4	pressedButtons	Bit field, see below for individual buttons
+			//0x0C	uint16	2	triggerOrBattery	Analog trigger value(user input) / Battery voltage ? (ping)
+			//0x0E	uint8	1	batteryCharge	Bit 7 : Charging / Bit 6..0 : Battery charge in percent
+			//0x10	uint32	4	hardwareID	Hardware ID(user input) / 0x00000000 (ping)
+			//0x14	int16	2	touchpadHorizontal	Horizontal thumb position(Left : -32768 / Right : 32767)
+			//0x16	int16	2	touchpadVertical	Vertical thumb position(Bottom : -32768 / Top : 32767)
+			//0x18 ? 2 ? unknown
+			//0x1A	uint16	2	triggerHighRes	Analog trigger value with higher resolution
+			//0x1C ? 24 ? unknown
+			//0x34	uint16	2	triggerRawMaybe	Analog trigger value, maybe raw sensor data
+			//0x36 ? 8 ? unknown
+			//0x3E	uint8	1	someBitFieldMaybe	0x00 : ping / 0x64 : user input
+			//0x3F ? 1 ? unknown
+
+			//typedef struct
+			//{
+			//	//uint8_t reportId;
+			//	uint16_t reportType;
+			//	uint32_t reportCount;
+			//	uint32_t pressedButtons;
+			//	uint16_t  triggerOrBattery;
+			//	uint8_t batteryCharge;
+			//	uint32_t hardwareId;
+			//	int16_t  touchpadHorizontal;
+			//	int16_t  touchpadVertical;
+			//	uint16_t unknown1;
+			//	uint16_t triggerHighRes;
+			//	uint8_t  unknown2;
+			//	uint8_t  unknown3;
+			//	uint8_t  unknown4;
+			//	uint16_t triggerRaw;
+			//	uint8_t  unknown5;
+			//	uint8_t  unknown6; // maybe some bitfield?
+			//	uint8_t  unknown7;
+			//} usb_buttons_raw;
+
+			//usb_buttons_raw *raw = (usb_buttons_raw*) readdata;
+			if (*((uint16_t*)(&(readdata[0x0]))) == 0x100)
+			{
+				buttonEvent bEvent;
+				memset(&bEvent, 0, sizeof(bEvent));
+
+				bEvent.pressedButtonsValid = 1;
+				bEvent.pressedButtons = *((uint32_t*)(&(readdata[0x07])));
+				bEvent.triggerHighResValid = 1;
+				//bEvent.triggerHighRes = raw->triggerHighRes; 
+				//bEvent.triggerHighRes = (raw->pressedButtons & 0xff000000) >> 24; // this seems to provide the same data at 2x the resolution as above
+				//bEvent.triggerHighRes = raw->triggerRaw;
+				
+				bEvent.triggerHighRes = *((uint16_t*)(&(readdata[0x19])));
+				bEvent.touchpadHorizontalValid = 1;
+				//bEvent.touchpadHorizontal = raw->touchpadHorizontal;
+				bEvent.touchpadHorizontal = *((int16_t*)(&(readdata[0x13])));
+				bEvent.touchpadVerticalValid = 1;
+				//bEvent.touchpadVertical = raw->touchpadVertical;
+				bEvent.touchpadVertical = *((int16_t*)(&(readdata[0x15])));
+
+				//printf("%4.4x\n", bEvent.triggerHighRes);
+				registerButtonEvent(obj, &bEvent);
+
+				//printf("Buttons: %8.8x\n", raw->pressedButtons);
+			}
+			int a = 0;
+		}
+		else
+		{
+			int a = 0;// breakpoint here
+		}
 	}
+	default:
+	{
+		int a = 0; // breakpoint here
+	}
+
 	}
 }
 
@@ -1269,171 +1648,22 @@ void survive_data_cb( SurviveUSBInterface * si )
 
 
 
-static int jsoneq(const char *json, jsmntok_t *tok, const char *s) {
- if (tok->type == JSMN_STRING && (int) strlen(s) == tok->end - tok->start &&
-    strncmp(json + tok->start, s, tok->end - tok->start) == 0) {
-		return 0;
-	}
-	return -1;
-}
-
-
-static int ParsePoints( SurviveContext * ctx, SurviveObject * so, char * ct0conf, FLT ** floats_out, jsmntok_t * t, int i )
-{
-	int k;
-	int pts = t[i+1].size;
-	jsmntok_t * tk;
-
-	so->nr_locations = 0;
-	*floats_out = malloc( sizeof( **floats_out ) * 32 * 3 );
-
-	for( k = 0; k < pts; k++ )
-	{
-		tk = &t[i+2+k*4];
-
-		int m;
-		for( m = 0; m < 3; m++ )
-		{
-			char ctt[128];
-
-			tk++;
-			int elemlen = tk->end - tk->start;
-
-			if( tk->type != 4 || elemlen > sizeof( ctt )-1 )
-			{
-				SV_ERROR( "Parse error in JSON\n" );
-				return 1;
-			}
-
-			memcpy( ctt, ct0conf + tk->start, elemlen );
-			ctt[elemlen] = 0;
-			FLT f = atof( ctt );
-			int id = so->nr_locations*3+m;
-			(*floats_out)[id] = f;
-		}
-		so->nr_locations++;
-	}
-	return 0;
-}
-
 static int LoadConfig( SurviveViveData * sv, SurviveObject * so, int devno, int iface, int extra_magic )
 {
 	SurviveContext * ctx = sv->ctx;
 	char * ct0conf = 0;
 	int len = survive_get_config( &ct0conf, sv, devno, iface, extra_magic );
-printf( "Loading config: %d\n", len );
-#if 0
-	char fname[100];
-	sprintf( fname, "%s_config.json", so->codename );
-	FILE * f = fopen( fname, "w" );
-	fwrite( ct0conf, strlen(ct0conf), 1, f );
-	fclose( f );
-#endif
+	printf( "Loading config: %d\n", len );
 
-	if( len > 0 )
 	{
-
-		//From JSMN example.
-		jsmn_parser p;
-		jsmntok_t t[4096];
-		jsmn_init(&p);
-		int i;
-		int r = jsmn_parse(&p, ct0conf, len, t, sizeof(t)/sizeof(t[0]));	
-		if (r < 0) {
-			SV_INFO("Failed to parse JSON in HMD configuration: %d\n", r);
-			return -1;
-		}
-		if (r < 1 || t[0].type != JSMN_OBJECT) {
-			SV_INFO("Object expected in HMD configuration\n");
-			return -2;
-		}
-
-		for (i = 1; i < r; i++) {
-			jsmntok_t * tk = &t[i];
-
-			char ctxo[100];
-			int ilen = tk->end - tk->start;
-			if( ilen > 99 ) ilen = 99;
-			memcpy(ctxo, ct0conf + tk->start, ilen);
-			ctxo[ilen] = 0;
-
-//				printf( "%d / %d / %d / %d %s %d\n", tk->type, tk->start, tk->end, tk->size, ctxo, jsoneq(ct0conf, &t[i], "modelPoints") );
-//				printf( "%.*s\n", ilen, ct0conf + tk->start );
-
-			if (jsoneq(ct0conf, tk, "modelPoints") == 0) {
-				if( ParsePoints( ctx, so, ct0conf, &so->sensor_locations, t, i  ) )
-				{
-					break;
-				}
-			}
-			if (jsoneq(ct0conf, tk, "modelNormals") == 0) {
-				if( ParsePoints( ctx, so, ct0conf, &so->sensor_normals, t, i  ) )
-				{
-					break;
-				}
-			}
-
-
-			if (jsoneq(ct0conf, tk, "acc_bias") == 0) {
-				int32_t count = (tk+1)->size;
-				FLT* values = NULL;
-				if ( parse_float_array(ct0conf, tk+2, &values, count) >0 ) {
-					so->acc_bias = values;
-					so->acc_bias[0] *= .125; //XXX Wat?  Observed by CNL.  Biasing by more than this seems to hose things.
-					so->acc_bias[1] *= .125;
-					so->acc_bias[2] *= .125;
-				}
-			}
-			if (jsoneq(ct0conf, tk, "acc_scale") == 0) {
-				int32_t count = (tk+1)->size;
-				FLT* values = NULL;
-				if ( parse_float_array(ct0conf, tk+2, &values, count) >0 ) {
-					so->acc_scale = values;
-				}
-			}
-
-			if (jsoneq(ct0conf, tk, "gyro_bias") == 0) {
-				int32_t count = (tk+1)->size;
-				FLT* values = NULL;
-				if ( parse_float_array(ct0conf, tk+2, &values, count) >0 ) {
-					so->gyro_bias = values;
-				}
-			}
-			if (jsoneq(ct0conf, tk, "gyro_scale") == 0) {
-				int32_t count = (tk+1)->size;
-				FLT* values = NULL;
-				if ( parse_float_array(ct0conf, tk+2, &values, count) >0 ) {
-					so->gyro_scale = values;
-				}
-			}
-		}
+	  char raw_fname[100];
+	  sprintf( raw_fname, "%s_config.json", so->codename );
+	  FILE * f = fopen( raw_fname, "w" );
+	  fwrite( ct0conf, strlen(ct0conf), 1, f );
+	  fclose( f );
 	}
-	else
-	{
-		//TODO: Cleanup any remaining USB stuff.
-		return 1;
-	}
-
-	char fname[64];
 
-	sprintf( fname, "calinfo/%s_points.csv", so->codename );
-	FILE * f = fopen( fname, "w" );
-	int j;
-	for( j = 0; j < so->nr_locations; j++ )
-	{
-		fprintf( f, "%f %f %f\n", so->sensor_locations[j*3+0], so->sensor_locations[j*3+1], so->sensor_locations[j*3+2] );
-	}
-	fclose( f );
-
-	sprintf( fname, "calinfo/%s_normals.csv", so->codename );
-	f = fopen( fname, "w" );
-	for( j = 0; j < so->nr_locations; j++ )
-	{
-		fprintf( f, "%f %f %f\n", so->sensor_normals[j*3+0], so->sensor_normals[j*3+1], so->sensor_normals[j*3+2] );
-	}
-	fclose( f );
-
-	return 0;
+	return survive_load_htc_config_format(ct0conf, len, so);
 }
 
 
@@ -1450,23 +1680,27 @@ void init_SurviveObject(SurviveObject* so) {
 	so->acc_bias = NULL;
 	so->gyro_scale = NULL;
 	so->gyro_bias = NULL;
+	so->haptic = NULL;
+	so->PoserData = NULL;
+	so->disambiguator_data = NULL;
 }
 
 int DriverRegHTCVive( SurviveContext * ctx )
 {
-	int r;
-	SurviveObject * hmd = calloc( 1, sizeof( SurviveObject ) );
-	SurviveObject * wm0 = calloc( 1, sizeof( SurviveObject ) );
-	SurviveObject * wm1 = calloc( 1, sizeof( SurviveObject ) );
-	SurviveObject * tr0 = calloc( 1, sizeof( SurviveObject ) );
-	SurviveObject * ww0 = calloc( 1, sizeof( SurviveObject ) );
-	SurviveViveData * sv = calloc( 1, sizeof( SurviveViveData ) );
-
-	init_SurviveObject(hmd);
-	init_SurviveObject(wm0);
-	init_SurviveObject(wm1);
-	init_SurviveObject(tr0);
-	init_SurviveObject(ww0);
+	const char* playback_dir = config_read_str(ctx->global_config_values,
+						   "PlaybackDir", "");
+	if(strlen(playback_dir) != 0) {
+	  SV_INFO("Playback is active; disabling USB driver");
+	  return 0;
+	}
+	
+	int r;	
+	SurviveViveData * sv = calloc(1, sizeof(SurviveViveData) );
+	SurviveObject * hmd = survive_create_hmd(ctx, "HTC", sv);
+	SurviveObject * wm0 = survive_create_wm0(ctx, "HTC", sv, 0);
+	SurviveObject * wm1 = survive_create_wm1(ctx, "HTC", sv, 0);
+	SurviveObject * tr0 = survive_create_tr0(ctx, "HTC", sv);
+	SurviveObject * ww0 = survive_create_ww0(ctx, "HTC", sv);
 
 	sv->ctx = ctx;
 	
@@ -1478,23 +1712,6 @@ int DriverRegHTCVive( SurviveContext * ctx )
 		mkdir( "calinfo", 0755 );
 	#endif
 
-
-	hmd->ctx = ctx;
-	memcpy( hmd->codename, "HMD", 4 );
-	memcpy( hmd->drivername, "HTC", 4 );
-	wm0->ctx = ctx;
-	memcpy( wm0->codename, "WM0", 4 );
-	memcpy( wm0->drivername, "HTC", 4 );
-	wm1->ctx = ctx;
-	memcpy( wm1->codename, "WM1", 4 );
-	memcpy( wm1->drivername, "HTC", 4 );
-	tr0->ctx = ctx;
-	memcpy( tr0->codename, "TR0", 4 );
-	memcpy( tr0->drivername, "HTC", 4 );
-	ww0->ctx = ctx;
-	memcpy( ww0->codename, "WW0", 4 );
-	memcpy( ww0->drivername, "HTC", 4 );
-
 	//USB must happen last.
 	if( r = survive_usb_init( sv, hmd, wm0, wm1, tr0, ww0) )
 	{
@@ -1508,55 +1725,7 @@ int DriverRegHTCVive( SurviveContext * ctx )
 	if( sv->udev[USB_DEV_WATCHMAN2] && LoadConfig( sv, wm1, 3, 0, 1 )) { SV_INFO( "Watchman 1 config issue." ); }
 	if( sv->udev[USB_DEV_TRACKER0]  && LoadConfig( sv, tr0, 4, 0, 0 )) { SV_INFO( "Tracker 0 config issue." ); }
 	if( sv->udev[USB_DEV_W_WATCHMAN1]  && LoadConfig( sv, ww0, 5, 0, 0 )) { SV_INFO( "Wired Watchman 0 config issue." ); }
-
-	hmd->timebase_hz = wm0->timebase_hz = wm1->timebase_hz = 48000000;
-	tr0->timebase_hz = ww0->timebase_hz = hmd->timebase_hz;
-
-	hmd->pulsedist_max_ticks = wm0->pulsedist_max_ticks = wm1->pulsedist_max_ticks = 500000;
-	tr0->pulsedist_max_ticks = ww0->pulsedist_max_ticks = hmd->pulsedist_max_ticks;
-
-	hmd->pulselength_min_sync = wm0->pulselength_min_sync = wm1->pulselength_min_sync = 2200;
-	tr0->pulselength_min_sync = ww0->pulselength_min_sync = hmd->pulselength_min_sync;
-
-	hmd->pulse_in_clear_time = wm0->pulse_in_clear_time = wm1->pulse_in_clear_time = 35000;
-	tr0->pulse_in_clear_time = ww0->pulse_in_clear_time = hmd->pulse_in_clear_time;
-
-	hmd->pulse_max_for_sweep = wm0->pulse_max_for_sweep = wm1->pulse_max_for_sweep = 1800;
-	tr0->pulse_max_for_sweep = ww0->pulse_max_for_sweep = hmd->pulse_max_for_sweep;
-
-	hmd->pulse_synctime_offset = wm0->pulse_synctime_offset = wm1->pulse_synctime_offset = 20000;
-	tr0->pulse_synctime_offset = ww0->pulse_synctime_offset = hmd->pulse_synctime_offset;
-
-	hmd->pulse_synctime_slack = wm0->pulse_synctime_slack = wm1->pulse_synctime_slack = 5000;
-	tr0->pulse_synctime_slack = ww0->pulse_synctime_slack = hmd->pulse_synctime_slack;
-
-	hmd->timecenter_ticks = hmd->timebase_hz / 240;
-	wm0->timecenter_ticks = wm0->timebase_hz / 240;
-	wm1->timecenter_ticks = wm1->timebase_hz / 240;
-	tr0->timecenter_ticks = tr0->timebase_hz / 240;
-	ww0->timecenter_ticks = ww0->timebase_hz / 240;
-/*
-	int i;
-	int locs = hmd->nr_locations;
-	printf( "Locs: %d\n", locs );
-	if (hmd->sensor_locations )
-	{
-		printf( "POSITIONS:\n" );
-		for( i = 0; i < locs*3; i+=3 )
-		{
-			printf( "%f %f %f\n", hmd->sensor_locations[i+0], hmd->sensor_locations[i+1], hmd->sensor_locations[i+2] );
-		}
-	}
-	if( hmd->sensor_normals )
-	{
-		printf( "NORMALS:\n" );
-		for( i = 0; i < locs*3; i+=3 )
-		{
-			printf( "%f %f %f\n", hmd->sensor_normals[i+0], hmd->sensor_normals[i+1], hmd->sensor_normals[i+2] );
-		}
-	}
-*/
-
+	
 	//Add the drivers.
 	if( sv->udev[USB_DEV_HMD_IMU_LH]       ) { survive_add_object( ctx, hmd ); }
 	if( sv->udev[USB_DEV_WATCHMAN1] ) { survive_add_object( ctx, wm0 ); }
@@ -1564,7 +1733,15 @@ int DriverRegHTCVive( SurviveContext * ctx )
 	if( sv->udev[USB_DEV_TRACKER0]  ) { survive_add_object( ctx, tr0 ); }
 	if( sv->udev[USB_DEV_W_WATCHMAN1]  ) { survive_add_object( ctx, ww0 ); }
 
-	survive_add_driver( ctx, sv, survive_vive_usb_poll, survive_vive_close, survive_vive_send_magic );
+	if( sv->udev[USB_DEV_HMD_IMU_LH] ||
+	    sv->udev[USB_DEV_WATCHMAN1]  ||
+	    sv->udev[USB_DEV_WATCHMAN2]  ||
+	    sv->udev[USB_DEV_TRACKER0]   ||
+	    sv->udev[USB_DEV_W_WATCHMAN1] ) {
+	  survive_add_driver( ctx, sv, survive_vive_usb_poll, survive_vive_close, survive_vive_send_magic );
+	} else {
+	  fprintf(stderr, "No USB devices detected\n");
+	}
 
 	return 0;
 fail_gracefully: