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."

1 files changed, 34 insertions, 3 deletions

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)