Merge pull request #48 from mwturvey/ToriPoserHomeStretch

Tori poser home stretch

6 files changed, 457 insertions, 73 deletions

diff --git a/include/libsurvive/poser.h b/include/libsurvive/poser.h
index cf11e0c..497b009 100644
--- a/include/libsurvive/poser.h
+++ b/include/libsurvive/poser.h
@@ -32,7 +32,8 @@ typedef struct
 {
 	PoserType pt;
 	int sensor_id;
-	int acode;			//OOTX Code associated with this sweep. base_station = acode >> 2;  axis = acode & 1;
+	int acode;			//OOTX Code associated with this sweep. bit 1 indicates vertical(1) or horizontal(0) sweep
+	int lh;             //Lighthouse making this sweep
 	uint32_t timecode;  //In object-local ticks.
 	FLT length;			//In seconds
 	FLT angle;			//In radians from center of lighthouse.
diff --git a/src/poser_octavioradii.c b/src/poser_octavioradii.c
index 3893085..0d8674c 100644
--- a/src/poser_octavioradii.c
+++ b/src/poser_octavioradii.c
@@ -4,8 +4,12 @@
 
 typedef struct
 {
-	int something;
-	//Stuff
+#define OLD_ANGLES_BUFF_LEN 3
+	FLT oldAngles[SENSORS_PER_OBJECT][2][NUM_LIGHTHOUSES][OLD_ANGLES_BUFF_LEN]; // sensor, sweep axis, lighthouse, instance
+	int angleIndex[NUM_LIGHTHOUSES][2]; // index into circular buffer ahead. separate index for each axis.
+	int lastAxis[NUM_LIGHTHOUSES];
+
+	int hitCount[SENSORS_PER_OBJECT][NUM_LIGHTHOUSES][2];
 } OctavioRadiiData;
 
 #include <stdio.h>
@@ -45,6 +49,7 @@ typedef struct
 	Point normal; // unit vector indicating the normal for the sensor
 	double theta; // "horizontal" angular measurement from lighthouse radians
 	double phi; // "vertical" angular measurement from lighthouse in radians.
+	int id;
 } TrackedSensor;
 
 typedef struct
@@ -382,10 +387,10 @@ static RefineEstimateUsingGradientDescentRadii(FLT *estimateOut, SensorAngles *a
 
 
 	}
-	printf("\ni=%d\n", i);
+	printf(" i=%d ", i);
 }
 
-void SolveForLighthouseRadii(Point *objPosition, FLT *objOrientation, TrackedObject *obj)
+static void SolveForLighthouseRadii(Point *objPosition, FLT *objOrientation, TrackedObject *obj)
 {
 	FLT estimate[MAX_RADII];
 
@@ -394,6 +399,12 @@ void SolveForLighthouseRadii(Point *objPosition, FLT *objOrientation, TrackedObj
 		estimate[i] = 2.38;
 	}
 
+
+	//for (int i=0; i < obj->numSensors; i++)
+	//{
+	//	printf("%d, ", obj->sensor[i].id);
+	//}
+
 	SensorAngles angles[MAX_RADII];
 	PointPair pairs[MAX_POINT_PAIRS];
 
@@ -423,22 +434,119 @@ void SolveForLighthouseRadii(Point *objPosition, FLT *objOrientation, TrackedObj
 
 	// we should now have an estimate of the radii.
 
-	for (int i = 0; i < obj->numSensors; i++)
+	//for (int i = 0; i < obj->numSensors; i++)
+	for (int i = 0; i < 1; i++)
 	{
 		printf("radius[%d]: %f\n", i, estimate[i]);
 	}
+
 	// (FLT *estimateOut, SensorAngles *angles, FLT *initialEstimate, size_t numRadii, PointPair *pairs, size_t numPairs, FILE *logFile)
 
 	return;
 }
 
+static void QuickPose(SurviveObject *so)
+{
+	OctavioRadiiData * td = so->PoserData;
+
+
+	//for (int i=0; i < so->nr_locations; i++)
+	//{
+	//	FLT x0=td->oldAngles[i][0][0][td->angleIndex[0][0]];
+	//	FLT y0=td->oldAngles[i][1][0][td->angleIndex[0][1]];
+	//	//FLT x1=td->oldAngles[i][0][1][td->angleIndex[1][0]];
+	//	//FLT y1=td->oldAngles[i][1][1][td->angleIndex[1][1]];
+	//	//printf("%2d: %8.8f, %8.8f   %8.8f, %8.8f   \n", 
+	//	//	i,
+	//	//	x0,
+	//	//	y0,
+	//	//	x1,
+	//	//	y1
+	//	//	);
+	//	printf("%2d: %8.8f, %8.8f   \n", 
+	//		i,
+	//		x0,
+	//		y0
+	//		);
+	//}
+	//printf("\n");
+
+	TrackedObject *to;
+
+	to = malloc(sizeof(TrackedObject) + (SENSORS_PER_OBJECT * sizeof(TrackedSensor)));
+
+	{
+		int sensorCount = 0;
+
+		for (int i = 0; i < so->nr_locations; i++)
+		{
+			int lh = 0;
+			//printf("%d[%d], ",i,td->hitCount[i][lh][0]);
+
+			int angleIndex0 = (td->angleIndex[lh][0] + 1 + OLD_ANGLES_BUFF_LEN) % OLD_ANGLES_BUFF_LEN;
+			int angleIndex1 = (td->angleIndex[lh][1] + 1 + OLD_ANGLES_BUFF_LEN) % OLD_ANGLES_BUFF_LEN;
+			if ((td->oldAngles[i][0][lh][angleIndex0] != 0 && td->oldAngles[i][1][lh][angleIndex1] != 0))
+				
+				
+			{
+				if (td->hitCount[i][lh][0] > 10 && td->hitCount[i][lh][1] > 10)
+				{
+					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] };
+
+					to->sensor[sensorCount].normal.x = norm[0];
+					to->sensor[sensorCount].normal.y = norm[1];
+					to->sensor[sensorCount].normal.z = norm[2];
+					to->sensor[sensorCount].point.x = point[0];
+					to->sensor[sensorCount].point.y = point[1];
+					to->sensor[sensorCount].point.z = point[2];
+					to->sensor[sensorCount].theta = td->oldAngles[i][0][lh][angleIndex0] + LINMATHPI / 2; // lighthouse 0, angle 0 (horizontal)
+					to->sensor[sensorCount].phi = td->oldAngles[i][1][lh][angleIndex1] + LINMATHPI / 2; // lighthouse 0, angle 1 (vertical)
+					to->sensor[sensorCount].id=i;
+
+
+
+					//printf("%2d: %8.8f, %8.8f   \n", 
+					//	i,
+					//	to->sensor[sensorCount].theta,
+					//	to->sensor[sensorCount].phi
+					//	);
+
+					sensorCount++;
+				}
+			}
+		}
+		//printf("\n");
+		to->numSensors = sensorCount;
+
+		if (sensorCount > 4)
+		{
+			FLT pos[3];
+			FLT orient[4];
+			SolveForLighthouseRadii(pos, orient, to);
+		}
+
+		
+	}
+
+
+	free(to);
+
+}
+
+
 int PoserOctavioRadii( SurviveObject * so, PoserData * pd )
 {
 	PoserType pt = pd->pt;
 	SurviveContext * ctx = so->ctx;
 	OctavioRadiiData * dd = so->PoserData;
 
-	if( !dd ) so->PoserData = dd = malloc( sizeof(OctavioRadiiData) );
+	if( !dd )
+	{
+		so->PoserData = dd = malloc( sizeof(OctavioRadiiData) );
+		memset(dd, 0, sizeof(OctavioRadiiData));
+	}
+
 
 	switch( pt )
 	{
@@ -451,9 +559,81 @@ int PoserOctavioRadii( SurviveObject * so, PoserData * pd )
 	case POSERDATA_LIGHT:
 	{
 		PoserDataLight * l = (PoserDataLight*)pd;
+
+		if (l->lh >= NUM_LIGHTHOUSES || l->lh < 0)
+		{
+			// should never happen.  Famous last words...
+			break;
+		}
+		int axis = l->acode & 0x1;
+
+		//printf("%d ", l->sensor_id);
+
+
 		//printf( "LIG:%s %d @ %f rad, %f s (AC %d) (TC %d)\n", so->codename, l->sensor_id, l->angle, l->length, l->acode, l->timecode );
-		break;
-	}
+		if ((dd->lastAxis[l->lh] != (l->acode & 0x1)) )
+		{
+			int lastAxis = dd->lastAxis[l->lh];
+	//printf("\n");
+			//if (0 == l->lh)
+			//	printf("or[%d,%d] ", l->lh,lastAxis);
+
+			for (int i=0; i < SENSORS_PER_OBJECT; i++)
+			{
+	//FLT oldAngles[SENSORS_PER_OBJECT][2][NUM_LIGHTHOUSES][OLD_ANGLES_BUFF_LEN]; // sensor, sweep axis, lighthouse, instance
+				int index = dd->angleIndex[l->lh][axis];
+				if (dd->oldAngles[i][axis][l->lh][dd->angleIndex[l->lh][axis]] != 0)
+				{
+					//if (0 == l->lh)
+					//	printf("%d ", i);
+
+					dd->hitCount[i][l->lh][axis]++;
+				}
+				else
+				{
+					dd->hitCount[i][l->lh][axis] *= 0.5;
+				}
+			}
+			//if (0 == l->lh)
+			//	printf("\n");
+			//int foo = l->acode & 0x1;
+			//printf("%d", foo);
+
+
+			//if (axis)
+			{
+				if (0 == l->lh && axis) // only once per full cycle...
+				{
+					static unsigned int counter = 1;
+
+					counter++;
+
+					// let's just do this occasionally for now...
+					if (counter % 4 == 0)
+						QuickPose(so);
+				}
+				// axis changed, time to increment the circular buffer index.
+
+
+				dd->angleIndex[l->lh][axis]++;
+				dd->angleIndex[l->lh][axis] = dd->angleIndex[l->lh][axis] % OLD_ANGLES_BUFF_LEN;
+
+				// and clear out the data.
+				for (int i=0; i < SENSORS_PER_OBJECT; i++)
+				{
+					dd->oldAngles[i][axis][l->lh][dd->angleIndex[l->lh][axis]] = 0;
+				}
+
+			}
+			dd->lastAxis[l->lh] = axis;
+		}
+
+		//if (0 == l->lh)
+		//	printf("(%d) ", l->sensor_id);
+
+	//FLT oldAngles[SENSORS_PER_OBJECT][2][NUM_LIGHTHOUSES][OLD_ANGLES_BUFF_LEN]; // sensor, sweep axis, lighthouse, instance
+		dd->oldAngles[l->sensor_id][axis][l->lh][dd->angleIndex[l->lh][axis]] = l->angle;
+		break;	}
 	case POSERDATA_FULL_SCENE:
 	{
 		TrackedObject *to;
@@ -482,6 +662,7 @@ int PoserOctavioRadii( SurviveObject * so, PoserData * pd )
 					to->sensor[sensorCount].point.z = so->sensor_locations[i * 3 + 2];
 					to->sensor[sensorCount].theta = fs->angles[i][0][0] + LINMATHPI / 2; // lighthouse 0, angle 0 (horizontal)
 					to->sensor[sensorCount].phi = fs->angles[i][0][1] + LINMATHPI / 2; // lighthosue 0, angle 1 (vertical)
+					to->sensor[sensorCount].id=i;
 					sensorCount++;
 				}
 			}
@@ -509,6 +690,7 @@ int PoserOctavioRadii( SurviveObject * so, PoserData * pd )
 					to->sensor[sensorCount].point.z = so->sensor_locations[i * 3 + 2];
 					to->sensor[sensorCount].theta = fs->angles[i][lh][0] + LINMATHPI / 2; // lighthouse 0, angle 0 (horizontal)
 					to->sensor[sensorCount].phi = fs->angles[i][lh][1] + LINMATHPI / 2; // lighthosue 0, angle 1 (vertical)
+					to->sensor[sensorCount].id=i;
 					sensorCount++;
 				}
 			}
diff --git a/src/poser_turveytori.c b/src/poser_turveytori.c
index 5c3350b..76dd7fe 100644
--- a/src/poser_turveytori.c
+++ b/src/poser_turveytori.c
@@ -71,8 +71,14 @@ static const float DefaultPointsPerOuterDiameter = 60;
 
 typedef struct
 {
+	FLT down[3];  // populated by the IMU for posing
 	int something;
 	//Stuff
+
+#define OLD_ANGLES_BUFF_LEN 3
+	FLT oldAngles[SENSORS_PER_OBJECT][2][NUM_LIGHTHOUSES][OLD_ANGLES_BUFF_LEN]; // sensor, sweep axis, lighthouse, instance
+	int angleIndex[NUM_LIGHTHOUSES][2]; // index into circular buffer ahead. separate index for each axis.
+	int lastAxis[NUM_LIGHTHOUSES];
 } ToriData;
 
 
@@ -525,7 +531,7 @@ static Point RefineEstimateUsingModifiedGradientDescent1(Point initialEstimate,
 
 
 	}
-	printf("\ni=%d\n", i);
+	printf(" i=%d ", i);
 
 	return lastPoint;
 }
@@ -844,12 +850,12 @@ void getNormalizedAndScaledRotationGradient(FLT *vectorToScale, FLT desiredMagni
 
 static void WhereIsTheTrackedObjectAxisAngle(FLT *rotation, Point lhPoint)
 {
-	FLT reverseRotation[4] = {rotation[0], rotation[1], rotation[2], -rotation[3]};
+	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]);
 
-	printf("The tracked object is at location (%f, %f, %f)\n", objPoint[0], objPoint[1], objPoint[2]);
+	printf("{%8.8f, %8.8f, %8.8f}  ", objPoint[0], objPoint[1], objPoint[2]);
 }
 
 static void RefineRotationEstimateAxisAngle(FLT *rotOut, Point lhPoint, FLT *initialEstimate, TrackedObject *obj)
@@ -872,7 +878,7 @@ static void RefineRotationEstimateAxisAngle(FLT *rotOut, Point lhPoint, FLT *ini
 	//   in fact, it probably could probably be 1 without any issue.  The main place where g is decremented
 	//   is in the block below when we've made a jump that results in a worse fitness than we're starting at.
 	//   In those cases, we don't take the jump, and instead lower the value of g and try again.
-	for (FLT g = 0.1; g > 0.000000001; g *= 0.99)
+	for (FLT g = 0.1; g > 0.000000001 || i > 10000; g *= 0.99)
 	{
 		i++;
 		FLT point1[4];
@@ -946,7 +952,7 @@ static void RefineRotationEstimateAxisAngle(FLT *rotOut, Point lhPoint, FLT *ini
 
 
 	}
-	printf("\nRi=%d\n", i);
+	printf(" Ri=%d ", i);
 }
 static void WhereIsTheTrackedObjectQuaternion(FLT *rotation, Point lhPoint)
 {
@@ -955,7 +961,7 @@ static void WhereIsTheTrackedObjectQuaternion(FLT *rotation, Point lhPoint)
 	
 	//rotatearoundaxis(objPoint, objPoint, reverseRotation, reverseRotation[3]);
 	quatrotatevector(objPoint, rotation, objPoint);
-	printf("The tracked object is at location (%f, %f, %f)\n", objPoint[0], objPoint[1], objPoint[2]);
+	printf("(%f, %f, %f)\n", objPoint[0], objPoint[1], objPoint[2]);
 }
 
 
@@ -1055,7 +1061,7 @@ static void RefineRotationEstimateQuaternion(FLT *rotOut, Point lhPoint, FLT *in
 
 
 	}
-	printf("\nRi=%d  Fitness=%3f\n", i, lastMatchFitness);
+	printf("Ri=%3d  Fitness=%3f ", i, lastMatchFitness);
 }
 
 
@@ -1087,8 +1093,23 @@ void SolveForRotation(FLT rotOut[4], TrackedObject *obj, Point lh)
 }
 
 
-Point SolveForLighthouse(TrackedObject *obj, char doLogOutput)
+static Point SolveForLighthouse(TrackedObject *obj, char doLogOutput)
 {
+	//printf("Solving for Lighthouse\n");
+
+	//printf("obj->numSensors = %d;\n", obj->numSensors);
+
+	//for (int i=0; i < obj->numSensors; i++)
+	//{
+	//	printf("obj->sensor[%d].normal.x = %f;\n", i, obj->sensor[i].normal.x);
+	//	printf("obj->sensor[%d].normal.y = %f;\n", i, obj->sensor[i].normal.y);
+	//	printf("obj->sensor[%d].normal.z = %f;\n", i, obj->sensor[i].normal.z);
+	//	printf("obj->sensor[%d].point.x = %f;\n", i, obj->sensor[i].point.x);
+	//	printf("obj->sensor[%d].point.y = %f;\n", i, obj->sensor[i].point.y);
+	//	printf("obj->sensor[%d].point.z = %f;\n", i, obj->sensor[i].point.z);
+	//	printf("obj->sensor[%d].phi = %f;\n", i, obj->sensor[i].phi);
+	//	printf("obj->sensor[%d].theta = %f;\n\n", i, obj->sensor[i].theta);
+	//}
 	PointsAndAngle pna[MAX_POINT_PAIRS];
 
 	volatile size_t sizeNeeded = sizeof(pna);
@@ -1167,13 +1188,17 @@ Point SolveForLighthouse(TrackedObject *obj, char doLogOutput)
 
 	FLT fitGd = getPointFitness(refinedEstimateGd, pna, pnaCount);
 
-	FLT distance = FLT_SQRT(SQUARED(refinedEstimateGd.x) + SQUARED(refinedEstimateGd.y) + SQUARED(refinedEstimateGd.z));
-	printf("(%4.4f, %4.4f, %4.4f)\n", refinedEstimateGd.x, refinedEstimateGd.y, refinedEstimateGd.z);
-	printf("Distance is %f,   Fitness is %f\n", distance, fitGd);
+	printf("(%4.4f, %4.4f, %4.4f) Dist: %8.8f Fit:%4f  ", refinedEstimateGd.x, refinedEstimateGd.y, refinedEstimateGd.z, distance, fitGd);
 
-	FLT rot[4];
-	SolveForRotation(rot, obj, refinedEstimateGd);
+	//if (fitGd > 5)
+	{
+		FLT distance = FLT_SQRT(SQUARED(refinedEstimateGd.x) + SQUARED(refinedEstimateGd.y) + SQUARED(refinedEstimateGd.z));
+		printf("(%4.4f, %4.4f, %4.4f) Dist: %8.8f Fit:%4f  ", refinedEstimateGd.x, refinedEstimateGd.y, refinedEstimateGd.z, distance, fitGd);
+		//printf("Distance is %f,   Fitness is %f\n", distance, fitGd);
 
+		FLT rot[4];
+		SolveForRotation(rot, obj, refinedEstimateGd);
+	}
 	if (logFile)
 	{
 		updateHeader(logFile);
@@ -1190,70 +1215,215 @@ Point SolveForLighthouse(TrackedObject *obj, char doLogOutput)
 
 
 
+static void QuickPose(SurviveObject *so)
+{
+	ToriData * td = so->PoserData;
+
+
+	//for (int i=0; i < so->nr_locations; i++)
+	//{
+	//	FLT x0=td->oldAngles[i][0][0][td->angleIndex[0][0]];
+	//	FLT y0=td->oldAngles[i][1][0][td->angleIndex[0][1]];
+	//	FLT x1=td->oldAngles[i][0][1][td->angleIndex[1][0]];
+	//	FLT y1=td->oldAngles[i][1][1][td->angleIndex[1][1]];
+	//	//printf("%2d: %8.8f, %8.8f   %8.8f, %8.8f   \n", 
+	//	//	i,
+	//	//	x0,
+	//	//	y0,
+	//	//	x1,
+	//	//	y1
+	//	//	);
+	//	printf("%2d: %8.8f, %8.8f   \n", 
+	//		i,
+	//		x0,
+	//		y0
+	//		);
+	//}
+	//printf("\n");
+
+	TrackedObject *to;
+
+	to = malloc(sizeof(TrackedObject) + (SENSORS_PER_OBJECT * sizeof(TrackedSensor)));
+
+	{
+		int sensorCount = 0;
+
+		for (int i = 0; i < so->nr_locations; i++)
+		{
+			int lh = 0;
+			int angleIndex0 = (td->angleIndex[lh][0] + 1 + OLD_ANGLES_BUFF_LEN) % OLD_ANGLES_BUFF_LEN;
+			int angleIndex1 = (td->angleIndex[lh][1] + 1 + OLD_ANGLES_BUFF_LEN) % OLD_ANGLES_BUFF_LEN;
+			if (td->oldAngles[i][0][lh][angleIndex0] != 0 && td->oldAngles[i][1][lh][angleIndex1] != 0) 
+			{
+				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] };
+
+				to->sensor[sensorCount].normal.x = norm[0];
+				to->sensor[sensorCount].normal.y = norm[1];
+				to->sensor[sensorCount].normal.z = norm[2];
+				to->sensor[sensorCount].point.x = point[0];
+				to->sensor[sensorCount].point.y = point[1];
+				to->sensor[sensorCount].point.z = point[2];
+				to->sensor[sensorCount].theta = td->oldAngles[i][0][lh][angleIndex0] + LINMATHPI / 2; // lighthouse 0, angle 0 (horizontal)
+				to->sensor[sensorCount].phi = td->oldAngles[i][1][lh][angleIndex1] + LINMATHPI / 2; // lighthouse 0, angle 1 (vertical)
+
+				//printf("%2d: %8.8f, %8.8f   \n", 
+				//	i,
+				//	to->sensor[sensorCount].theta,
+				//	to->sensor[sensorCount].phi
+				//	);
+
+				sensorCount++;
+			}
+		}
+		to->numSensors = sensorCount;
+
+		if (sensorCount > 4)
+		{
+			SolveForLighthouse(to, 0);
+			printf("!\n");
+		}
+
+		
+	}
+
+
+	free(to);
 
+}
 
 
 
-int PoserTurveyTori( SurviveObject * so, PoserData * pd )
+int PoserTurveyTori( SurviveObject * so, PoserData * poserData )
 {
-	PoserType pt = pd->pt;
+	PoserType pt = poserData->pt;
 	SurviveContext * ctx = so->ctx;
-	ToriData * dd = so->PoserData;
+	ToriData * td = so->PoserData;
 
-	if (!dd)
+
+	if (!td)
 	{
-		so->PoserData = dd = malloc(sizeof(ToriData));
-		memset(dd, 0, sizeof(ToriData));
+		so->PoserData = td = malloc(sizeof(ToriData));
+		memset(td, 0, sizeof(ToriData));
 	}
 
 	switch( pt )
 	{
 	case POSERDATA_IMU:
 	{
-		PoserDataIMU * imu = (PoserDataIMU*)pd;
-		//printf( "IMU:%s (%f %f %f) (%f %f %f)\n", so->codename, imu->accel[0], imu->accel[1], imu->accel[2], imu->gyro[0], imu->gyro[1], imu->gyro[2] );
+		PoserDataIMU * tmpImu = (PoserDataIMU*)poserData;
+		
+		// store off data we can use for figuring out what direction is down when doing calibration.
+		//TODO: looks like the data mask isn't getting set correctly.
+		//if (tmpImu->datamask & 1) // accelerometer data is present
+		{
+			td->down[0] = td->down[0] * 0.98 + 0.02 * tmpImu->accel[0];
+			td->down[1] = td->down[1] * 0.98 + 0.02 * tmpImu->accel[1];
+			td->down[2] = td->down[2] * 0.98 + 0.02 * tmpImu->accel[2];
+		}
+		//printf( "IMU:%s (%f %f %f) (%f %f %f)\n", so->codename, tmpImu->accel[0], tmpImu->accel[1], tmpImu->accel[2], tmpImu->gyro[0], tmpImu->gyro[1], tmpImu->gyro[2] );
+		//printf( "Down: (%f %f %f)\n", td->down[0], td->down[1], td->down[2] );
 		break;
 	}
 	case POSERDATA_LIGHT:
 	{
-		PoserDataLight * l = (PoserDataLight*)pd;
+		PoserDataLight * l = (PoserDataLight*)poserData;
+
+		if (l->lh >= NUM_LIGHTHOUSES || l->lh < 0)
+		{
+			// should never happen.  Famous last words...
+			break;
+		}
+		int axis = l->acode & 0x1;
 		//printf( "LIG:%s %d @ %f rad, %f s (AC %d) (TC %d)\n", so->codename, l->sensor_id, l->angle, l->length, l->acode, l->timecode );
+		if ((td->lastAxis[l->lh] != (l->acode & 0x1)) )
+		{
+			int foo = l->acode & 0x1;
+			//printf("%d", foo);
+
+
+			//if (axis)
+			{
+				if (0 == l->lh && axis) // only once per full cycle...
+				{
+					static unsigned int counter = 1;
+
+					counter++;
+
+					// let's just do this occasionally for now...
+					if (counter % 1 == 0)
+						QuickPose(so);
+				}
+				// axis changed, time to increment the circular buffer index.
+				td->angleIndex[l->lh][axis]++;
+				td->angleIndex[l->lh][axis] = td->angleIndex[l->lh][axis] % OLD_ANGLES_BUFF_LEN;
+
+				// and clear out the data.
+				for (int i=0; i < SENSORS_PER_OBJECT; i++)
+				{
+					td->oldAngles[i][axis][l->lh][td->angleIndex[l->lh][axis]] = 0;
+				}
+
+			}
+			td->lastAxis[l->lh] = axis;
+		}
+
+		td->oldAngles[l->sensor_id][axis][l->lh][td->angleIndex[l->lh][axis]] = l->angle;
 		break;
 	}
 	case POSERDATA_FULL_SCENE:
 	{
 		TrackedObject *to;
 
-		PoserDataFullScene * fs = (PoserDataFullScene*)pd;
+		PoserDataFullScene * fs = (PoserDataFullScene*)poserData;
 
 		to = malloc(sizeof(TrackedObject) + (SENSORS_PER_OBJECT * sizeof(TrackedSensor)));
 
-		//FLT  lengths[SENSORS_PER_OBJECT][NUM_LIGHTHOUSES][2];
-		//FLT  angles[SENSORS_PER_OBJECT][NUM_LIGHTHOUSES][2];  //2 Axes  (Angles in LH space)
-		//FLT  synctimes[SENSORS_PER_OBJECT][NUM_LIGHTHOUSES];
+		// if we rotate the internal reference frame of of the tracked object from having -z being arbitrary
+		// to being the down direction as defined by the accelerometer, then when we have come up
+		// with world coordinate system, it will have Z oriented correctly.
+
+		// let's get the quaternion that represents this rotation.
+		FLT downQuat[4];
+		FLT negZ[3] = { 0,0,-1 };
+		//quatfrom2vectors(downQuat, negZ, td->down);
+		quatfrom2vectors(downQuat, td->down, negZ);
 
-		//to->numSensors = so->nr_locations;
 		{
 			int sensorCount = 0;
 
+
 			for (int i = 0; i < so->nr_locations; i++)
 			{
 				if (fs->lengths[i][0][0] != -1 && fs->lengths[i][0][1] != -1) //lh 0
 				{
-					to->sensor[sensorCount].normal.x = so->sensor_normals[i * 3 + 0];
-					to->sensor[sensorCount].normal.y = so->sensor_normals[i * 3 + 1];
-					to->sensor[sensorCount].normal.z = so->sensor_normals[i * 3 + 2];
-					to->sensor[sensorCount].point.x = so->sensor_locations[i * 3 + 0];
-					to->sensor[sensorCount].point.y = so->sensor_locations[i * 3 + 1];
-					to->sensor[sensorCount].point.z = so->sensor_locations[i * 3 + 2];
+					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);
+
+					to->sensor[sensorCount].normal.x = norm[0];
+					to->sensor[sensorCount].normal.y = norm[1];
+					to->sensor[sensorCount].normal.z = norm[2];
+					to->sensor[sensorCount].point.x = point[0];
+					to->sensor[sensorCount].point.y = point[1];
+					to->sensor[sensorCount].point.z = point[2];
 					to->sensor[sensorCount].theta = fs->angles[i][0][0] + LINMATHPI / 2; // lighthouse 0, angle 0 (horizontal)
-					to->sensor[sensorCount].phi = fs->angles[i][0][1] + LINMATHPI / 2; // lighthosue 0, angle 1 (vertical)
+					to->sensor[sensorCount].phi = fs->angles[i][0][1] + LINMATHPI / 2; // lighthouse 0, angle 1 (vertical)
+
+					//printf("%2d: %8.8f, %8.8f   \n", 
+					//	i,
+					//	to->sensor[sensorCount].theta,
+					//	to->sensor[sensorCount].phi
+					//	);
+
 					sensorCount++;
 				}
 			}
-
 			to->numSensors = sensorCount;
 
+
 			SolveForLighthouse(to, 0);
 		}
 		{
@@ -1264,12 +1434,18 @@ int PoserTurveyTori( SurviveObject * so, PoserData * pd )
 			{
 				if (fs->lengths[i][lh][0] != -1 && fs->lengths[i][lh][1] != -1) 
 				{
-					to->sensor[sensorCount].normal.x = so->sensor_normals[i * 3 + 0];
-					to->sensor[sensorCount].normal.y = so->sensor_normals[i * 3 + 1];
-					to->sensor[sensorCount].normal.z = so->sensor_normals[i * 3 + 2];
-					to->sensor[sensorCount].point.x = so->sensor_locations[i * 3 + 0];
-					to->sensor[sensorCount].point.y = so->sensor_locations[i * 3 + 1];
-					to->sensor[sensorCount].point.z = so->sensor_locations[i * 3 + 2];
+					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);
+
+					to->sensor[sensorCount].normal.x = norm[0];
+					to->sensor[sensorCount].normal.y = norm[1];
+					to->sensor[sensorCount].normal.z = norm[2];
+					to->sensor[sensorCount].point.x = point[0];
+					to->sensor[sensorCount].point.y = point[1];
+					to->sensor[sensorCount].point.z = point[2];
 					to->sensor[sensorCount].theta = fs->angles[i][lh][0] + LINMATHPI / 2; // lighthouse 0, angle 0 (horizontal)
 					to->sensor[sensorCount].phi = fs->angles[i][lh][1] + LINMATHPI / 2; // lighthosue 0, angle 1 (vertical)
 					sensorCount++;
@@ -1280,13 +1456,15 @@ int PoserTurveyTori( SurviveObject * so, PoserData * pd )
 
 			SolveForLighthouse(to, 0);
 		}
+
+		free(to);
 		//printf( "Full scene data.\n" );
 		break;
 	}
 	case POSERDATA_DISASSOCIATE:
 	{
-		free( dd );
-		so->PoserData = 0;
+		free( so->PoserData );
+		so->PoserData = NULL;
 		//printf( "Need to disassociate.\n" );
 		break;
 	}
diff --git a/src/survive_data.c b/src/survive_data.c
index 9447104..3f16c7c 100644
--- a/src/survive_data.c
+++ b/src/survive_data.c
@@ -79,10 +79,40 @@ void handle_lightcap2_process_sweep_data(SurviveObject *so)
 			}
 		}
 
+		int allZero = 1;
+		for (int q=0; q< 32; q++)
+			if (lcd->sweep.sweep_len[q] != 0)
+				allZero=0;
+		//if (!allZero)
+		//	printf("a[%d]l[%d] ", lcd->per_sweep.activeAcode & 5,  lcd->per_sweep.activeLighthouse);
 		for (int i = 0; i < SENSORS_PER_OBJECT; i++)
 		{
+			{
+				static int counts[SENSORS_PER_OBJECT][2] = {0};
+
+				if (lcd->per_sweep.activeLighthouse == 0 && !allZero)
+				{
+					if (lcd->sweep.sweep_len[i] != 0)
+					{
+						//printf("%d  ", i);
+						//counts[i][lcd->per_sweep.activeAcode & 1] ++;
+					}
+					else
+					{
+						counts[i][lcd->per_sweep.activeAcode & 1] =0;
+					}
+
+					//if (counts[i][0] > 10 && counts[i][1] > 10)
+					//{
+						//printf("%d(%d,%d), ", i, counts[i][0], counts[i][1]);
+					//}
+				}
+			}
+
+
 			if (lcd->sweep.sweep_len[i] != 0) // if the sensor was hit, process it
 			{
+
 				int offset_from = lcd->sweep.sweep_time[i] - lcd->per_sweep.activeSweepStartTime + lcd->sweep.sweep_len[i] / 2;
 
 				if (offset_from < 380000 && offset_from > 70000)
@@ -94,6 +124,9 @@ void handle_lightcap2_process_sweep_data(SurviveObject *so)
 				}
 			}
 		}
+		//if (!allZero)
+		//	printf("   ..:..\n");
+
 	}
 	// clear out sweep data (could probably limit this to only after a "first" sync.  
 	// this is slightly more robust, so doing it here for now.
@@ -108,6 +141,7 @@ void handle_lightcap2_sync(SurviveObject * so, LightcapElement * le )
 	//static unsigned int recent_sync_count = -1;
 	//static unsigned int activeSweepStartTime;
 
+	int acode = handle_lightcap2_getAcodeFromSyncPulse(so, le->length);
 
 	// Process any sweep data we have
 	handle_lightcap2_process_sweep_data(so);
@@ -126,7 +160,6 @@ void handle_lightcap2_sync(SurviveObject * so, LightcapElement * le )
 		lcd->per_sweep.lh_pulse_len[lcd->per_sweep.current_lh] = le->length;
 		lcd->per_sweep.lh_start_time[lcd->per_sweep.current_lh] = le->timestamp;
 
-		int acode = handle_lightcap2_getAcodeFromSyncPulse(so, le->length);
 		if (!(acode >> 2 & 1)) // if the skip bit is not set
 		{
 			lcd->per_sweep.activeLighthouse = lcd->per_sweep.current_lh;
@@ -148,14 +181,18 @@ void handle_lightcap2_sync(SurviveObject * so, LightcapElement * le )
 		lcd->per_sweep.lh_pulse_len[lcd->per_sweep.current_lh] = le->length;
 		lcd->per_sweep.lh_start_time[lcd->per_sweep.current_lh] = le->timestamp;
 
-		int acode = handle_lightcap2_getAcodeFromSyncPulse(so, le->length);
-
 		if (!(acode >> 2 & 1)) // if the skip bit is not set
 		{
 			if (lcd->per_sweep.activeLighthouse != -1)
 			{
-				// hmm, it appears we got two non-skip pulses at the same time.  That should never happen
-				fprintf(stderr, "WARNING: Two non-skip pulses received on the same cycle!\n");
+				static int pulseWarningCount=0;
+
+				if (pulseWarningCount < 5)
+				{
+					pulseWarningCount++;
+					// hmm, it appears we got two non-skip pulses at the same time.  That should never happen
+					fprintf(stderr, "WARNING: Two non-skip pulses received on the same cycle!\n");
+				}
 			}
 			lcd->per_sweep.activeLighthouse = 1;
 			lcd->per_sweep.activeSweepStartTime = le->timestamp;
diff --git a/src/survive_process.c b/src/survive_process.c
index b58b344..3af2da9 100644
--- a/src/survive_process.c
+++ b/src/survive_process.c
@@ -57,6 +57,7 @@ void survive_default_angle_process( SurviveObject * so, int sensor_id, int acode
 			.timecode = timecode,
 			.length = length,
 			.angle = angle,
+			.lh = lh,
 		};
 		so->PoserFn( so, (PoserData *)&l );
 	}
diff --git a/src/survive_vive.c b/src/survive_vive.c
index a5c731d..757fa11 100755
--- a/src/survive_vive.c
+++ b/src/survive_vive.c
@@ -1105,6 +1105,7 @@ void survive_data_cb( SurviveUSBInterface * si )
 	}
 	case USB_IF_LIGHTHOUSE:
 	case USB_IF_W_WATCHMAN1:
+	case USB_IF_TRACKER0:
 	{
 		int i;
 		//printf( "%d -> ", size );
@@ -1156,22 +1157,6 @@ void survive_data_cb( SurviveUSBInterface * si )
 		}
 		break;
 	}
-	case USB_IF_TRACKER0:
-	{
-		SurviveObject * w = obj;
-		if( id == 32 )
-		{
-			// TODO: Looks like this will need to be handle_tracker, since
-			// it appears the interface is sufficiently different.
-			// More work needd to reverse engineer it.
-			//handle_wired_watchman( w, readdata, size);
-		}
-		else
-		{
-			SV_INFO( "Unknown tracker code %d\n", id );
-		}
-		break;
-	}
 	case USB_IF_LIGHTCAP:
 	{
 		int i;