Adaptive Acode Offset Compensation

4 files changed, 86 insertions, 65 deletions

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++ )