From 3ca8ba3d69de226ae8835bb29e45c7bcd35793fe Mon Sep 17 00:00:00 2001 From: mwturvey Date: Wed, 29 Mar 2017 16:39:47 -0700 Subject: 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." --- src/survive_data.c | 37 ++++++++++++++++++++++++++++++++++--- 1 file changed, 34 insertions(+), 3 deletions(-) (limited to 'src/survive_data.c') 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) -- cgit v1.2.3