plot_lighthouse/Makefile

1 files changed, 129 insertions, 56 deletions

diff --git a/src/survive_data.c b/src/survive_data.c
index d380d4a..ad834cf 100644
--- a/src/survive_data.c
+++ b/src/survive_data.c
@@ -11,7 +11,6 @@
 //All MIT/x11 Licensed Code in this file may be relicensed freely under the GPL or LGPL licenses.
 
 #include "survive_internal.h"
-#include "disambiguator.h"
 #include <stdint.h>
 #include <string.h>
 
@@ -32,89 +31,164 @@ struct LightcapElement
 //This is the disambiguator function, for taking light timing and figuring out place-in-sweep for a given photodiode.
 static void handle_lightcap( struct SurviveObject * so, struct LightcapElement * le )
 {
-	struct SurviveContext * ct = so->ctx;
-	int32_t deltat = (uint32_t)le->timestamp - (uint32_t)so->last_master_time;
+	struct SurviveContext * ctx = so->ctx;
+	//int32_t deltat = (uint32_t)le->timestamp - (uint32_t)so->last_master_time;
 
 //	printf( "%s %d %d %d %d %d\n", so->codename, le->sensor_id, le->type, le->length, le->timestamp, le->timestamp-so->tsl );
 
 	so->tsl = le->timestamp;
-	if( le->length < 20 ) return;
-#ifndef USE_OLD_DISAMBIGUATOR
-	int32_t offset = le->timestamp - so->d->last;
-	switch( disambiguator_step( so->d, le->timestamp, le->length ) ) {
-	default:
-	case P_SLAVE:
-		// this is only interesting for the OOTX data
-		break;
-	case P_UNKNOWN:
-		// not currently locked
-		break;
-	case P_MASTER:
-		ct->lightproc( so, le->sensor_id, -1, 0, le->timestamp, offset );
-		so->d->code = ((le->length+125)/250) - 12;
-		break;
-	case P_SWEEP:
-		if (so->d->code & 1) return;
-		ct->lightproc( so, le->sensor_id, so->d->code >> 1, offset, le->timestamp, le->length );
-		break;
+	if( le->length < 20 ) return;  ///Assuming 20 is an okay value for here.
+
+	//The sync pulse finder is taking Charles's old disambiguator code and mixing it with a more linear
+	//version of Julian Picht's disambiguator, available in 488c5e9.  Removed afterwards into this
+	//unified driver.
+	int ssn = so->sync_set_number;
+	if( ssn < 0 ) ssn = 0;
+	int last_sync_time  =  so->last_time  [ssn];
+	int last_sync_length = so->last_length[ssn];
+	int32_t delta = le->timestamp - last_sync_time;  //Handle time wrapping (be sure to be int32)
+
+	if( delta < -so->pulsedist_max_ticks || delta > so->pulsedist_max_ticks )
+	{
+		//Reset pulse, etc.
+		so->sync_set_number = -1;
+		delta = so->pulsedist_max_ticks;
 	}
-#else
-	if( le->length > 2200 ) //Pulse longer indicates a sync pulse.
+
+
+	if( le->length > so->pulselength_min_sync ) //Pulse longer indicates a sync pulse.
 	{
-		int32_t deltat = (uint32_t)le->timestamp - (uint32_t)so->last_master_time;
-		if( deltat > 2000 || deltat < -2000 )		//New pulse. (may be inverted)
+		int is_new_pulse = delta > so->pulselength_min_sync /*1500*/ + last_sync_length;
+
+		so->did_handle_ootx = 0;
+
+		if( is_new_pulse )
 		{
-			//See if this is a unique pulse, or another one in the same set we need to look at.
-			if( le->timestamp - so->last_master_time > 1500 + so->last_photo_length )
+			int is_master_sync_pulse = delta > so->pulse_in_clear_time /*40000*/; 
+
+			if( is_master_sync_pulse )
+			{
+				ssn = so->sync_set_number = 0;
+				so->last_time[ssn] = le->timestamp;
+				so->last_length[ssn] = le->length;
+			}
+			else if( so->sync_set_number == -1 )
+			{
+				//Do nothing.
+			}
+			else
 			{
-				// check if it is a slave pulse
-				if (le->timestamp - so->last_master_time < 70000) {
-					so->last_slave_time = le->timestamp;
-					return;
+				ssn = ++so->sync_set_number;
+				if( so->sync_set_number >= NUM_LIGHTHOUSES )
+				{
+					SV_INFO( "Warning.  Received an extra, unassociated sync pulse." );
+					ssn = so->sync_set_number = -1;
+				}
+				else
+				{
+					so->last_time[ssn] = le->timestamp;
+					so->last_length[ssn] = le->length;
 				}
-
-//				printf("%10u %10u %6u %6d\n", so->last_master_time, le->timestamp, (le->length - 2750)/500, (int32_t)le->timestamp - (int32_t)so->last_master_time);
-				so->last_master_time = le->timestamp;
-				so->last_photo_length = le->length;
-				ct->lightproc( so, le->sensor_id, -1, 0, le->timestamp, deltat );
-				deltat = 0;
 			}
 		}
-
-		//Find longest pulse-length from device in our window and use that one.
-		if( le->length > so->last_photo_length )
+		else
 		{
-//			printf("%10u %10u %6d %6d\n", so->last_master_time, le->timestamp, (le->length - 2750)/500, (int32_t)le->timestamp - (int32_t)so->last_master_time);
-			so->last_master_time = le->timestamp;
-			so->last_photo_length = le->length;
+			//Find the longest pulse.
+			if( le->length > last_sync_length )
+			{
+				if( so->last_time[ssn] > le->timestamp )
+				{
+					so->last_time[ssn] = le->timestamp;
+					so->last_length[ssn] = le->length;
+				}
+			}
 		}
 	}
+
+
+
 	//See if this is a valid actual pulse.
-	else if( le->length < 1800 && le->length > 40 && ( le->timestamp - so->last_master_time < 380000 ) )
+	else if( le->length < so->pulse_max_for_sweep && delta > so->pulse_in_clear_time && ssn >= 0 )
 	{
-		int32_t dl = so->last_master_time;
-		int32_t tpco = so->last_photo_length;
+		int32_t dl = so->last_time[0];
+		int32_t tpco = so->last_length[0];
+
+
+#if NUM_LIGHTHOUSES != 2
+		#error You are going to have to fix the code around here to allow for something other than two base stations.
+#endif
 
 		//Adding length 
 		//Long pulse-code from IR flood.
 		//Make sure it fits nicely into a divisible-by-500 time.
-		int32_t acode = (tpco+125+50)/250;  //+10, seems ike that's
-		if( acode & 1 ) return;
 
-		acode >>= 1;
-		acode -= 6;
-		if (acode > 3) {
-			dl = so->last_slave_time;
+		int32_t main_divisor = so->timebase_hz / 384000; //125 @ 48 MHz.
+
+		int32_t acode_array[2] =
+			{
+				(so->last_length[0]+main_divisor+50)/(main_divisor*2),  //+50 adds a small offset and seems to help always get it right. 
+				(so->last_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.
+			};
+
+		//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;
+
+
+		int acode = acode_array[0];
+
+		if( !so->did_handle_ootx )
+		{
+			int32_t delta1 = so->last_time[0] - so->recent_sync_time;
+			int32_t delta2 = so->last_time[1] - so->last_time[0];
+
+			ctx->lightproc( so, -1, acode_array[0], delta1, so->last_time[0], so->last_length[0] );
+			ctx->lightproc( so, -2, acode_array[1], delta2, so->last_time[1], so->last_length[1] );
+
+			so->recent_sync_time = so->last_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;
 		}
 
-		//printf( "%s / %d %d ++ %d %d\n", so->codename, dl, tpco, offset_from, acode );
+
+		if (acode > 3) {
+			if( ssn == 0 )
+			{
+				SV_INFO( "Warning: got a slave marker but only got a master sync." );
+			}
+			dl = so->last_time[1];
+			tpco = so->last_length[1];
+		}
 
 		int32_t offset_from = le->timestamp - dl + le->length/2;
 
 		//Make sure pulse is in valid window
 		if( offset_from < 380000 && offset_from > 70000 )
 		{
-			ct->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 );
 		}
 	}
 	else
@@ -122,7 +196,6 @@ static void handle_lightcap( struct SurviveObject * so, struct LightcapElement *
 		//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
 }