Adaptive Acode Offset Compensation

1 files changed, 66 insertions, 48 deletions

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));
 	}