From 4e3ac12016cc489c818709412acd293cac54cb78 Mon Sep 17 00:00:00 2001
From: Joshua Allen <axlecrusher@gmail.com>
Date: Fri, 17 Mar 2017 11:37:30 -0400
Subject: reusable function for decoding acode

---
 src/survive_data.c | 25 +++++++++++++++++--------
 1 file changed, 17 insertions(+), 8 deletions(-)

(limited to 'src/survive_data.c')

diff --git a/src/survive_data.c b/src/survive_data.c
index 4a2cfb6..d658e18 100644
--- a/src/survive_data.c
+++ b/src/survive_data.c
@@ -5,6 +5,19 @@
 #include <stdint.h>
 #include <string.h>
 
+
+int32_t decode_acode(uint32_t length, int32_t main_divisor) {
+	//+50 adds a small offset and seems to help always get it right. 
+	//Check the +50 in the future to see how well this works on a variety of hardware.
+
+	int32_t acode = (length+main_divisor+50)/(main_divisor*2);
+	if( acode & 1 ) return -1;
+
+	return (acode>>1) - 6;
+}
+
+}
+
 //This is the disambiguator function, for taking light timing and figuring out place-in-sweep for a given photodiode.
 void handle_lightcap( SurviveObject * so, LightcapElement * le )
 {
@@ -112,17 +125,13 @@ void handle_lightcap( SurviveObject * so, LightcapElement * le )
 
 		int32_t acode_array[2] =
 			{
-				(so->last_sync_length[0]+main_divisor+50)/(main_divisor*2),  //+50 adds a small offset and seems to help always get it right. 
-				(so->last_sync_length[1]+main_divisor+50)/(main_divisor*2),	//Check the +50 in the future to see how well this works on a variety of hardware.
+				decode_acode(so->last_sync_length[0],main_divisor),
+				decode_acode(so->last_sync_length[1],main_divisor)
 			};
 
 		//XXX: TODO: Capture error count here.
-		if( acode_array[0] & 1 ) return;
-		if( acode_array[1] & 1 ) return;
-
-		acode_array[0] = (acode_array[0]>>1) - 6;
-		acode_array[1] = (acode_array[1]>>1) - 6;
-
+		if( acode_array[0] < 0 ) return;
+		if( acode_array[1] < 0 ) return;
 
 		int acode = acode_array[0];
 
-- 
cgit v1.3.1


From 64e3b67aa0d6a970e1e1988881e77d04c4fb5b07 Mon Sep 17 00:00:00 2001
From: mwturvey <michael.w.turvey@intel.com>
Date: Fri, 17 Mar 2017 14:51:00 -0700
Subject: Remove extra debug print

---
 src/survive_data.c | 1 -
 1 file changed, 1 deletion(-)

(limited to 'src/survive_data.c')

diff --git a/src/survive_data.c b/src/survive_data.c
index b9099ac..5c5a5e9 100644
--- a/src/survive_data.c
+++ b/src/survive_data.c
@@ -39,7 +39,6 @@ void handle_lightcap( SurviveObject * so, LightcapElement * le )
 //		return; //if we don't know what lighthouse this is we don't care to do much else
 	}
 
-	printf("m sync %d %d %d %d\n", le->sensor_id, so->last_sync_time[ssn], le->timestamp, delta);
 
 	if( le->length > so->pulselength_min_sync ) //Pulse longer indicates a sync pulse.
 	{
-- 
cgit v1.3.1


From e1818d84e1ab0ae217d8035f8a12398f06508059 Mon Sep 17 00:00:00 2001
From: mwturvey <michael.w.turvey@intel.com>
Date: Fri, 17 Mar 2017 16:32:18 -0700
Subject: Alternate disambiguator and calibration updates

---
 src/survive_cal.c  | 50 ++++++++++++++++++++++++++--------
 src/survive_cal.h  |  6 +++--
 src/survive_data.c | 78 ++++++++++++++++++++++++++++++++++++++++++++++++++++++
 3 files changed, 121 insertions(+), 13 deletions(-)

(limited to 'src/survive_data.c')

diff --git a/src/survive_cal.c b/src/survive_cal.c
index 0eb9446..c36a48a 100755
--- a/src/survive_cal.c
+++ b/src/survive_cal.c
@@ -30,6 +30,11 @@ int mkdir(const char *);
 #define DRPTS_NEEDED_FOR_AVG ((int)(DRPTS*3/4))
 
 
+	//at stage 1, is when it branches to stage two or stage 7
+	//stage 2 checks for the presence of two watchmen and an HMD visible to both lighthouses.
+	//Stage 3 collects a bunch of data for statistical averageing
+	//stage 4 does the calculation for the poses (NOT DONE!)
+	//Stage 5 = System Calibrate.d
 
 
 static void handle_calibration( struct SurviveCalData *cd );
@@ -117,17 +122,36 @@ void survive_cal_install( struct SurviveContext * ctx )
 	cd->stage = 1;
 	cd->ctx = ctx;
 
-	cd->poseobjects[0] = survive_get_so_by_name( ctx, "HMD" );
-	cd->poseobjects[1] = survive_get_so_by_name( ctx, "WM0" );
-	cd->poseobjects[2] = survive_get_so_by_name( ctx, "WM1" );
+	cd->numPoseObjects = 0;
 
-	if( cd->poseobjects[0] == 0 || cd->poseobjects[1] == 0 || cd->poseobjects[2] == 0 )
+	for (int i=0; i < ctx->objs_ct; i++)
 	{
-		SV_ERROR( "Error: cannot find all devices needed for calibration." );
-		free( cd );
-		return;
+		// This would be a place where we could conditionally decide if we 
+		// want to include a certain device in the calibration routine.
+		if (1)
+		{
+			cd->poseobjects[i] = ctx->objs[i];
+			cd->numPoseObjects++;
+
+			SV_INFO("Calibration is using %s", cd->poseobjects[i]->codename);
+		}
 	}
 
+	// If we want to mandate that certain devices have been found
+
+
+
+	//cd->poseobjects[0] = survive_get_so_by_name( ctx, "HMD" );
+	//cd->poseobjects[1] = survive_get_so_by_name( ctx, "WM0" );
+	//cd->poseobjects[2] = survive_get_so_by_name( ctx, "WM1" );
+
+	//if( cd->poseobjects[0] == 0 || cd->poseobjects[1] == 0 || cd->poseobjects[2] == 0 )
+	//{
+	//	SV_ERROR( "Error: cannot find all devices needed for calibration." );
+	//	free( cd );
+	//	return;
+	//}
+
 //XXX TODO MWTourney, work on your code here.
 /*
 	if( !cd->hmd )
@@ -185,7 +209,9 @@ void survive_cal_light( struct SurviveObject * so, int sensor_id, int acode, int
 		if( sensor_id < 0 )
 		{
 			int lhid = -sensor_id-1;
-			if( lhid < NUM_LIGHTHOUSES && so->codename[0] == 'H' )
+			// Take the OOTX data from the first device. 
+			// TODO: Improve this so we'll watch all devices looking for OOTX data.  Why limit ourselves to just one?
+			if( lhid < NUM_LIGHTHOUSES && so == cd->poseobjects[0]  ) 
 			{
 				uint8_t dbit = (acode & 2)>>1;
 				ootx_pump_bit( &cd->ootx_decoders[lhid], dbit );
@@ -193,7 +219,9 @@ 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;  //If all lighthouses have their OOTX set, move on.
+			//if( i == NUM_LIGHTHOUSES ) cd->stage = 2;  //If all lighthouses have their OOTX set, move on.         <------- Revert This!!!!! 
+			if( i == 1 ) 
+				cd->stage = 2;  //If all lighthouses have their OOTX set, move on.
 		}
 		break;
 	}
@@ -498,11 +526,11 @@ static void handle_calibration( struct SurviveCalData *cd )
 	int obj;
 
 	//Poses of lighthouses relative to objects.
-	SurvivePose  objphl[POSE_OBJECTS][NUM_LIGHTHOUSES];
+	SurvivePose  objphl[MAX_POSE_OBJECTS][NUM_LIGHTHOUSES];
 
 	FILE * fobjp = fopen( "calinfo/objposes.csv", "w" );
 
-	for( obj = 0; obj < POSE_OBJECTS; obj++ )
+	for( obj = 0; obj < cd->numPoseObjects; obj++ )
 	{
 		int i, j;
 		PoserDataFullScene fsd;
diff --git a/src/survive_cal.h b/src/survive_cal.h
index 3d62051..8c488bd 100644
--- a/src/survive_cal.h
+++ b/src/survive_cal.h
@@ -37,7 +37,7 @@ void survive_cal_angle( SurviveObject * so, int sensor_id, int acode, uint32_t t
 #define MIN_PTS_BEFORE_CAL 24
 #define DRPTS 128
 
-#define POSE_OBJECTS 3
+#define MAX_POSE_OBJECTS 10
 
 #define MAX_CAL_PT_DAT (MAX_SENSORS_TO_CAL*NUM_LIGHTHOUSES*2)
 struct SurviveCalData
@@ -64,7 +64,9 @@ struct SurviveCalData
 
 	int senid_of_checkpt; //This is a point on a watchman that can be used to check the lh solution.
 
-	SurviveObject * poseobjects[POSE_OBJECTS];
+	SurviveObject * poseobjects[MAX_POSE_OBJECTS];
+
+	size_t numPoseObjects;
 
 	PoserCB ConfigPoserFn;
 
diff --git a/src/survive_data.c b/src/survive_data.c
index 5c5a5e9..e6538d1 100644
--- a/src/survive_data.c
+++ b/src/survive_data.c
@@ -5,9 +5,87 @@
 #include <stdint.h>
 #include <string.h>
 
+int getAcodeFromSyncPulse(int pulseLen)
+{
+	if (pulseLen < 3125) return 0;
+	if (pulseLen < 3625) return 1;
+	if (pulseLen < 4125) return 2;
+	if (pulseLen < 4625) return 3;
+	if (pulseLen < 5125) return 4;
+	if (pulseLen < 5625) return 5;
+	if (pulseLen < 6125) return 6;
+	return 7;
+}
+void handle_lightcap2_sync(SurviveObject * so, LightcapElement * le )
+{
+	fprintf(stderr, "%d\n", le->length);
+
+	if (le->timestamp - so->recent_sync_time < 24000)
+	{
+		// I do believe we are lighthouse B		
+		so->last_sync_time[1] = so->recent_sync_time = le->timestamp;
+		so->last_sync_length[1] = le->length;
+		if (le->length < 4625) // max non-skip pulse + 1/4 the difference in time between pulses.  
+		{
+			so->sync_set_number = 1;
+			so->ctx->lightproc(so, -2, getAcodeFromSyncPulse(le->length), le->length, le->timestamp, le->length); // Don't think I got this quite right.
+		}
+	}
+	else
+	{
+		// looks like this is the first sync pulse.  Cool!
+		if (le->length < 4625) // max non-skip pulse + 1/4 the difference in time between pulses.  
+		{
+			so->sync_set_number = 1;
+			so->ctx->lightproc(so, -1, getAcodeFromSyncPulse(le->length), le->length, le->timestamp, le->length); // Don't think I got this quite right.
+		}		
+	}
+
+
+//			ctx->lightproc( so, -2, acode_array[1], delta2, so->last_sync_time[1], so->last_sync_length[1] );
+
+
+//	typedef void (*light_process_func)( SurviveObject * so, int sensor_id, int acode, int timeinsweep, uint32_t timecode, uint32_t length );
+
+}
+
+void handle_lightcap2_sweep(SurviveObject * so, LightcapElement * le )
+{
+
+}
+
+void handle_lightcap2( SurviveObject * so, LightcapElement * le )
+{
+	SurviveContext * ctx = so->ctx;
+
+	if( le->sensor_id > SENSORS_PER_OBJECT )
+	{
+		return;
+	}
+
+	if (le->length > 6750)
+	{
+		// Should never get a reading so high.  Odd.
+		return;
+	}
+	if (le->length >= 2750)
+	{
+		// Looks like a sync pulse, process it!
+		handle_lightcap2_sync(so, le);
+	}
+
+	// must be a sweep pulse, process it!
+	handle_lightcap2_sweep(so, le);
+
+}
+
+
 //This is the disambiguator function, for taking light timing and figuring out place-in-sweep for a given photodiode.
 void handle_lightcap( SurviveObject * so, LightcapElement * le )
 {
+	handle_lightcap2(so,le);
+	return;
+
 	SurviveContext * ctx = so->ctx;
 	//int32_t deltat = (uint32_t)le->timestamp - (uint32_t)so->last_master_time;
 
-- 
cgit v1.3.1


From 0f94e39d96fbc2744e61b85d2884250d0783eb11 Mon Sep 17 00:00:00 2001
From: cnlohr <lohr85@gmail.com>
Date: Sat, 18 Mar 2017 15:21:24 -0400
Subject: Add more possible debugging if turned on.

---
 src/survive_data.c | 22 ++++++++++++++++++----
 1 file changed, 18 insertions(+), 4 deletions(-)

(limited to 'src/survive_data.c')

diff --git a/src/survive_data.c b/src/survive_data.c
index 5c5a5e9..0789203 100644
--- a/src/survive_data.c
+++ b/src/survive_data.c
@@ -8,17 +8,31 @@
 //This is the disambiguator function, for taking light timing and figuring out place-in-sweep for a given photodiode.
 void handle_lightcap( SurviveObject * so, LightcapElement * le )
 {
-	SurviveContext * ctx = so->ctx;
+	SurviveContext * ctx = so->ctx;	
 	//int32_t deltat = (uint32_t)le->timestamp - (uint32_t)so->last_master_time;
 
-	//if( so->codename[0] != 'H' )
-
-
 	if( le->sensor_id > SENSORS_PER_OBJECT )
 	{
 		return;
 	}
 
+#if 0
+	if( so->codename[0] == 'H' )
+	{
+		static int lt;
+		static int last;
+		if( le->length > 1000 )
+		{
+			int dl = le->timestamp - lt;
+			lt = le->timestamp;
+			if( dl > 10000 || dl < -10000 )
+				printf( "+++%s %3d %5d %9d  ", so->codename, le->sensor_id, le->length, dl );
+			if( dl > 100000 ) printf(" \n" );
+		}
+		last=le->length;
+	}
+#endif
+
 	so->tsl = le->timestamp;
 	if( le->length < 20 ) return;  ///Assuming 20 is an okay value for here.
 
-- 
cgit v1.3.1


From d4e7bc4037f9e0b4af3f11940b53520eb9f82ff6 Mon Sep 17 00:00:00 2001
From: Joshua Allen <axlecrusher@gmail.com>
Date: Sat, 18 Mar 2017 20:25:06 -0400
Subject: fix compile

---
 src/survive_data.c | 2 --
 1 file changed, 2 deletions(-)

(limited to 'src/survive_data.c')

diff --git a/src/survive_data.c b/src/survive_data.c
index 16a22e9..ffeacff 100644
--- a/src/survive_data.c
+++ b/src/survive_data.c
@@ -16,8 +16,6 @@ int32_t decode_acode(uint32_t length, int32_t main_divisor) {
 	return (acode>>1) - 6;
 }
 
-}
-
 //This is the disambiguator function, for taking light timing and figuring out place-in-sweep for a given photodiode.
 void handle_lightcap( SurviveObject * so, LightcapElement * le )
 {
-- 
cgit v1.3.1


From 8253e3c3adbb54ebc3d3c8e02c017e900a83edb0 Mon Sep 17 00:00:00 2001
From: cnlohr <lohr85@gmail.com>
Date: Mon, 20 Mar 2017 01:42:32 -0400
Subject: log synctimes.

---
 Makefile                   |  6 +++++
 calibrate.c                |  2 +-
 include/libsurvive/poser.h |  1 +
 src/survive_cal.c          | 56 +++++++++++++++++++++++++++++++++++++++++++++-
 src/survive_cal.h          |  3 +++
 src/survive_data.c         | 10 ++++++++-
 src/survive_process.c      |  3 +++
 7 files changed, 78 insertions(+), 3 deletions(-)

(limited to 'src/survive_data.c')

diff --git a/Makefile b/Makefile
index a642877..1899c4c 100644
--- a/Makefile
+++ b/Makefile
@@ -32,6 +32,12 @@ LIBSURVIVE_CORE:=$(LIBSURVIVE_CORE)
 LIBSURVIVE_O:=$(POSERS) $(REDISTS) $(LIBSURVIVE_CORE)
 LIBSURVIVE_C:=$(LIBSURVIVE_O:.o=.c)
 
+
+#If you want to use HIDAPI on Linux.
+#CFLAGS:=$(CFLAGS) -DHIDAPI
+#REDISTS:=$(REDISTS) redist/hid-linux.o
+#LDFLAGS:=$(LDFLAGS) -ludev
+
 #Useful Preprocessor Directives:
 # -DUSE_DOUBLE = use double instead of float for most operations.
 # -DNOZLIB = use puff.c
diff --git a/calibrate.c b/calibrate.c
index 726a8cc..f27131a 100644
--- a/calibrate.c
+++ b/calibrate.c
@@ -56,7 +56,7 @@ void my_light_process( struct SurviveObject * so, int sensor_id, int acode, int
 //	if( timeinsweep < 0 ) return;
 	survive_default_light_process( so, sensor_id, acode, timeinsweep, timecode, length );
 	if( sensor_id < 0 ) return;
-	if( acode == -1 ) return;
+	if( acode < 0 ) return;
 //return;
 	int jumpoffset = sensor_id;
 	if( strcmp( so->codename, "WM0" ) == 0 ) jumpoffset += 32;
diff --git a/include/libsurvive/poser.h b/include/libsurvive/poser.h
index 98c926e..cf11e0c 100644
--- a/include/libsurvive/poser.h
+++ b/include/libsurvive/poser.h
@@ -45,6 +45,7 @@ typedef struct
 	//If "lengths[...]" < 0, means not a valid piece of sweep information.
 	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];
 
 	PoserDataIMU lastimu;
 } PoserDataFullScene;
diff --git a/src/survive_cal.c b/src/survive_cal.c
index 9dc3af2..04931cc 100755
--- a/src/survive_cal.c
+++ b/src/survive_cal.c
@@ -196,6 +196,21 @@ void survive_cal_light( struct SurviveObject * so, int sensor_id, int acode, int
 			if( i == NUM_LIGHTHOUSES ) cd->stage = 2;  //If all lighthouses have their OOTX set, move on.
 		}
 		break;
+	case 3: //Look for light sync lengths.
+	{
+		if( acode >= -2 ) break;
+		else if( acode < -4 ) break;
+		int lh = (-acode) - 3;
+
+		if( strcmp( so->codename, "WM0" ) == 0 )
+			sensor_id += 32;
+		if( strcmp( so->codename, "WM1" ) == 0 )
+			sensor_id += 64;
+
+		cd->all_sync_times[sensor_id][lh][cd->all_sync_counts[sensor_id][lh]++] = length;
+		break;
+	}
+
 	}
 	
 }
@@ -333,6 +348,8 @@ static void reset_calibration( struct SurviveCalData * cd )
 	cd->found_common = 0;
 	cd->times_found_common = 0;
 	cd->stage = 2;
+
+	memset( cd->all_sync_counts, 0, sizeof( cd->all_sync_counts ) );
 }
 
 static void handle_calibration( struct SurviveCalData *cd )
@@ -358,9 +375,45 @@ static void handle_calibration( struct SurviveCalData *cd )
 #else
 	mkdir( "calinfo", 0755 );
 #endif
+	int sen, axis, lh;
+	FLT temp_syncs[SENSORS_PER_OBJECT][NUM_LIGHTHOUSES];
+
+	//Just to get it out of the way early, we'll calculate the sync-pulse-lengths here.
+	FILE * sync_time_info = fopen( "calinfo/synctime.csv", "w" );
+
+	for( sen = 0; sen < MAX_SENSORS_TO_CAL; sen++ )
+	for( lh = 0; lh < NUM_LIGHTHOUSES; lh++ )
+	{
+		int count = cd->all_sync_counts[sen][lh];
+		int i;
+		double totaltime;
+
+		totaltime = 0;
+
+		if( count < 20 ) continue;
+		for( i = 0; i < count; i++ )
+		{
+			totaltime += cd->all_sync_times[sen][lh][i];
+		}
+		FLT avg = totaltime/count;
+
+		double stddev = 0.0;
+		for( i = 0; i < count; i++ )
+		{
+			stddev += (cd->all_sync_times[sen][lh][i] - avg)*(cd->all_sync_times[sen][lh][i] - avg);
+		}
+		stddev /= count;
+
+		fprintf( sync_time_info, "%d %d %f %d %f\n", sen, lh, totaltime/count, count, stddev );
+	}
+
+	fclose( sync_time_info );
+
+
+
+
 	FILE * hists = fopen( "calinfo/histograms.csv", "w" );
 	FILE * ptinfo = fopen( "calinfo/ptinfo.csv", "w" );
-	int sen, axis, lh;
 	for( sen = 0; sen < MAX_SENSORS_TO_CAL; sen++ )
 	for( lh = 0; lh < NUM_LIGHTHOUSES; lh++ )
 	for( axis = 0; axis < 2; axis++ )
@@ -526,6 +579,7 @@ static void handle_calibration( struct SurviveCalData *cd )
 			fsd.lengths[i][j][1] = cd->avglens[dataindex+1];
 			fsd.angles[i][j][0] = cd->avgsweeps[dataindex+0];
 			fsd.angles[i][j][1] = cd->avgsweeps[dataindex+1];
+			fsd.synctimes[i][j] = temp_syncs[i][j];
 		}
 
 		int r = cd->ConfigPoserFn( cd->poseobjects[obj], (PoserData*)&fsd );
diff --git a/src/survive_cal.h b/src/survive_cal.h
index 701dc60..c64b8f9 100644
--- a/src/survive_cal.h
+++ b/src/survive_cal.h
@@ -56,6 +56,9 @@ struct SurviveCalData
 	int8_t found_common;
 	int8_t times_found_common;
 
+	FLT all_sync_times[MAX_SENSORS_TO_CAL][NUM_LIGHTHOUSES][DRPTS];
+	int16_t all_sync_counts[MAX_SENSORS_TO_CAL][NUM_LIGHTHOUSES];
+
 	//For camfind (4+)
 	//Index is calculated with:      int dataindex = sen*(2*NUM_LIGHTHOUSES)+lh*2+axis;
 	FLT avgsweeps[MAX_CAL_PT_DAT];
diff --git a/src/survive_data.c b/src/survive_data.c
index ffeacff..8c5c646 100644
--- a/src/survive_data.c
+++ b/src/survive_data.c
@@ -69,7 +69,7 @@ void handle_lightcap( SurviveObject * so, LightcapElement * le )
 	{
 		int is_new_pulse = delta > so->pulselength_min_sync /*1500*/ + last_sync_length;
 
-		//printf("m sync %d %d %d %d\n", le->sensor_id, so->last_sync_time[ssn], le->timestamp, delta);
+
 
 		so->did_handle_ootx = 0;
 
@@ -114,6 +114,14 @@ void handle_lightcap( SurviveObject * so, LightcapElement * le )
 				}
 			}
 		}
+
+		//Extra tidbit for storing length-of-sync-pulses.
+		{
+			int32_t main_divisor = so->timebase_hz / 384000; //125 @ 48 MHz.
+			int base_station = is_new_pulse;
+			printf( "%s %d %d %d\n", so->codename, le->sensor_id, so->sync_set_number, le->length );
+			ctx->lightproc( so, le->sensor_id, -3 - so->sync_set_number, 0, le->timestamp, le->length );
+		}
 	}
 
 
diff --git a/src/survive_process.c b/src/survive_process.c
index 8dc849a..9295638 100644
--- a/src/survive_process.c
+++ b/src/survive_process.c
@@ -16,6 +16,9 @@ void survive_default_light_process( SurviveObject * so, int sensor_id, int acode
 		survive_cal_light( so, sensor_id, acode, timeinsweep, timecode, length );
 	}
 
+	//We don't use sync times, yet.
+	if( acode < -1 ) return;
+
 	if( base_station > NUM_LIGHTHOUSES ) return;
 
 	//No loner need sync information past this point.
-- 
cgit v1.3.1


From 39ef5af74702c8825a82f65cf68e6af875a814ee Mon Sep 17 00:00:00 2001
From: ultramn <dchapm2@umbc.edu>
Date: Thu, 23 Mar 2017 00:41:22 -0400
Subject: Added a dynamic plotting tool for OrthoSolve.  Charles added
 orthogonalization to the rotation matrix.

---
 dave/AffineSolve      |  Bin 65424 -> 40104 bytes
 dave/AffineSolve.c    | 1241 +++++++++++++++++++++++++------------------------
 dave/Makefile         |   20 +-
 dave/OrthoPlot.c      |  451 ++++++++++++++++++
 dave/dclapack_test.c  |    2 +-
 dave/fileutil.c       |  133 ++++++
 dave/fileutil.h       |   35 ++
 src/poser_daveortho.c |   85 +++-
 src/survive_cal.h     |    2 +-
 src/survive_data.c    |    2 +-
 10 files changed, 1356 insertions(+), 615 deletions(-)
 create mode 100644 dave/OrthoPlot.c
 create mode 100644 dave/fileutil.c
 create mode 100644 dave/fileutil.h

(limited to 'src/survive_data.c')

diff --git a/dave/AffineSolve b/dave/AffineSolve
index 98a9590..7d51b34 100755
Binary files a/dave/AffineSolve and b/dave/AffineSolve differ
diff --git a/dave/AffineSolve.c b/dave/AffineSolve.c
index 4fba56b..685062e 100644
--- a/dave/AffineSolve.c
+++ b/dave/AffineSolve.c
@@ -11,10 +11,13 @@
 #include <math.h>
 #include "dclapack.h"
 #include <linmath.h>
+#define RegisterDriver(a,b)
+#include "poser_daveortho.c"
 #define LH_ID    0
 #define NUM_HMD 32
+#define INDIR "full_test_triangle_on_floor/"
 
-#define MAX_POINTS 128
+#define MAX_POINTS SENSORS_PER_OBJECT
 //#define _ABS(a)  ( (a)<=0 ? -(a) : (a) )
 #define _SIGN(a) ( (a)<=0 ? -1.0f : 1.0f )
 #define RANDF  ( (float)rand() / (float)RAND_MAX )
@@ -31,7 +34,7 @@ float hmd_pos[NUM_HMD][3];
 void ReadHmdPoints()
 {
     int i;
-    FILE *fin = fopen("HMD_points.csv","r");
+    FILE *fin = fopen(INDIR "HMD_points.csv","r");
     if (fin==NULL) {
         printf("ERROR: could not open HMD_points.csv for reading\n");
         exit(1);
@@ -52,7 +55,7 @@ void ReadPtinfo()
     for (i=0; i<NUM_HMD; i++) { hmd_angle[i][0]=-9999.0; hmd_angle[i][1]=-9999.0; }
 
     // Read ptinfo.csv
-    FILE *fin = fopen("ptinfo.csv", "r");
+    FILE *fin = fopen(INDIR "ptinfo.csv", "r");
     if (fin==NULL) { printf("ERROR: could not open ptinfo.csv for reading\n"); exit(1); }
     while (!feof(fin))
     {
@@ -74,667 +77,707 @@ void ReadPtinfo()
     fclose(fin);
 }
 
-#define PRINT_MAT(A,M,N) { \
-    int m,n; \
-    printf(#A "\n"); \
-    for (m=0; m<M; m++) { \
-        for (n=0; n<N; n++) { \
-            printf("%f\t", A[m][n]); \
-        } \
-        printf("\n"); \
-    } \
-}
-
-#define CrossProduct(ox,oy,oz,a,b,c,x,y,z) { \
-    ox=(b)*(z)-(c)*(y); \
-    oy=(c)*(x)-(a)*(z); \
-    oz=(a)*(y)-(b)*(x); }
-
-void OrthoSolve(
-    float T[4][4],               // OUTPUT: 4x4 transformation matrix
-    FLOAT S_out[2][MAX_POINTS],  // OUTPUT:  array of screenspace points
-    FLOAT S_in[2][MAX_POINTS],   // INPUT:  array of screenspace points
-    FLOAT X_in[3][MAX_POINTS],   // INPUT:  array of offsets
-    int nPoints)
+void AffineSolve(
+    float T[4][4],           // OUTPUT: transform
+    float O[MAX_POINTS][4],  // INPUT:  points, offsets
+    float N[MAX_POINTS][3],  // INPUT:  plane normals
+    float D[MAX_POINTS],     // INPUT:  plane offsets
+    int nPoints, int nIter,
+    float stepSizeRot, float stepSizePos, float falloff, int constrain)
 {
-    int i,j,k;
-    FLOAT R[3][3];           // OUTPUT: 3x3 rotation matrix
-    FLOAT trans[3];          // INPUT:  x,y,z translation vector
-
-    //--------------------
-    // Remove the center of the HMD offsets, and the screen space
-    //--------------------
-    FLOAT xbar[3] = {0.0, 0.0, 0.0};
-    FLOAT sbar[2] = {0.0, 0.0};
-    FLOAT S[2][MAX_POINTS];
-    FLOAT X[3][MAX_POINTS];
-    FLOAT inv_nPoints = 1.0 / nPoints;
-    for (i=0; i<nPoints; i++) {
-        xbar[0] += X_in[0][i];
-        xbar[1] += X_in[1][i];
-        xbar[2] += X_in[2][i];
-        sbar[0] += S_in[0][i];
-        sbar[1] += S_in[1][i];        
-    }
-    for (j=0; j<3; j++) { xbar[j] *= inv_nPoints; }
-    for (j=0; j<2; j++) { sbar[j] *= inv_nPoints; }
-    for (i=0; i<nPoints; i++) {
-        X[0][i] = X_in[0][i] - xbar[0];
-        X[1][i] = X_in[1][i] - xbar[1];
-        X[2][i] = X_in[2][i] - xbar[2];
-        S[0][i] = S_in[0][i] - sbar[0];
-        S[1][i] = S_in[1][i] - sbar[1];
-    }
+    int i,j,k,iter;
+    //T[3][3] = 1.0f;
     
-    //--------------------
-    // Solve for the morph matrix
-    //  S = M X
-    // thus
-    // (SX^t)(XX^t)^-1 = M
-    //--------------------
-    FLOAT Xt[MAX_POINTS][3];
-    FLOAT XXt[3][3];
-    FLOAT invXXt[3][3];
-    FLOAT SXt[2][3];
-    FLOAT M[2][3];           // Morph matrix! (2 by 3)
-    TRANSP(X,Xt,3,nPoints);
-    MUL(X,Xt,XXt,3,nPoints,3);
-    MUL(S,Xt,SXt,2,nPoints,3);
-    INV(XXt,invXXt,3);
-    MUL(SXt,invXXt,M,2,3,3);
-//PRINT(M,2,3);
-
-// Double checking work
-FLOAT S_morph[2][MAX_POINTS];
-MUL(M,X,S_morph,2,3,nPoints);
-for (i=0; i<nPoints; i++) { S_morph[0][i]+=sbar[0]; S_morph[1][i]+=sbar[1]; }
-
-    //--------------------
-    // Solve for the non-trivial vector
-    //  uf -- vector that goes into the camera
-    //--------------------
-    FLOAT uM[3][3] = {
-        { M[0][0], M[0][1], M[0][2] },
-        { M[1][0], M[1][1], M[1][2] },
-        { 3.14567, -1.2345, 4.32567 } };      // Morph matrix with appended row
-//PRINT(uM,3,3);
-// ToDo: Pick a number for the bottom that is NOT linearly separable with M[0] and M[1]
-    FLOAT B[3][1] = { {0.0}, {0.0}, {1.0} };
-    FLOAT inv_uM[3][3];
-    FLOAT uf[3][1];
-    INV(uM,inv_uM,3);
-    MUL(inv_uM,B,uf,3,3,1);
+    printf("iter x y z error\n");
     
-    //--------------------
-    // Solve for unit length vector
-    //  f that goes into the camera
-    //--------------------
-    FLOAT uf_len = sqrt( uf[0][0]*uf[0][0] + uf[1][0]*uf[1][0] + uf[2][0]*uf[2][0] );
-    FLOAT f[3][1] = { {uf[0][0]/uf_len}, {uf[1][0]/uf_len}, {uf[2][0]/uf_len} };
-//PRINT(uf,3,1);
-//PRINT(f,3,1);
+    float gradDot     = 1.0;
+    float prevGradDot = 1.0;
+    float de_dT[3][4];  // the gradient
+    float conj[3][4];   // the conjugate
+    float errorSq=0.0;
+    for (iter=0; iter<nIter; iter++)
+    {
+        //----------------------------------
+        // Calculate the gradient direction
+        //----------------------------------
+        errorSq = 0.0;
+        memset(de_dT, 0, 3*4*sizeof(float));
+        for (i=0; i<nPoints; i++)
+        {
+            // What is the plane deviation error
+            float Ei = -D[i];
+            for (j=0; j<3; j++) {
+                float Tj_oi = 0.0f;
+                for (k=0; k<4; k++) {
+                    Tj_oi += T[j][k] * O[i][k];
+                }
+                Ei += N[i][j] * Tj_oi;
+            }
+//            printf("E[%d] %f\n", i, Ei);
+        
+            // Figure out contribution to the error
+            for (j=0; j<3; j++) {
+                for (k=0; k<4; k++) {
+                    de_dT[j][k] += N[i][j] * O[i][k] * Ei;
+                }
+            }
+            
+            errorSq += Ei*Ei;
+        }
 
-//FLOAT check[3][1];
-//MUL(uM,uf,check,3,3,1);
-//PRINT(check,3,1);
+//        printf("%d %f %f %f %f\n", iter, T[0][3], T[1][3], T[2][3], sqrt(errorSq));
+//exit(1);
+        // Constrain the gradient (such that dot products are zero)
+        if (constrain)
+        {
+            float T0T1 = 0.0, T1T2 = 0.0, T2T0 = 0.0;
+            for (k=0; k<3; k++) {
+                T0T1 += T[0][k] * T[1][k];
+                T1T2 += T[1][k] * T[2][k];
+                T2T0 += T[2][k] * T[0][k];
+            }
+//            printf("T0T1 %f T1T2 %f T2T0 %f\n", T0T1, T1T2, T2T0);
+            for (k=0; k<3; k++) {
+                de_dT[0][k] += ORTHOG_PENALTY * 2.0 * T0T1 * T[1][k];
+                de_dT[0][k] += ORTHOG_PENALTY * 2.0 * T2T0 * T[2][k];
+                de_dT[1][k] += ORTHOG_PENALTY * 2.0 * T1T2 * T[2][k];
+                de_dT[1][k] += ORTHOG_PENALTY * 2.0 * T0T1 * T[0][k];
+                de_dT[2][k] += ORTHOG_PENALTY * 2.0 * T1T2 * T[1][k];
+                de_dT[2][k] += ORTHOG_PENALTY * 2.0 * T2T0 * T[0][k];
+            }
+        }
 
-    //--------------------
-    // take cross products to get vectors u,r
-    //--------------------
-    FLOAT u[3][1], r[3][1];
-    CrossProduct(u[0][0],u[1][0],u[2][0],f[0][0],f[1][0],f[2][0],1.0,0.0,0.0);
-    FLOAT inv_ulen = 1.0 / sqrt( u[0][0]*u[0][0] + u[1][0]*u[1][0] + u[2][0]*u[2][0] );
-    u[0][0]*=inv_ulen; u[1][0]*=inv_ulen; u[2][0]*=inv_ulen;
-    CrossProduct(r[0][0],r[1][0],r[2][0],f[0][0],f[1][0],f[2][0],u[0][0],u[1][0],u[2][0]);
-//PRINT(u,3,1);
-//PRINT(r,3,1);
+        // Calculate the gradient dot product
+        //  (used by conjugate gradient method)
+        prevGradDot = gradDot;
+        gradDot = 0.0;
+        for (j=0; j<3; j++) {
+            for (k=0; k<4; k++) {
+                gradDot += de_dT[j][k] * de_dT[j][k];
+            }
+        }
 
-    //--------------------
-    // Use morph matrix to get screen space
-    //  uhat,rhat
-    //--------------------
-    FLOAT uhat[2][1], rhat[2][1], fhat[2][1];
-    MUL(M,f,fhat,2,3,1);
-    MUL(M,u,uhat,2,3,1);
-    MUL(M,r,rhat,2,3,1);
-    FLOAT fhat_len = sqrt( fhat[0][0]*fhat[0][0] + fhat[1][0]*fhat[1][0] );
-    FLOAT uhat_len = sqrt( uhat[0][0]*uhat[0][0] + uhat[1][0]*uhat[1][0] );
-    FLOAT rhat_len = sqrt( rhat[0][0]*rhat[0][0] + rhat[1][0]*rhat[1][0] );
-    FLOAT urhat_len = 0.5 * (uhat_len + rhat_len);
-/*    
-printf("fhat %f %f (len %f)\n", fhat[0][0], fhat[1][0], fhat_len);
-printf("uhat %f %f (len %f)\n", uhat[0][0], uhat[1][0], uhat_len);
-printf("rhat %f %f (len %f)\n", rhat[0][0], rhat[1][0], rhat_len);
-*/
-//    FLOAT ydist1 = 1.0 /  uhat_len; //0.25*PI / uhat_len;
-//    FLOAT ydist2 = 1.0 /  rhat_len; //0.25*PI / rhat_len;
-    FLOAT ydist  = 1.0 / urhat_len;
-    //printf("ydist1 %f ydist2 %f ydist %f\n", ydist1, ydist2, ydist);
+//        printf("Iter %d error %f gradDot %f prevGradDot %f\n", iter, sqrt(errorSq), gradDot, prevGradDot);
 
-    //--------------------
-    // Rescale the axies to be of the proper length
-    //--------------------
-    FLOAT x[3][1] = { {M[0][0]*ydist}, {0.0}, {M[1][0]*ydist} };
-    FLOAT y[3][1] = { {M[0][1]*ydist}, {0.0}, {M[1][1]*ydist} };
-    FLOAT z[3][1] = { {M[0][2]*ydist}, {0.0}, {M[1][2]*ydist} };
+        //----------------------------------
+        // Calculate the conjugate direction
+        //----------------------------------
+//        if (iter==0) {
+            // First iteration, just use the gradient
+            for (j=0; j<3; j++) {
+                for (k=0; k<4; k++) {
+                    conj[j][k] = -de_dT[j][k];
+                }
+            }
+/*        } else {
+            // Calculate "beta" for Fletcher Reeves method
+            float beta = gradDot / prevGradDot;
+//printf("gradDot %f prevGradDot %f beta %f\n", gradDot, prevGradDot, beta);
 
-    // we know the distance into (or out of) the camera for the z axis,
-    //  but we don't know which direction . . .
-    FLOAT x_y = sqrt(1.0 - x[0][0]*x[0][0] - x[2][0]*x[2][0]);
-    FLOAT y_y = sqrt(1.0 - y[0][0]*y[0][0] - y[2][0]*y[2][0]);
-    FLOAT z_y = sqrt(1.0 - z[0][0]*z[0][0] - z[2][0]*z[2][0]);
+            // Update the conjugate
+            for (j=0; j<3; j++) {
+                for (k=0; k<4; k++) {
+                    conj[j][k] = beta*conj[j][k] - de_dT[j][k];
+                }
+            }            
+        }
+*/
 
-	if( x_y != x_y ) x_y = 0;
-	if( y_y != y_y ) y_y = 0;
-	if( z_y != z_y ) z_y = 0;
+//        PRINT_MAT(de_dT,4,4);
+//        exit(1);
 
-/*
-    // Exhaustively flip the minus sign of the z axis until we find the right one . . .
-    FLOAT bestErr = 9999.0;
-    FLOAT xy_dot2 = x[0][0]*y[0][0] + x[2][0]*y[2][0];
-    FLOAT yz_dot2 = y[0][0]*z[0][0] + y[2][0]*z[2][0];
-    FLOAT zx_dot2 = z[0][0]*x[0][0] + z[2][0]*x[2][0];
-    for (i=0;i<2;i++) {
-        for (j=0;j<2;j++) {
-            for(k=0;k<2;k++) {
+        //----------------------------------
+        // How large is the gradient ?
+        //----------------------------------
+        
+        double gradSizeRot = 0.0;
+        double gradSizePos = 0.0;
+        for (j=0; j<3; j++) {
+            for (k=0; k<3; k++) {
+                gradSizeRot += _ABS(conj[j][k]);
+            }
+            gradSizePos += _ABS(conj[j][k]);
+        }
+        if (gradSizeRot <= TOO_SMALL && gradSizePos <= TOO_SMALL) { break; }  // Quit, we've totally converged
+        
+        //----------------------------------
+        // Descend in the gradient direction
+        //----------------------------------
+        if (gradSizeRot > TOO_SMALL) {
+            float scaleRot = stepSizeRot / gradSizeRot;
+            for (j=0; j<3; j++) {
+                for (k=0; k<3; k++) {
+                    T[j][k] += scaleRot * conj[j][k];
+                }
+            }
+            stepSizeRot *= falloff;
+        }
+        
+        if (gradSizePos > TOO_SMALL) {
+            float scalePos = stepSizePos / gradSizePos;
+            for (j=0; j<3; j++) {
+                T[j][3] += scalePos * conj[j][3];
+            }
+            stepSizePos *= falloff;
+        }
+        
+        // Constrain the gradient (such that scaling is one)
+        if (constrain)
+        {
+            // Measure the scales
+            float len[3] = {0.0, 0.0, 0.0};
+            for (j=0; j<3; j++) {
+                double lenSq = 0.0;
+                for (k=0; k<3; k++) { lenSq += (double)T[j][k] * (double)T[j][k]; }
+                len[j] = sqrt(lenSq);
+            }
             
-                // Calculate the error term
-                FLOAT xy_dot = xy_dot2 + x_y*y_y;
-                FLOAT yz_dot = yz_dot2 + y_y*z_y;
-                FLOAT zx_dot = zx_dot2 + z_y*x_y;
-                FLOAT err = _ABS(xy_dot) + _ABS(yz_dot) + _ABS(zx_dot);
-                
-                // Calculate the handedness
-                FLOAT cx,cy,cz;
-                CrossProduct(cx,cy,cz,x[0][0],x_y,x[2][0],y[0][0],y_y,y[2][0]);
-                FLOAT hand = cx*z[0][0] + cy*z_y + cz*z[2][0];
-                printf("err %f hand %f\n", err, hand);
-                
-                // If we are the best right-handed frame so far
-                //if (hand > 0 && err < bestErr) { x[1][0]=x_y; y[1][0]=y_y; z[1][0]=z_y; bestErr=err; }
-				if ( i == 0 && j == 1 && k == 0) { x[1][0]=x_y; y[1][0]=y_y; z[1][0]=z_y; bestErr=err; }
-                z_y = -z_y;
+            // How far off is the scale?
+            float xzLen = 0.5 * (len[0] + len[2]);
+            if (xzLen > TOO_SMALL) {
+                float inv_xzLen = 1.0 / xzLen;
+                for (j=0; j<3; j++) {
+                    T[3][j] *= inv_xzLen;
+                }
+            }
+            
+            // Rescale the thing
+            for (j=0; j<3; j++)
+            {
+                if (len[j] > TOO_SMALL) {
+                    float inv_len = 1.0 / len[j];
+                    for (k=0; k<3; k++) { T[j][k] *= inv_len; }
+                }
             }
-            y_y = -y_y;
         }
-        x_y = -x_y;
     }
-    printf("bestErr %f\n", bestErr);
-*/
+    float dist = sqrt(T[0][3]*T[0][3] + T[1][3]*T[1][3] + T[2][3]*T[2][3]);
+    printf("AffineSolve: pos: %f %f %f dist: %f\n", T[0][3], T[1][3], T[2][3], dist);
+}
+
+int main()
+{
+    int i,j,k,sen,axis;
+        
+    // Read the data files
+    ReadHmdPoints();
+    ReadPtinfo();
 
     //-------------------------
-    // A test version of the rescaling to the proper length
+    // Package the lighthouse data for "AffineSolve"
     //-------------------------
-    FLOAT ydist2;
-    FLOAT bestBestErr = 9999.0;
-    FLOAT bestYdist = 0;
-    for (ydist2=ydist-0.1; ydist2<ydist+0.1; ydist2+=0.0001)
-    {
-        FLOAT x2[3][1] = { {M[0][0]*ydist2}, {0.0}, {M[1][0]*ydist2} };
-        FLOAT y2[3][1] = { {M[0][1]*ydist2}, {0.0}, {M[1][1]*ydist2} };
-        FLOAT z2[3][1] = { {M[0][2]*ydist2}, {0.0}, {M[1][2]*ydist2} };
 
-        // we know the distance into (or out of) the camera for the z axis,
-        //  but we don't know which direction . . .
-        FLOAT x_y = sqrt(1.0 - x2[0][0]*x2[0][0] - x2[2][0]*x2[2][0]);
-        FLOAT y_y = sqrt(1.0 - y2[0][0]*y2[0][0] - y2[2][0]*y2[2][0]);
-        FLOAT z_y = sqrt(1.0 - z2[0][0]*z2[0][0] - z2[2][0]*z2[2][0]);
+    // Data for the "iterative" affine solve formula
+    float Tcalc[4][4];
+    float O[MAX_POINTS][4];
+    float N[MAX_POINTS][3];
+    float D[MAX_POINTS];
+    int nPlanes = 0;
 
-        // Exhaustively flip the minus sign of the z axis until we find the right one . . .
-        FLOAT bestErr = 9999.0;
-        FLOAT xy_dot2 = x2[0][0]*y2[0][0] + x2[2][0]*y2[2][0];
-        FLOAT yz_dot2 = y2[0][0]*z2[0][0] + y2[2][0]*z2[2][0];
-        FLOAT zx_dot2 = z2[0][0]*x2[0][0] + z2[2][0]*x2[2][0];
-        for (i=0;i<2;i++) {
-            for (j=0;j<2;j++) {
-                for(k=0;k<2;k++) {
-            
-                    // Calculate the error term
-                    FLOAT xy_dot = xy_dot2 + x_y*y_y;
-                    FLOAT yz_dot = yz_dot2 + y_y*z_y;
-                    FLOAT zx_dot = zx_dot2 + z_y*x_y;
-                    FLOAT err = _ABS(xy_dot) + _ABS(yz_dot) + _ABS(zx_dot);
-                
-                    // Calculate the handedness
-                    FLOAT cx,cy,cz;
-                    CrossProduct(cx,cy,cz,x2[0][0],x_y,x2[2][0],y2[0][0],y_y,y2[2][0]);
-                    FLOAT hand = cx*z2[0][0] + cy*z_y + cz*z2[2][0];
-//                  printf("err %f hand %f\n", err, hand);
-                
-                    // If we are the best right-handed frame so far
-                    if (hand > 0 && err < bestErr) { x2[1][0]=x_y; y2[1][0]=y_y; z2[1][0]=z_y; bestErr=err; }
-                    z_y = -z_y;
-                }
-                y_y = -y_y;
-            }
-            x_y = -x_y;
-        }
-        printf("ydist2 %f bestErr %f\n",ydist2,bestErr);
+    for (sen=0; sen<NUM_HMD; sen++)
+    {
+        for (axis=0; axis<2; axis++)
+        {
+            if (hmd_angle[sen][axis] != -9999.0)
+            {
+                // Set the offset
+                O[nPlanes][0] = hmd_pos[sen][0];
+                O[nPlanes][1] = hmd_pos[sen][1];
+                O[nPlanes][2] = hmd_pos[sen][2];
+                O[nPlanes][3] = 1.0;
         
-        if (bestErr < bestBestErr) {
-            memcpy(x,x2,3*sizeof(FLOAT));
-            memcpy(y,y2,3*sizeof(FLOAT));
-            memcpy(z,z2,3*sizeof(FLOAT));
-            bestBestErr = bestErr;
-            bestYdist = ydist2;
+                // Calculate the plane equation
+                if (axis == 0) {   // Horizontal
+                    N[nPlanes][0] = -cos(hmd_angle[sen][axis]);
+                    N[nPlanes][1] = -sin(hmd_angle[sen][axis]);
+                    N[nPlanes][2] = 0.0;
+                    D[nPlanes]    = 0.0;
+                } else {           // Vertical
+                    N[nPlanes][0] = 0.0;
+                    N[nPlanes][1] = -sin(hmd_angle[sen][axis]);
+                    N[nPlanes][2] =  cos(hmd_angle[sen][axis]);
+                    D[nPlanes]    = 0.0;
+                }
+
+                printf("plane %d O %.3f %.3f %.3f %.3f  N %.3f %.3f %.3f  D %.3f\n",
+                    nPlanes,
+                    O[nPlanes][0], O[nPlanes][1], O[nPlanes][2], O[nPlanes][3], 
+                    N[nPlanes][0], N[nPlanes][1], N[nPlanes][2], 
+                    D[nPlanes]);
+                nPlanes++;
+            }
         }
     }
-    ydist = bestYdist;
 
-/*
-    for (i=0; i<nPoints; i++) {
-        float x1 = x[0][0]*X[0][i] + y[0][0]*X[1][i] + z[0][0]*X[2][i];
-        float y1 = x[1][0]*X[0][i] + y[1][0]*X[1][i] + z[1][0]*X[2][i];
-        float z1 = x[2][0]*X[0][i] + y[2][0]*X[1][i] + z[2][0]*X[2][i];
-        printf("x1z1 %f %f y1 %f\n", x1, z1, y1);
-    }
-*/
-/*    
-    //--------------------
-    // Combine uhat and rhat to figure out the unit x-vector
-    //--------------------
-    FLOAT xhat[2][1]  = { {0.0}, {1.0} };
-    FLOAT urhat[2][2] = {
-        {uhat[0][0], uhat[1][0]},
-        {rhat[0][0], rhat[1][0]} };
-    FLOAT inv_urhat[2][2];
-    FLOAT ab[2][1];
-    INV(urhat,inv_urhat,2);
-    MUL(inv_urhat,xhat,ab,2,2,1);
-PRINT(ab,2,1);
-    FLOAT a = ab[0][0], b = ab[1][0];
+    
+    printf("nPlanes %d\n", nPlanes);
+        
+    //}
+    
+    PRINT_MAT(Tcalc,4,4);
 
-    //-------------------
-    // calculate the xyz coordinate system
-    //-------------------
-    FLOAT y[3][1] = { {f[0][0]}, {f[1][0]}, {f[2][0]} };
-    FLOAT x[3][1] = { {a*u[0][0] + b*r[0][0]}, {a*u[1][0] + b*r[1][0]}, {a*u[2][0] + b*r[2][0]} };
-    FLOAT inv_xlen = 1.0 / sqrt( x[0][0]*x[0][0] + x[1][0]*x[1][0] + x[2][0]*x[2][0] );
-    x[0][0]*=inv_xlen; x[1][0]*=inv_xlen; x[2][0]*=inv_xlen;
-    FLOAT z[3][1];
-    CrossProduct(z[0][0],z[1][0],z[2][0],x[0][0],x[1][0],x[2][0],y[0][0],y[1][0],y[2][0]);
-*/
-    // Store into the rotation matrix
-    for (i=0; i<3; i++) { R[i][0] = x[i][0]; R[i][1] = y[i][0]; R[i][2] = z[i][0]; }
-//PRINT(R,3,3);
+    
+    //--------------------------------------------------
+    // Package the data for "OrthoSolve"
+    //--------------------------------------------------
 
-    //-------------------
-    // Calculate the translation of the centroid
-    //-------------------
-    trans[0]=tan(sbar[0]);  trans[1]=1.0;  trans[2]=tan(sbar[1]);
-    FLOAT inv_translen = ydist / sqrt( trans[0]*trans[0] + trans[1]*trans[1] + trans[2]*trans[2] );
-    trans[0]*=inv_translen; trans[1]*=inv_translen; trans[2]*=inv_translen;
+    // Data for the "fake" ortho solve formula
+    float Tortho[4][4];         // OUTPUT: 4x4 transformation matrix
+    FLOAT S_out[2][MAX_POINTS];  // INPUT:  array of screenspace points
+    FLOAT S_in[2][MAX_POINTS];  // INPUT:  array of screenspace points
+    FLOAT X_in[3][MAX_POINTS];  // INPUT:  array of offsets
+    int nPoints=0;  
 
-    //-------------------
-    // Add in the centroid point
-    //-------------------
-    trans[0] -= xbar[0]*R[0][0] + xbar[1]*R[0][1] + xbar[2]*R[0][2];
-    trans[1] -= xbar[0]*R[1][0] + xbar[1]*R[1][1] + xbar[2]*R[1][2];
-    trans[2] -= xbar[0]*R[2][0] + xbar[1]*R[2][1] + xbar[2]*R[2][2];
-    FLOAT transdist = sqrt( trans[0]*trans[0] + trans[1]*trans[1] + trans[2]*trans[2] );
+    // Transform into the "OrthoSolve" format
+    for (sen=0; sen<NUM_HMD; sen++)
+    {
+        if (hmd_angle[sen][0] != -9999.0 && hmd_angle[sen][1] != -9999.0)
+        {
+            S_in[0][nPoints] = hmd_angle[sen][0];
+            S_in[1][nPoints] = hmd_angle[sen][1];
+            X_in[0][nPoints] = hmd_pos[sen][0];
+            X_in[1][nPoints] = hmd_pos[sen][1];
+            X_in[2][nPoints] = hmd_pos[sen][2];
+            nPoints++;
+        }
+    }
+    printf("OrthoSolve nPoints %d\n", nPoints);
+    
+    //--------------------------------------------------
+    // Run the "OrthoSolve" and then the "AffineSolve"
+    //--------------------------------------------------    
+    
+    int loop;
+    for (loop=0; loop<1; loop++)
+    {
+        // Run OrthoSolve
+        OrthoSolve(
+            Tortho,     // OUTPUT: 4x4 transformation matrix
+            S_out,      // OUTPUT: array of output screenspace points
+            S_in,       // INPUT:  array of screenspace points
+            X_in,       // INPUT:  array of offsets
+            nPoints);
+			printf( "POS: %f %f %f\n", Tortho[0][3], Tortho[1][3], Tortho[2][3]);
+
+		// Come up with rotation and transposed version of Tortho
+		FLT TorthoTr[4][4], Rortho[4][4], RorthoTr[4][4];
+		TRANSP(Tortho,TorthoTr,4,4);
+		memcpy(Rortho,Tortho,4*4*sizeof(FLT));
+		Rortho[3][0]=0.0;  Rortho[3][1]=0.0;  Rortho[3][2]=0.0;
+		TRANSP(Rortho,RorthoTr,4,4);
+		PRINT(Tortho,4,4);
+		PRINT(Rortho,4,4);
+
+		// Print out some quaternions
+		FLT Tquat[4], TquatTr[4], Rquat[4], RquatTr[4];
+		quatfrommatrix(Tquat,   &Tortho[0][0] );
+		quatfrommatrix(TquatTr, &TorthoTr[0][0] );
+		quatfrommatrix(Rquat,   &Rortho[0][0] );
+		quatfrommatrix(RquatTr, &RorthoTr[0][0] );
+		printf( "Tquat  : %f %f %f %f = %f\n", Tquat  [0], Tquat  [1], Tquat  [2], Tquat  [3], quatmagnitude(Tquat));
+		printf( "TquatTr: %f %f %f %f = %f\n", TquatTr[0], TquatTr[1], TquatTr[2], TquatTr[3], quatmagnitude(TquatTr));
+		printf( "Rquat  : %f %f %f %f = %f\n", Rquat  [0], Rquat  [1], Rquat  [2], Rquat  [3], quatmagnitude(Rquat));
+		printf( "RquatTr: %f %f %f %f = %f\n", RquatTr[0], RquatTr[1], RquatTr[2], RquatTr[3], quatmagnitude(RquatTr));
+
+		// Flip y and z axies
+		FLT T2[4][4] = {
+			{ Tortho[0][0], -Tortho[0][2], -Tortho[0][1], 0.0 },
+			{ Tortho[1][0], -Tortho[1][2], -Tortho[1][1], 0.0 },
+			{ Tortho[2][0], -Tortho[2][2], -Tortho[2][1], 0.0 },
+			{          0.0,           0.0,           0.0, 1.0 } };
+PRINT(T2,4,4);
+
+		// Print out the quaternions
+		FLT T2quat[4];
+		quatfrommatrix(T2quat,   &T2[0][0] );
+		printf( "T2quat : %f %f %f %f = %f\n", T2quat [0], T2quat [1], T2quat [2], T2quat [3], quatmagnitude(T2quat));
+    }
 
-    //-------------------
-    // Pack into the 4x4 transformation matrix
-    //-------------------
-    T[0][0]=R[0][0]; T[0][1]=R[0][1]; T[0][2]=R[0][2]; T[0][3]=trans[0];
-    T[1][0]=R[1][0]; T[1][1]=R[1][1]; T[1][2]=R[1][2]; T[1][3]=trans[1];
-    T[2][0]=R[2][0]; T[2][1]=R[2][1]; T[2][2]=R[2][2]; T[2][3]=trans[2];
-    T[3][0]=0.0;     T[3][1]=0.0;     T[3][2]=0.0;     T[3][3]=1.0;
+    // Run the calculation for Tcalc
+    //int run;
+    //for (run=0; run<100; run++) {
+/*
+    // Initialize Tcalc to the identity matrix
+    memcpy(Tcalc, Tortho, 4*4*sizeof(float));
+    //memset(Tcalc, 0, 4*4*sizeof(float));
+    //for (i=0; i<4; i++) { Tcalc[i][i] = 1.0f; }
 
-    PRINT_MAT(T,4,4);
-    //-------------------
-    // Plot the output points
-    //-------------------
-    for (i=0; i<nPoints; i++) {
-        float Tx = T[0][0]*X_in[0][i] + T[0][1]*X_in[1][i] + T[0][2]*X_in[2][i] + T[0][3];
-        float Ty = T[1][0]*X_in[0][i] + T[1][1]*X_in[1][i] + T[1][2]*X_in[2][i] + T[1][3];
-        float Tz = T[2][0]*X_in[0][i] + T[2][1]*X_in[1][i] + T[2][2]*X_in[2][i] + T[2][3];
-        S_out[0][i] = atan2(Tx, Ty);   // horiz
-        S_out[1][i] = atan2(Tz, Ty);   // vert
-        //S_out[0][i] = Tx;
-        //S_out[1][i] = Tz;
-        printf("point %i Txyz %f %f %f in %f %f out %f %f morph %f %f\n", i, Tx,Ty,Tz, S_in[0][i], S_in[1][i], S_out[0][i], S_out[1][i], S_morph[0][i], S_morph[1][i]);
-    }
+    // Solve it!
+    AffineSolve(
+        Tcalc,           // OUTPUT: transform
+        O,  // INPUT:  points, offsets
+        N,  // INPUT:  plane normals
+        D,     // INPUT:  plane offsets
+        nPlanes, NITER,
+        STEP_SIZE_ROT, STEP_SIZE_POS, FALLOFF,
+        1);
+*/
+    // insert code here...
+    return 0;
+}
 
 
 
 
-	float quat[4];
-	float posoff[3] = { trans[0], trans[1], trans[2] };
-	float MT[4][4];
-	//matrix44transpose( MT, &T[0][0] );
-	matrix44copy( &MT[0][0], &T[0][0] );
 
-//QUAT: 0.657864 -0.305763 0.128486 0.265895 = 0.783252
-//QUAT: 0.657864 0.305763 -0.128486 -0.265895 = 0.783252
+#if 0
+#define PRINT_MAT(A,M,N) { \
+    int m,n; \
+    printf(#A "\n"); \
+    for (m=0; m<M; m++) { \
+        for (n=0; n<N; n++) { \
+            printf("%f\t", A[m][n]); \
+        } \
+        printf("\n"); \
+    } \
+}
 
+#define CrossProduct(ox,oy,oz,a,b,c,x,y,z) { \
+    ox=(b)*(z)-(c)*(y); \
+    oy=(c)*(x)-(a)*(z); \
+    oz=(a)*(y)-(b)*(x); }
 
-	quatfrommatrix( quat, &MT[0][0] );
-	printf( "QUAT: %f %f %f %f = %f\n", quat[0], quat[1], quat[2], quat[3], quatmagnitude(quat) );
-	//quat[2] -= 0.005; //fixes up lh0 in test data set.
-	quatnormalize( quat, quat );
-	printf( "QUAT: %f %f %f %f = %f\n", quat[0], quat[1], quat[2], quat[3], quatmagnitude(quat) );
+void OrthoSolve(
+    float T[4][4],               // OUTPUT: 4x4 transformation matrix
+    FLOAT S_out[2][MAX_POINTS],  // OUTPUT:  array of screenspace points
+    FLOAT S_in[2][MAX_POINTS],   // INPUT:  array of screenspace points
+    FLOAT X_in[3][MAX_POINTS],   // INPUT:  array of offsets
+    int nPoints)
+{
+    int i,j,k;
+    FLOAT R[3][3];           // OUTPUT: 3x3 rotation matrix
+    FLOAT trans[3];          // INPUT:  x,y,z translation vector
 
+    //--------------------
+    // Remove the center of the HMD offsets, and the screen space
+    //--------------------
+    FLOAT xbar[3] = {0.0, 0.0, 0.0};
+    FLOAT sbar[2] = {0.0, 0.0};
+    FLOAT S[2][MAX_POINTS];
+    FLOAT X[3][MAX_POINTS];
+    FLOAT inv_nPoints = 1.0 / nPoints;
+    for (i=0; i<nPoints; i++) {
+        xbar[0] += X_in[0][i];
+        xbar[1] += X_in[1][i];
+        xbar[2] += X_in[2][i];
+        sbar[0] += S_in[0][i];
+        sbar[1] += S_in[1][i];        
+    }
+    for (j=0; j<3; j++) { xbar[j] *= inv_nPoints; }
+    for (j=0; j<2; j++) { sbar[j] *= inv_nPoints; }
+    for (i=0; i<nPoints; i++) {
+        X[0][i] = X_in[0][i] - xbar[0];
+        X[1][i] = X_in[1][i] - xbar[1];
+        X[2][i] = X_in[2][i] - xbar[2];
+        S[0][i] = S_in[0][i] - sbar[0];
+        S[1][i] = S_in[1][i] - sbar[1];
+    }
+    
+    //--------------------
+    // Solve for the morph matrix
+    //  S = M X
+    // thus
+    // (SX^t)(XX^t)^-1 = M
+    //--------------------
+    FLOAT Xt[MAX_POINTS][3];
+    FLOAT XXt[3][3];
+    FLOAT invXXt[3][3];
+    FLOAT SXt[2][3];
+    FLOAT M[2][3];           // Morph matrix! (2 by 3)
+    TRANSP(X,Xt,3,nPoints);
+    MUL(X,Xt,XXt,3,nPoints,3);
+    MUL(S,Xt,SXt,2,nPoints,3);
+    INV(XXt,invXXt,3);
+    MUL(SXt,invXXt,M,2,3,3);
+//PRINT(M,2,3);
 
-	for( i = 0; i <nPoints;i++ )
-	{
-		float pt[3] = { X_in[0][i], X_in[1][i], X_in[2][i] };
-		quatrotatevector( pt, quat, pt );
-		add3d( pt, pt, posoff );
-		printf( "%f %f %f OUT %f %f %f ANGLE %f %f AOUT %f %f\n", 
-			X_in[0][i], X_in[1][i], X_in[2][i], pt[0], pt[1], pt[2],
-			S_in[0][i], S_in[1][i], atan2( pt[0], pt[1] ), atan2( pt[2], pt[1] ) );
-	}
-	quattomatrix( &MT[0][0], quat );
-    PRINT_MAT(MT,4,4);
+// Double checking work
+FLOAT S_morph[2][MAX_POINTS];
+MUL(M,X,S_morph,2,3,nPoints);
+for (i=0; i<nPoints; i++) { S_morph[0][i]+=sbar[0]; S_morph[1][i]+=sbar[1]; }
 
+    //--------------------
+    // Solve for the non-trivial vector
+    //  uf -- vector that goes into the camera
+    //--------------------
+    FLOAT uM[3][3] = {
+        { M[0][0], M[0][1], M[0][2] },
+        { M[1][0], M[1][1], M[1][2] },
+        { 3.14567, -1.2345, 4.32567 } };      // Morph matrix with appended row
+//PRINT(uM,3,3);
+// ToDo: Pick a number for the bottom that is NOT linearly separable with M[0] and M[1]
+    FLOAT B[3][1] = { {0.0}, {0.0}, {1.0} };
+    FLOAT inv_uM[3][3];
+    FLOAT uf[3][1];
+    INV(uM,inv_uM,3);
+    MUL(inv_uM,B,uf,3,3,1);
+    
+    //--------------------
+    // Solve for unit length vector
+    //  f that goes into the camera
+    //--------------------
+    FLOAT uf_len = sqrt( uf[0][0]*uf[0][0] + uf[1][0]*uf[1][0] + uf[2][0]*uf[2][0] );
+    FLOAT f[3][1] = { {uf[0][0]/uf_len}, {uf[1][0]/uf_len}, {uf[2][0]/uf_len} };
+//PRINT(uf,3,1);
+//PRINT(f,3,1);
 
+//FLOAT check[3][1];
+//MUL(uM,uf,check,3,3,1);
+//PRINT(check,3,1);
 
-//    printf("xbar %f %f %f\n", xbar[0], xbar[1], xbar[2]);
-//    printf("trans %f %f %f dist: %f\n", trans[0], trans[1], trans[2], transdist);
-}
+    //--------------------
+    // take cross products to get vectors u,r
+    //--------------------
+    FLOAT u[3][1], r[3][1];
+    CrossProduct(u[0][0],u[1][0],u[2][0],f[0][0],f[1][0],f[2][0],1.0,0.0,0.0);
+    FLOAT inv_ulen = 1.0 / sqrt( u[0][0]*u[0][0] + u[1][0]*u[1][0] + u[2][0]*u[2][0] );
+    u[0][0]*=inv_ulen; u[1][0]*=inv_ulen; u[2][0]*=inv_ulen;
+    CrossProduct(r[0][0],r[1][0],r[2][0],f[0][0],f[1][0],f[2][0],u[0][0],u[1][0],u[2][0]);
+//PRINT(u,3,1);
+//PRINT(r,3,1);
 
-void AffineSolve(
-    float T[4][4],           // OUTPUT: transform
-    float O[MAX_POINTS][4],  // INPUT:  points, offsets
-    float N[MAX_POINTS][3],  // INPUT:  plane normals
-    float D[MAX_POINTS],     // INPUT:  plane offsets
-    int nPoints, int nIter,
-    float stepSizeRot, float stepSizePos, float falloff, int constrain)
-{
-    int i,j,k,iter;
-    //T[3][3] = 1.0f;
-    
-    printf("iter x y z error\n");
-    
-    float gradDot     = 1.0;
-    float prevGradDot = 1.0;
-    float de_dT[3][4];  // the gradient
-    float conj[3][4];   // the conjugate
-    float errorSq=0.0;
-    for (iter=0; iter<nIter; iter++)
-    {
-        //----------------------------------
-        // Calculate the gradient direction
-        //----------------------------------
-        errorSq = 0.0;
-        memset(de_dT, 0, 3*4*sizeof(float));
-        for (i=0; i<nPoints; i++)
-        {
-            // What is the plane deviation error
-            float Ei = -D[i];
-            for (j=0; j<3; j++) {
-                float Tj_oi = 0.0f;
-                for (k=0; k<4; k++) {
-                    Tj_oi += T[j][k] * O[i][k];
-                }
-                Ei += N[i][j] * Tj_oi;
-            }
-//            printf("E[%d] %f\n", i, Ei);
-        
-            // Figure out contribution to the error
-            for (j=0; j<3; j++) {
-                for (k=0; k<4; k++) {
-                    de_dT[j][k] += N[i][j] * O[i][k] * Ei;
-                }
-            }
-            
-            errorSq += Ei*Ei;
-        }
+    //--------------------
+    // Use morph matrix to get screen space
+    //  uhat,rhat
+    //--------------------
+    FLOAT uhat[2][1], rhat[2][1], fhat[2][1];
+    MUL(M,f,fhat,2,3,1);
+    MUL(M,u,uhat,2,3,1);
+    MUL(M,r,rhat,2,3,1);
+    FLOAT fhat_len = sqrt( fhat[0][0]*fhat[0][0] + fhat[1][0]*fhat[1][0] );
+    FLOAT uhat_len = sqrt( uhat[0][0]*uhat[0][0] + uhat[1][0]*uhat[1][0] );
+    FLOAT rhat_len = sqrt( rhat[0][0]*rhat[0][0] + rhat[1][0]*rhat[1][0] );
+    FLOAT urhat_len = 0.5 * (uhat_len + rhat_len);
+/*    
+printf("fhat %f %f (len %f)\n", fhat[0][0], fhat[1][0], fhat_len);
+printf("uhat %f %f (len %f)\n", uhat[0][0], uhat[1][0], uhat_len);
+printf("rhat %f %f (len %f)\n", rhat[0][0], rhat[1][0], rhat_len);
+*/
+//    FLOAT ydist1 = 1.0 /  uhat_len; //0.25*PI / uhat_len;
+//    FLOAT ydist2 = 1.0 /  rhat_len; //0.25*PI / rhat_len;
+    FLOAT ydist  = 1.0 / urhat_len;
+    //printf("ydist1 %f ydist2 %f ydist %f\n", ydist1, ydist2, ydist);
 
-//        printf("%d %f %f %f %f\n", iter, T[0][3], T[1][3], T[2][3], sqrt(errorSq));
-//exit(1);
-        // Constrain the gradient (such that dot products are zero)
-        if (constrain)
-        {
-            float T0T1 = 0.0, T1T2 = 0.0, T2T0 = 0.0;
-            for (k=0; k<3; k++) {
-                T0T1 += T[0][k] * T[1][k];
-                T1T2 += T[1][k] * T[2][k];
-                T2T0 += T[2][k] * T[0][k];
-            }
-//            printf("T0T1 %f T1T2 %f T2T0 %f\n", T0T1, T1T2, T2T0);
-            for (k=0; k<3; k++) {
-                de_dT[0][k] += ORTHOG_PENALTY * 2.0 * T0T1 * T[1][k];
-                de_dT[0][k] += ORTHOG_PENALTY * 2.0 * T2T0 * T[2][k];
-                de_dT[1][k] += ORTHOG_PENALTY * 2.0 * T1T2 * T[2][k];
-                de_dT[1][k] += ORTHOG_PENALTY * 2.0 * T0T1 * T[0][k];
-                de_dT[2][k] += ORTHOG_PENALTY * 2.0 * T1T2 * T[1][k];
-                de_dT[2][k] += ORTHOG_PENALTY * 2.0 * T2T0 * T[0][k];
-            }
-        }
+    //--------------------
+    // Rescale the axies to be of the proper length
+    //--------------------
+    FLOAT x[3][1] = { {M[0][0]*ydist}, {0.0}, {M[1][0]*ydist} };
+    FLOAT y[3][1] = { {M[0][1]*ydist}, {0.0}, {M[1][1]*ydist} };
+    FLOAT z[3][1] = { {M[0][2]*ydist}, {0.0}, {M[1][2]*ydist} };
 
-        // Calculate the gradient dot product
-        //  (used by conjugate gradient method)
-        prevGradDot = gradDot;
-        gradDot = 0.0;
-        for (j=0; j<3; j++) {
-            for (k=0; k<4; k++) {
-                gradDot += de_dT[j][k] * de_dT[j][k];
-            }
-        }
+    // we know the distance into (or out of) the camera for the z axis,
+    //  but we don't know which direction . . .
+    FLOAT x_y = sqrt(1.0 - x[0][0]*x[0][0] - x[2][0]*x[2][0]);
+    FLOAT y_y = sqrt(1.0 - y[0][0]*y[0][0] - y[2][0]*y[2][0]);
+    FLOAT z_y = sqrt(1.0 - z[0][0]*z[0][0] - z[2][0]*z[2][0]);
 
-//        printf("Iter %d error %f gradDot %f prevGradDot %f\n", iter, sqrt(errorSq), gradDot, prevGradDot);
+	if( x_y != x_y ) x_y = 0;
+	if( y_y != y_y ) y_y = 0;
+	if( z_y != z_y ) z_y = 0;
 
-        //----------------------------------
-        // Calculate the conjugate direction
-        //----------------------------------
-//        if (iter==0) {
-            // First iteration, just use the gradient
-            for (j=0; j<3; j++) {
-                for (k=0; k<4; k++) {
-                    conj[j][k] = -de_dT[j][k];
-                }
+/*
+    // Exhaustively flip the minus sign of the z axis until we find the right one . . .
+    FLOAT bestErr = 9999.0;
+    FLOAT xy_dot2 = x[0][0]*y[0][0] + x[2][0]*y[2][0];
+    FLOAT yz_dot2 = y[0][0]*z[0][0] + y[2][0]*z[2][0];
+    FLOAT zx_dot2 = z[0][0]*x[0][0] + z[2][0]*x[2][0];
+    for (i=0;i<2;i++) {
+        for (j=0;j<2;j++) {
+            for(k=0;k<2;k++) {
+            
+                // Calculate the error term
+                FLOAT xy_dot = xy_dot2 + x_y*y_y;
+                FLOAT yz_dot = yz_dot2 + y_y*z_y;
+                FLOAT zx_dot = zx_dot2 + z_y*x_y;
+                FLOAT err = _ABS(xy_dot) + _ABS(yz_dot) + _ABS(zx_dot);
+                
+                // Calculate the handedness
+                FLOAT cx,cy,cz;
+                CrossProduct(cx,cy,cz,x[0][0],x_y,x[2][0],y[0][0],y_y,y[2][0]);
+                FLOAT hand = cx*z[0][0] + cy*z_y + cz*z[2][0];
+                printf("err %f hand %f\n", err, hand);
+                
+                // If we are the best right-handed frame so far
+                //if (hand > 0 && err < bestErr) { x[1][0]=x_y; y[1][0]=y_y; z[1][0]=z_y; bestErr=err; }
+				if ( i == 0 && j == 1 && k == 0) { x[1][0]=x_y; y[1][0]=y_y; z[1][0]=z_y; bestErr=err; }
+                z_y = -z_y;
             }
-/*        } else {
-            // Calculate "beta" for Fletcher Reeves method
-            float beta = gradDot / prevGradDot;
-//printf("gradDot %f prevGradDot %f beta %f\n", gradDot, prevGradDot, beta);
-
-            // Update the conjugate
-            for (j=0; j<3; j++) {
-                for (k=0; k<4; k++) {
-                    conj[j][k] = beta*conj[j][k] - de_dT[j][k];
-                }
-            }            
+            y_y = -y_y;
         }
+        x_y = -x_y;
+    }
+    printf("bestErr %f\n", bestErr);
 */
 
-//        PRINT_MAT(de_dT,4,4);
-//        exit(1);
+    //-------------------------
+    // A test version of the rescaling to the proper length
+    //-------------------------
+    FLOAT ydist2;
+    FLOAT bestBestErr = 9999.0;
+    FLOAT bestYdist = 0;
+    for (ydist2=ydist-0.1; ydist2<ydist+0.1; ydist2+=0.0001)
+    {
+        FLOAT x2[3][1] = { {M[0][0]*ydist2}, {0.0}, {M[1][0]*ydist2} };
+        FLOAT y2[3][1] = { {M[0][1]*ydist2}, {0.0}, {M[1][1]*ydist2} };
+        FLOAT z2[3][1] = { {M[0][2]*ydist2}, {0.0}, {M[1][2]*ydist2} };
 
-        //----------------------------------
-        // How large is the gradient ?
-        //----------------------------------
-        
-        double gradSizeRot = 0.0;
-        double gradSizePos = 0.0;
-        for (j=0; j<3; j++) {
-            for (k=0; k<3; k++) {
-                gradSizeRot += _ABS(conj[j][k]);
-            }
-            gradSizePos += _ABS(conj[j][k]);
-        }
-        if (gradSizeRot <= TOO_SMALL && gradSizePos <= TOO_SMALL) { break; }  // Quit, we've totally converged
-        
-        //----------------------------------
-        // Descend in the gradient direction
-        //----------------------------------
-        if (gradSizeRot > TOO_SMALL) {
-            float scaleRot = stepSizeRot / gradSizeRot;
-            for (j=0; j<3; j++) {
-                for (k=0; k<3; k++) {
-                    T[j][k] += scaleRot * conj[j][k];
-                }
-            }
-            stepSizeRot *= falloff;
-        }
-        
-        if (gradSizePos > TOO_SMALL) {
-            float scalePos = stepSizePos / gradSizePos;
-            for (j=0; j<3; j++) {
-                T[j][3] += scalePos * conj[j][3];
-            }
-            stepSizePos *= falloff;
-        }
-        
-        // Constrain the gradient (such that scaling is one)
-        if (constrain)
-        {
-            // Measure the scales
-            float len[3] = {0.0, 0.0, 0.0};
-            for (j=0; j<3; j++) {
-                double lenSq = 0.0;
-                for (k=0; k<3; k++) { lenSq += (double)T[j][k] * (double)T[j][k]; }
-                len[j] = sqrt(lenSq);
-            }
-            
-            // How far off is the scale?
-            float xzLen = 0.5 * (len[0] + len[2]);
-            if (xzLen > TOO_SMALL) {
-                float inv_xzLen = 1.0 / xzLen;
-                for (j=0; j<3; j++) {
-                    T[3][j] *= inv_xzLen;
-                }
-            }
+        // we know the distance into (or out of) the camera for the z axis,
+        //  but we don't know which direction . . .
+        FLOAT x_y = sqrt(1.0 - x2[0][0]*x2[0][0] - x2[2][0]*x2[2][0]);
+        FLOAT y_y = sqrt(1.0 - y2[0][0]*y2[0][0] - y2[2][0]*y2[2][0]);
+        FLOAT z_y = sqrt(1.0 - z2[0][0]*z2[0][0] - z2[2][0]*z2[2][0]);
+
+        // Exhaustively flip the minus sign of the z axis until we find the right one . . .
+        FLOAT bestErr = 9999.0;
+        FLOAT xy_dot2 = x2[0][0]*y2[0][0] + x2[2][0]*y2[2][0];
+        FLOAT yz_dot2 = y2[0][0]*z2[0][0] + y2[2][0]*z2[2][0];
+        FLOAT zx_dot2 = z2[0][0]*x2[0][0] + z2[2][0]*x2[2][0];
+        for (i=0;i<2;i++) {
+            for (j=0;j<2;j++) {
+                for(k=0;k<2;k++) {
             
-            // Rescale the thing
-            for (j=0; j<3; j++)
-            {
-                if (len[j] > TOO_SMALL) {
-                    float inv_len = 1.0 / len[j];
-                    for (k=0; k<3; k++) { T[j][k] *= inv_len; }
+                    // Calculate the error term
+                    FLOAT xy_dot = xy_dot2 + x_y*y_y;
+                    FLOAT yz_dot = yz_dot2 + y_y*z_y;
+                    FLOAT zx_dot = zx_dot2 + z_y*x_y;
+                    FLOAT err = _ABS(xy_dot) + _ABS(yz_dot) + _ABS(zx_dot);
+                
+                    // Calculate the handedness
+                    FLOAT cx,cy,cz;
+                    CrossProduct(cx,cy,cz,x2[0][0],x_y,x2[2][0],y2[0][0],y_y,y2[2][0]);
+                    FLOAT hand = cx*z2[0][0] + cy*z_y + cz*z2[2][0];
+//                  printf("err %f hand %f\n", err, hand);
+                
+                    // If we are the best right-handed frame so far
+                    if (hand > 0 && err < bestErr) { x2[1][0]=x_y; y2[1][0]=y_y; z2[1][0]=z_y; bestErr=err; }
+                    z_y = -z_y;
                 }
+                y_y = -y_y;
             }
+            x_y = -x_y;
+        }
+        printf("ydist2 %f bestErr %f\n",ydist2,bestErr);
+        
+        if (bestErr < bestBestErr) {
+            memcpy(x,x2,3*sizeof(FLOAT));
+            memcpy(y,y2,3*sizeof(FLOAT));
+            memcpy(z,z2,3*sizeof(FLOAT));
+            bestBestErr = bestErr;
+            bestYdist = ydist2;
         }
     }
-    float dist = sqrt(T[0][3]*T[0][3] + T[1][3]*T[1][3] + T[2][3]*T[2][3]);
-    printf("AffineSolve: pos: %f %f %f dist: %f\n", T[0][3], T[1][3], T[2][3], dist);
-}
+    ydist = bestYdist;
 
-int main()
-{
-    int i,j,k,sen,axis;
-        
-    // Read the data files
-    ReadHmdPoints();
-    ReadPtinfo();
+/*
+    for (i=0; i<nPoints; i++) {
+        float x1 = x[0][0]*X[0][i] + y[0][0]*X[1][i] + z[0][0]*X[2][i];
+        float y1 = x[1][0]*X[0][i] + y[1][0]*X[1][i] + z[1][0]*X[2][i];
+        float z1 = x[2][0]*X[0][i] + y[2][0]*X[1][i] + z[2][0]*X[2][i];
+        printf("x1z1 %f %f y1 %f\n", x1, z1, y1);
+    }
+*/
+/*    
+    //--------------------
+    // Combine uhat and rhat to figure out the unit x-vector
+    //--------------------
+    FLOAT xhat[2][1]  = { {0.0}, {1.0} };
+    FLOAT urhat[2][2] = {
+        {uhat[0][0], uhat[1][0]},
+        {rhat[0][0], rhat[1][0]} };
+    FLOAT inv_urhat[2][2];
+    FLOAT ab[2][1];
+    INV(urhat,inv_urhat,2);
+    MUL(inv_urhat,xhat,ab,2,2,1);
+PRINT(ab,2,1);
+    FLOAT a = ab[0][0], b = ab[1][0];
 
-    //-------------------------
-    // Package the lighthouse data for "AffineSolve"
-    //-------------------------
+    //-------------------
+    // calculate the xyz coordinate system
+    //-------------------
+    FLOAT y[3][1] = { {f[0][0]}, {f[1][0]}, {f[2][0]} };
+    FLOAT x[3][1] = { {a*u[0][0] + b*r[0][0]}, {a*u[1][0] + b*r[1][0]}, {a*u[2][0] + b*r[2][0]} };
+    FLOAT inv_xlen = 1.0 / sqrt( x[0][0]*x[0][0] + x[1][0]*x[1][0] + x[2][0]*x[2][0] );
+    x[0][0]*=inv_xlen; x[1][0]*=inv_xlen; x[2][0]*=inv_xlen;
+    FLOAT z[3][1];
+    CrossProduct(z[0][0],z[1][0],z[2][0],x[0][0],x[1][0],x[2][0],y[0][0],y[1][0],y[2][0]);
+*/
+    // Store into the rotation matrix
+    for (i=0; i<3; i++) { R[i][0] = x[i][0]; R[i][1] = y[i][0]; R[i][2] = z[i][0]; }
+//PRINT(R,3,3);
 
-    // Data for the "iterative" affine solve formula
-    float Tcalc[4][4];
-    float O[MAX_POINTS][4];
-    float N[MAX_POINTS][3];
-    float D[MAX_POINTS];
-    int nPlanes = 0;
+    //-------------------
+    // Calculate the translation of the centroid
+    //-------------------
+    trans[0]=tan(sbar[0]);  trans[1]=1.0;  trans[2]=tan(sbar[1]);
+    FLOAT inv_translen = ydist / sqrt( trans[0]*trans[0] + trans[1]*trans[1] + trans[2]*trans[2] );
+    trans[0]*=inv_translen; trans[1]*=inv_translen; trans[2]*=inv_translen;
 
-    for (sen=0; sen<NUM_HMD; sen++)
-    {
-        for (axis=0; axis<2; axis++)
-        {
-            if (hmd_angle[sen][axis] != -9999.0)
-            {
-                // Set the offset
-                O[nPlanes][0] = hmd_pos[sen][0];
-                O[nPlanes][1] = hmd_pos[sen][1];
-                O[nPlanes][2] = hmd_pos[sen][2];
-                O[nPlanes][3] = 1.0;
-        
-                // Calculate the plane equation
-                if (axis == 0) {   // Horizontal
-                    N[nPlanes][0] = -cos(hmd_angle[sen][axis]);
-                    N[nPlanes][1] = -sin(hmd_angle[sen][axis]);
-                    N[nPlanes][2] = 0.0;
-                    D[nPlanes]    = 0.0;
-                } else {           // Vertical
-                    N[nPlanes][0] = 0.0;
-                    N[nPlanes][1] = -sin(hmd_angle[sen][axis]);
-                    N[nPlanes][2] =  cos(hmd_angle[sen][axis]);
-                    D[nPlanes]    = 0.0;
-                }
+    //-------------------
+    // Add in the centroid point
+    //-------------------
+    trans[0] -= xbar[0]*R[0][0] + xbar[1]*R[0][1] + xbar[2]*R[0][2];
+    trans[1] -= xbar[0]*R[1][0] + xbar[1]*R[1][1] + xbar[2]*R[1][2];
+    trans[2] -= xbar[0]*R[2][0] + xbar[1]*R[2][1] + xbar[2]*R[2][2];
+    FLOAT transdist = sqrt( trans[0]*trans[0] + trans[1]*trans[1] + trans[2]*trans[2] );
 
-                printf("plane %d O %.3f %.3f %.3f %.3f  N %.3f %.3f %.3f  D %.3f\n",
-                    nPlanes,
-                    O[nPlanes][0], O[nPlanes][1], O[nPlanes][2], O[nPlanes][3], 
-                    N[nPlanes][0], N[nPlanes][1], N[nPlanes][2], 
-                    D[nPlanes]);
-                nPlanes++;
-            }
-        }
+    //-------------------
+    // Pack into the 4x4 transformation matrix
+    //-------------------
+    T[0][0]=R[0][0]; T[0][1]=R[0][1]; T[0][2]=R[0][2]; T[0][3]=trans[0];
+    T[1][0]=R[1][0]; T[1][1]=R[1][1]; T[1][2]=R[1][2]; T[1][3]=trans[1];
+    T[2][0]=R[2][0]; T[2][1]=R[2][1]; T[2][2]=R[2][2]; T[2][3]=trans[2];
+    T[3][0]=0.0;     T[3][1]=0.0;     T[3][2]=0.0;     T[3][3]=1.0;
+
+    PRINT_MAT(T,4,4);
+    //-------------------
+    // Plot the output points
+    //-------------------
+    for (i=0; i<nPoints; i++) {
+        float Tx = T[0][0]*X_in[0][i] + T[0][1]*X_in[1][i] + T[0][2]*X_in[2][i] + T[0][3];
+        float Ty = T[1][0]*X_in[0][i] + T[1][1]*X_in[1][i] + T[1][2]*X_in[2][i] + T[1][3];
+        float Tz = T[2][0]*X_in[0][i] + T[2][1]*X_in[1][i] + T[2][2]*X_in[2][i] + T[2][3];
+        S_out[0][i] = atan2(Tx, Ty);   // horiz
+        S_out[1][i] = atan2(Tz, Ty);   // vert
+        //S_out[0][i] = Tx;
+        //S_out[1][i] = Tz;
+        printf("point %i Txyz %f %f %f in %f %f out %f %f morph %f %f\n", i, Tx,Ty,Tz, S_in[0][i], S_in[1][i], S_out[0][i], S_out[1][i], S_morph[0][i], S_morph[1][i]);
     }
 
-    
-    printf("nPlanes %d\n", nPlanes);
-        
-    //}
-    
-    PRINT_MAT(Tcalc,4,4);
 
-    
-    //--------------------------------------------------
-    // Package the data for "OrthoSolve"
-    //--------------------------------------------------
 
-    // Data for the "fake" ortho solve formula
-    float Tortho[4][4];         // OUTPUT: 4x4 transformation matrix
-    FLOAT S_out[2][MAX_POINTS];  // INPUT:  array of screenspace points
-    FLOAT S_in[2][MAX_POINTS];  // INPUT:  array of screenspace points
-    FLOAT X_in[3][MAX_POINTS];  // INPUT:  array of offsets
-    int nPoints=0;  
 
-    // Transform into the "OrthoSolve" format
-    for (sen=0; sen<NUM_HMD; sen++)
-    {
-        if (hmd_angle[sen][0] != -9999.0 && hmd_angle[sen][1] != -9999.0)
-        {
-            S_in[0][nPoints] = hmd_angle[sen][0];
-            S_in[1][nPoints] = hmd_angle[sen][1];
-            X_in[0][nPoints] = hmd_pos[sen][0];
-            X_in[1][nPoints] = hmd_pos[sen][1];
-            X_in[2][nPoints] = hmd_pos[sen][2];
-            nPoints++;
-        }
-    }
-    printf("OrthoSolve nPoints %d\n", nPoints);
-    
-    //--------------------------------------------------
-    // Run the "OrthoSolve" and then the "AffineSolve"
-    //--------------------------------------------------    
-    
-    int loop;
-    for (loop=0; loop<1; loop++)
-    {
-        // Run OrthoSolve
-        OrthoSolve(
-            Tortho,     // OUTPUT: 4x4 transformation matrix
-            S_out,      // OUTPUT: array of output screenspace points
-            S_in,       // INPUT:  array of screenspace points
-            X_in,       // INPUT:  array of offsets
-            nPoints);
-    }
+	float quat[4];
+	float posoff[3] = { trans[0], trans[1], trans[2] };
+	float MT[4][4];
+	//matrix44transpose( MT, &T[0][0] );
+	matrix44copy( &MT[0][0], &T[0][0] );
 
-    // Run the calculation for Tcalc
-    //int run;
-    //for (run=0; run<100; run++) {
-/*
-    // Initialize Tcalc to the identity matrix
-    memcpy(Tcalc, Tortho, 4*4*sizeof(float));
-    //memset(Tcalc, 0, 4*4*sizeof(float));
-    //for (i=0; i<4; i++) { Tcalc[i][i] = 1.0f; }
+//QUAT: 0.657864 -0.305763 0.128486 0.265895 = 0.783252
+//QUAT: 0.657864 0.305763 -0.128486 -0.265895 = 0.783252
 
-    // Solve it!
-    AffineSolve(
-        Tcalc,           // OUTPUT: transform
-        O,  // INPUT:  points, offsets
-        N,  // INPUT:  plane normals
-        D,     // INPUT:  plane offsets
-        nPlanes, NITER,
-        STEP_SIZE_ROT, STEP_SIZE_POS, FALLOFF,
-        1);
-*/
-    // insert code here...
-    return 0;
+
+	quatfrommatrix( quat, &MT[0][0] );
+	printf( "QUAT: %f %f %f %f = %f\n", quat[0], quat[1], quat[2], quat[3], quatmagnitude(quat) );
+	//quat[2] -= 0.005; //fixes up lh0 in test data set.
+	quatnormalize( quat, quat );
+	printf( "QUAT: %f %f %f %f = %f\n", quat[0], quat[1], quat[2], quat[3], quatmagnitude(quat) );
+
+
+	for( i = 0; i <nPoints;i++ )
+	{
+		float pt[3] = { X_in[0][i], X_in[1][i], X_in[2][i] };
+		quatrotatevector( pt, quat, pt );
+		add3d( pt, pt, posoff );
+		printf( "%f %f %f OUT %f %f %f ANGLE %f %f AOUT %f %f\n", 
+			X_in[0][i], X_in[1][i], X_in[2][i], pt[0], pt[1], pt[2],
+			S_in[0][i], S_in[1][i], atan2( pt[0], pt[1] ), atan2( pt[2], pt[1] ) );
+	}
+	quattomatrix( &MT[0][0], quat );
+    PRINT_MAT(MT,4,4);
+
+
+
+//    printf("xbar %f %f %f\n", xbar[0], xbar[1], xbar[2]);
+//    printf("trans %f %f %f dist: %f\n", trans[0], trans[1], trans[2], transdist);
 }
+#endif
diff --git a/dave/Makefile b/dave/Makefile
index bf62837..330bda3 100644
--- a/dave/Makefile
+++ b/dave/Makefile
@@ -1,7 +1,21 @@
+UNAME:=$(shell uname)
+
+CFLAGS:= -lm -I../redist -I../src -I../include/libsurvive
+
+ifeq ($(UNAME), Linux)
+CFLAGS:= $(CFLAGS) -lGL -lGLU -lglut -lX11 -I../../redist -DLINUX -lm -lpthread -DLINUX
+endif
+
+# Darwin is Mac OSX !!
+ifeq ($(UNAME), Darwin)
+CFLAGS:= $(CFLAGS) -I../../redist -w -framework OpenGL -framework GLUT
+endif
+
+
 all:
 #	gcc -O3 -o kalman_filter kalman_filter.c main.c
-	gcc -O3 -o dclapack_test dclapack_test.c -lm
-	gcc  -O0 -g -o AffineSolve   AffineSolve.c -lm -I../redist ../redist/linmath.c #-Wall
-
+	gcc -O3 -o dclapack_test dclapack_test.c $(CFLAGS)
+	gcc  -O3 -o AffineSolve   AffineSolve.c $(CFLAGS) ../redist/linmath.c #-Wall
+	gcc -O3 -o OrthoPlot OrthoPlot.c fileutil.c ../redist/linmath.c ../redist/os_generic.c $(CFLAGS)
 clean:
 	rm -f kalman_filter dclapack_test
diff --git a/dave/OrthoPlot.c b/dave/OrthoPlot.c
new file mode 100644
index 0000000..222828c
--- /dev/null
+++ b/dave/OrthoPlot.c
@@ -0,0 +1,451 @@
+/*
+ When creating your project, uncheck OWL,
+ uncheck Class Library, select Static
+ instead of Dynamic and change the target
+ model to Console from GUI.
+ Also link glut.lib to your project once its done.
+ */
+
+#include <stdio.h>   // Standard Header For Most Programs
+#include <stdlib.h>
+#include <string.h>
+#include <math.h>
+#include "os_generic.h"
+#include "linmath.h"
+#include "fileutil.h"
+
+#ifdef __APPLE__
+#include <OpenGL/gl.h>   // The GL Header File
+#include <GLUT/glut.h>   // The GL Utility Toolkit (Glut) Header
+#else
+#include <GL/gl.h>
+#include <GL/glu.h>
+#endif
+#ifdef __linux__
+#include <GL/freeglut.h>
+#endif
+
+#define RegisterDriver(a,b)
+#include "poser_daveortho.c"
+
+
+// Required to set up a window
+#define WIDTH  1280
+#define HEIGHT 1280
+#define FULLSCREEN 0
+int    keys[256];               // Regular keyboard keys
+int sp_keys[256];               // Special keyboard keycodes (GLUT specific)
+
+#define LH_ID    0
+#define NUM_HMD 32
+#define INDIR "dave/full_test_triangle_on_floor/"
+#define MAX_POINTS SENSORS_PER_OBJECT
+
+#define PI 3.1415926535897932386264
+#define MOVESPEED 1.0
+#define ROTSPEED  5.0
+
+// View space
+float posx=0.0f;
+float posy=0.0f;
+float posz=0.0f;
+float rotx=0.0f;
+float roty=0.0f;
+float rotz=0.0f;
+
+// Data for the "fake" ortho solve formula
+float Tortho[4][4];         // OUTPUT: 4x4 transformation matrix
+FLOAT S_out[2][MAX_POINTS];  // INPUT:  array of screenspace points
+FLOAT S_in[2][MAX_POINTS];  // INPUT:  array of screenspace points
+FLOAT X_in[3][MAX_POINTS];  // INPUT:  array of offsets
+int nPoints=0;  
+
+//--------------------------------------------------------------------
+// 
+//--------------------------------------------------------------------
+
+void DrawGrid(
+	float minX, float maxX,
+	float minY, float maxY,
+	float minZ, float maxZ,
+	float stepX, float stepY, float stepZ);
+
+void DrawCoordinateSystem(
+	float x, float y, float z,
+	float qx, float qy, float qz, float qr);
+
+
+float hmd_pos[NUM_HMD][3];
+void ReadHmdPoints()
+{
+    int i;
+    FILE *fin = fopen(INDIR "HMD_points.csv","r");
+    if (fin==NULL) {
+        printf("ERROR: could not open " INDIR "HMD_points.csv for reading\n");
+        exit(1);
+    }
+    
+    for (i=0; i<NUM_HMD; i++) {
+        fscanf(fin, "%f %f %f", &(hmd_pos[i][0]), &(hmd_pos[i][1]), &(hmd_pos[i][2]));
+    }
+    
+    fclose(fin);
+}
+
+float hmd_angle[NUM_HMD][2];
+void ReadPtinfo()
+{
+    // Initialize to -9999
+    int i;
+    for (i=0; i<NUM_HMD; i++) { hmd_angle[i][0]=-9999.0; hmd_angle[i][1]=-9999.0; }
+
+    // Read ptinfo.csv
+    FILE *fin = fopen(INDIR "ptinfo.csv", "r");
+    if (fin==NULL) { printf("ERROR: could not open ptinfo.csv for reading\n"); exit(1); }
+    while (!feof(fin))
+    {
+        // Read the angle
+        int sen,lh,axis,count;
+        float angle, avglen, stddevang, stddevlen;
+        float max_outlier_length, max_outlier_angle;
+        int rt = fscanf( fin, "%d %d %d %d %f %f %f %f %f %f\n",
+                        &sen, &lh, &axis, &count,
+                        &angle, &avglen, &stddevang, &stddevlen,
+                        &max_outlier_length, &max_outlier_angle);
+        if (rt != 10) { break; }
+        
+        // If it's valid, store in the result
+        if (lh == LH_ID && sen < NUM_HMD) {
+            hmd_angle[sen][axis] = angle;
+        }
+    }
+    fclose(fin);
+}
+
+
+//--------------------------------------------------------------------
+// 
+//--------------------------------------------------------------------
+
+/*
+ * init() is called at program startup
+ */
+void init()
+{
+    int i,j,k,sen,axis;
+        
+    // Read the data files
+    ReadHmdPoints();
+    ReadPtinfo();
+
+    //--------------------------------------------------
+    // Package the data for "OrthoSolve"
+    //--------------------------------------------------
+
+    // Transform into the "OrthoSolve" format
+    for (sen=0; sen<NUM_HMD; sen++)
+    {
+        if (hmd_angle[sen][0] != -9999.0 && hmd_angle[sen][1] != -9999.0)
+        {
+            S_in[0][nPoints] = hmd_angle[sen][0];
+            S_in[1][nPoints] = hmd_angle[sen][1];
+            X_in[0][nPoints] = hmd_pos[sen][0];
+            X_in[1][nPoints] = hmd_pos[sen][1];
+            X_in[2][nPoints] = hmd_pos[sen][2];
+            nPoints++;
+        }
+    }
+    printf("OrthoSolve nPoints %d\n", nPoints);
+    //--------------------------------------------------
+    // Run the "OrthoSolve" and then the "AffineSolve"
+    //--------------------------------------------------    
+    
+    // Run OrthoSolve
+    OrthoSolve(
+		Tortho,     // OUTPUT: 4x4 transformation matrix
+		S_out,      // OUTPUT: array of output screenspace points
+		S_in,       // INPUT:  array of screenspace points
+		X_in,       // INPUT:  array of offsets
+		nPoints);
+		//printf( "POS: %f %f %f\n", Tortho[0][3], Tortho[1][3], Tortho[2][3]);
+
+	//--------------------------------------------------
+	// Spawn a thread to read the HMD angles
+	//--------------------------------------------------
+	OGCreateThread(ThreadReadHmtAngles,0);
+
+    //--------------------------------------------------
+	// Initialize OpenGL
+    //--------------------------------------------------
+	glShadeModel(GL_SMOOTH);							// Enable Smooth Shading
+	glClearColor(0.0f, 0.0f, 0.0f, 1.0f);				// Black Background
+	glClearDepth(1.0f);									// Depth Buffer Setup
+	glEnable(GL_DEPTH_TEST);							// Enables Depth Testing
+	glDepthFunc(GL_LEQUAL);								// The Type Of Depth Testing To Do
+	glEnable ( GL_COLOR_MATERIAL );
+	glDisable(GL_CULL_FACE);
+	glHint(GL_PERSPECTIVE_CORRECTION_HINT, GL_NICEST);
+}
+
+/*
+ * draw() is called once every frame
+ */
+void draw()
+{
+	int i,j;
+
+    //------------------------
+    // Check for keyboard input
+    //------------------------
+    if (keys['w'] || keys['W']) {
+        posz += MOVESPEED;
+    }
+    if (keys['s'] || keys['S']) {
+        posz -= MOVESPEED;
+    }
+    if (keys['a'] || keys['A']) {
+        roty += ROTSPEED;
+    }
+    if (keys['d'] || keys['D']) {
+        roty -= ROTSPEED;
+    }
+    if (keys[27]) {
+        exit(0);
+    }
+    
+    //------------------------
+    // Update the scene
+    //------------------------
+    
+    //------------------------
+    // Draw using OpenGL
+    //------------------------
+    
+    // Clear the screen
+    glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);	// Clear Screen And Depth Buffer
+ 
+    // Translate and rotate the camera
+    glLoadIdentity();  // Reset The Current Modelview Matrix
+//    glTranslatef(-posx,-posy,posz);						// Move Left 1.5 Units And Into The    
+
+	// Bouning box around the points (in radians)
+	float x0=-45.0 * (PI/180.0);
+	float x1= 45.0 * (PI/180.0);
+	float y0=-45.0 * (PI/180.0);
+	float y1= 45.0 * (PI/180.0);
+
+
+	//------------------------
+	// Read the angles from stdin
+	//------------------------
+	FLT hmdAngles[4][NUM_HMD];
+
+	// Read the hmd angles
+	OGLockMutex(read_mutex);
+	for (i=0; i<4; i++) {
+		for (j=0; j<NUM_HMD; j++) {
+			hmdAngles[i][j] = read_hmdAngles[i][j];
+		}
+	}
+	OGUnlockMutex(read_mutex);
+
+	// Draw the hmd bearing angles
+	nPoints=0;
+	for (i=0; i<NUM_HMD; i++) {
+		const double dist=10.0;
+		
+		int sweepx = (LH_ID==0) ? SWEEP_LX : SWEEP_RX;
+		int sweepy = (LH_ID==0) ? SWEEP_LY : SWEEP_RY;
+
+		// If the left lighthouse sees it
+		if (read_hmdAngleViewed[sweepx][i] >= read_frameno-6 &&
+		    read_hmdAngleViewed[sweepy][i] >= read_frameno-6 &&
+		    hmdAngles[sweepx][i]!=-9999.0 && hmdAngles[sweepy][i]!=-9999.0)
+		{
+			S_in[0][nPoints] = hmdAngles[sweepy][i];
+			S_in[1][nPoints] = hmdAngles[sweepx][i];
+            X_in[0][nPoints] = hmd_pos[i][0];
+            X_in[1][nPoints] = hmd_pos[i][1];
+            X_in[2][nPoints] = hmd_pos[i][2];
+printf("i %d S %f %f X %f %f %f frno %d %d currfr %d\n",
+	i, S_in[0][nPoints], S_in[1][nPoints],
+	X_in[0][nPoints], X_in[1][nPoints], X_in[2][nPoints],
+	read_hmdAngleViewed[sweepx][i], read_hmdAngleViewed[sweepy][i], read_frameno);
+			nPoints++;
+		}
+	}
+
+	read_frameno++;
+
+    //--------------------------------------------------
+    // Run the "OrthoSolve" and then the "AffineSolve"
+    //--------------------------------------------------    
+    
+    // Run OrthoSolve
+    OrthoSolve(
+		Tortho,     // OUTPUT: 4x4 transformation matrix
+		S_out,      // OUTPUT: array of output screenspace points
+		S_in,       // INPUT:  array of screenspace points
+		X_in,       // INPUT:  array of offsets
+		nPoints);
+		printf( "POS: %f %f %f\n", Tortho[0][3], Tortho[1][3], Tortho[2][3]);
+
+
+	//------------------------
+	// Draw the inputs
+	//------------------------
+	glPointSize(3.0);
+
+	// Draw the input points
+	glColor3f(1.0,0.5,0.5);
+	glBegin(GL_POINTS);
+	for (i=0; i<nPoints; i++) {
+		glVertex2f(  (S_in[0][i]-x0)/(x1-x0),  (S_in[1][i]-y0)/(y1-y0)  );
+	}
+	glEnd();
+
+	// Draw the output points
+	glColor3f(0.5,0.5,1.0);
+	glBegin(GL_POINTS);
+	for (i=0; i<nPoints; i++) {
+		glVertex2f(  (S_out[0][i]-x0)/(x1-x0),  (S_out[1][i]-y0)/(y1-y0)  );
+	}
+	glEnd();
+
+
+    //
+    // We're Done
+    //
+    glutSwapBuffers ( );         // Swap The Buffers To Not Be Left With A Clear Screen
+}
+
+/*
+ * resize() is called when we change the screen size
+ */
+void resize(int width, int height)		// Resize And Initialize The GL Window
+{
+    glViewport(0,0,width,height);						// Reset The Current Viewport
+    
+    glMatrixMode(GL_PROJECTION);						// Select The Projection Matrix
+    glLoadIdentity();									// Reset The Projection Matrix
+    
+    // Uncomment For a 3D perspective
+    //gluPerspective(45.0f,(float)width/(float)height,0.1f,1000.0f);
+    // Uncomment for a 2D perspective
+    glOrtho(0.0, 1.0, 0.0, 1.0, -10.0, 10.0);
+    
+    glMatrixMode(GL_MODELVIEW);							// Select The Modelview Matrix
+    glLoadIdentity();									// Reset The Modelview Matrix
+}
+
+/*
+ * These functions are called whenever a key is pressed
+ */
+void keyboardDown ( unsigned char key, int x, int y )  // Create Keyboard Function
+{
+    keys[key] = 1;
+}
+void keyboardUp ( unsigned char key, int x, int y )
+{
+    keys[key] = 0;
+}
+
+void specialKeyDown ( int key, int x, int y )  // Create Special Function (required for arrow keys)
+{
+    if (key<256) {
+        sp_keys[key] = 1;
+    }
+}
+void specialKeyUp (int key, int x, int y)
+{
+    if (key<256) {
+        sp_keys[key] = 0;
+    }
+}
+
+int main ( int argc, char** argv )   // Create Main Function For Bringing It All Together
+{
+    glutInit            ( &argc, argv ); // Erm Just Write It =)
+    glutInitDisplayMode ( GLUT_RGB | GLUT_DEPTH | GLUT_DOUBLE ); // Display Mode
+    glutInitWindowSize  ( WIDTH, HEIGHT ); // If glutFullScreen wasn't called this is the window size
+    glutCreateWindow    ( "OpenGL" ); // Window Title (argv[0] for current directory as title)
+    if (FULLSCREEN) {
+        glutFullScreen      ( );          // Put Into Full Screen
+    }
+    glutDisplayFunc     ( draw );  // Matching Earlier Functions To Their Counterparts
+    glutIdleFunc        ( draw );
+    glutReshapeFunc     ( resize );
+    glutKeyboardFunc    ( keyboardDown );
+    glutKeyboardUpFunc  ( keyboardUp );
+    glutSpecialFunc     ( specialKeyDown );
+    glutSpecialUpFunc   ( specialKeyUp );
+    init ();
+    glutMainLoop        ( );          // Initialize The Main Loop
+    return 0;
+}
+
+
+
+void DrawGrid(
+	float minX, float maxX,
+	float minY, float maxY,
+	float minZ, float maxZ,
+	float stepX, float stepY, float stepZ)
+{
+	float x,y,z;
+	
+	glBegin(GL_LINES);
+
+	// X grid stripes
+	for (y=minY; y<maxY; y+=stepY) {
+		for (z=minZ; z<maxZ; z+=stepZ) {
+			glVertex3f(minX, y, z);
+			glVertex3f(maxX, y, z);
+		}
+	}
+
+	// Y grid stripes
+	for (x=minX; x<maxX; x+=stepX) {
+		for (z=minZ; z<maxZ; z+=stepZ) {
+			glVertex3f(x, minY, z);
+			glVertex3f(x, maxY, z);
+		}
+	}
+
+	// Z grid stripes
+	for (y=minY; y<maxY; y+=stepY) {
+		for (x=minX; x<maxX; x+=stepX) {
+			glVertex3f(x, y, minZ);
+			glVertex3f(x, y, maxZ);
+		}
+	}
+
+	glEnd();
+}
+
+
+void DrawCoordinateSystem(
+	float x, float y, float z,
+	float qx, float qy, float qz, float qr)
+{
+	FLT i0[3],j0[3],k0[3];
+	FLT i[3],j[3],k[3];
+	FLT q[4];
+	
+	i0[0]=1.0; i0[1]=0.0; i0[2]=0.0;
+	j0[0]=0.0; j0[1]=1.0; j0[2]=0.0;
+	k0[0]=0.0; k0[1]=0.0; k0[2]=1.0;
+	q [0]=qr;  q [1]=qx;  q [2]=qy;  q [3]=qz;
+	
+	quatrotatevector(i, q, i0);
+	quatrotatevector(j, q, j0);
+	quatrotatevector(k, q, k0);
+
+	glBegin(GL_LINES);
+	glColor3f(1, 0, 0); glVertex3f(x,z,y); glVertex3f(x+i[0],z+i[2],y+i[1]);
+	glColor3f(0, 1, 0); glVertex3f(x,z,y); glVertex3f(x+j[0],z+j[2],y+j[1]);
+	glColor3f(0, 0, 1); glVertex3f(x,z,y); glVertex3f(x+k[0],z+k[2],y+k[1]);
+	glEnd();
+}
+
+
diff --git a/dave/dclapack_test.c b/dave/dclapack_test.c
index b0667ef..3f48b08 100644
--- a/dave/dclapack_test.c
+++ b/dave/dclapack_test.c
@@ -1,6 +1,6 @@
 #define FLOAT float
 #define ORDER 50
-#include "dclapack.h"
+#include "../redist/dclapack.h"
 
 #include <stdio.h>
 #include <stdlib.h>
diff --git a/dave/fileutil.c b/dave/fileutil.c
new file mode 100644
index 0000000..04dc241
--- /dev/null
+++ b/dave/fileutil.c
@@ -0,0 +1,133 @@
+#include "fileutil.h"
+#include <stdio.h>
+#include <stdlib.h>
+
+#define PI 3.14159265358979323846264
+
+
+og_mutex_t  read_mutex;
+og_thread_t read_thread;
+double read_hmdAngles[NUM_SWEEP][NUM_HMD];
+int read_hmdAngleViewed[NUM_SWEEP][NUM_HMD];
+int read_frameno=0;
+
+static FILE *fopen_orDie(const char *path, const char *flag)
+{
+	FILE *f = fopen(path, flag);
+	if (f == NULL) {
+		printf("ERROR could not oepn %s for %s\n", path, flag);
+		exit(1);
+	}
+	return f;
+}
+
+static void SeekToken(FILE *f, const char *keyword)
+{
+	char token[4096];
+	do {
+		fscanf(f, "%s", token);
+	} while( strcmp(token,keyword)!=0 && !feof(f) );
+}
+
+void LoadLighthousePos(
+        const char *path,
+        float *x, float *y, float *z,
+        float *qi, float *qj, float *qk, float *qreal)
+{
+	FILE *f = fopen_orDie(path,"r");
+	SeekToken(f, "POS:");
+	fscanf(f, "%f %f %f\n", x, y, z);
+	SeekToken(f, "QUAT:");
+	fscanf(f, "%f %f %f %f\n", qreal, qi, qj, qk);
+	fclose (f);
+}
+
+void LoadHmdProcessedDataAngles(
+	const char *path,
+	double angles[NUM_SWEEP][NUM_HMD])
+{
+	int i,j;
+	char   type[256];
+	char   sweep[256];
+	int    id;
+	int    nSweep;
+	double ang;
+	double d1,d2,d3;  // revisit these numbers later
+
+	// Initialize all of the angles to -9999
+	for (i=0; i<NUM_SWEEP; i++) {
+		for (j=0; j<NUM_HMD; j++) {
+			angles[i][j] = -9999.0;   // Initially no value
+		}
+	}
+
+	FILE *f = fopen_orDie(path, "r");
+
+	while (!feof(f))
+	{
+		// Read the line from the file
+		int rt=fscanf(f, "%s %s %d %d %lf %lf %lf %lf",
+			&type, &sweep, &id, &nSweep, &ang,
+			&d1,&d2,&d3);
+
+		if (rt<8) { break; }
+
+		// Only hmd points
+		if ( strcmp(type,"HMD")!=0 ) { continue; }
+
+		// Which sweep is it?
+		int sweepId=-1;
+		if      ( strcmp(sweep,"LX")==0 ) { sweepId=SWEEP_LX; }
+		else if ( strcmp(sweep,"LY")==0 ) { sweepId=SWEEP_LY; }
+		else if ( strcmp(sweep,"RX")==0 ) { sweepId=SWEEP_RX; }
+		else if ( strcmp(sweep,"RY")==0 ) { sweepId=SWEEP_RY; }
+		else { continue; }
+		
+		// Convert the angle from ticks to radians
+		angles[sweepId][id] = (PI / 400000.0) * ( ang-200000.0 );
+	}
+
+	fclose(f);
+}
+
+void *ThreadReadHmtAngles(void *junk)
+{
+	char house[256];
+	char xy[256];
+	char hmd[256];
+	double ts;
+	int id;
+	int syncid;
+	int timeInSweep;
+	int length;
+	//lighthouse sweep hmdOrwmd timestamp id syncid timeinsweep length
+
+	while(1) {
+		int rt=scanf("%s %s %s %lf %d %d %d %d", house, xy, hmd, &ts, &id, &syncid, &timeInSweep, &length);
+		if (rt==8)
+		{
+			//int rt=scanf("%s %s %s %lf %d %d %d %d", house, xy, hmd, &ts, &id, &syncid, &timeInSweep, &length);
+			//printf( "%s %s %s %f %d %d %d %d\n", house, xy, hmd, ts,id, syncid, timeInSweep, length );
+
+			if( id < 0 ) continue;
+			int sweepId=0;
+			if ( house[0]=='R' ) { sweepId+=2; }
+			if ( xy[0]   =='Y' ) { sweepId++; }
+			double angle = (PI / 400000.0) * ( (double)timeInSweep-200000.0 );
+		
+			if      ( strcmp(hmd,"HMD")==0 ) { id += 0;  } 
+			else if ( strcmp(hmd,"WM0")==0 ) { id += 32; }
+			else if ( strcmp(hmd,"WM1")==0 ) { id += 56; }
+			else    { continue; }
+		
+			if ( id<0 || id >NUM_HMD) { continue; }
+
+			OGLockMutex  (read_mutex);
+			read_hmdAngles[sweepId][id] = angle;
+			OGUnlockMutex(read_mutex);
+			read_hmdAngleViewed[sweepId][id] = read_frameno;
+		}
+	}
+}
+
+
diff --git a/dave/fileutil.h b/dave/fileutil.h
new file mode 100644
index 0000000..e5da244
--- /dev/null
+++ b/dave/fileutil.h
@@ -0,0 +1,35 @@
+#ifndef _fileutil_h_
+#define _fileutil_h_
+
+#include <pthread.h>
+#include "os_generic.h"
+
+void LoadLighthousePos(
+	const char *path, 
+	float *x, float *y, float *z,
+	float *qi, float *qj, float *qk, float *qreal);
+
+
+// first 32 are hmd, next 24 wm0 next 24 wm1
+#define NUM_HMD   80
+#define NUM_SWEEP 4
+#define SWEEP_LX  0
+#define SWEEP_LY  1
+#define SWEEP_RX  2
+#define SWEEP_RY  3
+void LoadHmdProcessedDataAngles(
+	const char *path,
+	double angle[NUM_SWEEP][NUM_HMD]);
+
+
+extern og_mutex_t  read_mutex;
+extern og_thread_t read_thread;
+extern double read_hmdAngles[NUM_SWEEP][NUM_HMD];
+extern int read_hmdAngleViewed[NUM_SWEEP][NUM_HMD];
+extern int read_frameno;
+void *ThreadReadHmtAngles(void *junk);
+
+
+#endif // __fileutil_h_
+
+
diff --git a/src/poser_daveortho.c b/src/poser_daveortho.c
index e81e154..80f65a9 100644
--- a/src/poser_daveortho.c
+++ b/src/poser_daveortho.c
@@ -265,7 +265,8 @@ printf("rhat %f %f (len %f)\n", rhat[0][0], rhat[1][0], rhat_len);
 //    FLT ydist1 = 1.0 /  uhat_len; //0.25*PI / uhat_len;
 //    FLT ydist2 = 1.0 /  rhat_len; //0.25*PI / rhat_len;
     FLT ydist  = 1.0 / urhat_len;
-    //printf("ydist1 %f ydist2 %f ydist %f\n", ydist1, ydist2, ydist);
+    printf("ydist %f\n", ydist);
+//    printf("ydist1 %f ydist2 %f ydist %f\n", ydist1, ydist2, ydist);
 
     //--------------------
     // Rescale the axies to be of the proper length
@@ -282,7 +283,7 @@ printf("rhat %f %f (len %f)\n", rhat[0][0], rhat[1][0], rhat_len);
 	if( x_y != x_y ) x_y = 0;
 	if( y_y != y_y ) y_y = 0;
 	if( z_y != z_y ) z_y = 0;
-/*
+
     // Exhaustively flip the minus sign of the z axis until we find the right one . . .
     FLT bestErr = 9999.0;
     FLT xy_dot2 = x[0][0]*y[0][0] + x[2][0]*y[2][0];
@@ -302,7 +303,7 @@ printf("rhat %f %f (len %f)\n", rhat[0][0], rhat[1][0], rhat_len);
                 FLT cx,cy,cz;
                 CrossProduct(cx,cy,cz,x[0][0],x_y,x[2][0],y[0][0],y_y,y[2][0]);
                 FLT hand = cx*z[0][0] + cy*z_y + cz*z[2][0];
-                printf("err %f hand %f\n", err, hand);
+//                printf("err %f hand %f\n", err, hand);
                 
                 // If we are the best right-handed frame so far
                 //if (hand > 0 && err < bestErr) { x[1][0]=x_y; y[1][0]=y_y; z[1][0]=z_y; bestErr=err; }
@@ -313,9 +314,9 @@ printf("rhat %f %f (len %f)\n", rhat[0][0], rhat[1][0], rhat_len);
         }
         x_y = -x_y;
     }
-    printf("bestErr %f\n", bestErr);
-*/
+//    printf("bestErr %f\n", bestErr);
 
+/*
     //-------------------------
     // A test version of the rescaling to the proper length
     //-------------------------
@@ -338,7 +339,7 @@ printf("rhat %f %f (len %f)\n", rhat[0][0], rhat[1][0], rhat_len);
 		if( y_y != y_y ) y_y = 0;
 		if( z_y != z_y ) z_y = 0;
 
-		printf( "---> %f %f %f\n", x_y, y_y, z_y );
+//		printf( "---> %f %f %f\n", x_y, y_y, z_y );
 
         // Exhaustively flip the minus sign of the z axis until we find the right one . . .
         FLT bestErr = 9999.0;
@@ -359,7 +360,7 @@ printf("rhat %f %f (len %f)\n", rhat[0][0], rhat[1][0], rhat_len);
                     FLT cx,cy,cz;
                     CrossProduct(cx,cy,cz,x2[0][0],x_y,x2[2][0],y2[0][0],y_y,y2[2][0]);
                     FLT hand = cx*z2[0][0] + cy*z_y + cz*z2[2][0];
-                  printf("err %f hand %f\n", err, hand);
+                  //printf("err %f hand %f\n", err, hand);
                 
                     // If we are the best right-handed frame so far
                     if (hand > 0 && err < bestErr) { x2[1][0]=x_y; y2[1][0]=y_y; z2[1][0]=z_y; bestErr=err; }
@@ -380,7 +381,7 @@ printf("rhat %f %f (len %f)\n", rhat[0][0], rhat[1][0], rhat_len);
         }
     }
     ydist = bestYdist;
-
+*/
 /*
     for (i=0; i<nPoints; i++) {
         FLT x1 = x[0][0]*X[0][i] + y[0][0]*X[1][i] + z[0][0]*X[2][i];
@@ -441,7 +442,71 @@ PRINT(ab,2,1);
     T[2][0]=R[2][0]; T[2][1]=R[2][1]; T[2][2]=R[2][2]; T[2][3]=trans[2];
     T[3][0]=0.0;     T[3][1]=0.0;     T[3][2]=0.0;     T[3][3]=1.0;
 
-    PRINT_MAT(T,4,4);
+
+	FLT T2[4][4];
+
+	//-------------------
+	// Orthogonalize the matrix
+	//-------------------
+	FLT temp[4][4];
+	FLT quat[4], quatNorm[4];
+	FLT euler[3];
+
+
+	//-------------------
+	// Orthogonalize the matrix
+	//-------------------
+
+	PRINT_MAT(T,4,4);
+matrix44transpose(T2, T);
+//matrix44copy(T2,T);
+	cross3d( &T2[0][0], &T2[1][0], &T2[2][0] );
+	cross3d( &T2[2][0], &T2[0][0], &T2[1][0] );
+	normalize3d( &T2[0][0], &T2[0][0] );
+	normalize3d( &T2[1][0], &T2[1][0] );
+	normalize3d( &T2[2][0], &T2[2][0] );
+//matrix44copy(T2,T);
+matrix44transpose(T, T2);
+	PRINT_MAT(T,4,4);
+
+
+
+	//memcpy(T2,T,16*sizeof(float));
+//	matrix44copy(T2,T);
+	matrix44transpose(T2,T);
+	quatfrommatrix( quat, &T2[0][0] );
+	quatnormalize(quatNorm,quat);
+	quattoeuler(euler,quatNorm);
+	quattomatrix( T2, quatNorm );
+	printf("rot %f %f %f len %f\n", euler[0], euler[1], euler[2], quatmagnitude(quat));
+	PRINT(T,4,4);
+
+//	matrix44copy(temp,T2);
+	matrix44transpose(temp,T2);
+
+	memcpy(T2,temp,16*sizeof(float));
+	for (i=0; i<3; i++) {
+		for (j=0; j<3; j++) {
+			T[i][j] = T2[j][i];
+		}
+	}
+	PRINT(T2,4,4);
+
+
+/*
+    CrossProduct(T[0][2],T[1][2],T[2][2],  T[0][0],T[1][0],T[2][0],  T[0][1],T[1][1],T[2][1]);
+    CrossProduct(T[0][0],T[1][0],T[2][0],  T[0][1],T[1][1],T[2][1],  T[0][2],T[1][2],T[2][2]);
+    CrossProduct(T[0][1],T[1][1],T[2][1],  T[0][2],T[1][2],T[2][2],  T[0][0],T[1][0],T[2][0]);
+	float xlen = sqrt(T[0][0]*T[0][0] + T[1][0]*T[1][0] + T[2][0]*T[2][0]);
+	float ylen = sqrt(T[0][1]*T[0][1] + T[1][1]*T[1][1] + T[2][1]*T[2][1]);
+	float zlen = sqrt(T[0][2]*T[0][2] + T[1][0]*T[1][2] + T[2][2]*T[2][2]);
+	T[0][0]/=xlen; T[1][0]/=xlen; T[2][0]/=xlen;
+	T[0][1]/=ylen; T[1][1]/=ylen; T[2][1]/=ylen;
+	T[0][2]/=zlen; T[1][2]/=zlen; T[2][2]/=zlen;
+*/
+
+
+ //   PRINT_MAT(T,4,4);
     //-------------------
     // Plot the output points
     //-------------------
@@ -453,7 +518,7 @@ PRINT(ab,2,1);
         S_out[1][i] = atan2(Tz, Ty);   // vert
         //S_out[0][i] = Tx;
         //S_out[1][i] = Tz;
-        printf("point %i Txyz %f %f %f in %f %f out %f %f morph %f %f\n", i, Tx,Ty,Tz, S_in[0][i], S_in[1][i], S_out[0][i], S_out[1][i], S_morph[0][i], S_morph[1][i]);
+//        printf("point %i Txyz %f %f %f in %f %f out %f %f morph %f %f\n", i, Tx,Ty,Tz, S_in[0][i], S_in[1][i], S_out[0][i], S_out[1][i], S_morph[0][i], S_morph[1][i]);
     }
 
 }
diff --git a/src/survive_cal.h b/src/survive_cal.h
index c64b8f9..45b77f6 100644
--- a/src/survive_cal.h
+++ b/src/survive_cal.h
@@ -37,7 +37,7 @@ void survive_cal_angle( SurviveObject * so, int sensor_id, int acode, uint32_t t
 #define MIN_PTS_BEFORE_CAL 24
 
 
-#define DRPTS 1024  //Number of samples required in collection phase.
+#define DRPTS 32  //Number of samples required in collection phase.
 
 #define POSE_OBJECTS 3
 
diff --git a/src/survive_data.c b/src/survive_data.c
index 8c5c646..0873f7f 100644
--- a/src/survive_data.c
+++ b/src/survive_data.c
@@ -119,7 +119,7 @@ void handle_lightcap( SurviveObject * so, LightcapElement * le )
 		{
 			int32_t main_divisor = so->timebase_hz / 384000; //125 @ 48 MHz.
 			int base_station = is_new_pulse;
-			printf( "%s %d %d %d\n", so->codename, le->sensor_id, so->sync_set_number, le->length );
+			//printf( "%s %d %d %d\n", so->codename, le->sensor_id, so->sync_set_number, le->length );
 			ctx->lightproc( so, le->sensor_id, -3 - so->sync_set_number, 0, le->timestamp, le->length );
 		}
 	}
-- 
cgit v1.3.1


From 472d05c2a356ec6d71669d67fb599e401a6f4a76 Mon Sep 17 00:00:00 2001
From: Mike Turvey <mturvey6@gmail.com>
Date: Thu, 23 Mar 2017 00:25:25 -0700
Subject: Updated disambiguator.  Solid OOTX

---
 calibrate.c                  |   4 +-
 data_recorder.c              |   4 +-
 include/libsurvive/survive.h |   3 +-
 src/survive_cal.c            |   9 +-
 src/survive_data.c           | 211 +++++++++++++++++++++++++++++++++++++++----
 src/survive_process.c        |   6 +-
 6 files changed, 208 insertions(+), 29 deletions(-)

(limited to 'src/survive_data.c')

diff --git a/calibrate.c b/calibrate.c
index 726a8cc..0f2d6ac 100644
--- a/calibrate.c
+++ b/calibrate.c
@@ -51,10 +51,10 @@ int bufferpts[32*2*3][2];
 char buffermts[32*128*3];
 int buffertimeto[32*3][2];
 
-void my_light_process( struct SurviveObject * so, int sensor_id, int acode, int timeinsweep, uint32_t timecode, uint32_t length  )
+void my_light_process( struct SurviveObject * so, int sensor_id, int acode, int timeinsweep, uint32_t timecode, uint32_t length, int lh)
 {
 //	if( timeinsweep < 0 ) return;
-	survive_default_light_process( so, sensor_id, acode, timeinsweep, timecode, length );
+	survive_default_light_process( so, sensor_id, acode, timeinsweep, timecode, length, lh);
 	if( sensor_id < 0 ) return;
 	if( acode == -1 ) return;
 //return;
diff --git a/data_recorder.c b/data_recorder.c
index 46f3427..6432f4f 100644
--- a/data_recorder.c
+++ b/data_recorder.c
@@ -43,9 +43,9 @@ int bufferpts[32*2*3];
 char buffermts[32*128*3];
 int buffertimeto[32*3];
 
-void my_light_process( struct SurviveObject * so, int sensor_id, int acode, int timeinsweep, uint32_t timecode, uint32_t length  )
+void my_light_process( struct SurviveObject * so, int sensor_id, int acode, int timeinsweep, uint32_t timecode, uint32_t length, int lh)
 {
-	survive_default_light_process( so, sensor_id, acode, timeinsweep, timecode, length );
+	survive_default_light_process( so, sensor_id, acode, timeinsweep, timecode, length, lh);
 
 	if( acode == -1 ) return;
 //return;
diff --git a/include/libsurvive/survive.h b/include/libsurvive/survive.h
index e04586c..c3b6a03 100644
--- a/include/libsurvive/survive.h
+++ b/include/libsurvive/survive.h
@@ -47,6 +47,7 @@ struct SurviveObject
 	int32_t pulse_synctime_slack; //5,000 for normal vive hardware.    (guessed)
 
 	//Flood info, for calculating which laser is currently sweeping.
+	void * disambiguator_data;
 	int8_t   oldcode;
 	int8_t   sync_set_number; //0 = master, 1 = slave, -1 = fault. 
 	int8_t   did_handle_ootx; //If unset, will send lightcap data for sync pulses next time a sensor is hit.
@@ -129,7 +130,7 @@ void survive_cal_install( SurviveContext * ctx );  //XXX This will be removed if
 
 //Call these from your callback if overridden.  
 //Accept higher-level data.
-void survive_default_light_process( SurviveObject * so, int sensor_id, int acode, int timeinsweep, uint32_t timecode, uint32_t length );
+void survive_default_light_process( SurviveObject * so, int sensor_id, int acode, int timeinsweep, uint32_t timecode, uint32_t length , int lh);
 void survive_default_imu_process( SurviveObject * so, int mode, FLT * accelgyro, uint32_t timecode, int id );
 void survive_default_angle_process( SurviveObject * so, int sensor_id, int acode, uint32_t timecode, FLT length, FLT angle );
 
diff --git a/src/survive_cal.c b/src/survive_cal.c
index c36a48a..51d12dd 100755
--- a/src/survive_cal.c
+++ b/src/survive_cal.c
@@ -190,7 +190,7 @@ void survive_cal_install( struct SurviveContext * ctx )
 }
 
 
-void survive_cal_light( struct SurviveObject * so, int sensor_id, int acode, int timeinsweep, uint32_t timecode, uint32_t length  )
+void survive_cal_light( struct SurviveObject * so, int sensor_id, int acode, int timeinsweep, uint32_t timecode, uint32_t length, int lh)
 {
 	struct SurviveContext * ctx = so->ctx;
 	struct SurviveCalData * cd = ctx->calptr;
@@ -210,7 +210,6 @@ void survive_cal_light( struct SurviveObject * so, int sensor_id, int acode, int
 		{
 			int lhid = -sensor_id-1;
 			// Take the OOTX data from the first device. 
-			// TODO: Improve this so we'll watch all devices looking for OOTX data.  Why limit ourselves to just one?
 			if( lhid < NUM_LIGHTHOUSES && so == cd->poseobjects[0]  ) 
 			{
 				uint8_t dbit = (acode & 2)>>1;
@@ -219,9 +218,9 @@ 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;  //If all lighthouses have their OOTX set, move on.         <------- Revert This!!!!! 
-			if( i == 1 ) 
-				cd->stage = 2;  //If all lighthouses have their OOTX set, move on.
+			if( i == NUM_LIGHTHOUSES ) cd->stage = 2;  //If all lighthouses have their OOTX set, move on.         <------- Revert This!!!!! 
+			//if( i == 1 ) 
+				//cd->stage = 2;  //If all lighthouses have their OOTX set, move on.
 		}
 		break;
 	}
diff --git a/src/survive_data.c b/src/survive_data.c
index e6538d1..2ef4940 100644
--- a/src/survive_data.c
+++ b/src/survive_data.c
@@ -5,7 +5,32 @@
 #include <stdint.h>
 #include <string.h>
 
-int getAcodeFromSyncPulse(int pulseLen)
+typedef struct
+{
+	unsigned int sweep_time[SENSORS_PER_OBJECT];
+	unsigned int sweep_len[SENSORS_PER_OBJECT];
+} lightcaps_sweep_data;
+typedef struct
+{
+	lightcaps_sweep_data sweep;
+} lightcap2_data;
+
+typedef struct
+{
+	int recent_sync_time;
+	int activeLighthouse;
+	int activeSweepStartTime;
+	int activeAcode;
+
+	int lh_pulse_len[NUM_LIGHTHOUSES];
+	int lh_start_time[NUM_LIGHTHOUSES];
+	int current_lh; // used knowing which sync pulse we're looking at.
+
+} lightcap2_global_data;
+
+static lightcap2_global_data lcgd = { 0 };
+
+int handle_lightcap2_getAcodeFromSyncPulse(int pulseLen)
 {
 	if (pulseLen < 3125) return 0;
 	if (pulseLen < 3625) return 1;
@@ -16,48 +41,201 @@ int getAcodeFromSyncPulse(int pulseLen)
 	if (pulseLen < 6125) return 6;
 	return 7;
 }
+void handle_lightcap2_process_sweep_data(SurviveObject *so)
+{
+	lightcap2_data *lcd = so->disambiguator_data;
+
+	// look at all of the sensors we found, and process the ones that were hit.
+	// TODO: find the sensor(s) with the longest pulse length, and assume 
+	// those are the "highest quality".  Then, reject any pulses that are sufficiently
+	// different from those values, assuming that they are reflections.
+	{
+		unsigned int longest_pulse = 0;
+		unsigned int timestamp_of_longest_pulse = 0;
+		for (int i = 0; i < SENSORS_PER_OBJECT; i++)
+		{
+			if (lcd->sweep.sweep_len[i] > longest_pulse)
+			{
+				longest_pulse = lcd->sweep.sweep_len[i];
+				timestamp_of_longest_pulse = lcd->sweep.sweep_time[i];
+			}
+		}
+
+		for (int i = 0; i < SENSORS_PER_OBJECT; i++)
+		{
+			if (lcd->sweep.sweep_len[i] != 0) // if the sensor was hit, process it
+			{
+				int offset_from = lcd->sweep.sweep_time[i] - lcgd.activeSweepStartTime + lcd->sweep.sweep_len[i] / 2;
+
+				if (offset_from < 380000 && offset_from > 70000)
+				{
+					int timeDelta = abs(timestamp_of_longest_pulse - lcd->sweep.sweep_time[i]);
+					if (timeDelta < 15000) // if this sweep point is within ~7 degrees of the point with the longest pulse.
+					{
+						so->ctx->lightproc(so, i, lcgd.activeAcode, offset_from, lcd->sweep.sweep_time[i], lcd->sweep.sweep_len[i], lcgd.activeLighthouse);
+					}
+				}
+			}
+		}
+	}
+	// 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.
+	memset(&(((lightcap2_data*)so->disambiguator_data)->sweep), 0, sizeof(lightcaps_sweep_data));
+}
 void handle_lightcap2_sync(SurviveObject * so, LightcapElement * le )
 {
-	fprintf(stderr, "%d\n", le->length);
+	//fprintf(stderr, "%6.6d %4.4d \n", le->timestamp - so->recent_sync_time, le->length);
+	lightcap2_data *lcd = so->disambiguator_data;
 
-	if (le->timestamp - so->recent_sync_time < 24000)
+	//static unsigned int recent_sync_time = 0;
+	//static unsigned int recent_sync_count = -1;
+	//static unsigned int activeSweepStartTime;
+
+
+	// Process any sweep data we have
+	handle_lightcap2_process_sweep_data(so);
+
+	int time_since_last_sync = (le->timestamp - lcgd.recent_sync_time);
+
+	fprintf(stderr, "            %2d %8d %d\n", le->sensor_id, time_since_last_sync, le->length);
+	// need to store up sync pulses, so we can take the earliest starting time for all sensors.
+	if (time_since_last_sync < 2400)
 	{
+		lcgd.recent_sync_time = le->timestamp;
+		// it's the same sync pulse;
+		so->sync_set_number = 1;
+		//so->ctx->lightproc(so, recent_sync_count, getAcodeFromSyncPulse(le->length), le->length, le->timestamp, le->length); // Don't think I got this quite right.
+		so->recent_sync_time = le->timestamp;
+
+		lcgd.lh_pulse_len[lcgd.current_lh] = le->length;
+		lcgd.lh_start_time[lcgd.current_lh] = le->timestamp;
+
+		int acode = handle_lightcap2_getAcodeFromSyncPulse(le->length);
+		if (!(acode >> 2 & 1)) // if the skip bit is not set
+		{
+			lcgd.activeLighthouse = lcgd.current_lh;
+			lcgd.activeSweepStartTime = le->timestamp;
+			lcgd.activeAcode = acode;
+		}
+		else
+		{
+			lcgd.activeLighthouse = -1;
+			lcgd.activeSweepStartTime = 0;
+			lcgd.activeAcode = 0;
+		}
+	}
+	else if (time_since_last_sync < 24000)
+	{
+		//recent_sync_count--;
+		lcgd.recent_sync_time = le->timestamp;
 		// I do believe we are lighthouse B		
-		so->last_sync_time[1] = so->recent_sync_time = le->timestamp;
-		so->last_sync_length[1] = le->length;
-		if (le->length < 4625) // max non-skip pulse + 1/4 the difference in time between pulses.  
+		lcgd.current_lh = 1;
+		lcgd.lh_pulse_len[lcgd.current_lh] = le->length;
+		lcgd.lh_start_time[lcgd.current_lh] = le->timestamp;
+
+		int acode = handle_lightcap2_getAcodeFromSyncPulse(le->length);
+
+		//{
+			//fprintf(stderr, "2");
+			//so->ctx->lightproc(so, -2, acode, le->length, le->timestamp, le->length); // Don't think I got this quite right.
+
+
+		//}
+		if (!(acode >> 2 & 1)) // if the skip bit is not set
 		{
-			so->sync_set_number = 1;
-			so->ctx->lightproc(so, -2, getAcodeFromSyncPulse(le->length), le->length, le->timestamp, le->length); // Don't think I got this quite right.
+			if (lcgd.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");
+			}
+			lcgd.activeLighthouse = 1;
+			lcgd.activeSweepStartTime = le->timestamp;
+			lcgd.activeAcode = acode;
 		}
+
 	}
-	else
+	else if (time_since_last_sync > 370000)
 	{
 		// looks like this is the first sync pulse.  Cool!
-		if (le->length < 4625) // max non-skip pulse + 1/4 the difference in time between pulses.  
+
+		// first, send out the sync pulse data for the last round (for OOTX decoding
 		{
-			so->sync_set_number = 1;
-			so->ctx->lightproc(so, -1, getAcodeFromSyncPulse(le->length), le->length, le->timestamp, le->length); // Don't think I got this quite right.
-		}		
-	}
+			if (lcgd.lh_pulse_len[0] != 0)
+			{
+				so->ctx->lightproc(
+					so,
+					-1,
+					handle_lightcap2_getAcodeFromSyncPulse(lcgd.lh_pulse_len[0]),
+					lcgd.lh_pulse_len[0],
+					lcgd.lh_start_time[0],
+					0,
+					0);
+			}
+			if (lcgd.lh_pulse_len[1] != 0)
+			{
+				so->ctx->lightproc(
+					so,
+					-2,
+					handle_lightcap2_getAcodeFromSyncPulse(lcgd.lh_pulse_len[1]),
+					lcgd.lh_pulse_len[1],
+					lcgd.lh_start_time[1],
+					0,
+					1);
+			}
+		}
 
+		// initialize here.
+		memset(&lcgd, 0, sizeof(lcgd));
+		lcgd.activeLighthouse = -1; 
 
-//			ctx->lightproc( so, -2, acode_array[1], delta2, so->last_sync_time[1], so->last_sync_length[1] );
 
 
-//	typedef void (*light_process_func)( SurviveObject * so, int sensor_id, int acode, int timeinsweep, uint32_t timecode, uint32_t length );
+		lcgd.recent_sync_time = le->timestamp;
+		// I do believe we are lighthouse A		
+		lcgd.current_lh = 0;
+		lcgd.lh_pulse_len[lcgd.current_lh] = le->length;
+		lcgd.lh_start_time[lcgd.current_lh] = le->timestamp;
+
+		int acode = handle_lightcap2_getAcodeFromSyncPulse(le->length);
+		//{
+		//	so->ctx->lightproc(so, -1, acode, le->length, le->timestamp, le->length); // Don't think I got this quite right.
+		//}	
+
+		if (!(acode >> 2 & 1)) // if the skip bit is not set
+		{
+			lcgd.activeLighthouse = 0;
+			lcgd.activeSweepStartTime = le->timestamp;
+			lcgd.activeAcode = acode;
+		}
+
+	}
 
 }
 
 void handle_lightcap2_sweep(SurviveObject * so, LightcapElement * le )
 {
+	lightcap2_data *lcd = so->disambiguator_data;
 
+	// If we see multiple "hits" on the sweep for a given sensor,
+	// assume that the longest (i.e. strongest signal) is most likely 
+	// the non-reflected signal. 
+	if (lcd->sweep.sweep_len[le->sensor_id] < le->length)
+	{
+		lcd->sweep.sweep_len[le->sensor_id] = le->length;
+		lcd->sweep.sweep_time[le->sensor_id] = le->timestamp;
+	}
 }
 
 void handle_lightcap2( SurviveObject * so, LightcapElement * le )
 {
 	SurviveContext * ctx = so->ctx;
 
+	if (so->disambiguator_data == NULL)
+	{
+		so->disambiguator_data = malloc(sizeof(lightcap2_data));
+		memset(so->disambiguator_data, 0, sizeof(lightcap2_data));
+	}
+
 	if( le->sensor_id > SENSORS_PER_OBJECT )
 	{
 		return;
@@ -72,6 +250,7 @@ void handle_lightcap2( SurviveObject * so, LightcapElement * le )
 	{
 		// Looks like a sync pulse, process it!
 		handle_lightcap2_sync(so, le);
+		return;
 	}
 
 	// must be a sweep pulse, process it!
diff --git a/src/survive_process.c b/src/survive_process.c
index 8dc849a..6735f10 100644
--- a/src/survive_process.c
+++ b/src/survive_process.c
@@ -6,14 +6,14 @@
 //XXX TODO: Once data is avialble in the context, use the stuff here to handle converting from time codes to
 //proper angles, then from there perform the rest of the solution. 
 
-void survive_default_light_process( SurviveObject * so, int sensor_id, int acode, int timeinsweep, uint32_t timecode, uint32_t length  )
+void survive_default_light_process( SurviveObject * so, int sensor_id, int acode, int timeinsweep, uint32_t timecode, uint32_t length, int lh)
 {
 	SurviveContext * ctx = so->ctx;
-	int base_station = acode >> 2;
+	int base_station = lh;
 	int axis = acode & 1;
 	if( ctx->calptr )
 	{
-		survive_cal_light( so, sensor_id, acode, timeinsweep, timecode, length );
+		survive_cal_light( so, sensor_id, acode, timeinsweep, timecode, length, lh);
 	}
 
 	if( base_station > NUM_LIGHTHOUSES ) return;
-- 
cgit v1.3.1


From 7701d14440d5d005b1ec3513ff8646e2f6b101ef Mon Sep 17 00:00:00 2001
From: Mike Turvey <mturvey6@gmail.com>
Date: Thu, 23 Mar 2017 08:11:36 -0700
Subject: Cleanup

---
 src/survive_data.c | 27 +++++++++++----------------
 1 file changed, 11 insertions(+), 16 deletions(-)

(limited to 'src/survive_data.c')

diff --git a/src/survive_data.c b/src/survive_data.c
index 2ef4940..d92db36 100644
--- a/src/survive_data.c
+++ b/src/survive_data.c
@@ -69,8 +69,7 @@ void handle_lightcap2_process_sweep_data(SurviveObject *so)
 
 				if (offset_from < 380000 && offset_from > 70000)
 				{
-					int timeDelta = abs(timestamp_of_longest_pulse - lcd->sweep.sweep_time[i]);
-					if (timeDelta < 15000) // if this sweep point is within ~7 degrees of the point with the longest pulse.
+					if (longest_pulse *10 / 8 < -lcd->sweep.sweep_len[i]) 
 					{
 						so->ctx->lightproc(so, i, lcgd.activeAcode, offset_from, lcd->sweep.sweep_time[i], lcd->sweep.sweep_len[i], lcgd.activeLighthouse);
 					}
@@ -97,14 +96,13 @@ void handle_lightcap2_sync(SurviveObject * so, LightcapElement * le )
 
 	int time_since_last_sync = (le->timestamp - lcgd.recent_sync_time);
 
-	fprintf(stderr, "            %2d %8d %d\n", le->sensor_id, time_since_last_sync, le->length);
+	//fprintf(stderr, "            %2d %8d %d\n", le->sensor_id, time_since_last_sync, le->length);
 	// need to store up sync pulses, so we can take the earliest starting time for all sensors.
 	if (time_since_last_sync < 2400)
 	{
 		lcgd.recent_sync_time = le->timestamp;
 		// it's the same sync pulse;
 		so->sync_set_number = 1;
-		//so->ctx->lightproc(so, recent_sync_count, getAcodeFromSyncPulse(le->length), le->length, le->timestamp, le->length); // Don't think I got this quite right.
 		so->recent_sync_time = le->timestamp;
 
 		lcgd.lh_pulse_len[lcgd.current_lh] = le->length;
@@ -126,7 +124,6 @@ void handle_lightcap2_sync(SurviveObject * so, LightcapElement * le )
 	}
 	else if (time_since_last_sync < 24000)
 	{
-		//recent_sync_count--;
 		lcgd.recent_sync_time = le->timestamp;
 		// I do believe we are lighthouse B		
 		lcgd.current_lh = 1;
@@ -135,12 +132,6 @@ void handle_lightcap2_sync(SurviveObject * so, LightcapElement * le )
 
 		int acode = handle_lightcap2_getAcodeFromSyncPulse(le->length);
 
-		//{
-			//fprintf(stderr, "2");
-			//so->ctx->lightproc(so, -2, acode, le->length, le->timestamp, le->length); // Don't think I got this quite right.
-
-
-		//}
 		if (!(acode >> 2 & 1)) // if the skip bit is not set
 		{
 			if (lcgd.activeLighthouse != -1)
@@ -197,9 +188,6 @@ void handle_lightcap2_sync(SurviveObject * so, LightcapElement * le )
 		lcgd.lh_start_time[lcgd.current_lh] = le->timestamp;
 
 		int acode = handle_lightcap2_getAcodeFromSyncPulse(le->length);
-		//{
-		//	so->ctx->lightproc(so, -1, acode, le->length, le->timestamp, le->length); // Don't think I got this quite right.
-		//}	
 
 		if (!(acode >> 2 & 1)) // if the skip bit is not set
 		{
@@ -207,9 +195,7 @@ void handle_lightcap2_sync(SurviveObject * so, LightcapElement * le )
 			lcgd.activeSweepStartTime = le->timestamp;
 			lcgd.activeAcode = acode;
 		}
-
 	}
-
 }
 
 void handle_lightcap2_sweep(SurviveObject * so, LightcapElement * le )
@@ -219,6 +205,15 @@ void handle_lightcap2_sweep(SurviveObject * so, LightcapElement * le )
 	// If we see multiple "hits" on the sweep for a given sensor,
 	// assume that the longest (i.e. strongest signal) is most likely 
 	// the non-reflected signal. 
+
+	if (le->length < 80)
+	{
+		// this is a low-quality read.  Better to throw it out than to use it.
+		//fprintf(stderr, "%2d %d\n", le->sensor_id, le->length);
+		return;
+	}
+	fprintf(stderr, "%2d %d\n", le->sensor_id, le->length);
+
 	if (lcd->sweep.sweep_len[le->sensor_id] < le->length)
 	{
 		lcd->sweep.sweep_len[le->sensor_id] = le->length;
-- 
cgit v1.3.1


From 280a6599fea76a7d2c16cfe0fcc5c8f37fde66de Mon Sep 17 00:00:00 2001
From: mwturvey <michael.w.turvey@intel.com>
Date: Thu, 23 Mar 2017 09:47:38 -0700
Subject: More cleanup

---
 calibrate.c                        |  2 +-
 data_recorder.c                    |  2 +-
 include/libsurvive/survive.h       |  2 +-
 include/libsurvive/survive_types.h |  2 +-
 src/survive_cal.c                  |  2 +-
 src/survive_cal.h                  |  2 +-
 src/survive_data.c                 | 96 +++++++++++++++++++-------------------
 src/survive_process.c              |  2 +-
 src/survive_vive.c                 |  2 +-
 9 files changed, 57 insertions(+), 55 deletions(-)

(limited to 'src/survive_data.c')

diff --git a/calibrate.c b/calibrate.c
index 0f2d6ac..82869e0 100644
--- a/calibrate.c
+++ b/calibrate.c
@@ -51,7 +51,7 @@ int bufferpts[32*2*3][2];
 char buffermts[32*128*3];
 int buffertimeto[32*3][2];
 
-void my_light_process( struct SurviveObject * so, int sensor_id, int acode, int timeinsweep, uint32_t timecode, uint32_t length, int lh)
+void my_light_process( struct SurviveObject * so, int sensor_id, int acode, int timeinsweep, uint32_t timecode, uint32_t length, uint32_t lh)
 {
 //	if( timeinsweep < 0 ) return;
 	survive_default_light_process( so, sensor_id, acode, timeinsweep, timecode, length, lh);
diff --git a/data_recorder.c b/data_recorder.c
index 6432f4f..04a219a 100644
--- a/data_recorder.c
+++ b/data_recorder.c
@@ -43,7 +43,7 @@ int bufferpts[32*2*3];
 char buffermts[32*128*3];
 int buffertimeto[32*3];
 
-void my_light_process( struct SurviveObject * so, int sensor_id, int acode, int timeinsweep, uint32_t timecode, uint32_t length, int lh)
+void my_light_process( struct SurviveObject * so, int sensor_id, int acode, int timeinsweep, uint32_t timecode, uint32_t length, uint32_t lh)
 {
 	survive_default_light_process( so, sensor_id, acode, timeinsweep, timecode, length, lh);
 
diff --git a/include/libsurvive/survive.h b/include/libsurvive/survive.h
index c3b6a03..1165e9d 100644
--- a/include/libsurvive/survive.h
+++ b/include/libsurvive/survive.h
@@ -130,7 +130,7 @@ void survive_cal_install( SurviveContext * ctx );  //XXX This will be removed if
 
 //Call these from your callback if overridden.  
 //Accept higher-level data.
-void survive_default_light_process( SurviveObject * so, int sensor_id, int acode, int timeinsweep, uint32_t timecode, uint32_t length , int lh);
+void survive_default_light_process( SurviveObject * so, int sensor_id, int acode, int timeinsweep, uint32_t timecode, uint32_t length , uint32_t lh);
 void survive_default_imu_process( SurviveObject * so, int mode, FLT * accelgyro, uint32_t timecode, int id );
 void survive_default_angle_process( SurviveObject * so, int sensor_id, int acode, uint32_t timecode, FLT length, FLT angle );
 
diff --git a/include/libsurvive/survive_types.h b/include/libsurvive/survive_types.h
index 1600e11..def30b8 100644
--- a/include/libsurvive/survive_types.h
+++ b/include/libsurvive/survive_types.h
@@ -28,7 +28,7 @@ typedef struct BaseStationData BaseStationData;
 typedef struct SurviveCalData SurviveCalData;   //XXX Warning: This may be removed.  Check at a later time for its defunctness.
 
 typedef void (*text_feedback_func)( SurviveContext * ctx, const char * fault );
-typedef void (*light_process_func)( SurviveObject * so, int sensor_id, int acode, int timeinsweep, uint32_t timecode, uint32_t length );
+typedef void (*light_process_func)( SurviveObject * so, int sensor_id, int acode, int timeinsweep, uint32_t timecode, uint32_t length, uint32_t lighthouse);
 typedef void (*imu_process_func)( SurviveObject * so, int mask, FLT * accelgyro, uint32_t timecode, int id );
 typedef void (*angle_process_func)( SurviveObject * so, int sensor_id, int acode, uint32_t timecode, FLT length, FLT angle );
 
diff --git a/src/survive_cal.c b/src/survive_cal.c
index 51d12dd..dfa2e85 100755
--- a/src/survive_cal.c
+++ b/src/survive_cal.c
@@ -190,7 +190,7 @@ void survive_cal_install( struct SurviveContext * ctx )
 }
 
 
-void survive_cal_light( struct SurviveObject * so, int sensor_id, int acode, int timeinsweep, uint32_t timecode, uint32_t length, int lh)
+void survive_cal_light( struct SurviveObject * so, int sensor_id, int acode, int timeinsweep, uint32_t timecode, uint32_t length, uint32_t lh)
 {
 	struct SurviveContext * ctx = so->ctx;
 	struct SurviveCalData * cd = ctx->calptr;
diff --git a/src/survive_cal.h b/src/survive_cal.h
index 8c488bd..9d7b3a9 100644
--- a/src/survive_cal.h
+++ b/src/survive_cal.h
@@ -29,7 +29,7 @@ int survive_cal_get_status( SurviveContext * ctx, char * description, int descri
 //void survive_cal_teardown( struct SurviveContext * ctx );
 
 //Called from survive_default_light_process
-void survive_cal_light( SurviveObject * so, int sensor_id, int acode, int timeinsweep, uint32_t timecode, uint32_t length  );
+void survive_cal_light( SurviveObject * so, int sensor_id, int acode, int timeinsweep, uint32_t timecode, uint32_t length, uint32_t lighthouse);
 void survive_cal_angle( SurviveObject * so, int sensor_id, int acode, uint32_t timecode, FLT length, FLT angle );
 
 #define MAX_SENSORS_TO_CAL 96
diff --git a/src/survive_data.c b/src/survive_data.c
index d92db36..00e66f0 100644
--- a/src/survive_data.c
+++ b/src/survive_data.c
@@ -10,11 +10,6 @@ typedef struct
 	unsigned int sweep_time[SENSORS_PER_OBJECT];
 	unsigned int sweep_len[SENSORS_PER_OBJECT];
 } lightcaps_sweep_data;
-typedef struct
-{
-	lightcaps_sweep_data sweep;
-} lightcap2_data;
-
 typedef struct
 {
 	int recent_sync_time;
@@ -28,7 +23,14 @@ typedef struct
 
 } lightcap2_global_data;
 
-static lightcap2_global_data lcgd = { 0 };
+typedef struct
+{
+	lightcaps_sweep_data sweep;
+	lightcap2_global_data global;
+} lightcap2_data;
+
+
+//static lightcap2_global_data lcgd = { 0 };
 
 int handle_lightcap2_getAcodeFromSyncPulse(int pulseLen)
 {
@@ -65,13 +67,13 @@ void handle_lightcap2_process_sweep_data(SurviveObject *so)
 		{
 			if (lcd->sweep.sweep_len[i] != 0) // if the sensor was hit, process it
 			{
-				int offset_from = lcd->sweep.sweep_time[i] - lcgd.activeSweepStartTime + lcd->sweep.sweep_len[i] / 2;
+				int offset_from = lcd->sweep.sweep_time[i] - lcd->global.activeSweepStartTime + lcd->sweep.sweep_len[i] / 2;
 
 				if (offset_from < 380000 && offset_from > 70000)
 				{
-					if (longest_pulse *10 / 8 < -lcd->sweep.sweep_len[i]) 
+					if (longest_pulse *10 / 8 < lcd->sweep.sweep_len[i]) 
 					{
-						so->ctx->lightproc(so, i, lcgd.activeAcode, offset_from, lcd->sweep.sweep_time[i], lcd->sweep.sweep_len[i], lcgd.activeLighthouse);
+						so->ctx->lightproc(so, i, lcd->global.activeAcode, offset_from, lcd->sweep.sweep_time[i], lcd->sweep.sweep_len[i], lcd->global.activeLighthouse);
 					}
 				}
 			}
@@ -94,54 +96,54 @@ void handle_lightcap2_sync(SurviveObject * so, LightcapElement * le )
 	// Process any sweep data we have
 	handle_lightcap2_process_sweep_data(so);
 
-	int time_since_last_sync = (le->timestamp - lcgd.recent_sync_time);
+	int time_since_last_sync = (le->timestamp - lcd->global.recent_sync_time);
 
 	//fprintf(stderr, "            %2d %8d %d\n", le->sensor_id, time_since_last_sync, le->length);
 	// need to store up sync pulses, so we can take the earliest starting time for all sensors.
 	if (time_since_last_sync < 2400)
 	{
-		lcgd.recent_sync_time = le->timestamp;
+		lcd->global.recent_sync_time = le->timestamp;
 		// it's the same sync pulse;
 		so->sync_set_number = 1;
 		so->recent_sync_time = le->timestamp;
 
-		lcgd.lh_pulse_len[lcgd.current_lh] = le->length;
-		lcgd.lh_start_time[lcgd.current_lh] = le->timestamp;
+		lcd->global.lh_pulse_len[lcd->global.current_lh] = le->length;
+		lcd->global.lh_start_time[lcd->global.current_lh] = le->timestamp;
 
 		int acode = handle_lightcap2_getAcodeFromSyncPulse(le->length);
 		if (!(acode >> 2 & 1)) // if the skip bit is not set
 		{
-			lcgd.activeLighthouse = lcgd.current_lh;
-			lcgd.activeSweepStartTime = le->timestamp;
-			lcgd.activeAcode = acode;
+			lcd->global.activeLighthouse = lcd->global.current_lh;
+			lcd->global.activeSweepStartTime = le->timestamp;
+			lcd->global.activeAcode = acode;
 		}
 		else
 		{
-			lcgd.activeLighthouse = -1;
-			lcgd.activeSweepStartTime = 0;
-			lcgd.activeAcode = 0;
+			lcd->global.activeLighthouse = -1;
+			lcd->global.activeSweepStartTime = 0;
+			lcd->global.activeAcode = 0;
 		}
 	}
 	else if (time_since_last_sync < 24000)
 	{
-		lcgd.recent_sync_time = le->timestamp;
+		lcd->global.recent_sync_time = le->timestamp;
 		// I do believe we are lighthouse B		
-		lcgd.current_lh = 1;
-		lcgd.lh_pulse_len[lcgd.current_lh] = le->length;
-		lcgd.lh_start_time[lcgd.current_lh] = le->timestamp;
+		lcd->global.current_lh = 1;
+		lcd->global.lh_pulse_len[lcd->global.current_lh] = le->length;
+		lcd->global.lh_start_time[lcd->global.current_lh] = le->timestamp;
 
 		int acode = handle_lightcap2_getAcodeFromSyncPulse(le->length);
 
 		if (!(acode >> 2 & 1)) // if the skip bit is not set
 		{
-			if (lcgd.activeLighthouse != -1)
+			if (lcd->global.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");
 			}
-			lcgd.activeLighthouse = 1;
-			lcgd.activeSweepStartTime = le->timestamp;
-			lcgd.activeAcode = acode;
+			lcd->global.activeLighthouse = 1;
+			lcd->global.activeSweepStartTime = le->timestamp;
+			lcd->global.activeAcode = acode;
 		}
 
 	}
@@ -151,49 +153,49 @@ void handle_lightcap2_sync(SurviveObject * so, LightcapElement * le )
 
 		// first, send out the sync pulse data for the last round (for OOTX decoding
 		{
-			if (lcgd.lh_pulse_len[0] != 0)
+			if (lcd->global.lh_pulse_len[0] != 0)
 			{
 				so->ctx->lightproc(
 					so,
 					-1,
-					handle_lightcap2_getAcodeFromSyncPulse(lcgd.lh_pulse_len[0]),
-					lcgd.lh_pulse_len[0],
-					lcgd.lh_start_time[0],
+					handle_lightcap2_getAcodeFromSyncPulse(lcd->global.lh_pulse_len[0]),
+					lcd->global.lh_pulse_len[0],
+					lcd->global.lh_start_time[0],
 					0,
 					0);
 			}
-			if (lcgd.lh_pulse_len[1] != 0)
+			if (lcd->global.lh_pulse_len[1] != 0)
 			{
 				so->ctx->lightproc(
 					so,
 					-2,
-					handle_lightcap2_getAcodeFromSyncPulse(lcgd.lh_pulse_len[1]),
-					lcgd.lh_pulse_len[1],
-					lcgd.lh_start_time[1],
+					handle_lightcap2_getAcodeFromSyncPulse(lcd->global.lh_pulse_len[1]),
+					lcd->global.lh_pulse_len[1],
+					lcd->global.lh_start_time[1],
 					0,
 					1);
 			}
 		}
 
 		// initialize here.
-		memset(&lcgd, 0, sizeof(lcgd));
-		lcgd.activeLighthouse = -1; 
+		memset(&lcd->global, 0, sizeof(lcd->global));
+		lcd->global.activeLighthouse = -1; 
 
 
 
-		lcgd.recent_sync_time = le->timestamp;
+		lcd->global.recent_sync_time = le->timestamp;
 		// I do believe we are lighthouse A		
-		lcgd.current_lh = 0;
-		lcgd.lh_pulse_len[lcgd.current_lh] = le->length;
-		lcgd.lh_start_time[lcgd.current_lh] = le->timestamp;
+		lcd->global.current_lh = 0;
+		lcd->global.lh_pulse_len[lcd->global.current_lh] = le->length;
+		lcd->global.lh_start_time[lcd->global.current_lh] = le->timestamp;
 
 		int acode = handle_lightcap2_getAcodeFromSyncPulse(le->length);
 
 		if (!(acode >> 2 & 1)) // if the skip bit is not set
 		{
-			lcgd.activeLighthouse = 0;
-			lcgd.activeSweepStartTime = le->timestamp;
-			lcgd.activeAcode = acode;
+			lcd->global.activeLighthouse = 0;
+			lcd->global.activeSweepStartTime = le->timestamp;
+			lcd->global.activeAcode = acode;
 		}
 	}
 }
@@ -383,8 +385,8 @@ void handle_lightcap( SurviveObject * so, LightcapElement * le )
 			int32_t delta1 = so->last_sync_time[0] - so->recent_sync_time;
 			int32_t delta2 = so->last_sync_time[1] - so->last_sync_time[0];
 
-			ctx->lightproc( so, -1, acode_array[0], delta1, so->last_sync_time[0], so->last_sync_length[0] );
-			ctx->lightproc( so, -2, acode_array[1], delta2, so->last_sync_time[1], so->last_sync_length[1] );
+			ctx->lightproc( so, -1, acode_array[0], delta1, so->last_sync_time[0], so->last_sync_length[0], 0 );
+			ctx->lightproc( so, -2, acode_array[1], delta2, so->last_sync_time[1], so->last_sync_length[1], 1 );
 
 			so->recent_sync_time = so->last_sync_time[1];
 
@@ -427,7 +429,7 @@ void handle_lightcap( SurviveObject * so, LightcapElement * le )
 		//Make sure pulse is in valid window
 		if( offset_from < 380000 && offset_from > 70000 )
 		{
-			ctx->lightproc( so, le->sensor_id, acode, offset_from, le->timestamp, le->length );
+			ctx->lightproc( so, le->sensor_id, acode, offset_from, le->timestamp, le->length, so->sync_set_number );
 		}
 	}
 	else
diff --git a/src/survive_process.c b/src/survive_process.c
index 6735f10..463481a 100644
--- a/src/survive_process.c
+++ b/src/survive_process.c
@@ -6,7 +6,7 @@
 //XXX TODO: Once data is avialble in the context, use the stuff here to handle converting from time codes to
 //proper angles, then from there perform the rest of the solution. 
 
-void survive_default_light_process( SurviveObject * so, int sensor_id, int acode, int timeinsweep, uint32_t timecode, uint32_t length, int lh)
+void survive_default_light_process( SurviveObject * so, int sensor_id, int acode, int timeinsweep, uint32_t timecode, uint32_t length, uint32_t lh)
 {
 	SurviveContext * ctx = so->ctx;
 	int base_station = lh;
diff --git a/src/survive_vive.c b/src/survive_vive.c
index a9b295f..c04fc03 100755
--- a/src/survive_vive.c
+++ b/src/survive_vive.c
@@ -141,7 +141,7 @@ void survive_data_cb( SurviveUSBInterface * si );
 
 //USB Subsystem 
 void survive_usb_close( SurviveContext * t );
-int survive_usb_init( SurviveViveData * sv, SurviveObject * hmd, SurviveObject *wm0, SurviveObject * wm1, SurviveObject * tr0 );
+int survive_usb_init( SurviveViveData * sv, SurviveObject * hmd, SurviveObject *wm0, SurviveObject * wm1, SurviveObject * tr0, struct SurviveObject * ww0 );
 int survive_usb_poll( SurviveContext * ctx );
 int survive_get_config( char ** config, SurviveViveData * ctx, int devno, int iface, int send_extra_magic );
 int survive_vive_send_magic(struct SurviveContext * ctx, void * drv, int magic_code, void * data, int datalen );
-- 
cgit v1.3.1


From f33018188bf5bdf6f6b8af0d646e3f8c519d9d71 Mon Sep 17 00:00:00 2001
From: mwturvey <michael.w.turvey@intel.com>
Date: Thu, 23 Mar 2017 12:13:44 -0700
Subject: Added support for empty string in config.json & other cleanup.

---
 redist/json_helpers.c |  3 ++-
 src/survive_cal.c     | 50 +++++++++++++++++++++-----------------------------
 src/survive_config.c  | 10 +++++++++-
 src/survive_data.c    |  2 +-
 4 files changed, 33 insertions(+), 32 deletions(-)

(limited to 'src/survive_data.c')

diff --git a/redist/json_helpers.c b/redist/json_helpers.c
index 0267932..3b5cc0d 100644
--- a/redist/json_helpers.c
+++ b/redist/json_helpers.c
@@ -117,7 +117,8 @@ char* load_file_to_mem(const char* path) {
 	fseek( f, 0, SEEK_END );
 	int len = ftell( f );
 	fseek( f, 0, SEEK_SET );
-	char * JSON_STRING = malloc( len );
+	char * JSON_STRING = malloc( len + 1);
+	memset(JSON_STRING,0,len+1); 
 	fread( JSON_STRING, len, 1, f );
 	fclose( f );
 	return JSON_STRING;
diff --git a/src/survive_cal.c b/src/survive_cal.c
index 19eb3ca..f9ec595 100755
--- a/src/survive_cal.c
+++ b/src/survive_cal.c
@@ -124,50 +124,42 @@ void survive_cal_install( struct SurviveContext * ctx )
 
 	cd->numPoseObjects = 0;
 
-	for (int i=0; i < ctx->objs_ct; i++)
+	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 );
+	size_t requiredTrackersFound = 0;
+
+	for (int j=0; j < ctx->objs_ct; j++)
 	{
-		// This would be a place where we could conditionally decide if we 
-		// want to include a certain device in the calibration routine.
-		if (1)
+		// Add the tracker if we allow all trackers for calibration, or if it's in the list 
+		// of required trackers.
+		int isRequiredTracker = strstr(RequiredTrackersForCal, ctx->objs[j]->codename) != NULL;
+
+		if (isRequiredTracker)
 		{
-			cd->poseobjects[i] = ctx->objs[i];
+			requiredTrackersFound++;
+		}
+
+		if (AllowAllTrackersForCal || isRequiredTracker)
+		{
+			cd->poseobjects[j] = ctx->objs[j];
 			cd->numPoseObjects++;
 
-			SV_INFO("Calibration is using %s", cd->poseobjects[i]->codename);
+			SV_INFO("Calibration is using %s", cd->poseobjects[j]->codename);
 		}
+
 	}
 
 	// If we want to mandate that certain devices have been found
 
-
-
-	//cd->poseobjects[0] = survive_get_so_by_name( ctx, "HMD" );
-	//cd->poseobjects[1] = survive_get_so_by_name( ctx, "WM0" );
-	//cd->poseobjects[2] = survive_get_so_by_name( ctx, "WM1" );
-
-	//if( cd->poseobjects[0] == 0 || cd->poseobjects[1] == 0 || cd->poseobjects[2] == 0 )
-	//{
-	//	SV_ERROR( "Error: cannot find all devices needed for calibration." );
-	//	free( cd );
-	//	return;
-	//}
-
-//XXX TODO MWTourney, work on your code here.
-/*
-	if( !cd->hmd )
+	if (strlen(RequiredTrackersForCal) > 0)
 	{
-		cd->hmd = survive_get_so_by_name( ctx, "TR0" );
-
-		if( !cd->hmd )
+		if (requiredTrackersFound != ((strlen(RequiredTrackersForCal) + 1) / 4))
 		{
-			SV_ERROR( "Error: cannot find any devices labeled HMD. Required for calibration" );
+			SV_ERROR( "Error: cannot find all devices needed for calibration." );
 			free( cd );
 			return;
 		}
-		SV_INFO( "HMD not found, calibrating using Tracker" );
 	}
-*/
-
 
 	const char * DriverName;
 	const char * PreferredPoser = config_read_str( ctx->global_config_values, "ConfigPoser", "PoserCharlesSlow" );
diff --git a/src/survive_config.c b/src/survive_config.c
index 0810280..005cfaf 100644
--- a/src/survive_config.c
+++ b/src/survive_config.c
@@ -175,7 +175,13 @@ const char* config_set_str(config_group *cg, const char *tag, const char* value)
 	if (cv == NULL) cv = next_unused_entry(cg);
 
 	sstrcpy(&(cv->tag), tag);
-	sstrcpy(&(cv->data), value);
+
+	if (NULL != value){
+		sstrcpy(&(cv->data), value);
+	}
+	else {
+		sstrcpy(&(cv->data), "");
+	}
 	cv->type = CONFIG_STRING;
 
 	return value;
@@ -357,9 +363,11 @@ void handle_tag_value(char* tag, char** values, uint8_t count) {
 	print_json_value(tag,values,count);
 	config_group* cg = cg_stack[cg_stack_head];
 
+	if (NULL != *values){	
 	if (parse_uint32(tag,values,count) > 0) return; //parse integers first, stricter rules
 
 	if (parse_floats(tag,values,count) > 0) return;
+	}
 
 	//should probably also handle string arrays
 	config_set_str(cg,tag,values[0]);
diff --git a/src/survive_data.c b/src/survive_data.c
index ee180b1..da4e0a2 100644
--- a/src/survive_data.c
+++ b/src/survive_data.c
@@ -372,7 +372,7 @@ void handle_lightcap( SurviveObject * so, LightcapElement * le )
 			int32_t main_divisor = so->timebase_hz / 384000; //125 @ 48 MHz.
 			int base_station = is_new_pulse;
 			//printf( "%s %d %d %d\n", so->codename, le->sensor_id, so->sync_set_number, le->length );
-			ctx->lightproc( so, le->sensor_id, -3 - so->sync_set_number, 0, le->timestamp, le->length );
+			ctx->lightproc( so, le->sensor_id, -3 - so->sync_set_number, 0, le->timestamp, le->length, base_station);
 	}
 	}
 
-- 
cgit v1.3.1


From 3d86dda66d50d1e1955d4e0cd5f374e0aed1789f Mon Sep 17 00:00:00 2001
From: mwturvey <michael.w.turvey@intel.com>
Date: Thu, 23 Mar 2017 16:26:27 -0700
Subject: Fixed Windows USB Interface Enumeration

---
 src/survive_cal.c  |  4 ++--
 src/survive_data.c | 21 +++++++++++----------
 src/survive_vive.c | 18 ++++++++++++++----
 3 files changed, 27 insertions(+), 16 deletions(-)

(limited to 'src/survive_data.c')

diff --git a/src/survive_cal.c b/src/survive_cal.c
index f9ec595..81889ca 100755
--- a/src/survive_cal.c
+++ b/src/survive_cal.c
@@ -155,7 +155,7 @@ void survive_cal_install( struct SurviveContext * ctx )
 	{
 		if (requiredTrackersFound != ((strlen(RequiredTrackersForCal) + 1) / 4))
 		{
-			SV_ERROR( "Error: cannot find all devices needed for calibration." );
+			SV_ERROR( "Error: Did not find all devices required for calibration." );
 			free( cd );
 			return;
 		}
@@ -201,7 +201,7 @@ void survive_cal_light( struct SurviveObject * so, int sensor_id, int acode, int
 		if( sensor_id < 0 )
 		{
 			int lhid = -sensor_id-1;
-			// Take the OOTX data from the first device. 
+			// Take the OOTX data from the first device.  (if using HMD, WM0, WM1 only, this will be HMD)
 			if( lhid < NUM_LIGHTHOUSES && so == cd->poseobjects[0]  ) 
 			{
 				uint8_t dbit = (acode & 2)>>1;
diff --git a/src/survive_data.c b/src/survive_data.c
index da4e0a2..ce263ed 100644
--- a/src/survive_data.c
+++ b/src/survive_data.c
@@ -208,13 +208,14 @@ void handle_lightcap2_sweep(SurviveObject * so, LightcapElement * le )
 	// assume that the longest (i.e. strongest signal) is most likely 
 	// the non-reflected signal. 
 
-	if (le->length < 80)
-	{
-		// this is a low-quality read.  Better to throw it out than to use it.
-		//fprintf(stderr, "%2d %d\n", le->sensor_id, le->length);
-		return;
-	}
-	fprintf(stderr, "%2d %d\n", le->sensor_id, le->length);
+	//if (le->length < 80)
+	//{
+	//	// this is a low-quality read.  Better to throw it out than to use it.
+	//	//fprintf(stderr, "%2d %d\n", le->sensor_id, le->length);
+	//	return;
+	//}
+	//fprintf(stderr, "%2d %d\n", le->sensor_id, le->length);
+	//fprintf(stderr, ".");
 
 	if (lcd->sweep.sweep_len[le->sensor_id] < le->length)
 	{
@@ -269,8 +270,8 @@ int32_t decode_acode(uint32_t length, int32_t main_divisor) {
 void handle_lightcap( SurviveObject * so, LightcapElement * le )
 {
 	SurviveContext * ctx = so->ctx;
-//	handle_lightcap2(so,le);
-//	return;
+	handle_lightcap2(so,le);
+	return;
 
 	//int32_t deltat = (uint32_t)le->timestamp - (uint32_t)so->last_master_time;
 
@@ -373,7 +374,7 @@ void handle_lightcap( SurviveObject * so, LightcapElement * le )
 			int base_station = is_new_pulse;
 			//printf( "%s %d %d %d\n", so->codename, le->sensor_id, so->sync_set_number, le->length );
 			ctx->lightproc( so, le->sensor_id, -3 - so->sync_set_number, 0, le->timestamp, le->length, base_station);
-	}
+		}
 	}
 
 
diff --git a/src/survive_vive.c b/src/survive_vive.c
index f6465b2..bfe4926 100755
--- a/src/survive_vive.c
+++ b/src/survive_vive.c
@@ -340,7 +340,7 @@ int survive_usb_init( SurviveViveData * sv, SurviveObject * hmd, SurviveObject *
 			if (cur_dev->vendor_id == vendor_id &&
 				cur_dev->product_id == product_id)
 			{
-				if( menum == enumid )
+				if( cur_dev->interface_number == enumid )
 				{
 					path_to_open = cur_dev->path;
 					break;
@@ -521,12 +521,21 @@ int survive_vive_send_magic(SurviveContext * ctx, void * drv, int magic_code, vo
 
 		if (sv->udev[USB_DEV_W_WATCHMAN1])
 		{
+			//static uint8_t vive_magic_power_on[64] = {  0x04, 0x78, 0x29, 0x38 };
+			static uint8_t vive_magic_power_on[5] = { 0x04 };
+			//static uint8_t vive_magic_power_on[5] = { 0x04, 0x00, 0xc3, 0xe2, 0x04 };
+			r = update_feature_report( sv->udev[USB_DEV_W_WATCHMAN1], 0, vive_magic_power_on, sizeof( vive_magic_power_on ) );
+			if( r != sizeof( vive_magic_power_on ) ) return 5;
+		}
+		if (sv->udev[USB_DEV_W_WATCHMAN1_LIGHTCAP])
+		{
+			//static uint8_t vive_magic_enable_lighthouse[64] = {  0x04, 0x78, 0x29, 0x38 };
 			static uint8_t vive_magic_enable_lighthouse[5] = { 0x04 };
-			r = update_feature_report( sv->udev[USB_DEV_W_WATCHMAN1], 0, vive_magic_enable_lighthouse, sizeof( vive_magic_enable_lighthouse ) );
+			r = update_feature_report( sv->udev[USB_DEV_W_WATCHMAN1_LIGHTCAP], 0, vive_magic_enable_lighthouse, sizeof( vive_magic_enable_lighthouse ) );
 			if( r != sizeof( vive_magic_enable_lighthouse ) ) return 5;
 
 			static uint8_t vive_magic_enable_lighthouse2[5] = { 0x07, 0x02 };  //Switch to 0x25 mode (able to get more light updates)
-			r = update_feature_report( sv->udev[USB_DEV_W_WATCHMAN1], 0, vive_magic_enable_lighthouse2, sizeof( vive_magic_enable_lighthouse2 ) );
+			r = update_feature_report( sv->udev[USB_DEV_W_WATCHMAN1_LIGHTCAP], 0, vive_magic_enable_lighthouse2, sizeof( vive_magic_enable_lighthouse2 ) );
 			if( r != sizeof( vive_magic_enable_lighthouse2 ) ) return 5;
 		}
 
@@ -534,7 +543,8 @@ int survive_vive_send_magic(SurviveContext * ctx, void * drv, int magic_code, vo
 		for( int i = 0; i < 256; i++ )
 		{
 			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_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 );
-- 
cgit v1.3.1


From c55c5928485da0432140be4befe5165c68ae4ab0 Mon Sep 17 00:00:00 2001
From: mwturvey <michael.w.turvey@intel.com>
Date: Thu, 23 Mar 2017 16:46:04 -0700
Subject: Change disambiguator filter

---
 src/survive_data.c |  2 +-
 src/survive_vive.c | 45 +++++++++++++++++++++++++++++++++------------
 2 files changed, 34 insertions(+), 13 deletions(-)

(limited to 'src/survive_data.c')

diff --git a/src/survive_data.c b/src/survive_data.c
index ce263ed..849b722 100644
--- a/src/survive_data.c
+++ b/src/survive_data.c
@@ -71,7 +71,7 @@ void handle_lightcap2_process_sweep_data(SurviveObject *so)
 
 				if (offset_from < 380000 && offset_from > 70000)
 				{
-					if (longest_pulse *10 / 8 < lcd->sweep.sweep_len[i]) 
+					//if (longest_pulse *10 / 8 < lcd->sweep.sweep_len[i]) 
 					{
 						so->ctx->lightproc(so, i, lcd->global.activeAcode, offset_from, lcd->sweep.sweep_time[i], lcd->sweep.sweep_len[i], lcd->global.activeLighthouse);
 					}
diff --git a/src/survive_vive.c b/src/survive_vive.c
index bfe4926..f58fb79 100755
--- a/src/survive_vive.c
+++ b/src/survive_vive.c
@@ -47,6 +47,7 @@ const short vidpids[] = {
 	0x28de, 0x2012, 0, //Valve Watchman, USB connected
 #ifdef HIDAPI
 	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)
 #endif
 }; //length MAX_USB_INTERFACES*2
@@ -60,6 +61,7 @@ const char * devnames[] = {
 	"Wired Watchman 1",
 #ifdef HIDAPI
 	"HMD Lightcap",
+	"Tracker 0 Lightcap",
 	"Wired Watchman 1 Lightcap",
 #endif
 }; //length MAX_USB_INTERFACES
@@ -74,8 +76,9 @@ const char * devnames[] = {
 
 #ifdef HIDAPI
 #define USB_DEV_LIGHTHOUSEB 6
-#define USB_DEV_W_WATCHMAN1_LIGHTCAP 7
-#define MAX_USB_DEVS		8
+#define USB_DEV_TRACKER0_LIGHTCAP 7
+#define USB_DEV_W_WATCHMAN1_LIGHTCAP 8
+#define MAX_USB_DEVS		9
 #else
 #define MAX_USB_DEVS		6
 #endif
@@ -87,8 +90,9 @@ const char * devnames[] = {
 #define USB_IF_TRACKER0		4
 #define USB_IF_W_WATCHMAN1	5
 #define USB_IF_LIGHTCAP		6
-#define USB_IF_W_WATCHMAN1_LIGHTCAP		7
-#define MAX_INTERFACES		8
+#define USB_IF_TRACKER0_LIGHTCAP		7
+#define USB_IF_W_WATCHMAN1_LIGHTCAP		8
+#define MAX_INTERFACES		9
 
 typedef struct SurviveUSBInterface SurviveUSBInterface;
 typedef struct SurviveViveData SurviveViveData;
@@ -475,9 +479,11 @@ int survive_usb_init( SurviveViveData * sv, SurviveObject * hmd, SurviveObject *
 #ifdef HIDAPI
 	//Tricky: use other interface for actual lightcap.  XXX THIS IS NOT YET RIGHT!!!
 	if( sv->udev[USB_DEV_LIGHTHOUSEB] && AttachInterface( sv, hmd, USB_IF_LIGHTCAP, sv->udev[USB_DEV_LIGHTHOUSEB], 0x82, survive_data_cb, "Lightcap")) { return -12; }
+	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; }
 #else
 	if( sv->udev[USB_DEV_LIGHTHOUSE] && AttachInterface( sv, hmd, USB_IF_LIGHTCAP, sv->udev[USB_DEV_LIGHTHOUSE], 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_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." );
@@ -521,9 +527,7 @@ int survive_vive_send_magic(SurviveContext * ctx, void * drv, int magic_code, vo
 
 		if (sv->udev[USB_DEV_W_WATCHMAN1])
 		{
-			//static uint8_t vive_magic_power_on[64] = {  0x04, 0x78, 0x29, 0x38 };
 			static uint8_t vive_magic_power_on[5] = { 0x04 };
-			//static uint8_t vive_magic_power_on[5] = { 0x04, 0x00, 0xc3, 0xe2, 0x04 };
 			r = update_feature_report( sv->udev[USB_DEV_W_WATCHMAN1], 0, vive_magic_power_on, sizeof( vive_magic_power_on ) );
 			if( r != sizeof( vive_magic_power_on ) ) return 5;
 		}
@@ -539,6 +543,23 @@ int survive_vive_send_magic(SurviveContext * ctx, void * drv, int magic_code, vo
 			if( r != sizeof( vive_magic_enable_lighthouse2 ) ) return 5;
 		}
 
+		if (sv->udev[USB_DEV_TRACKER0])
+		{
+			static uint8_t vive_magic_power_on[5] = { 0x04 };
+			r = update_feature_report( sv->udev[USB_DEV_TRACKER0], 0, vive_magic_power_on, sizeof( vive_magic_power_on ) );
+			if( r != sizeof( vive_magic_power_on ) ) return 5;
+		}
+		if (sv->udev[USB_DEV_TRACKER0_LIGHTCAP])
+		{
+			static uint8_t vive_magic_enable_lighthouse[5] = { 0x04 };
+			r = update_feature_report( sv->udev[USB_DEV_TRACKER0_LIGHTCAP], 0, vive_magic_enable_lighthouse, sizeof( vive_magic_enable_lighthouse ) );
+			if( r != sizeof( vive_magic_enable_lighthouse ) ) return 5;
+
+			static uint8_t vive_magic_enable_lighthouse2[5] = { 0x07, 0x02 };  //Switch to 0x25 mode (able to get more light updates)
+			r = update_feature_report( sv->udev[USB_DEV_TRACKER0_LIGHTCAP], 0, vive_magic_enable_lighthouse2, sizeof( vive_magic_enable_lighthouse2 ) );
+			if( r != sizeof( vive_magic_enable_lighthouse2 ) ) return 5;
+		}
+
 #if 0
 		for( int i = 0; i < 256; i++ )
 		{
@@ -551,12 +572,12 @@ int survive_vive_send_magic(SurviveContext * ctx, void * drv, int magic_code, vo
 		}
 #endif
 
-		if (sv->udev[USB_DEV_TRACKER0])
-		{
-			static uint8_t vive_magic_power_on[64] = {  0x04, 0x78, 0x29, 0x38 };
-			r = update_feature_report( sv->udev[USB_DEV_TRACKER0], 0, vive_magic_power_on, sizeof( vive_magic_power_on ) );
-			if( r != sizeof( vive_magic_power_on ) ) return 5;
-		}
+		//if (sv->udev[USB_DEV_TRACKER0])
+		//{
+		//	static uint8_t vive_magic_power_on[64] = {  0x04, 0x78, 0x29, 0x38 };
+		//	r = update_feature_report( sv->udev[USB_DEV_TRACKER0], 0, vive_magic_power_on, sizeof( vive_magic_power_on ) );
+		//	if( r != sizeof( vive_magic_power_on ) ) return 5;
+		//}
 
 		SV_INFO( "Powered unit on." );
 	}
-- 
cgit v1.3.1


From a576703242adea11c012af5afdff38af84d22e2e Mon Sep 17 00:00:00 2001
From: mwturvey <michael.w.turvey@intel.com>
Date: Fri, 24 Mar 2017 13:44:38 -0700
Subject: Adaptive Acode Offset Compensation

---
 calibrate.c        |   2 +-
 src/survive_cal.c  |   5 +--
 src/survive_data.c | 114 +++++++++++++++++++++++++++++++----------------------
 src/survive_vive.c |  30 ++++++++------
 4 files changed, 86 insertions(+), 65 deletions(-)

(limited to 'src/survive_data.c')

diff --git a/calibrate.c b/calibrate.c
index 62acbf6..6e2b5d1 100644
--- a/calibrate.c
+++ b/calibrate.c
@@ -59,7 +59,7 @@ void my_light_process( struct SurviveObject * so, int sensor_id, int acode, int
 	if( acode < 0 ) return;
 //return;
 	int jumpoffset = sensor_id;
-	if( strcmp( so->codename, "WM0" ) == 0 ) jumpoffset += 32;
+	if( strcmp( so->codename, "WM0" ) == 0 || strcmp( so->codename, "WW0" ) == 0) jumpoffset += 32;
 	else if( strcmp( so->codename, "WM1" ) == 0 ) jumpoffset += 64;
 
 
diff --git a/src/survive_cal.c b/src/survive_cal.c
index 81889ca..22a8eff 100755
--- a/src/survive_cal.c
+++ b/src/survive_cal.c
@@ -200,6 +200,7 @@ void survive_cal_light( struct SurviveObject * so, int sensor_id, int acode, int
 		//Collecting OOTX data.
 		if( sensor_id < 0 )
 		{
+				//fprintf(stderr, "%s\n", so->codename);
 			int lhid = -sensor_id-1;
 			// Take the OOTX data from the first device.  (if using HMD, WM0, WM1 only, this will be HMD)
 			if( lhid < NUM_LIGHTHOUSES && so == cd->poseobjects[0]  ) 
@@ -210,9 +211,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;  //If all lighthouses have their OOTX set, move on.         <------- Revert This!!!!! 
-			//if( i == 1 ) 
-				//cd->stage = 2;  //If all lighthouses have their OOTX set, move on.
+			if( i == NUM_LIGHTHOUSES ) cd->stage = 2;  //TODO: Make this configuratble to allow single lighthouse.
 		}
 		break;
 	case 3: //Look for light sync lengths.
diff --git a/src/survive_data.c b/src/survive_data.c
index 849b722..4e2479a 100644
--- a/src/survive_data.c
+++ b/src/survive_data.c
@@ -21,26 +21,42 @@ typedef struct
 	int lh_start_time[NUM_LIGHTHOUSES];
 	int current_lh; // used knowing which sync pulse we're looking at.
 
+} lightcap2_per_sweep_data;
+
+typedef struct
+{
+	float acode_offset;
 } lightcap2_global_data;
 
 typedef struct
 {
 	lightcaps_sweep_data sweep;
+	lightcap2_per_sweep_data per_sweep;
 	lightcap2_global_data global;
 } lightcap2_data;
 
 
 //static lightcap2_global_data lcgd = { 0 };
 
-int handle_lightcap2_getAcodeFromSyncPulse(int pulseLen)
+int handle_lightcap2_getAcodeFromSyncPulse(SurviveObject * so, int pulseLen)
 {
-	if (pulseLen < 3125) return 0;
-	if (pulseLen < 3625) return 1;
-	if (pulseLen < 4125) return 2;
-	if (pulseLen < 4625) return 3;
-	if (pulseLen < 5125) return 4;
-	if (pulseLen < 5625) return 5;
-	if (pulseLen < 6125) return 6;
+	float oldOffset = ((lightcap2_data*)so->disambiguator_data)->global.acode_offset;
+
+	int modifiedPulseLen = pulseLen - (int)oldOffset;
+
+	float newOffset = (((pulseLen) + 250) % 500) - 250;
+
+	((lightcap2_data*)so->disambiguator_data)->global.acode_offset = oldOffset * 0.9 + newOffset * 0.1;
+
+//fprintf(stderr, "    %f\n", oldOffset);
+#define ACODE_OFFSET 0
+	if (pulseLen < 3250 - ACODE_OFFSET) return 0;
+	if (pulseLen < 3750 - ACODE_OFFSET) return 1;
+	if (pulseLen < 4250 - ACODE_OFFSET) return 2;
+	if (pulseLen < 4750 - ACODE_OFFSET) return 3;
+	if (pulseLen < 5250 - ACODE_OFFSET) return 4;
+	if (pulseLen < 5750 - ACODE_OFFSET) return 5;
+	if (pulseLen < 6250 - ACODE_OFFSET) return 6;
 	return 7;
 }
 void handle_lightcap2_process_sweep_data(SurviveObject *so)
@@ -67,13 +83,13 @@ void handle_lightcap2_process_sweep_data(SurviveObject *so)
 		{
 			if (lcd->sweep.sweep_len[i] != 0) // if the sensor was hit, process it
 			{
-				int offset_from = lcd->sweep.sweep_time[i] - lcd->global.activeSweepStartTime + lcd->sweep.sweep_len[i] / 2;
+				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)
 				{
 					//if (longest_pulse *10 / 8 < lcd->sweep.sweep_len[i]) 
 					{
-						so->ctx->lightproc(so, i, lcd->global.activeAcode, offset_from, lcd->sweep.sweep_time[i], lcd->sweep.sweep_len[i], lcd->global.activeLighthouse);
+						so->ctx->lightproc(so, i, lcd->per_sweep.activeAcode, offset_from, lcd->sweep.sweep_time[i], lcd->sweep.sweep_len[i], lcd->per_sweep.activeLighthouse);
 					}
 				}
 			}
@@ -96,54 +112,54 @@ void handle_lightcap2_sync(SurviveObject * so, LightcapElement * le )
 	// Process any sweep data we have
 	handle_lightcap2_process_sweep_data(so);
 
-	int time_since_last_sync = (le->timestamp - lcd->global.recent_sync_time);
+	int time_since_last_sync = (le->timestamp - lcd->per_sweep.recent_sync_time);
 
 	//fprintf(stderr, "            %2d %8d %d\n", le->sensor_id, time_since_last_sync, le->length);
 	// need to store up sync pulses, so we can take the earliest starting time for all sensors.
 	if (time_since_last_sync < 2400)
 	{
-		lcd->global.recent_sync_time = le->timestamp;
+		lcd->per_sweep.recent_sync_time = le->timestamp;
 		// it's the same sync pulse;
 		so->sync_set_number = 1;
 		so->recent_sync_time = le->timestamp;
 
-		lcd->global.lh_pulse_len[lcd->global.current_lh] = le->length;
-		lcd->global.lh_start_time[lcd->global.current_lh] = le->timestamp;
+		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(le->length);
+		int acode = handle_lightcap2_getAcodeFromSyncPulse(so, le->length);
 		if (!(acode >> 2 & 1)) // if the skip bit is not set
 		{
-			lcd->global.activeLighthouse = lcd->global.current_lh;
-			lcd->global.activeSweepStartTime = le->timestamp;
-			lcd->global.activeAcode = acode;
+			lcd->per_sweep.activeLighthouse = lcd->per_sweep.current_lh;
+			lcd->per_sweep.activeSweepStartTime = le->timestamp;
+			lcd->per_sweep.activeAcode = acode;
 		}
 		else
 		{
-			lcd->global.activeLighthouse = -1;
-			lcd->global.activeSweepStartTime = 0;
-			lcd->global.activeAcode = 0;
+			lcd->per_sweep.activeLighthouse = -1;
+			lcd->per_sweep.activeSweepStartTime = 0;
+			lcd->per_sweep.activeAcode = 0;
 		}
 	}
 	else if (time_since_last_sync < 24000)
 	{
-		lcd->global.recent_sync_time = le->timestamp;
+		lcd->per_sweep.recent_sync_time = le->timestamp;
 		// I do believe we are lighthouse B		
-		lcd->global.current_lh = 1;
-		lcd->global.lh_pulse_len[lcd->global.current_lh] = le->length;
-		lcd->global.lh_start_time[lcd->global.current_lh] = le->timestamp;
+		lcd->per_sweep.current_lh = 1;
+		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(le->length);
+		int acode = handle_lightcap2_getAcodeFromSyncPulse(so, le->length);
 
 		if (!(acode >> 2 & 1)) // if the skip bit is not set
 		{
-			if (lcd->global.activeLighthouse != -1)
+			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");
 			}
-			lcd->global.activeLighthouse = 1;
-			lcd->global.activeSweepStartTime = le->timestamp;
-			lcd->global.activeAcode = acode;
+			lcd->per_sweep.activeLighthouse = 1;
+			lcd->per_sweep.activeSweepStartTime = le->timestamp;
+			lcd->per_sweep.activeAcode = acode;
 		}
 
 	}
@@ -153,49 +169,50 @@ void handle_lightcap2_sync(SurviveObject * so, LightcapElement * le )
 
 		// first, send out the sync pulse data for the last round (for OOTX decoding
 		{
-			if (lcd->global.lh_pulse_len[0] != 0)
+			if (lcd->per_sweep.lh_pulse_len[0] != 0)
 			{
 				so->ctx->lightproc(
 					so,
 					-1,
-					handle_lightcap2_getAcodeFromSyncPulse(lcd->global.lh_pulse_len[0]),
-					lcd->global.lh_pulse_len[0],
-					lcd->global.lh_start_time[0],
+					handle_lightcap2_getAcodeFromSyncPulse(so, lcd->per_sweep.lh_pulse_len[0]),
+					lcd->per_sweep.lh_pulse_len[0],
+					lcd->per_sweep.lh_start_time[0],
 					0,
 					0);
 			}
-			if (lcd->global.lh_pulse_len[1] != 0)
+			if (lcd->per_sweep.lh_pulse_len[1] != 0)
 			{
 				so->ctx->lightproc(
 					so,
 					-2,
-					handle_lightcap2_getAcodeFromSyncPulse(lcd->global.lh_pulse_len[1]),
-					lcd->global.lh_pulse_len[1],
-					lcd->global.lh_start_time[1],
+					handle_lightcap2_getAcodeFromSyncPulse(so, lcd->per_sweep.lh_pulse_len[1]),
+					lcd->per_sweep.lh_pulse_len[1],
+					lcd->per_sweep.lh_start_time[1],
 					0,
 					1);
 			}
 		}
 
+		//fprintf(stderr, "************************************ Reinitializing Disambiguator!!!\n");
 		// initialize here.
-		memset(&lcd->global, 0, sizeof(lcd->global));
-		lcd->global.activeLighthouse = -1; 
+		memset(&lcd->per_sweep, 0, sizeof(lcd->per_sweep));
+		lcd->per_sweep.activeLighthouse = -1; 
 
 
 
-		lcd->global.recent_sync_time = le->timestamp;
+		lcd->per_sweep.recent_sync_time = le->timestamp;
 		// I do believe we are lighthouse A		
-		lcd->global.current_lh = 0;
-		lcd->global.lh_pulse_len[lcd->global.current_lh] = le->length;
-		lcd->global.lh_start_time[lcd->global.current_lh] = le->timestamp;
+		lcd->per_sweep.current_lh = 0;
+		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(le->length);
+		int acode = handle_lightcap2_getAcodeFromSyncPulse(so, le->length);
 
 		if (!(acode >> 2 & 1)) // if the skip bit is not set
 		{
-			lcd->global.activeLighthouse = 0;
-			lcd->global.activeSweepStartTime = le->timestamp;
-			lcd->global.activeAcode = acode;
+			lcd->per_sweep.activeLighthouse = 0;
+			lcd->per_sweep.activeSweepStartTime = le->timestamp;
+			lcd->per_sweep.activeAcode = acode;
 		}
 	}
 }
@@ -230,6 +247,7 @@ void handle_lightcap2( SurviveObject * so, LightcapElement * le )
 
 	if (so->disambiguator_data == NULL)
 	{
+		fprintf(stderr, "Initializing Disambiguator Data\n");
 		so->disambiguator_data = malloc(sizeof(lightcap2_data));
 		memset(so->disambiguator_data, 0, sizeof(lightcap2_data));
 	}
diff --git a/src/survive_vive.c b/src/survive_vive.c
index c1bcb26..55e949a 100755
--- a/src/survive_vive.c
+++ b/src/survive_vive.c
@@ -479,6 +479,9 @@ int survive_usb_init( SurviveViveData * sv, SurviveObject * hmd, SurviveObject *
 #ifdef HIDAPI
 	//Tricky: use other interface for actual lightcap.  XXX THIS IS NOT YET RIGHT!!!
 	if( sv->udev[USB_DEV_LIGHTHOUSEB] && AttachInterface( sv, hmd, USB_IF_LIGHTCAP, sv->udev[USB_DEV_LIGHTHOUSEB], 0x82, survive_data_cb, "Lightcap")) { return -12; }
+
+	// 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; }
 #else
@@ -564,18 +567,18 @@ int survive_vive_send_magic(SurviveContext * ctx, void * drv, int magic_code, vo
 			r = update_feature_report( sv->udev[USB_DEV_TRACKER0], 0, vive_magic_power_on, sizeof( vive_magic_power_on ) );
 			if( r != sizeof( vive_magic_power_on ) ) return 5;
 		}
-#ifdef HIDAPI
-		if (sv->udev[USB_DEV_TRACKER0_LIGHTCAP])
-		{
-			static uint8_t vive_magic_enable_lighthouse[5] = { 0x04 };
-			r = update_feature_report( sv->udev[USB_DEV_TRACKER0_LIGHTCAP], 0, vive_magic_enable_lighthouse, sizeof( vive_magic_enable_lighthouse ) );
-			if( r != sizeof( vive_magic_enable_lighthouse ) ) return 5;
-
-			static uint8_t vive_magic_enable_lighthouse2[5] = { 0x07, 0x02 };  //Switch to 0x25 mode (able to get more light updates)
-			r = update_feature_report( sv->udev[USB_DEV_TRACKER0_LIGHTCAP], 0, vive_magic_enable_lighthouse2, sizeof( vive_magic_enable_lighthouse2 ) );
-			if( r != sizeof( vive_magic_enable_lighthouse2 ) ) return 5;
-		}
-#else
+//#ifdef HIDAPI
+//		if (sv->udev[USB_DEV_TRACKER0_LIGHTCAP])
+//		{
+//			static uint8_t vive_magic_enable_lighthouse[5] = { 0x04 };
+//			r = update_feature_report( sv->udev[USB_DEV_TRACKER0_LIGHTCAP], 0, vive_magic_enable_lighthouse, sizeof( vive_magic_enable_lighthouse ) );
+//			if( r != sizeof( vive_magic_enable_lighthouse ) ) return 5;
+//
+//			static uint8_t vive_magic_enable_lighthouse2[5] = { 0x07, 0x02 };  //Switch to 0x25 mode (able to get more light updates)
+//			r = update_feature_report( sv->udev[USB_DEV_TRACKER0_LIGHTCAP], 0, vive_magic_enable_lighthouse2, sizeof( vive_magic_enable_lighthouse2 ) );
+//			if( r != sizeof( vive_magic_enable_lighthouse2 ) ) return 5;
+//		}
+//#else		
 		if (sv->udev[USB_DEV_TRACKER0])
 		{
 			static uint8_t vive_magic_enable_lighthouse[5] = { 0x04 };
@@ -587,7 +590,7 @@ int survive_vive_send_magic(SurviveContext * ctx, void * drv, int magic_code, vo
 			if( r != sizeof( vive_magic_enable_lighthouse2 ) ) return 5;
 		}
 
-#endif
+//#endif
 
 #if 0
 		for( int i = 0; i < 256; i++ )
@@ -1186,6 +1189,7 @@ void survive_data_cb( SurviveUSBInterface * si )
 		break;
 	}
 	case USB_IF_W_WATCHMAN1_LIGHTCAP:
+	case USB_IF_TRACKER0_LIGHTCAP:
 	{
 		int i=0;
 		for( i = 0; i < 7; i++ )
-- 
cgit v1.3.1


From 4dc1d72785c660c206f8def9d8c8aa32289c2709 Mon Sep 17 00:00:00 2001
From: mwturvey <michael.w.turvey@intel.com>
Date: Fri, 24 Mar 2017 15:19:59 -0700
Subject: More cleanup & finishing genericization of calibrator

---
 calibrate.c                        |  4 ++--
 include/libsurvive/survive.h       |  2 +-
 include/libsurvive/survive_types.h |  2 +-
 redist/CNFGWinDriver.c             |  2 +-
 redist/json_helpers.c              |  2 +-
 src/survive_cal.c                  | 30 +++++++++++++++++++-----------
 src/survive_cal.h                  |  6 ++++--
 src/survive_data.c                 |  6 +++---
 src/survive_process.c              |  6 +++---
 src/survive_vive.c                 |  6 ++----
 10 files changed, 37 insertions(+), 29 deletions(-)

(limited to 'src/survive_data.c')

diff --git a/calibrate.c b/calibrate.c
index ee22abf..abf592a 100644
--- a/calibrate.c
+++ b/calibrate.c
@@ -99,9 +99,9 @@ void my_imu_process( struct SurviveObject * so, int mask, FLT * accelgyro, uint3
 }
 
 
-void my_angle_process( struct SurviveObject * so, int sensor_id, int acode, uint32_t timecode, FLT length, FLT angle )
+void my_angle_process( struct SurviveObject * so, int sensor_id, int acode, uint32_t timecode, FLT length, FLT angle, uint32_t lh)
 {
-	survive_default_angle_process( so, sensor_id, acode, timecode, length, angle );
+	survive_default_angle_process( so, sensor_id, acode, timecode, length, angle, lh );
 }
 
 char* sensor_name[32];
diff --git a/include/libsurvive/survive.h b/include/libsurvive/survive.h
index 1165e9d..e13312d 100644
--- a/include/libsurvive/survive.h
+++ b/include/libsurvive/survive.h
@@ -132,7 +132,7 @@ void survive_cal_install( SurviveContext * ctx );  //XXX This will be removed if
 //Accept higher-level data.
 void survive_default_light_process( SurviveObject * so, int sensor_id, int acode, int timeinsweep, uint32_t timecode, uint32_t length , uint32_t lh);
 void survive_default_imu_process( SurviveObject * so, int mode, FLT * accelgyro, uint32_t timecode, int id );
-void survive_default_angle_process( SurviveObject * so, int sensor_id, int acode, uint32_t timecode, FLT length, FLT angle );
+void survive_default_angle_process( SurviveObject * so, int sensor_id, int acode, uint32_t timecode, FLT length, FLT angle, uint32_t lh );
 
 
 ////////////////////// Survive Drivers ////////////////////////////
diff --git a/include/libsurvive/survive_types.h b/include/libsurvive/survive_types.h
index def30b8..bfd0b1d 100644
--- a/include/libsurvive/survive_types.h
+++ b/include/libsurvive/survive_types.h
@@ -30,7 +30,7 @@ typedef struct SurviveCalData SurviveCalData;   //XXX Warning: This may be remov
 typedef void (*text_feedback_func)( SurviveContext * ctx, const char * fault );
 typedef void (*light_process_func)( SurviveObject * so, int sensor_id, int acode, int timeinsweep, uint32_t timecode, uint32_t length, uint32_t lighthouse);
 typedef void (*imu_process_func)( SurviveObject * so, int mask, FLT * accelgyro, uint32_t timecode, int id );
-typedef void (*angle_process_func)( SurviveObject * so, int sensor_id, int acode, uint32_t timecode, FLT length, FLT angle );
+typedef void (*angle_process_func)( SurviveObject * so, int sensor_id, int acode, uint32_t timecode, FLT length, FLT angle, uint32_t lh);
 
 
 //Device drivers (prefix your drivers with "DriverReg") i.e.
diff --git a/redist/CNFGWinDriver.c b/redist/CNFGWinDriver.c
index c5da925..b1c1eb0 100644
--- a/redist/CNFGWinDriver.c
+++ b/redist/CNFGWinDriver.c
@@ -232,7 +232,7 @@ void CNFGHandleInput()
 		case WM_MBUTTONUP:		HandleButton( (msg.lParam & 0xFFFF), (msg.lParam>>16) & 0xFFFF, 3, 0 ); break;
 		case WM_KEYDOWN:
 		case WM_KEYUP:
-			HandleKey( tolower( msg.wParam ), (msg.message==WM_KEYDOWN) );
+			HandleKey( tolower( (int)(msg.wParam) ), (msg.message==WM_KEYDOWN) );
 			break;
 		default:
 			DispatchMessage(&msg);
diff --git a/redist/json_helpers.c b/redist/json_helpers.c
index 3b5cc0d..29d48bd 100644
--- a/redist/json_helpers.c
+++ b/redist/json_helpers.c
@@ -174,7 +174,7 @@ void json_load_file(const char* path) {
 
 	int16_t children = -1;
 
-	for (i=0; i<(int)items; i+=2)
+	for (i=0; i<(unsigned int)items; i+=2)
 	{
 		//increment i on each successful tag + values combination, not individual tokens
 		jsmntok_t* tag_t = tokens+i;
diff --git a/src/survive_cal.c b/src/survive_cal.c
index 22a8eff..6c153b4 100755
--- a/src/survive_cal.c
+++ b/src/survive_cal.c
@@ -220,10 +220,13 @@ void survive_cal_light( struct SurviveObject * so, int sensor_id, int acode, int
 		else if( acode < -4 ) break;
 		int lh = (-acode) - 3;
 
-		if( strcmp( so->codename, "WM0" ) == 0 )
-			sensor_id += 32;
-		if( strcmp( so->codename, "WM1" ) == 0 )
-			sensor_id += 64;
+		for (int i=0; i < min(MAX_DEVICES_TO_CAL, cd->numPoseObjects); i++)
+		{
+			if( strcmp( so->codename, cd->poseobjects[i]->codename ) == 0 )
+			{
+				sensor_id += i*32;
+			}
+		}
 
 		cd->all_sync_times[sensor_id][lh][cd->all_sync_counts[sensor_id][lh]++] = length;
 		break;
@@ -233,7 +236,7 @@ void survive_cal_light( struct SurviveObject * so, int sensor_id, int acode, int
 	
 }
 
-void survive_cal_angle( struct SurviveObject * so, int sensor_id, int acode, uint32_t timecode, FLT length, FLT angle )
+void survive_cal_angle( struct SurviveObject * so, int sensor_id, int acode, uint32_t timecode, FLT length, FLT angle, uint32_t lh )
 {
 	struct SurviveContext * ctx = so->ctx;
 	struct SurviveCalData * cd = ctx->calptr;
@@ -241,14 +244,18 @@ void survive_cal_angle( struct SurviveObject * so, int sensor_id, int acode, uin
 	if( !cd ) return;
 
 	int sensid = sensor_id;
-	if( strcmp( so->codename, "WM0" ) == 0 )
-		sensid += 32;
-	if( strcmp( so->codename, "WM1" ) == 0 )
-		sensid += 64;
+
+	for (int i=0; i < min(MAX_DEVICES_TO_CAL, cd->numPoseObjects); i++)
+	{
+		if( strcmp( so->codename, cd->poseobjects[i]->codename ) == 0 )
+		{
+			sensid += i*32;
+		}
+	}
 
 	if( sensid >= MAX_SENSORS_TO_CAL || sensid < 0 ) return;
 
-	int lighthouse = acode>>2;
+	int lighthouse = lh;
 	int axis = acode & 1;
 
 	switch( cd->stage )
@@ -292,7 +299,8 @@ void survive_cal_angle( struct SurviveObject * so, int sensor_id, int acode, uin
 			int min_peaks = PTS_BEFORE_COMMON;
 			int i, j, k;
 			cd->found_common = 1;
-			for( i = 0; i < MAX_SENSORS_TO_CAL/SENSORS_PER_OBJECT; i++ )
+			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++ )
 			{
 				int sensors_visible = 0;
diff --git a/src/survive_cal.h b/src/survive_cal.h
index 8f4e4de..ae644d1 100644
--- a/src/survive_cal.h
+++ b/src/survive_cal.h
@@ -30,9 +30,11 @@ int survive_cal_get_status( SurviveContext * ctx, char * description, int descri
 
 //Called from survive_default_light_process
 void survive_cal_light( SurviveObject * so, int sensor_id, int acode, int timeinsweep, uint32_t timecode, uint32_t length, uint32_t lighthouse);
-void survive_cal_angle( SurviveObject * so, int sensor_id, int acode, uint32_t timecode, FLT length, FLT angle );
+void survive_cal_angle( SurviveObject * so, int sensor_id, int acode, uint32_t timecode, FLT length, FLT angle, uint32_t lh );
 
-#define MAX_SENSORS_TO_CAL 96
+#define MAX_SENSORS_PER_DEVICE 32
+#define MAX_DEVICES_TO_CAL 3
+#define MAX_SENSORS_TO_CAL (MAX_SENSORS_PER_DEVICE * MAX_DEVICES_TO_CAL)
 
 #define MIN_PTS_BEFORE_CAL 24
 
diff --git a/src/survive_data.c b/src/survive_data.c
index 4e2479a..9447104 100644
--- a/src/survive_data.c
+++ b/src/survive_data.c
@@ -25,7 +25,7 @@ typedef struct
 
 typedef struct
 {
-	float acode_offset;
+	double acode_offset;
 } lightcap2_global_data;
 
 typedef struct
@@ -40,11 +40,11 @@ typedef struct
 
 int handle_lightcap2_getAcodeFromSyncPulse(SurviveObject * so, int pulseLen)
 {
-	float oldOffset = ((lightcap2_data*)so->disambiguator_data)->global.acode_offset;
+	double oldOffset = ((lightcap2_data*)so->disambiguator_data)->global.acode_offset;
 
 	int modifiedPulseLen = pulseLen - (int)oldOffset;
 
-	float newOffset = (((pulseLen) + 250) % 500) - 250;
+	double newOffset = (((pulseLen) + 250) % 500) - 250;
 
 	((lightcap2_data*)so->disambiguator_data)->global.acode_offset = oldOffset * 0.9 + newOffset * 0.1;
 
diff --git a/src/survive_process.c b/src/survive_process.c
index d4604d8..b58b344 100644
--- a/src/survive_process.c
+++ b/src/survive_process.c
@@ -37,16 +37,16 @@ void survive_default_light_process( SurviveObject * so, int sensor_id, int acode
 #endif
 
 	FLT length_sec = length / (FLT)so->timebase_hz;
-	ctx->angleproc( so, sensor_id, acode, timecode, length_sec, angle );
+	ctx->angleproc( so, sensor_id, acode, timecode, length_sec, angle, lh);
 }
 
 
-void survive_default_angle_process( SurviveObject * so, int sensor_id, int acode, uint32_t timecode, FLT length, FLT angle )
+void survive_default_angle_process( SurviveObject * so, int sensor_id, int acode, uint32_t timecode, FLT length, FLT angle, uint32_t lh)
 {
 	SurviveContext * ctx = so->ctx;
 	if( ctx->calptr )
 	{
-		survive_cal_angle( so, sensor_id, acode, timecode, length, angle );
+		survive_cal_angle( so, sensor_id, acode, timecode, length, angle, lh );
 	}
 	if( so->PoserFn )
 	{
diff --git a/src/survive_vive.c b/src/survive_vive.c
index 55e949a..a5c731d 100755
--- a/src/survive_vive.c
+++ b/src/survive_vive.c
@@ -834,7 +834,6 @@ static void handle_watchman( SurviveObject * w, uint8_t * readdata )
 	qty-=2;
 	int propset = 0;
 	int doimu = 0;
-	int i;
 
 	if( (type & 0xf0) == 0xf0 )
 	{
@@ -911,10 +910,9 @@ static void handle_watchman( SurviveObject * w, uint8_t * readdata )
 		*readdata = type; //Put 'type' back on stack.
 		uint8_t * mptr = readdata + qty-3-1; //-3 for timecode, -1 to 
 
-//#define DEBUG_WATCHMAN
 #ifdef DEBUG_WATCHMAN
 		printf( "_%s ", w->codename);
-		for( i = 0; i < qty; i++ )
+		for(int i = 0; i < qty; i++ )
 		{
 			printf( "%02x ", readdata[i] );
 		}
@@ -1198,7 +1196,7 @@ void survive_data_cb( SurviveUSBInterface * si )
 			unsigned short *length = (unsigned short *)(&(readdata[2]));
 			unsigned long *time = (unsigned long *)(&(readdata[4]));
 			LightcapElement le;
-			le.sensor_id = POP2;
+			le.sensor_id = (uint8_t)POP2;
 			le.length = POP2;
 			le.timestamp = POP4;
 			if( le.sensor_id == 0xff ) break;
-- 
cgit v1.3.1


From b3bdcdb838ed57986f359b2181a624bfd1acbbf1 Mon Sep 17 00:00:00 2001
From: mwturvey <michael.w.turvey@intel.com>
Date: Tue, 28 Mar 2017 10:15:36 -0700
Subject: Fix Tracker IMU

---
 src/poser_turveytori.c |  2 +-
 src/survive_data.c     | 10 ++++++++--
 src/survive_vive.c     | 17 +----------------
 3 files changed, 10 insertions(+), 19 deletions(-)

(limited to 'src/survive_data.c')

diff --git a/src/poser_turveytori.c b/src/poser_turveytori.c
index 5c3350b..df691e3 100644
--- a/src/poser_turveytori.c
+++ b/src/poser_turveytori.c
@@ -1211,7 +1211,7 @@ int PoserTurveyTori( SurviveObject * so, PoserData * pd )
 	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] );
+		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] );
 		break;
 	}
 	case POSERDATA_LIGHT:
diff --git a/src/survive_data.c b/src/survive_data.c
index 9447104..3eb5890 100644
--- a/src/survive_data.c
+++ b/src/survive_data.c
@@ -154,8 +154,14 @@ void handle_lightcap2_sync(SurviveObject * so, LightcapElement * le )
 		{
 			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_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;
-- 
cgit v1.3.1


From 3ca8ba3d69de226ae8835bb29e45c7bcd35793fe Mon Sep 17 00:00:00 2001
From: mwturvey <michael.w.turvey@intel.com>
Date: Wed, 29 Mar 2017 16:39:47 -0700
Subject: Tori Poser Works!

There's a ton of code cruft, and the algorithm is currently too slow.  BUT I can track an object using only 1 lighthouse for tracking, at (I believe) an update rate of at least 7.5 HZ.  By tracking, I know the position and orientation of the lighthouses relative to the tracked object, and I know the tracked object's location relative to the lighthouse.  I don't have the orientation of the tracked object relative to the lighthouse yet, but that should be easy given the rest of the "knowns."
---
 src/poser_octavioradii.c | 198 +++++++++++++++++++++++++++++++++++++++++++++--
 src/poser_turveytori.c   | 174 +++++++++++++++++++++++++++++++++++++----
 src/survive_data.c       |  37 ++++++++-
 3 files changed, 381 insertions(+), 28 deletions(-)

(limited to 'src/survive_data.c')

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 8b92860..22098d0 100644
--- a/src/poser_turveytori.c
+++ b/src/poser_turveytori.c
@@ -74,6 +74,11 @@ 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;
 
 
@@ -526,7 +531,7 @@ static Point RefineEstimateUsingModifiedGradientDescent1(Point initialEstimate,
 
 
 	}
-	printf("\ni=%d\n", i);
+	printf(" i=%d ", i);
 
 	return lastPoint;
 }
@@ -845,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)
@@ -873,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];
@@ -947,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)
 {
@@ -956,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]);
 }
 
 
@@ -1056,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);
 }
 
 
@@ -1088,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);
@@ -1168,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);
@@ -1191,7 +1215,82 @@ 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);
+
+}
 
 
 
@@ -1229,14 +1328,47 @@ int PoserTurveyTori( SurviveObject * so, PoserData * poserData )
 	case POSERDATA_LIGHT:
 	{
 		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 (0 == l->lh)
+		if ((td->lastAxis[l->lh] != (l->acode & 0x1)) )
 		{
-			if (l->acode & 0x1)
+			int foo = l->acode & 0x1;
+			//printf("%d", foo);
+
+
+			//if (axis)
 			{
-				printf("%2d: %8f\n", l->sensor_id, l->angle);
+				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.
+				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:
@@ -1278,12 +1410,20 @@ int PoserTurveyTori( SurviveObject * so, PoserData * poserData )
 					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);
 		}
 		{
diff --git a/src/survive_data.c b/src/survive_data.c
index 3eb5890..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,8 +181,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
 		{
 			if (lcd->per_sweep.activeLighthouse != -1)
-- 
cgit v1.3.1


From b6787c60171724360f097a16cc7ec967995ff335 Mon Sep 17 00:00:00 2001
From: cnlohr <lohr85@gmail.com>
Date: Fri, 7 Apr 2017 00:55:21 -0400
Subject: Update, fixing charles' disambiguator, adding flag for
 TURVEYBIGUATOR!

---
 src/survive_data.c | 21 +++++++++++++++------
 1 file changed, 15 insertions(+), 6 deletions(-)

(limited to 'src/survive_data.c')

diff --git a/src/survive_data.c b/src/survive_data.c
index 3f16c7c..4bb1526 100644
--- a/src/survive_data.c
+++ b/src/survive_data.c
@@ -5,6 +5,10 @@
 #include <stdint.h>
 #include <string.h>
 
+#define USE_TURVEYBIGUATOR
+
+#ifdef USE_TURVEYBIGUATOR
+
 typedef struct
 {
 	unsigned int sweep_time[SENSORS_PER_OBJECT];
@@ -311,6 +315,9 @@ void handle_lightcap2( SurviveObject * so, LightcapElement * le )
 
 }
 
+
+#endif
+
 int32_t decode_acode(uint32_t length, int32_t main_divisor) {
 	//+50 adds a small offset and seems to help always get it right. 
 	//Check the +50 in the future to see how well this works on a variety of hardware.
@@ -325,9 +332,13 @@ int32_t decode_acode(uint32_t length, int32_t main_divisor) {
 void handle_lightcap( SurviveObject * so, LightcapElement * le )
 {
 	SurviveContext * ctx = so->ctx;
+
+#ifdef USE_TURVEYBIGUATOR
 	handle_lightcap2(so,le);
 	return;
 
+#else
+
 	//int32_t deltat = (uint32_t)le->timestamp - (uint32_t)so->last_master_time;
 
 	if( le->sensor_id > SENSORS_PER_OBJECT )
@@ -377,8 +388,6 @@ void handle_lightcap( SurviveObject * so, LightcapElement * le )
 	{
 		int is_new_pulse = delta > so->pulselength_min_sync /*1500*/ + last_sync_length;
 
-
-
 		so->did_handle_ootx = 0;
 
 		if( is_new_pulse )
@@ -398,6 +407,7 @@ void handle_lightcap( SurviveObject * so, LightcapElement * le )
 			else
 			{
 				ssn = ++so->sync_set_number;
+
 				if( so->sync_set_number >= NUM_LIGHTHOUSES )
 				{
 					SV_INFO( "Warning.  Received an extra, unassociated sync pulse." );
@@ -432,8 +442,6 @@ void handle_lightcap( SurviveObject * so, LightcapElement * le )
 		}
 	}
 
-
-
 	//See if this is a valid actual pulse.
 	else if( le->length < so->pulse_max_for_sweep && delta > so->pulse_in_clear_time && ssn >= 0 )
 	{
@@ -496,7 +504,6 @@ void handle_lightcap( SurviveObject * so, LightcapElement * le )
 			so->did_handle_ootx = 1;
 		}
 
-
 		if (acode > 3) {
 			if( ssn == 0 )
 			{
@@ -512,7 +519,7 @@ void handle_lightcap( SurviveObject * so, LightcapElement * le )
 		//Make sure pulse is in valid window
 		if( offset_from < 380000 && offset_from > 70000 )
 		{
-			ctx->lightproc( so, le->sensor_id, acode, offset_from, le->timestamp, le->length, so->sync_set_number );
+			ctx->lightproc( so, le->sensor_id, acode, offset_from, le->timestamp, le->length, !(acode>>2) );
 		}
 	}
 	else
@@ -520,6 +527,8 @@ void handle_lightcap( SurviveObject * so, LightcapElement * le )
 		//printf( "FAIL %d   %d - %d = %d\n", le->length, so->last_photo_time, le->timestamp, so->last_photo_time - le->timestamp );
 		//Runt pulse, or no sync pulses available.
 	}
+#endif
+
 }
 
 
-- 
cgit v1.3.1


From 454bb09f5ac0a72149c82a2231a9460e06657dea Mon Sep 17 00:00:00 2001
From: Joshua Allen <axlecrusher@gmail.com>
Date: Sat, 8 Apr 2017 10:32:59 -0400
Subject: Delay picking active lighthouse until the sweep pass so we can be
 sure we have the longest synce pulse time and acode. HMD reads both
 lighthouses.

---
 src/survive_data.c | 80 ++++++++++++++++++++++++++++++++++++++++++------------
 1 file changed, 62 insertions(+), 18 deletions(-)

(limited to 'src/survive_data.c')

diff --git a/src/survive_data.c b/src/survive_data.c
index 4bb1526..f61ae8b 100644
--- a/src/survive_data.c
+++ b/src/survive_data.c
@@ -21,8 +21,10 @@ typedef struct
 	int activeSweepStartTime;
 	int activeAcode;
 
-	int lh_pulse_len[NUM_LIGHTHOUSES];
+//	int lh_pulse_len[NUM_LIGHTHOUSES];
 	int lh_start_time[NUM_LIGHTHOUSES];
+	int lh_max_pulse_length[NUM_LIGHTHOUSES];
+	uint16_t lh_acode[NUM_LIGHTHOUSES];
 	int current_lh; // used knowing which sync pulse we're looking at.
 
 } lightcap2_per_sweep_data;
@@ -145,25 +147,34 @@ 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);
+	int acode = handle_lightcap2_getAcodeFromSyncPulse(so, le->length); //acode for this sensor reading
 
 	// Process any sweep data we have
 	handle_lightcap2_process_sweep_data(so);
 
 	int time_since_last_sync = (le->timestamp - lcd->per_sweep.recent_sync_time);
 
-	//fprintf(stderr, "            %2d %8d %d\n", le->sensor_id, time_since_last_sync, le->length);
+
+	fprintf(stderr, "            %2d %8d %d\n", le->sensor_id, time_since_last_sync, le->length);
 	// need to store up sync pulses, so we can take the earliest starting time for all sensors.
 	if (time_since_last_sync < 2400)
 	{
 		lcd->per_sweep.recent_sync_time = le->timestamp;
 		// it's the same sync pulse;
-		so->sync_set_number = 1;
+//		so->sync_set_number = 1;
 		so->recent_sync_time = le->timestamp;
 
-		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;
+//		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;
 
+		if (le->length > lcd->per_sweep.lh_max_pulse_length[lcd->per_sweep.current_lh]) {
+			lcd->per_sweep.lh_max_pulse_length[lcd->per_sweep.current_lh] = le->length;
+			lcd->per_sweep.lh_start_time[lcd->per_sweep.current_lh] = le->timestamp;
+			lcd->per_sweep.lh_acode[lcd->per_sweep.current_lh] = acode;
+		}
+
+/*
+		//this stuff should probably be happening on the sweep so that we can filter out erroneous a codes
 		if (!(acode >> 2 & 1)) // if the skip bit is not set
 		{
 			lcd->per_sweep.activeLighthouse = lcd->per_sweep.current_lh;
@@ -172,19 +183,26 @@ void handle_lightcap2_sync(SurviveObject * so, LightcapElement * le )
 		}
 		else
 		{
+			//this causes the master lighthouse to be ignored from the HMD
 			lcd->per_sweep.activeLighthouse = -1;
 			lcd->per_sweep.activeSweepStartTime = 0;
 			lcd->per_sweep.activeAcode = 0;
 		}
+		*/
 	}
 	else if (time_since_last_sync < 24000)
 	{
+		lcd->per_sweep.activeLighthouse != -1;
+
 		lcd->per_sweep.recent_sync_time = le->timestamp;
 		// I do believe we are lighthouse B		
 		lcd->per_sweep.current_lh = 1;
-		lcd->per_sweep.lh_pulse_len[lcd->per_sweep.current_lh] = le->length;
+//		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;
+		lcd->per_sweep.lh_max_pulse_length[lcd->per_sweep.current_lh] = le->length;
+		lcd->per_sweep.lh_acode[lcd->per_sweep.current_lh] = acode;
 
+/*
 		if (!(acode >> 2 & 1)) // if the skip bit is not set
 		{
 			if (lcd->per_sweep.activeLighthouse != -1)
@@ -198,10 +216,12 @@ void handle_lightcap2_sync(SurviveObject * so, LightcapElement * le )
 					fprintf(stderr, "WARNING: Two non-skip pulses received on the same cycle!\n");
 				}
 			}
+
 			lcd->per_sweep.activeLighthouse = 1;
 			lcd->per_sweep.activeSweepStartTime = le->timestamp;
 			lcd->per_sweep.activeAcode = acode;
 		}
+		*/
 
 	}
 	else if (time_since_last_sync > 370000)
@@ -210,24 +230,24 @@ void handle_lightcap2_sync(SurviveObject * so, LightcapElement * le )
 
 		// first, send out the sync pulse data for the last round (for OOTX decoding
 		{
-			if (lcd->per_sweep.lh_pulse_len[0] != 0)
+			if (lcd->per_sweep.lh_max_pulse_length[0] != 0)
 			{
 				so->ctx->lightproc(
 					so,
 					-1,
-					handle_lightcap2_getAcodeFromSyncPulse(so, lcd->per_sweep.lh_pulse_len[0]),
-					lcd->per_sweep.lh_pulse_len[0],
+					handle_lightcap2_getAcodeFromSyncPulse(so, lcd->per_sweep.lh_max_pulse_length[0]),
+					lcd->per_sweep.lh_max_pulse_length[0],
 					lcd->per_sweep.lh_start_time[0],
 					0,
 					0);
 			}
-			if (lcd->per_sweep.lh_pulse_len[1] != 0)
+			if (lcd->per_sweep.lh_max_pulse_length[1] != 0)
 			{
 				so->ctx->lightproc(
 					so,
 					-2,
-					handle_lightcap2_getAcodeFromSyncPulse(so, lcd->per_sweep.lh_pulse_len[1]),
-					lcd->per_sweep.lh_pulse_len[1],
+					handle_lightcap2_getAcodeFromSyncPulse(so, lcd->per_sweep.lh_max_pulse_length[1]),
+					lcd->per_sweep.lh_max_pulse_length[1],
 					lcd->per_sweep.lh_start_time[1],
 					0,
 					1);
@@ -239,23 +259,27 @@ void handle_lightcap2_sync(SurviveObject * so, LightcapElement * le )
 		memset(&lcd->per_sweep, 0, sizeof(lcd->per_sweep));
 		lcd->per_sweep.activeLighthouse = -1; 
 
-
-
 		lcd->per_sweep.recent_sync_time = le->timestamp;
 		// I do believe we are lighthouse A		
 		lcd->per_sweep.current_lh = 0;
-		lcd->per_sweep.lh_pulse_len[lcd->per_sweep.current_lh] = le->length;
+//		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;
+		lcd->per_sweep.lh_max_pulse_length[lcd->per_sweep.current_lh] = le->length;
+		lcd->per_sweep.lh_acode[lcd->per_sweep.current_lh] = acode;
 
-		int acode = handle_lightcap2_getAcodeFromSyncPulse(so, le->length);
+//		int acode = handle_lightcap2_getAcodeFromSyncPulse(so, le->length);
 
+/*
 		if (!(acode >> 2 & 1)) // if the skip bit is not set
 		{
 			lcd->per_sweep.activeLighthouse = 0;
 			lcd->per_sweep.activeSweepStartTime = le->timestamp;
 			lcd->per_sweep.activeAcode = acode;
 		}
+		*/
 	}
+
+//	printf("%d %d\n", acode, lcd->per_sweep.activeLighthouse );
 }
 
 void handle_lightcap2_sweep(SurviveObject * so, LightcapElement * le )
@@ -275,6 +299,25 @@ void handle_lightcap2_sweep(SurviveObject * so, LightcapElement * le )
 	//fprintf(stderr, "%2d %d\n", le->sensor_id, le->length);
 	//fprintf(stderr, ".");
 
+	lcd->per_sweep.activeLighthouse = -1;
+	lcd->per_sweep.activeSweepStartTime = 0;
+	lcd->per_sweep.activeAcode = 0;
+
+	for (uint8_t i=0; i< NUM_LIGHTHOUSES;++i) {
+		int acode = lcd->per_sweep.lh_acode[i];
+		if ((acode>0) && (!(acode >> 2 & 1))) {
+			lcd->per_sweep.activeLighthouse = i;
+			lcd->per_sweep.activeSweepStartTime = lcd->per_sweep.lh_start_time[i];
+			lcd->per_sweep.activeAcode = acode;
+		}
+	}
+
+	if (lcd->per_sweep.activeLighthouse < 0) {
+		fprintf(stderr, "WARNING: No active lighthouse!\n");
+		fprintf(stderr, "            %2d %8d\n", le->sensor_id, le->length);
+		return;
+	}
+
 	if (lcd->sweep.sweep_len[le->sensor_id] < le->length)
 	{
 		lcd->sweep.sweep_len[le->sensor_id] = le->length;
@@ -303,7 +346,8 @@ void handle_lightcap2( SurviveObject * so, LightcapElement * le )
 		// Should never get a reading so high.  Odd.
 		return;
 	}
-	if (le->length >= 2750)
+//	if (le->length >= 2750)
+	if (le->length >= 2500)
 	{
 		// Looks like a sync pulse, process it!
 		handle_lightcap2_sync(so, le);
-- 
cgit v1.3.1


From 96cec93d3c7461587673b5dc35c6fba1e9db899f Mon Sep 17 00:00:00 2001
From: Joshua Allen <axlecrusher@gmail.com>
Date: Sat, 8 Apr 2017 13:36:42 -0400
Subject: clear acode at beginning of pulses. only process valid acodes sweeps.
 makes HMD work again with single or double lighthouses.

---
 src/survive_data.c | 18 +++++++++++++-----
 1 file changed, 13 insertions(+), 5 deletions(-)

(limited to 'src/survive_data.c')

diff --git a/src/survive_data.c b/src/survive_data.c
index f61ae8b..989eb59 100644
--- a/src/survive_data.c
+++ b/src/survive_data.c
@@ -24,7 +24,7 @@ typedef struct
 //	int lh_pulse_len[NUM_LIGHTHOUSES];
 	int lh_start_time[NUM_LIGHTHOUSES];
 	int lh_max_pulse_length[NUM_LIGHTHOUSES];
-	uint16_t lh_acode[NUM_LIGHTHOUSES];
+	int8_t lh_acode[NUM_LIGHTHOUSES];
 	int current_lh; // used knowing which sync pulse we're looking at.
 
 } lightcap2_per_sweep_data;
@@ -96,7 +96,8 @@ void handle_lightcap2_process_sweep_data(SurviveObject *so)
 			{
 				static int counts[SENSORS_PER_OBJECT][2] = {0};
 
-				if (lcd->per_sweep.activeLighthouse == 0 && !allZero)
+//				if (lcd->per_sweep.activeLighthouse == 0 && !allZero)
+				if (lcd->per_sweep.activeLighthouse > -1 && !allZero)
 				{
 					if (lcd->sweep.sweep_len[i] != 0)
 					{
@@ -226,6 +227,9 @@ void handle_lightcap2_sync(SurviveObject * so, LightcapElement * le )
 	}
 	else if (time_since_last_sync > 370000)
 	{
+		// XXX CAUTION: if we lose sight of a lighthouse then, the remaining lighthouse will default to master
+		//this should probably be fixed. Maybe some kind of timing based guess at which lighthouse.
+
 		// looks like this is the first sync pulse.  Cool!
 
 		// first, send out the sync pulse data for the last round (for OOTX decoding
@@ -259,6 +263,10 @@ void handle_lightcap2_sync(SurviveObject * so, LightcapElement * le )
 		memset(&lcd->per_sweep, 0, sizeof(lcd->per_sweep));
 		lcd->per_sweep.activeLighthouse = -1; 
 
+		for (uint8_t i=0; i < NUM_LIGHTHOUSES;++i) {
+			lcd->per_sweep.lh_acode[i] = -1;
+		}
+
 		lcd->per_sweep.recent_sync_time = le->timestamp;
 		// I do believe we are lighthouse A		
 		lcd->per_sweep.current_lh = 0;
@@ -303,9 +311,9 @@ void handle_lightcap2_sweep(SurviveObject * so, LightcapElement * le )
 	lcd->per_sweep.activeSweepStartTime = 0;
 	lcd->per_sweep.activeAcode = 0;
 
-	for (uint8_t i=0; i< NUM_LIGHTHOUSES;++i) {
+	for (uint8_t i=0; i < NUM_LIGHTHOUSES;++i) {
 		int acode = lcd->per_sweep.lh_acode[i];
-		if ((acode>0) && (!(acode >> 2 & 1))) {
+		if ( (acode>=0) && !(acode >> 2 & 1)) {
 			lcd->per_sweep.activeLighthouse = i;
 			lcd->per_sweep.activeSweepStartTime = lcd->per_sweep.lh_start_time[i];
 			lcd->per_sweep.activeAcode = acode;
@@ -314,7 +322,7 @@ 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\n", le->sensor_id, le->length);
+		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;
 	}
 
-- 
cgit v1.3.1


From e2d553191e8693bb8937aac8b991d9c09e0df6cc Mon Sep 17 00:00:00 2001
From: Joshua Allen <axlecrusher@gmail.com>
Date: Sat, 8 Apr 2017 17:42:28 -0400
Subject: try to remove outliers

---
 src/survive_data.c | 63 +++++++++++++++++++++++++++++++++++++++++++++++++++---
 1 file changed, 60 insertions(+), 3 deletions(-)

(limited to 'src/survive_data.c')

diff --git a/src/survive_data.c b/src/survive_data.c
index 989eb59..647a5d7 100644
--- a/src/survive_data.c
+++ b/src/survive_data.c
@@ -65,10 +65,64 @@ int handle_lightcap2_getAcodeFromSyncPulse(SurviveObject * so, int pulseLen)
 	if (pulseLen < 6250 - ACODE_OFFSET) return 6;
 	return 7;
 }
+
+uint8_t remove_outliers(SurviveObject *so) {
+	lightcap2_data *lcd = so->disambiguator_data;
+
+	uint32_t sum = 0;
+	uint8_t non_zero_count = 0;
+	uint32_t mean = 0;
+
+	for (int i = 0; i < SENSORS_PER_OBJECT; i++)
+	{
+		if (lcd->sweep.sweep_len[i] > 0) {
+			sum += lcd->sweep.sweep_len[i];
+			++non_zero_count;
+		}
+	}
+
+	if (non_zero_count==0) return 0;
+
+	mean = sum/non_zero_count;
+
+	float standard_deviation = 0.0f;
+	sum = 0;
+	for (int i = 0; i < SENSORS_PER_OBJECT; i++)
+	{
+		uint16_t len = lcd->sweep.sweep_len[i];
+		if (len > 0) {
+			sum += (len - mean)*(len - mean);
+		}
+	}
+	standard_deviation = sqrt( ((float)sum)/((float)non_zero_count) );
+
+//	printf("%f\n", standard_deviation);
+
+	float fake_tao_test = standard_deviation*2;
+	uint8_t removed_outliers = 0;
+
+	for (int i = 0; i < SENSORS_PER_OBJECT; i++)
+	{
+		uint16_t len = lcd->sweep.sweep_len[i];
+		if (len == 0) continue;
+
+		if ( abs(len-mean) > fake_tao_test )
+		{
+//			fprintf(stderr, "removing %d\n", len);
+			lcd->sweep.sweep_len[i] = 0;
+			removed_outliers = 1;
+		}
+	}
+
+	return removed_outliers;
+}
+
 void handle_lightcap2_process_sweep_data(SurviveObject *so)
 {
 	lightcap2_data *lcd = so->disambiguator_data;
 
+	while(remove_outliers(so));
+
 	// look at all of the sensors we found, and process the ones that were hit.
 	// TODO: find the sensor(s) with the longest pulse length, and assume 
 	// those are the "highest quality".  Then, reject any pulses that are sufficiently
@@ -93,6 +147,7 @@ void handle_lightcap2_process_sweep_data(SurviveObject *so)
 		//	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};
 
@@ -119,10 +174,10 @@ void handle_lightcap2_process_sweep_data(SurviveObject *so)
 
 			if (lcd->sweep.sweep_len[i] != 0) // if the sensor was hit, process it
 			{
-
+//printf("%4d\n", lcd->sweep.sweep_len[i]);
 				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)
+//				if (offset_from < 380000 && offset_from > 70000)
 				{
 					//if (longest_pulse *10 / 8 < lcd->sweep.sweep_len[i]) 
 					{
@@ -134,7 +189,9 @@ void handle_lightcap2_process_sweep_data(SurviveObject *so)
 		//if (!allZero)
 		//	printf("   ..:..\n");
 
+//		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.
 	memset(&(((lightcap2_data*)so->disambiguator_data)->sweep), 0, sizeof(lightcaps_sweep_data));
@@ -156,7 +213,7 @@ void handle_lightcap2_sync(SurviveObject * so, LightcapElement * le )
 	int time_since_last_sync = (le->timestamp - lcd->per_sweep.recent_sync_time);
 
 
-	fprintf(stderr, "            %2d %8d %d\n", le->sensor_id, time_since_last_sync, le->length);
+//	fprintf(stderr, "            %2d %8d %d\n", le->sensor_id, time_since_last_sync, le->length);
 	// need to store up sync pulses, so we can take the earliest starting time for all sensors.
 	if (time_since_last_sync < 2400)
 	{
-- 
cgit v1.3.1


From 7686ad97d5f2542338f88e6f6e8055953609651c Mon Sep 17 00:00:00 2001
From: Josh Allen <josh@allensw.com>
Date: Sun, 9 Apr 2017 08:38:40 -0400
Subject: untested tau test

---
 src/survive_data.c | 68 ++++++++++++++++++++++++++++++++++++++++++------------
 1 file changed, 53 insertions(+), 15 deletions(-)

(limited to 'src/survive_data.c')

diff --git a/src/survive_data.c b/src/survive_data.c
index 647a5d7..8d115fd 100644
--- a/src/survive_data.c
+++ b/src/survive_data.c
@@ -4,16 +4,25 @@
 #include "survive_internal.h"
 #include <stdint.h>
 #include <string.h>
+#include <math.h>
 
 #define USE_TURVEYBIGUATOR
 
 #ifdef USE_TURVEYBIGUATOR
 
+static const float tau_table[33] = { 0, 0, 0, 1.151140982, 1.425, 1.5712213707, 1.656266074, 1.7110275587, 1.7490784054,
+	1.7770229476, 1.798410005, 1.8153056661, 1.8289916275, 1.8403044103, 1.8498129961, 1.8579178211,
+	1.864908883, 1.8710013691, 1.8763583296, 1.881105575, 1.885341741, 1.8891452542, 1.8925792599,
+	1.8956951735, 1.8985352854, 1.9011347009, 1.9035228046, 1.9057243816, 1.9077604832, 1.9096491058,
+	1.9114057255, 1.9130437248, 1.914574735
+};
+
 typedef struct
 {
 	unsigned int sweep_time[SENSORS_PER_OBJECT];
-	unsigned int sweep_len[SENSORS_PER_OBJECT];
+	uint16_t sweep_len[SENSORS_PER_OBJECT]; //might want to align this to cache lines, will be hot for frequent access
 } lightcaps_sweep_data;
+
 typedef struct
 {
 	int recent_sync_time;
@@ -73,12 +82,16 @@ uint8_t remove_outliers(SurviveObject *so) {
 	uint8_t non_zero_count = 0;
 	uint32_t mean = 0;
 
-	for (int i = 0; i < SENSORS_PER_OBJECT; i++)
+	uint16_t* min = NULL;
+	uint16_t* max = NULL;
+	uint8_t found_first = 0;
+
+	//future: https://gcc.gnu.org/projects/tree-ssa/vectorization.html#vectorizab
+
+	for (uint8_t i = 0; i < SENSORS_PER_OBJECT; i++)
 	{
-		if (lcd->sweep.sweep_len[i] > 0) {
-			sum += lcd->sweep.sweep_len[i];
-			++non_zero_count;
-		}
+		sum += lcd->sweep.sweep_len[i];
+		if (lcd->sweep.sweep_len[i] > 0) ++non_zero_count;
 	}
 
 	if (non_zero_count==0) return 0;
@@ -87,34 +100,59 @@ uint8_t remove_outliers(SurviveObject *so) {
 
 	float standard_deviation = 0.0f;
 	sum = 0;
-	for (int i = 0; i < SENSORS_PER_OBJECT; i++)
+	for (uint8_t i = 0; i < SENSORS_PER_OBJECT; i++)
 	{
 		uint16_t len = lcd->sweep.sweep_len[i];
 		if (len > 0) {
 			sum += (len - mean)*(len - mean);
+
+			if (found_first==0) {
+				max = min = lcd->sweep.sweep_len + i;
+				found_first=1;
+			} else {
+				if(lcd->sweep.sweep_len[i] < *min) min=lcd->sweep.sweep_len + i;
+				if(lcd->sweep.sweep_len[i] > *max) max=lcd->sweep.sweep_len + i;
+			}
 		}
 	}
-	standard_deviation = sqrt( ((float)sum)/((float)non_zero_count) );
+	standard_deviation = sqrtf( ((float)sum)/((float)non_zero_count) );
 
 //	printf("%f\n", standard_deviation);
 
-	float fake_tao_test = standard_deviation*2;
-	uint8_t removed_outliers = 0;
+	float tau_test = standard_deviation;
+
+	if (non_zero_count > 2) tau_test = standard_deviation*tau_table[non_zero_count];
 
-	for (int i = 0; i < SENSORS_PER_OBJECT; i++)
+//	uint8_t removed_outliers = 0;
+
+	uint32_t d1 = *min - mean;
+	uint32_t d2 = *max - mean;
+
+	if (d1>d2 && d1>tau_test) {
+		*min = 0;
+		return 1;
+	}
+	else if (d2>tau_test) {
+		*max = 0;
+		return 1;
+	}
+
+	return 0;
+/*
+	for (uint8_t i = 0; i < SENSORS_PER_OBJECT; i++)
 	{
 		uint16_t len = lcd->sweep.sweep_len[i];
 		if (len == 0) continue;
 
-		if ( abs(len-mean) > fake_tao_test )
+		if ( abs(len-mean) > tau_test )
 		{
 //			fprintf(stderr, "removing %d\n", len);
 			lcd->sweep.sweep_len[i] = 0;
 			removed_outliers = 1;
 		}
 	}
-
-	return removed_outliers;
+*/
+//	return removed_outliers;
 }
 
 void handle_lightcap2_process_sweep_data(SurviveObject *so)
@@ -250,7 +288,7 @@ void handle_lightcap2_sync(SurviveObject * so, LightcapElement * le )
 	}
 	else if (time_since_last_sync < 24000)
 	{
-		lcd->per_sweep.activeLighthouse != -1;
+		lcd->per_sweep.activeLighthouse = -1;
 
 		lcd->per_sweep.recent_sync_time = le->timestamp;
 		// I do believe we are lighthouse B		
-- 
cgit v1.3.1


From 8dee94008fec1506a3a2f58292a45e1f93b6d333 Mon Sep 17 00:00:00 2001
From: Josh Allen <josh@allensw.com>
Date: Sun, 9 Apr 2017 08:51:29 -0400
Subject: should be absolute value if statement should not be combined

---
 src/survive_data.c | 12 +++++++-----
 1 file changed, 7 insertions(+), 5 deletions(-)

(limited to 'src/survive_data.c')

diff --git a/src/survive_data.c b/src/survive_data.c
index 8d115fd..22ce8c2 100644
--- a/src/survive_data.c
+++ b/src/survive_data.c
@@ -125,12 +125,14 @@ uint8_t remove_outliers(SurviveObject *so) {
 
 //	uint8_t removed_outliers = 0;
 
-	uint32_t d1 = *min - mean;
-	uint32_t d2 = *max - mean;
+	uint32_t d1 = abs(*min - mean);
+	uint32_t d2 = abs(*max - mean);
 
-	if (d1>d2 && d1>tau_test) {
-		*min = 0;
-		return 1;
+	if (d1>d2) {
+		if (d1 > tau_test) {
+			*min = 0;
+			return 1;
+		}
 	}
 	else if (d2>tau_test) {
 		*max = 0;
-- 
cgit v1.3.1


From 4b842a1a73a2713726c3d999ab4b6886e7e9fb69 Mon Sep 17 00:00:00 2001
From: Mike Turvey <mturvey6@gmail.com>
Date: Tue, 11 Apr 2017 17:42:04 -0700
Subject: Disabling remove_outliers because it's removing too many non-outliers

---
 src/survive_data.c | 1 +
 1 file changed, 1 insertion(+)

(limited to 'src/survive_data.c')

diff --git a/src/survive_data.c b/src/survive_data.c
index 647a5d7..0273532 100644
--- a/src/survive_data.c
+++ b/src/survive_data.c
@@ -67,6 +67,7 @@ int handle_lightcap2_getAcodeFromSyncPulse(SurviveObject * so, int pulseLen)
 }
 
 uint8_t remove_outliers(SurviveObject *so) {
+	return 0; // disabling this for now because it seems remove almost all the points for wired watchman and wired tracker.
 	lightcap2_data *lcd = so->disambiguator_data;
 
 	uint32_t sum = 0;
-- 
cgit v1.3.1


From a7d4ccbde10be9c14ae690d3c9a21627195cfaa5 Mon Sep 17 00:00:00 2001
From: "Dr. Orion Lawlor" <lawlor@alaska.edu>
Date: Mon, 17 Apr 2017 21:42:05 -0800
Subject: Fix warnings by including <math.h>

---
 src/survive_data.c | 1 +
 1 file changed, 1 insertion(+)

(limited to 'src/survive_data.c')

diff --git a/src/survive_data.c b/src/survive_data.c
index 0273532..7b9d051 100644
--- a/src/survive_data.c
+++ b/src/survive_data.c
@@ -4,6 +4,7 @@
 #include "survive_internal.h"
 #include <stdint.h>
 #include <string.h>
+#include <math.h> /* for sqrt */
 
 #define USE_TURVEYBIGUATOR
 
-- 
cgit v1.3.1


From f177f87e98637de8f07488d820390f322c45413c Mon Sep 17 00:00:00 2001
From: cnlohr <lohr85@gmail.com>
Date: Sun, 30 Apr 2017 00:13:42 -0400
Subject: Update the charles disambiguator

---
 include/libsurvive/survive.h |  2 +-
 src/survive_data.c           | 53 +++++++++++++++++++++++++++++++-------------
 2 files changed, 39 insertions(+), 16 deletions(-)

(limited to 'src/survive_data.c')

diff --git a/include/libsurvive/survive.h b/include/libsurvive/survive.h
index 3c35bd2..e85fe48 100644
--- a/include/libsurvive/survive.h
+++ b/include/libsurvive/survive.h
@@ -42,7 +42,7 @@ struct SurviveObject
 
 	//Timing sensitive data (mostly for disambiguation)
 	int32_t timebase_hz;		//48,000,000 for normal vive hardware.  (checked)
-	int32_t timecenter_ticks; 	//200,000 for normal vive hardware.     (checked)
+	int32_t timecenter_ticks; 	//200,000 for normal vive hardware.     (checked)  (This doubles-up as 2x this = full sweep length)
 	int32_t pulsedist_max_ticks; //500,000 for normal vive hardware.   (guessed)
 	int32_t pulselength_min_sync; //2,200 for normal vive hardware.    (guessed)
 	int32_t pulse_in_clear_time; //35,000 for normal vive hardware.    (guessed)
diff --git a/src/survive_data.c b/src/survive_data.c
index 7b9d051..0e2e828 100644
--- a/src/survive_data.c
+++ b/src/survive_data.c
@@ -6,7 +6,7 @@
 #include <string.h>
 #include <math.h> /* for sqrt */
 
-#define USE_TURVEYBIGUATOR
+//#define USE_TURVEYBIGUATOR
 
 #ifdef USE_TURVEYBIGUATOR
 
@@ -449,6 +449,7 @@ void handle_lightcap( SurviveObject * so, LightcapElement * le )
 	return;
 
 #else
+	//printf( "LE%3d%6d%12d\n", le->sensor_id, le->length, le->timestamp );
 
 	//int32_t deltat = (uint32_t)le->timestamp - (uint32_t)so->last_master_time;
 
@@ -482,6 +483,9 @@ void handle_lightcap( SurviveObject * so, LightcapElement * le )
 	//unified driver.
 	int ssn = so->sync_set_number; //lighthouse number
 	if( ssn < 0 ) ssn = 0;
+#ifdef DEBUG
+	if( ssn >= NUM_LIGHTHOUSES ) { SV_INFO( "ALGORITHMIC WARNING: ssn exceeds NUM_LIGHTHOUSES" ); }
+#endif
 	int last_sync_time  =  so->last_sync_time  [ssn];
 	int last_sync_length = so->last_sync_length[ssn];
 	int32_t delta = le->timestamp - last_sync_time;  //Handle time wrapping (be sure to be int32)
@@ -498,16 +502,29 @@ void handle_lightcap( SurviveObject * so, LightcapElement * le )
 	if( le->length > so->pulselength_min_sync ) //Pulse longer indicates a sync pulse.
 	{
 		int is_new_pulse = delta > so->pulselength_min_sync /*1500*/ + last_sync_length;
-
+		//printf( "INP: %d %d\n", is_new_pulse, so->sync_set_number );
 		so->did_handle_ootx = 0;
 
 		if( is_new_pulse )
 		{
 			int is_master_sync_pulse = delta > so->pulse_in_clear_time /*40000*/; 
+			int is_pulse_from_same_lh_as_last_sweep;
+			int tp = delta % ( so->timecenter_ticks * 2);
+			is_pulse_from_same_lh_as_last_sweep = tp < so->pulse_synctime_slack && tp > -so->pulse_synctime_slack;
 
-			if( is_master_sync_pulse )
+			if( is_master_sync_pulse )  //Could also be called by slave if no master was seen.
 			{
-				ssn = so->sync_set_number = 0;
+				ssn = so->sync_set_number = is_pulse_from_same_lh_as_last_sweep?(so->sync_set_number):0; //If repeated lighthouse, just back off one.
+				if( ssn < 0 ) { SV_INFO( "SEVERE WARNING: Pulse codes for tracking not able to be backed out.\n" ); ssn = 0; }
+				if( ssn != 0 )
+				{
+					//If it's the slave that is repeated, be sure to zero out its sync info.
+					so->last_sync_length[0] = 0;
+				}
+				else
+				{
+					so->last_sync_length[1] = 0;
+				}
 				so->last_sync_time[ssn] = le->timestamp;
 				so->last_sync_length[ssn] = le->length;
 			}
@@ -544,15 +561,19 @@ void handle_lightcap( SurviveObject * so, LightcapElement * le )
 			}
 		}
 
-		//Extra tidbit for storing length-of-sync-pulses.
+#if 0
+		//Extra tidbit for storing length-of-sync-pulses, if you want to try to use this to determine AoI or distance to LH.
 		{
 			int32_t main_divisor = so->timebase_hz / 384000; //125 @ 48 MHz.
 			int base_station = is_new_pulse;
-			//printf( "%s %d %d %d\n", so->codename, le->sensor_id, so->sync_set_number, le->length );
-			ctx->lightproc( so, le->sensor_id, -3 - so->sync_set_number, 0, le->timestamp, le->length, base_station);
+			printf( "%s %d %d %d\n", so->codename, le->sensor_id, so->sync_set_number, le->length ); //XXX sync_set_number is wrong here.
+			ctx->lightproc( so, le->sensor_id, -3 - so->sync_set_number, 0, le->timestamp, le->length, base_station); //XXX sync_set_number is wrong here.
 		}
+#endif
 	}
 
+	//Any else- statements below here are 
+
 	//See if this is a valid actual pulse.
 	else if( le->length < so->pulse_max_for_sweep && delta > so->pulse_in_clear_time && ssn >= 0 )
 	{
@@ -576,9 +597,11 @@ void handle_lightcap( SurviveObject * so, LightcapElement * le )
 				decode_acode(so->last_sync_length[1],main_divisor)
 			};
 
+		//printf( "%d %d\n", acode_array[0], acode_array[1] );
+
 		//XXX: TODO: Capture error count here.
-		if( acode_array[0] < 0 ) return;
-		if( acode_array[1] < 0 ) return;
+		//if( acode_array[0] < 0 ) return;
+		//if( acode_array[1] < 0 ) return;
 
 		int acode = acode_array[0];
 
@@ -587,13 +610,14 @@ void handle_lightcap( SurviveObject * so, LightcapElement * le )
 			int32_t delta1 = so->last_sync_time[0] - so->recent_sync_time;
 			int32_t delta2 = so->last_sync_time[1] - so->last_sync_time[0];
 
-			ctx->lightproc( so, -1, acode_array[0], delta1, so->last_sync_time[0], so->last_sync_length[0], 0 );
-			ctx->lightproc( so, -2, acode_array[1], delta2, so->last_sync_time[1], so->last_sync_length[1], 1 );
+			//printf( "%p %p %d %d %d  %p\n", ctx, so, so->last_sync_time[0], acode_array, so->last_sync_length[0], ctx->lightproc );
+			if( acode_array[0] >= 0 ) ctx->lightproc( so, -1, acode_array[0], delta1, so->last_sync_time[0], so->last_sync_length[0], 0 );
+			if( acode_array[1] >= 0 ) ctx->lightproc( so, -2, acode_array[1], delta2, so->last_sync_time[1], so->last_sync_length[1], 1 );
 
 			so->recent_sync_time = so->last_sync_time[1];
 
 			//Throw out everything if our sync pulses look like they're bad.
-
+/*
 			int32_t center_1 = so->timecenter_ticks*2 - so->pulse_synctime_offset;
 			int32_t center_2 = so->pulse_synctime_offset;
 			int32_t slack = so->pulse_synctime_slack;
@@ -611,7 +635,7 @@ void handle_lightcap( SurviveObject * so, LightcapElement * le )
 				so->sync_set_number = -1;
 				return;
 			}
-
+*/
 			so->did_handle_ootx = 1;
 		}
 
@@ -628,7 +652,7 @@ void handle_lightcap( SurviveObject * so, LightcapElement * le )
 		int32_t offset_from = le->timestamp - dl + le->length/2;
 
 		//Make sure pulse is in valid window
-		if( offset_from < 380000 && offset_from > 70000 )
+		if( offset_from < so->timecenter_ticks*2-so->pulse_in_clear_time && offset_from > so->pulse_in_clear_time && acode >= 0 )
 		{
 			ctx->lightproc( so, le->sensor_id, acode, offset_from, le->timestamp, le->length, !(acode>>2) );
 		}
@@ -639,7 +663,6 @@ void handle_lightcap( SurviveObject * so, LightcapElement * le )
 		//Runt pulse, or no sync pulses available.
 	}
 #endif
-
 }
 
 
-- 
cgit v1.3.1


From 947dfaff73c0b758a21a410776543340e732d705 Mon Sep 17 00:00:00 2001
From: cnlohr <lohr85@gmail.com>
Date: Sun, 30 Apr 2017 00:46:57 -0400
Subject: Fix charles' disambiguator to work with one lighthouse and
 @axlecrusher's OOTX reader.

---
 src/survive_data.c | 125 ++++++++++++++++++++++++++++-------------------------
 test.c             |   0
 2 files changed, 65 insertions(+), 60 deletions(-)
 mode change 100755 => 100644 test.c

(limited to 'src/survive_data.c')

diff --git a/src/survive_data.c b/src/survive_data.c
index 0e2e828..4f94106 100644
--- a/src/survive_data.c
+++ b/src/survive_data.c
@@ -432,13 +432,59 @@ void handle_lightcap2( SurviveObject * so, LightcapElement * le )
 int32_t decode_acode(uint32_t length, int32_t main_divisor) {
 	//+50 adds a small offset and seems to help always get it right. 
 	//Check the +50 in the future to see how well this works on a variety of hardware.
-
+	if( !main_divisor ) return -1;
 	int32_t acode = (length+main_divisor+50)/(main_divisor*2);
 	if( acode & 1 ) return -1;
 
 	return (acode>>1) - 6;
 }
 
+
+void HandleOOTX( SurviveContext * ctx, SurviveObject * so )
+{
+	int32_t main_divisor = so->timebase_hz / 384000; //125 @ 48 MHz.
+
+	int32_t acode_array[2] =
+		{
+			decode_acode(so->last_sync_length[0],main_divisor),
+			decode_acode(so->last_sync_length[1],main_divisor)
+		};
+
+
+	int32_t delta1 = so->last_sync_time[0] - so->recent_sync_time;
+	int32_t delta2 = so->last_sync_time[1] - so->last_sync_time[0];
+
+	//printf( "%p %p %d %d %d  %p\n", ctx, so, so->last_sync_time[0], acode_array, so->last_sync_length[0], ctx->lightproc );
+	if( acode_array[0] >= 0 ) ctx->lightproc( so, -1, acode_array[0], delta1, so->last_sync_time[0], so->last_sync_length[0], 0 );
+	if( acode_array[1] >= 0 ) ctx->lightproc( so, -2, acode_array[1], delta2, so->last_sync_time[1], so->last_sync_length[1], 1 );
+
+	so->recent_sync_time = so->last_sync_time[1];
+
+/*
+	//Throw out everything if our sync pulses look like they're bad.
+	//This actually doesn't seem to hold anymore, now that we're looking for multiple LHs.
+	int32_t center_1 = so->timecenter_ticks*2 - so->pulse_synctime_offset;
+	int32_t center_2 = so->pulse_synctime_offset;
+	int32_t slack = so->pulse_synctime_slack;
+
+	if( delta1 < center_1 - slack || delta1 > center_1 + slack )
+	{
+		//XXX: TODO: Count faults.
+		so->sync_set_number = -1;
+		return;
+	}
+
+	if( delta2 < center_2 - slack || delta2 > center_2 + slack )
+	{
+		//XXX: TODO: Count faults.
+		so->sync_set_number = -1;
+		return;
+	}
+*/
+	so->did_handle_ootx = 1;
+}
+		
+
 //This is the disambiguator function, for taking light timing and figuring out place-in-sweep for a given photodiode.
 void handle_lightcap( SurviveObject * so, LightcapElement * le )
 {
@@ -502,9 +548,18 @@ void handle_lightcap( SurviveObject * so, LightcapElement * le )
 	if( le->length > so->pulselength_min_sync ) //Pulse longer indicates a sync pulse.
 	{
 		int is_new_pulse = delta > so->pulselength_min_sync /*1500*/ + last_sync_length;
-		//printf( "INP: %d %d\n", is_new_pulse, so->sync_set_number );
+
+		//TRICKY: If we didn't see anything from the other lighthouse, we might just not see it... But, we still have to send our sync
+		//information to the rest of libsurvive.  This could be turned into a function and combined with the code below.
+		if( !so->did_handle_ootx && is_new_pulse )
+		{
+			HandleOOTX( ctx, so );
+		}
 		so->did_handle_ootx = 0;
 
+
+		//printf( "INP: %d %d\n", is_new_pulse, so->sync_set_number );
+
 		if( is_new_pulse )
 		{
 			int is_master_sync_pulse = delta > so->pulse_in_clear_time /*40000*/; 
@@ -563,6 +618,7 @@ void handle_lightcap( SurviveObject * so, LightcapElement * le )
 
 #if 0
 		//Extra tidbit for storing length-of-sync-pulses, if you want to try to use this to determine AoI or distance to LH.
+		//We don't actually use this anywhere, and I doubt we ever will?  Though, it could be useful at a later time to improve tracking.
 		{
 			int32_t main_divisor = so->timebase_hz / 384000; //125 @ 48 MHz.
 			int base_station = is_new_pulse;
@@ -590,71 +646,20 @@ void handle_lightcap( SurviveObject * so, LightcapElement * le )
 		//Make sure it fits nicely into a divisible-by-500 time.
 
 		int32_t main_divisor = so->timebase_hz / 384000; //125 @ 48 MHz.
-
-		int32_t acode_array[2] =
-			{
-				decode_acode(so->last_sync_length[0],main_divisor),
-				decode_acode(so->last_sync_length[1],main_divisor)
-			};
-
-		//printf( "%d %d\n", acode_array[0], acode_array[1] );
-
-		//XXX: TODO: Capture error count here.
-		//if( acode_array[0] < 0 ) return;
-		//if( acode_array[1] < 0 ) return;
-
-		int acode = acode_array[0];
+		int acode = decode_acode(so->last_sync_length[0],main_divisor);
 
 		if( !so->did_handle_ootx )
-		{
-			int32_t delta1 = so->last_sync_time[0] - so->recent_sync_time;
-			int32_t delta2 = so->last_sync_time[1] - so->last_sync_time[0];
-
-			//printf( "%p %p %d %d %d  %p\n", ctx, so, so->last_sync_time[0], acode_array, so->last_sync_length[0], ctx->lightproc );
-			if( acode_array[0] >= 0 ) ctx->lightproc( so, -1, acode_array[0], delta1, so->last_sync_time[0], so->last_sync_length[0], 0 );
-			if( acode_array[1] >= 0 ) ctx->lightproc( so, -2, acode_array[1], delta2, so->last_sync_time[1], so->last_sync_length[1], 1 );
-
-			so->recent_sync_time = so->last_sync_time[1];
-
-			//Throw out everything if our sync pulses look like they're bad.
-/*
-			int32_t center_1 = so->timecenter_ticks*2 - so->pulse_synctime_offset;
-			int32_t center_2 = so->pulse_synctime_offset;
-			int32_t slack = so->pulse_synctime_slack;
-
-			if( delta1 < center_1 - slack || delta1 > center_1 + slack )
-			{
-				//XXX: TODO: Count faults.
-				so->sync_set_number = -1;
-				return;
-			}
-
-			if( delta2 < center_2 - slack || delta2 > center_2 + slack )
-			{
-				//XXX: TODO: Count faults.
-				so->sync_set_number = -1;
-				return;
-			}
-*/
-			so->did_handle_ootx = 1;
-		}
-
-		if (acode > 3) {
-			if( ssn == 0 )
-			{
-				//SV_INFO( "Warning: got a slave marker but only got a master sync." );
-				//This happens too frequently.  Consider further examination.
-			}
-			dl = so->last_sync_time[1];
-			tpco = so->last_sync_length[1];
-		}
+			HandleOOTX( ctx, so );
 
 		int32_t offset_from = le->timestamp - dl + le->length/2;
 
 		//Make sure pulse is in valid window
-		if( offset_from < so->timecenter_ticks*2-so->pulse_in_clear_time && offset_from > so->pulse_in_clear_time && acode >= 0 )
+		if( offset_from < so->timecenter_ticks*2-so->pulse_in_clear_time && offset_from > so->pulse_in_clear_time )
 		{
-			ctx->lightproc( so, le->sensor_id, acode, offset_from, le->timestamp, le->length, !(acode>>2) );
+			int whichlh;
+			if( acode < 0 ) whichlh = 1;
+			else whichlh = !(acode>>2);
+			ctx->lightproc( so, le->sensor_id, acode, offset_from, le->timestamp, le->length, whichlh );
 		}
 	}
 	else
diff --git a/test.c b/test.c
old mode 100755
new mode 100644
-- 
cgit v1.3.1


From 78ac5da64c299bcf0f2e1c8d19c876e7723faccf Mon Sep 17 00:00:00 2001
From: cnlohr <lohr85@gmail.com>
Date: Sat, 13 May 2017 03:47:35 -0400
Subject: Fix my disambiguator. Seems to work with either-or lighthouse.

---
 src/survive_data.c | 46 ++++++++++++++++------------------------------
 1 file changed, 16 insertions(+), 30 deletions(-)

(limited to 'src/survive_data.c')

diff --git a/src/survive_data.c b/src/survive_data.c
index df8df8e..157650d 100644
--- a/src/survive_data.c
+++ b/src/survive_data.c
@@ -468,6 +468,7 @@ void handle_lightcap2( SurviveObject * so, LightcapElement * le )
 
 #endif
 
+
 int32_t decode_acode(uint32_t length, int32_t main_divisor) {
 	//+50 adds a small offset and seems to help always get it right. 
 	//Check the +50 in the future to see how well this works on a variety of hardware.
@@ -478,6 +479,10 @@ int32_t decode_acode(uint32_t length, int32_t main_divisor) {
 	return (acode>>1) - 6;
 }
 
+////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////	
+///////The charles disambiguator.  Don't use this, mostly here for debugging.///////////////////////////////////////////////////////	
+////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////	
+
 
 void HandleOOTX( SurviveContext * ctx, SurviveObject * so )
 {
@@ -499,30 +504,9 @@ void HandleOOTX( SurviveContext * ctx, SurviveObject * so )
 
 	so->recent_sync_time = so->last_sync_time[1];
 
-/*
-	//Throw out everything if our sync pulses look like they're bad.
-	//This actually doesn't seem to hold anymore, now that we're looking for multiple LHs.
-	int32_t center_1 = so->timecenter_ticks*2 - so->pulse_synctime_offset;
-	int32_t center_2 = so->pulse_synctime_offset;
-	int32_t slack = so->pulse_synctime_slack;
-
-	if( delta1 < center_1 - slack || delta1 > center_1 + slack )
-	{
-		//XXX: TODO: Count faults.
-		so->sync_set_number = -1;
-		return;
-	}
-
-	if( delta2 < center_2 - slack || delta2 > center_2 + slack )
-	{
-		//XXX: TODO: Count faults.
-		so->sync_set_number = -1;
-		return;
-	}
-*/
 	so->did_handle_ootx = 1;
 }
-		
+
 
 //This is the disambiguator function, for taking light timing and figuring out place-in-sweep for a given photodiode.
 void handle_lightcap( SurviveObject * so, LightcapElement * le )
@@ -588,16 +572,8 @@ void handle_lightcap( SurviveObject * so, LightcapElement * le )
 	{
 		int is_new_pulse = delta > so->pulselength_min_sync /*1500*/ + last_sync_length;
 
-		//TRICKY: If we didn't see anything from the other lighthouse, we might just not see it... But, we still have to send our sync
-		//information to the rest of libsurvive.  This could be turned into a function and combined with the code below.
-		if( !so->did_handle_ootx && is_new_pulse )
-		{
-			HandleOOTX( ctx, so );
-		}
-		so->did_handle_ootx = 0;
 
 
-		//printf( "INP: %d %d\n", is_new_pulse, so->sync_set_number );
 
 		if( is_new_pulse )
 		{
@@ -606,6 +582,16 @@ void handle_lightcap( SurviveObject * so, LightcapElement * le )
 			int tp = delta % ( so->timecenter_ticks * 2);
 			is_pulse_from_same_lh_as_last_sweep = tp < so->pulse_synctime_slack && tp > -so->pulse_synctime_slack;
 
+			if( !so->did_handle_ootx )
+			{
+				HandleOOTX( ctx, so );
+			}
+			if( !is_master_sync_pulse )
+			{
+				so->did_handle_ootx = 0;
+			}
+
+
 			if( is_master_sync_pulse )  //Could also be called by slave if no master was seen.
 			{
 				ssn = so->sync_set_number = is_pulse_from_same_lh_as_last_sweep?(so->sync_set_number):0; //If repeated lighthouse, just back off one.
-- 
cgit v1.3.1