Alternate disambiguator and calibration updates

3 files changed, 121 insertions, 13 deletions

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;