From 4e3ac12016cc489c818709412acd293cac54cb78 Mon Sep 17 00:00:00 2001 From: Joshua Allen Date: Fri, 17 Mar 2017 11:37:30 -0400 Subject: reusable function for decoding acode --- src/survive_data.c | 25 +++++++++++++++++-------- 1 file changed, 17 insertions(+), 8 deletions(-) (limited to 'src/survive_data.c') diff --git a/src/survive_data.c b/src/survive_data.c index 4a2cfb6..d658e18 100644 --- a/src/survive_data.c +++ b/src/survive_data.c @@ -5,6 +5,19 @@ #include #include + +int32_t decode_acode(uint32_t length, int32_t main_divisor) { + //+50 adds a small offset and seems to help always get it right. + //Check the +50 in the future to see how well this works on a variety of hardware. + + int32_t acode = (length+main_divisor+50)/(main_divisor*2); + if( acode & 1 ) return -1; + + return (acode>>1) - 6; +} + +} + //This is the disambiguator function, for taking light timing and figuring out place-in-sweep for a given photodiode. void handle_lightcap( SurviveObject * so, LightcapElement * le ) { @@ -112,17 +125,13 @@ void handle_lightcap( SurviveObject * so, LightcapElement * le ) int32_t acode_array[2] = { - (so->last_sync_length[0]+main_divisor+50)/(main_divisor*2), //+50 adds a small offset and seems to help always get it right. - (so->last_sync_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. + decode_acode(so->last_sync_length[0],main_divisor), + decode_acode(so->last_sync_length[1],main_divisor) }; //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; - + if( acode_array[0] < 0 ) return; + if( acode_array[1] < 0 ) return; int acode = acode_array[0]; -- cgit v1.2.3 From 64e3b67aa0d6a970e1e1988881e77d04c4fb5b07 Mon Sep 17 00:00:00 2001 From: mwturvey Date: Fri, 17 Mar 2017 14:51:00 -0700 Subject: Remove extra debug print --- src/survive_data.c | 1 - 1 file changed, 1 deletion(-) (limited to 'src/survive_data.c') diff --git a/src/survive_data.c b/src/survive_data.c index b9099ac..5c5a5e9 100644 --- a/src/survive_data.c +++ b/src/survive_data.c @@ -39,7 +39,6 @@ void handle_lightcap( SurviveObject * so, LightcapElement * le ) // return; //if we don't know what lighthouse this is we don't care to do much else } - printf("m sync %d %d %d %d\n", le->sensor_id, so->last_sync_time[ssn], le->timestamp, delta); if( le->length > so->pulselength_min_sync ) //Pulse longer indicates a sync pulse. { -- cgit v1.2.3 From e1818d84e1ab0ae217d8035f8a12398f06508059 Mon Sep 17 00:00:00 2001 From: mwturvey Date: Fri, 17 Mar 2017 16:32:18 -0700 Subject: Alternate disambiguator and calibration updates --- src/survive_data.c | 78 ++++++++++++++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 78 insertions(+) (limited to 'src/survive_data.c') diff --git a/src/survive_data.c b/src/survive_data.c index 5c5a5e9..e6538d1 100644 --- a/src/survive_data.c +++ b/src/survive_data.c @@ -5,9 +5,87 @@ #include #include +int getAcodeFromSyncPulse(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; + return 7; +} +void handle_lightcap2_sync(SurviveObject * so, LightcapElement * le ) +{ + fprintf(stderr, "%d\n", le->length); + + if (le->timestamp - so->recent_sync_time < 24000) + { + // I do believe we are lighthouse B + so->last_sync_time[1] = so->recent_sync_time = le->timestamp; + so->last_sync_length[1] = le->length; + if (le->length < 4625) // max non-skip pulse + 1/4 the difference in time between pulses. + { + so->sync_set_number = 1; + so->ctx->lightproc(so, -2, getAcodeFromSyncPulse(le->length), le->length, le->timestamp, le->length); // Don't think I got this quite right. + } + } + else + { + // looks like this is the first sync pulse. Cool! + if (le->length < 4625) // max non-skip pulse + 1/4 the difference in time between pulses. + { + so->sync_set_number = 1; + so->ctx->lightproc(so, -1, getAcodeFromSyncPulse(le->length), le->length, le->timestamp, le->length); // Don't think I got this quite right. + } + } + + +// ctx->lightproc( so, -2, acode_array[1], delta2, so->last_sync_time[1], so->last_sync_length[1] ); + + +// typedef void (*light_process_func)( SurviveObject * so, int sensor_id, int acode, int timeinsweep, uint32_t timecode, uint32_t length ); + +} + +void handle_lightcap2_sweep(SurviveObject * so, LightcapElement * le ) +{ + +} + +void handle_lightcap2( SurviveObject * so, LightcapElement * le ) +{ + SurviveContext * ctx = so->ctx; + + if( le->sensor_id > SENSORS_PER_OBJECT ) + { + return; + } + + if (le->length > 6750) + { + // Should never get a reading so high. Odd. + return; + } + if (le->length >= 2750) + { + // Looks like a sync pulse, process it! + handle_lightcap2_sync(so, le); + } + + // must be a sweep pulse, process it! + handle_lightcap2_sweep(so, le); + +} + + //This is the disambiguator function, for taking light timing and figuring out place-in-sweep for a given photodiode. void handle_lightcap( SurviveObject * so, LightcapElement * le ) { + handle_lightcap2(so,le); + return; + SurviveContext * ctx = so->ctx; //int32_t deltat = (uint32_t)le->timestamp - (uint32_t)so->last_master_time; -- cgit v1.2.3 From 0f94e39d96fbc2744e61b85d2884250d0783eb11 Mon Sep 17 00:00:00 2001 From: cnlohr Date: Sat, 18 Mar 2017 15:21:24 -0400 Subject: Add more possible debugging if turned on. --- src/survive_data.c | 22 ++++++++++++++++++---- 1 file changed, 18 insertions(+), 4 deletions(-) (limited to 'src/survive_data.c') diff --git a/src/survive_data.c b/src/survive_data.c index 5c5a5e9..0789203 100644 --- a/src/survive_data.c +++ b/src/survive_data.c @@ -8,17 +8,31 @@ //This is the disambiguator function, for taking light timing and figuring out place-in-sweep for a given photodiode. void handle_lightcap( SurviveObject * so, LightcapElement * le ) { - SurviveContext * ctx = so->ctx; + SurviveContext * ctx = so->ctx; //int32_t deltat = (uint32_t)le->timestamp - (uint32_t)so->last_master_time; - //if( so->codename[0] != 'H' ) - - if( le->sensor_id > SENSORS_PER_OBJECT ) { return; } +#if 0 + if( so->codename[0] == 'H' ) + { + static int lt; + static int last; + if( le->length > 1000 ) + { + int dl = le->timestamp - lt; + lt = le->timestamp; + if( dl > 10000 || dl < -10000 ) + printf( "+++%s %3d %5d %9d ", so->codename, le->sensor_id, le->length, dl ); + if( dl > 100000 ) printf(" \n" ); + } + last=le->length; + } +#endif + so->tsl = le->timestamp; if( le->length < 20 ) return; ///Assuming 20 is an okay value for here. -- cgit v1.2.3 From d4e7bc4037f9e0b4af3f11940b53520eb9f82ff6 Mon Sep 17 00:00:00 2001 From: Joshua Allen Date: Sat, 18 Mar 2017 20:25:06 -0400 Subject: fix compile --- src/survive_data.c | 2 -- 1 file changed, 2 deletions(-) (limited to 'src/survive_data.c') diff --git a/src/survive_data.c b/src/survive_data.c index 16a22e9..ffeacff 100644 --- a/src/survive_data.c +++ b/src/survive_data.c @@ -16,8 +16,6 @@ int32_t decode_acode(uint32_t length, int32_t main_divisor) { return (acode>>1) - 6; } -} - //This is the disambiguator function, for taking light timing and figuring out place-in-sweep for a given photodiode. void handle_lightcap( SurviveObject * so, LightcapElement * le ) { -- cgit v1.2.3 From 8253e3c3adbb54ebc3d3c8e02c017e900a83edb0 Mon Sep 17 00:00:00 2001 From: cnlohr Date: Mon, 20 Mar 2017 01:42:32 -0400 Subject: log synctimes. --- src/survive_data.c | 10 +++++++++- 1 file changed, 9 insertions(+), 1 deletion(-) (limited to 'src/survive_data.c') diff --git a/src/survive_data.c b/src/survive_data.c index ffeacff..8c5c646 100644 --- a/src/survive_data.c +++ b/src/survive_data.c @@ -69,7 +69,7 @@ void handle_lightcap( SurviveObject * so, LightcapElement * le ) { int is_new_pulse = delta > so->pulselength_min_sync /*1500*/ + last_sync_length; - //printf("m sync %d %d %d %d\n", le->sensor_id, so->last_sync_time[ssn], le->timestamp, delta); + so->did_handle_ootx = 0; @@ -114,6 +114,14 @@ void handle_lightcap( SurviveObject * so, LightcapElement * le ) } } } + + //Extra tidbit for storing length-of-sync-pulses. + { + int32_t main_divisor = so->timebase_hz / 384000; //125 @ 48 MHz. + int base_station = is_new_pulse; + printf( "%s %d %d %d\n", so->codename, le->sensor_id, so->sync_set_number, le->length ); + ctx->lightproc( so, le->sensor_id, -3 - so->sync_set_number, 0, le->timestamp, le->length ); + } } -- cgit v1.2.3 From 39ef5af74702c8825a82f65cf68e6af875a814ee Mon Sep 17 00:00:00 2001 From: ultramn Date: Thu, 23 Mar 2017 00:41:22 -0400 Subject: Added a dynamic plotting tool for OrthoSolve. Charles added orthogonalization to the rotation matrix. --- src/survive_data.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) (limited to 'src/survive_data.c') diff --git a/src/survive_data.c b/src/survive_data.c index 8c5c646..0873f7f 100644 --- a/src/survive_data.c +++ b/src/survive_data.c @@ -119,7 +119,7 @@ void handle_lightcap( SurviveObject * so, LightcapElement * le ) { int32_t main_divisor = so->timebase_hz / 384000; //125 @ 48 MHz. int base_station = is_new_pulse; - printf( "%s %d %d %d\n", so->codename, le->sensor_id, so->sync_set_number, le->length ); + //printf( "%s %d %d %d\n", so->codename, le->sensor_id, so->sync_set_number, le->length ); ctx->lightproc( so, le->sensor_id, -3 - so->sync_set_number, 0, le->timestamp, le->length ); } } -- cgit v1.2.3 From 472d05c2a356ec6d71669d67fb599e401a6f4a76 Mon Sep 17 00:00:00 2001 From: Mike Turvey Date: Thu, 23 Mar 2017 00:25:25 -0700 Subject: Updated disambiguator. Solid OOTX --- src/survive_data.c | 211 +++++++++++++++++++++++++++++++++++++++++++++++++---- 1 file changed, 195 insertions(+), 16 deletions(-) (limited to 'src/survive_data.c') diff --git a/src/survive_data.c b/src/survive_data.c index e6538d1..2ef4940 100644 --- a/src/survive_data.c +++ b/src/survive_data.c @@ -5,7 +5,32 @@ #include #include -int getAcodeFromSyncPulse(int pulseLen) +typedef struct +{ + unsigned int sweep_time[SENSORS_PER_OBJECT]; + unsigned int sweep_len[SENSORS_PER_OBJECT]; +} lightcaps_sweep_data; +typedef struct +{ + lightcaps_sweep_data sweep; +} lightcap2_data; + +typedef struct +{ + int recent_sync_time; + int activeLighthouse; + int activeSweepStartTime; + int activeAcode; + + int lh_pulse_len[NUM_LIGHTHOUSES]; + int lh_start_time[NUM_LIGHTHOUSES]; + int current_lh; // used knowing which sync pulse we're looking at. + +} lightcap2_global_data; + +static lightcap2_global_data lcgd = { 0 }; + +int handle_lightcap2_getAcodeFromSyncPulse(int pulseLen) { if (pulseLen < 3125) return 0; if (pulseLen < 3625) return 1; @@ -16,48 +41,201 @@ int getAcodeFromSyncPulse(int pulseLen) if (pulseLen < 6125) return 6; return 7; } +void handle_lightcap2_process_sweep_data(SurviveObject *so) +{ + lightcap2_data *lcd = so->disambiguator_data; + + // look at all of the sensors we found, and process the ones that were hit. + // TODO: find the sensor(s) with the longest pulse length, and assume + // those are the "highest quality". Then, reject any pulses that are sufficiently + // different from those values, assuming that they are reflections. + { + unsigned int longest_pulse = 0; + unsigned int timestamp_of_longest_pulse = 0; + for (int i = 0; i < SENSORS_PER_OBJECT; i++) + { + if (lcd->sweep.sweep_len[i] > longest_pulse) + { + longest_pulse = lcd->sweep.sweep_len[i]; + timestamp_of_longest_pulse = lcd->sweep.sweep_time[i]; + } + } + + for (int i = 0; i < SENSORS_PER_OBJECT; i++) + { + if (lcd->sweep.sweep_len[i] != 0) // if the sensor was hit, process it + { + int offset_from = lcd->sweep.sweep_time[i] - lcgd.activeSweepStartTime + lcd->sweep.sweep_len[i] / 2; + + if (offset_from < 380000 && offset_from > 70000) + { + int timeDelta = abs(timestamp_of_longest_pulse - lcd->sweep.sweep_time[i]); + if (timeDelta < 15000) // if this sweep point is within ~7 degrees of the point with the longest pulse. + { + so->ctx->lightproc(so, i, lcgd.activeAcode, offset_from, lcd->sweep.sweep_time[i], lcd->sweep.sweep_len[i], lcgd.activeLighthouse); + } + } + } + } + } + // 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. + memset(&(((lightcap2_data*)so->disambiguator_data)->sweep), 0, sizeof(lightcaps_sweep_data)); +} void handle_lightcap2_sync(SurviveObject * so, LightcapElement * le ) { - fprintf(stderr, "%d\n", le->length); + //fprintf(stderr, "%6.6d %4.4d \n", le->timestamp - so->recent_sync_time, le->length); + lightcap2_data *lcd = so->disambiguator_data; - if (le->timestamp - so->recent_sync_time < 24000) + //static unsigned int recent_sync_time = 0; + //static unsigned int recent_sync_count = -1; + //static unsigned int activeSweepStartTime; + + + // Process any sweep data we have + handle_lightcap2_process_sweep_data(so); + + int time_since_last_sync = (le->timestamp - lcgd.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) { + lcgd.recent_sync_time = le->timestamp; + // it's the same sync pulse; + so->sync_set_number = 1; + //so->ctx->lightproc(so, recent_sync_count, getAcodeFromSyncPulse(le->length), le->length, le->timestamp, le->length); // Don't think I got this quite right. + so->recent_sync_time = le->timestamp; + + lcgd.lh_pulse_len[lcgd.current_lh] = le->length; + lcgd.lh_start_time[lcgd.current_lh] = le->timestamp; + + int acode = handle_lightcap2_getAcodeFromSyncPulse(le->length); + if (!(acode >> 2 & 1)) // if the skip bit is not set + { + lcgd.activeLighthouse = lcgd.current_lh; + lcgd.activeSweepStartTime = le->timestamp; + lcgd.activeAcode = acode; + } + else + { + lcgd.activeLighthouse = -1; + lcgd.activeSweepStartTime = 0; + lcgd.activeAcode = 0; + } + } + else if (time_since_last_sync < 24000) + { + //recent_sync_count--; + lcgd.recent_sync_time = le->timestamp; // I do believe we are lighthouse B - so->last_sync_time[1] = so->recent_sync_time = le->timestamp; - so->last_sync_length[1] = le->length; - if (le->length < 4625) // max non-skip pulse + 1/4 the difference in time between pulses. + lcgd.current_lh = 1; + lcgd.lh_pulse_len[lcgd.current_lh] = le->length; + lcgd.lh_start_time[lcgd.current_lh] = le->timestamp; + + int acode = handle_lightcap2_getAcodeFromSyncPulse(le->length); + + //{ + //fprintf(stderr, "2"); + //so->ctx->lightproc(so, -2, acode, le->length, le->timestamp, le->length); // Don't think I got this quite right. + + + //} + if (!(acode >> 2 & 1)) // if the skip bit is not set { - so->sync_set_number = 1; - so->ctx->lightproc(so, -2, getAcodeFromSyncPulse(le->length), le->length, le->timestamp, le->length); // Don't think I got this quite right. + if (lcgd.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"); + } + lcgd.activeLighthouse = 1; + lcgd.activeSweepStartTime = le->timestamp; + lcgd.activeAcode = acode; } + } - else + else if (time_since_last_sync > 370000) { // looks like this is the first sync pulse. Cool! - if (le->length < 4625) // max non-skip pulse + 1/4 the difference in time between pulses. + + // first, send out the sync pulse data for the last round (for OOTX decoding { - so->sync_set_number = 1; - so->ctx->lightproc(so, -1, getAcodeFromSyncPulse(le->length), le->length, le->timestamp, le->length); // Don't think I got this quite right. - } - } + if (lcgd.lh_pulse_len[0] != 0) + { + so->ctx->lightproc( + so, + -1, + handle_lightcap2_getAcodeFromSyncPulse(lcgd.lh_pulse_len[0]), + lcgd.lh_pulse_len[0], + lcgd.lh_start_time[0], + 0, + 0); + } + if (lcgd.lh_pulse_len[1] != 0) + { + so->ctx->lightproc( + so, + -2, + handle_lightcap2_getAcodeFromSyncPulse(lcgd.lh_pulse_len[1]), + lcgd.lh_pulse_len[1], + lcgd.lh_start_time[1], + 0, + 1); + } + } + // initialize here. + memset(&lcgd, 0, sizeof(lcgd)); + lcgd.activeLighthouse = -1; -// ctx->lightproc( so, -2, acode_array[1], delta2, so->last_sync_time[1], so->last_sync_length[1] ); -// typedef void (*light_process_func)( SurviveObject * so, int sensor_id, int acode, int timeinsweep, uint32_t timecode, uint32_t length ); + lcgd.recent_sync_time = le->timestamp; + // I do believe we are lighthouse A + lcgd.current_lh = 0; + lcgd.lh_pulse_len[lcgd.current_lh] = le->length; + lcgd.lh_start_time[lcgd.current_lh] = le->timestamp; + + int acode = handle_lightcap2_getAcodeFromSyncPulse(le->length); + //{ + // so->ctx->lightproc(so, -1, acode, le->length, le->timestamp, le->length); // Don't think I got this quite right. + //} + + if (!(acode >> 2 & 1)) // if the skip bit is not set + { + lcgd.activeLighthouse = 0; + lcgd.activeSweepStartTime = le->timestamp; + lcgd.activeAcode = acode; + } + + } } void handle_lightcap2_sweep(SurviveObject * so, LightcapElement * le ) { + lightcap2_data *lcd = so->disambiguator_data; + // If we see multiple "hits" on the sweep for a given sensor, + // assume that the longest (i.e. strongest signal) is most likely + // the non-reflected signal. + if (lcd->sweep.sweep_len[le->sensor_id] < le->length) + { + lcd->sweep.sweep_len[le->sensor_id] = le->length; + lcd->sweep.sweep_time[le->sensor_id] = le->timestamp; + } } void handle_lightcap2( SurviveObject * so, LightcapElement * le ) { SurviveContext * ctx = so->ctx; + if (so->disambiguator_data == NULL) + { + so->disambiguator_data = malloc(sizeof(lightcap2_data)); + memset(so->disambiguator_data, 0, sizeof(lightcap2_data)); + } + if( le->sensor_id > SENSORS_PER_OBJECT ) { return; @@ -72,6 +250,7 @@ void handle_lightcap2( SurviveObject * so, LightcapElement * le ) { // Looks like a sync pulse, process it! handle_lightcap2_sync(so, le); + return; } // must be a sweep pulse, process it! -- cgit v1.2.3 From 7701d14440d5d005b1ec3513ff8646e2f6b101ef Mon Sep 17 00:00:00 2001 From: Mike Turvey Date: Thu, 23 Mar 2017 08:11:36 -0700 Subject: Cleanup --- src/survive_data.c | 27 +++++++++++---------------- 1 file changed, 11 insertions(+), 16 deletions(-) (limited to 'src/survive_data.c') diff --git a/src/survive_data.c b/src/survive_data.c index 2ef4940..d92db36 100644 --- a/src/survive_data.c +++ b/src/survive_data.c @@ -69,8 +69,7 @@ void handle_lightcap2_process_sweep_data(SurviveObject *so) if (offset_from < 380000 && offset_from > 70000) { - int timeDelta = abs(timestamp_of_longest_pulse - lcd->sweep.sweep_time[i]); - if (timeDelta < 15000) // if this sweep point is within ~7 degrees of the point with the longest pulse. + if (longest_pulse *10 / 8 < -lcd->sweep.sweep_len[i]) { so->ctx->lightproc(so, i, lcgd.activeAcode, offset_from, lcd->sweep.sweep_time[i], lcd->sweep.sweep_len[i], lcgd.activeLighthouse); } @@ -97,14 +96,13 @@ void handle_lightcap2_sync(SurviveObject * so, LightcapElement * le ) int time_since_last_sync = (le->timestamp - lcgd.recent_sync_time); - fprintf(stderr, " %2d %8d %d\n", le->sensor_id, time_since_last_sync, le->length); + //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) { lcgd.recent_sync_time = le->timestamp; // it's the same sync pulse; so->sync_set_number = 1; - //so->ctx->lightproc(so, recent_sync_count, getAcodeFromSyncPulse(le->length), le->length, le->timestamp, le->length); // Don't think I got this quite right. so->recent_sync_time = le->timestamp; lcgd.lh_pulse_len[lcgd.current_lh] = le->length; @@ -126,7 +124,6 @@ void handle_lightcap2_sync(SurviveObject * so, LightcapElement * le ) } else if (time_since_last_sync < 24000) { - //recent_sync_count--; lcgd.recent_sync_time = le->timestamp; // I do believe we are lighthouse B lcgd.current_lh = 1; @@ -135,12 +132,6 @@ void handle_lightcap2_sync(SurviveObject * so, LightcapElement * le ) int acode = handle_lightcap2_getAcodeFromSyncPulse(le->length); - //{ - //fprintf(stderr, "2"); - //so->ctx->lightproc(so, -2, acode, le->length, le->timestamp, le->length); // Don't think I got this quite right. - - - //} if (!(acode >> 2 & 1)) // if the skip bit is not set { if (lcgd.activeLighthouse != -1) @@ -197,9 +188,6 @@ void handle_lightcap2_sync(SurviveObject * so, LightcapElement * le ) lcgd.lh_start_time[lcgd.current_lh] = le->timestamp; int acode = handle_lightcap2_getAcodeFromSyncPulse(le->length); - //{ - // so->ctx->lightproc(so, -1, acode, le->length, le->timestamp, le->length); // Don't think I got this quite right. - //} if (!(acode >> 2 & 1)) // if the skip bit is not set { @@ -207,9 +195,7 @@ void handle_lightcap2_sync(SurviveObject * so, LightcapElement * le ) lcgd.activeSweepStartTime = le->timestamp; lcgd.activeAcode = acode; } - } - } void handle_lightcap2_sweep(SurviveObject * so, LightcapElement * le ) @@ -219,6 +205,15 @@ void handle_lightcap2_sweep(SurviveObject * so, LightcapElement * le ) // If we see multiple "hits" on the sweep for a given sensor, // assume that the longest (i.e. strongest signal) is most likely // the non-reflected signal. + + if (le->length < 80) + { + // this is a low-quality read. Better to throw it out than to use it. + //fprintf(stderr, "%2d %d\n", le->sensor_id, le->length); + return; + } + fprintf(stderr, "%2d %d\n", le->sensor_id, le->length); + if (lcd->sweep.sweep_len[le->sensor_id] < le->length) { lcd->sweep.sweep_len[le->sensor_id] = le->length; -- cgit v1.2.3 From 280a6599fea76a7d2c16cfe0fcc5c8f37fde66de Mon Sep 17 00:00:00 2001 From: mwturvey Date: Thu, 23 Mar 2017 09:47:38 -0700 Subject: More cleanup --- src/survive_data.c | 96 ++++++++++++++++++++++++++++-------------------------- 1 file changed, 49 insertions(+), 47 deletions(-) (limited to 'src/survive_data.c') diff --git a/src/survive_data.c b/src/survive_data.c index d92db36..00e66f0 100644 --- a/src/survive_data.c +++ b/src/survive_data.c @@ -10,11 +10,6 @@ typedef struct unsigned int sweep_time[SENSORS_PER_OBJECT]; unsigned int sweep_len[SENSORS_PER_OBJECT]; } lightcaps_sweep_data; -typedef struct -{ - lightcaps_sweep_data sweep; -} lightcap2_data; - typedef struct { int recent_sync_time; @@ -28,7 +23,14 @@ typedef struct } lightcap2_global_data; -static lightcap2_global_data lcgd = { 0 }; +typedef struct +{ + lightcaps_sweep_data sweep; + lightcap2_global_data global; +} lightcap2_data; + + +//static lightcap2_global_data lcgd = { 0 }; int handle_lightcap2_getAcodeFromSyncPulse(int pulseLen) { @@ -65,13 +67,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] - lcgd.activeSweepStartTime + lcd->sweep.sweep_len[i] / 2; + int offset_from = lcd->sweep.sweep_time[i] - lcd->global.activeSweepStartTime + lcd->sweep.sweep_len[i] / 2; if (offset_from < 380000 && offset_from > 70000) { - if (longest_pulse *10 / 8 < -lcd->sweep.sweep_len[i]) + if (longest_pulse *10 / 8 < lcd->sweep.sweep_len[i]) { - so->ctx->lightproc(so, i, lcgd.activeAcode, offset_from, lcd->sweep.sweep_time[i], lcd->sweep.sweep_len[i], lcgd.activeLighthouse); + so->ctx->lightproc(so, i, lcd->global.activeAcode, offset_from, lcd->sweep.sweep_time[i], lcd->sweep.sweep_len[i], lcd->global.activeLighthouse); } } } @@ -94,54 +96,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 - lcgd.recent_sync_time); + int time_since_last_sync = (le->timestamp - lcd->global.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) { - lcgd.recent_sync_time = le->timestamp; + lcd->global.recent_sync_time = le->timestamp; // it's the same sync pulse; so->sync_set_number = 1; so->recent_sync_time = le->timestamp; - lcgd.lh_pulse_len[lcgd.current_lh] = le->length; - lcgd.lh_start_time[lcgd.current_lh] = le->timestamp; + lcd->global.lh_pulse_len[lcd->global.current_lh] = le->length; + lcd->global.lh_start_time[lcd->global.current_lh] = le->timestamp; int acode = handle_lightcap2_getAcodeFromSyncPulse(le->length); if (!(acode >> 2 & 1)) // if the skip bit is not set { - lcgd.activeLighthouse = lcgd.current_lh; - lcgd.activeSweepStartTime = le->timestamp; - lcgd.activeAcode = acode; + lcd->global.activeLighthouse = lcd->global.current_lh; + lcd->global.activeSweepStartTime = le->timestamp; + lcd->global.activeAcode = acode; } else { - lcgd.activeLighthouse = -1; - lcgd.activeSweepStartTime = 0; - lcgd.activeAcode = 0; + lcd->global.activeLighthouse = -1; + lcd->global.activeSweepStartTime = 0; + lcd->global.activeAcode = 0; } } else if (time_since_last_sync < 24000) { - lcgd.recent_sync_time = le->timestamp; + lcd->global.recent_sync_time = le->timestamp; // I do believe we are lighthouse B - lcgd.current_lh = 1; - lcgd.lh_pulse_len[lcgd.current_lh] = le->length; - lcgd.lh_start_time[lcgd.current_lh] = le->timestamp; + 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; int acode = handle_lightcap2_getAcodeFromSyncPulse(le->length); if (!(acode >> 2 & 1)) // if the skip bit is not set { - if (lcgd.activeLighthouse != -1) + if (lcd->global.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"); } - lcgd.activeLighthouse = 1; - lcgd.activeSweepStartTime = le->timestamp; - lcgd.activeAcode = acode; + lcd->global.activeLighthouse = 1; + lcd->global.activeSweepStartTime = le->timestamp; + lcd->global.activeAcode = acode; } } @@ -151,49 +153,49 @@ void handle_lightcap2_sync(SurviveObject * so, LightcapElement * le ) // first, send out the sync pulse data for the last round (for OOTX decoding { - if (lcgd.lh_pulse_len[0] != 0) + if (lcd->global.lh_pulse_len[0] != 0) { so->ctx->lightproc( so, -1, - handle_lightcap2_getAcodeFromSyncPulse(lcgd.lh_pulse_len[0]), - lcgd.lh_pulse_len[0], - lcgd.lh_start_time[0], + handle_lightcap2_getAcodeFromSyncPulse(lcd->global.lh_pulse_len[0]), + lcd->global.lh_pulse_len[0], + lcd->global.lh_start_time[0], 0, 0); } - if (lcgd.lh_pulse_len[1] != 0) + if (lcd->global.lh_pulse_len[1] != 0) { so->ctx->lightproc( so, -2, - handle_lightcap2_getAcodeFromSyncPulse(lcgd.lh_pulse_len[1]), - lcgd.lh_pulse_len[1], - lcgd.lh_start_time[1], + handle_lightcap2_getAcodeFromSyncPulse(lcd->global.lh_pulse_len[1]), + lcd->global.lh_pulse_len[1], + lcd->global.lh_start_time[1], 0, 1); } } // initialize here. - memset(&lcgd, 0, sizeof(lcgd)); - lcgd.activeLighthouse = -1; + memset(&lcd->global, 0, sizeof(lcd->global)); + lcd->global.activeLighthouse = -1; - lcgd.recent_sync_time = le->timestamp; + lcd->global.recent_sync_time = le->timestamp; // I do believe we are lighthouse A - lcgd.current_lh = 0; - lcgd.lh_pulse_len[lcgd.current_lh] = le->length; - lcgd.lh_start_time[lcgd.current_lh] = le->timestamp; + 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; int acode = handle_lightcap2_getAcodeFromSyncPulse(le->length); if (!(acode >> 2 & 1)) // if the skip bit is not set { - lcgd.activeLighthouse = 0; - lcgd.activeSweepStartTime = le->timestamp; - lcgd.activeAcode = acode; + lcd->global.activeLighthouse = 0; + lcd->global.activeSweepStartTime = le->timestamp; + lcd->global.activeAcode = acode; } } } @@ -383,8 +385,8 @@ void handle_lightcap( SurviveObject * so, LightcapElement * le ) int32_t delta1 = so->last_sync_time[0] - so->recent_sync_time; int32_t delta2 = so->last_sync_time[1] - so->last_sync_time[0]; - ctx->lightproc( so, -1, acode_array[0], delta1, so->last_sync_time[0], so->last_sync_length[0] ); - ctx->lightproc( so, -2, acode_array[1], delta2, so->last_sync_time[1], so->last_sync_length[1] ); + ctx->lightproc( so, -1, acode_array[0], delta1, so->last_sync_time[0], so->last_sync_length[0], 0 ); + ctx->lightproc( so, -2, acode_array[1], delta2, so->last_sync_time[1], so->last_sync_length[1], 1 ); so->recent_sync_time = so->last_sync_time[1]; @@ -427,7 +429,7 @@ void handle_lightcap( SurviveObject * so, LightcapElement * le ) //Make sure pulse is in valid window if( offset_from < 380000 && offset_from > 70000 ) { - ctx->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, so->sync_set_number ); } } else -- cgit v1.2.3 From f33018188bf5bdf6f6b8af0d646e3f8c519d9d71 Mon Sep 17 00:00:00 2001 From: mwturvey Date: Thu, 23 Mar 2017 12:13:44 -0700 Subject: Added support for empty string in config.json & other cleanup. --- src/survive_data.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) (limited to 'src/survive_data.c') diff --git a/src/survive_data.c b/src/survive_data.c index ee180b1..da4e0a2 100644 --- a/src/survive_data.c +++ b/src/survive_data.c @@ -372,7 +372,7 @@ void handle_lightcap( SurviveObject * so, LightcapElement * le ) int32_t main_divisor = so->timebase_hz / 384000; //125 @ 48 MHz. int base_station = is_new_pulse; //printf( "%s %d %d %d\n", so->codename, le->sensor_id, so->sync_set_number, le->length ); - ctx->lightproc( so, le->sensor_id, -3 - so->sync_set_number, 0, le->timestamp, le->length ); + ctx->lightproc( so, le->sensor_id, -3 - so->sync_set_number, 0, le->timestamp, le->length, base_station); } } -- cgit v1.2.3 From 3d86dda66d50d1e1955d4e0cd5f374e0aed1789f Mon Sep 17 00:00:00 2001 From: mwturvey Date: Thu, 23 Mar 2017 16:26:27 -0700 Subject: Fixed Windows USB Interface Enumeration --- src/survive_data.c | 21 +++++++++++---------- 1 file changed, 11 insertions(+), 10 deletions(-) (limited to 'src/survive_data.c') diff --git a/src/survive_data.c b/src/survive_data.c index da4e0a2..ce263ed 100644 --- a/src/survive_data.c +++ b/src/survive_data.c @@ -208,13 +208,14 @@ void handle_lightcap2_sweep(SurviveObject * so, LightcapElement * le ) // assume that the longest (i.e. strongest signal) is most likely // the non-reflected signal. - if (le->length < 80) - { - // this is a low-quality read. Better to throw it out than to use it. - //fprintf(stderr, "%2d %d\n", le->sensor_id, le->length); - return; - } - fprintf(stderr, "%2d %d\n", le->sensor_id, le->length); + //if (le->length < 80) + //{ + // // this is a low-quality read. Better to throw it out than to use it. + // //fprintf(stderr, "%2d %d\n", le->sensor_id, le->length); + // return; + //} + //fprintf(stderr, "%2d %d\n", le->sensor_id, le->length); + //fprintf(stderr, "."); if (lcd->sweep.sweep_len[le->sensor_id] < le->length) { @@ -269,8 +270,8 @@ int32_t decode_acode(uint32_t length, int32_t main_divisor) { void handle_lightcap( SurviveObject * so, LightcapElement * le ) { SurviveContext * ctx = so->ctx; -// handle_lightcap2(so,le); -// return; + handle_lightcap2(so,le); + return; //int32_t deltat = (uint32_t)le->timestamp - (uint32_t)so->last_master_time; @@ -373,7 +374,7 @@ void handle_lightcap( SurviveObject * so, LightcapElement * le ) int base_station = is_new_pulse; //printf( "%s %d %d %d\n", so->codename, le->sensor_id, so->sync_set_number, le->length ); ctx->lightproc( so, le->sensor_id, -3 - so->sync_set_number, 0, le->timestamp, le->length, base_station); - } + } } -- cgit v1.2.3 From c55c5928485da0432140be4befe5165c68ae4ab0 Mon Sep 17 00:00:00 2001 From: mwturvey Date: Thu, 23 Mar 2017 16:46:04 -0700 Subject: Change disambiguator filter --- src/survive_data.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) (limited to 'src/survive_data.c') diff --git a/src/survive_data.c b/src/survive_data.c index ce263ed..849b722 100644 --- a/src/survive_data.c +++ b/src/survive_data.c @@ -71,7 +71,7 @@ void handle_lightcap2_process_sweep_data(SurviveObject *so) if (offset_from < 380000 && offset_from > 70000) { - if (longest_pulse *10 / 8 < lcd->sweep.sweep_len[i]) + //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); } -- cgit v1.2.3 From a576703242adea11c012af5afdff38af84d22e2e Mon Sep 17 00:00:00 2001 From: mwturvey Date: Fri, 24 Mar 2017 13:44:38 -0700 Subject: Adaptive Acode Offset Compensation --- src/survive_data.c | 114 +++++++++++++++++++++++++++++++---------------------- 1 file changed, 66 insertions(+), 48 deletions(-) (limited to 'src/survive_data.c') 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)); } -- cgit v1.2.3 From 4dc1d72785c660c206f8def9d8c8aa32289c2709 Mon Sep 17 00:00:00 2001 From: mwturvey Date: Fri, 24 Mar 2017 15:19:59 -0700 Subject: More cleanup & finishing genericization of calibrator --- src/survive_data.c | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) (limited to 'src/survive_data.c') diff --git a/src/survive_data.c b/src/survive_data.c index 4e2479a..9447104 100644 --- a/src/survive_data.c +++ b/src/survive_data.c @@ -25,7 +25,7 @@ typedef struct typedef struct { - float acode_offset; + double acode_offset; } lightcap2_global_data; typedef struct @@ -40,11 +40,11 @@ typedef struct int handle_lightcap2_getAcodeFromSyncPulse(SurviveObject * so, int pulseLen) { - float oldOffset = ((lightcap2_data*)so->disambiguator_data)->global.acode_offset; + double oldOffset = ((lightcap2_data*)so->disambiguator_data)->global.acode_offset; int modifiedPulseLen = pulseLen - (int)oldOffset; - float newOffset = (((pulseLen) + 250) % 500) - 250; + double newOffset = (((pulseLen) + 250) % 500) - 250; ((lightcap2_data*)so->disambiguator_data)->global.acode_offset = oldOffset * 0.9 + newOffset * 0.1; -- cgit v1.2.3 From b3bdcdb838ed57986f359b2181a624bfd1acbbf1 Mon Sep 17 00:00:00 2001 From: mwturvey Date: Tue, 28 Mar 2017 10:15:36 -0700 Subject: Fix Tracker IMU --- src/survive_data.c | 10 ++++++++-- 1 file changed, 8 insertions(+), 2 deletions(-) (limited to 'src/survive_data.c') diff --git a/src/survive_data.c b/src/survive_data.c index 9447104..3eb5890 100644 --- a/src/survive_data.c +++ b/src/survive_data.c @@ -154,8 +154,14 @@ void handle_lightcap2_sync(SurviveObject * so, LightcapElement * le ) { 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"); + static int pulseWarningCount=0; + + if (pulseWarningCount < 5) + { + pulseWarningCount++; + // 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->per_sweep.activeLighthouse = 1; lcd->per_sweep.activeSweepStartTime = le->timestamp; -- cgit v1.2.3 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 From b6787c60171724360f097a16cc7ec967995ff335 Mon Sep 17 00:00:00 2001 From: cnlohr Date: Fri, 7 Apr 2017 00:55:21 -0400 Subject: Update, fixing charles' disambiguator, adding flag for TURVEYBIGUATOR! --- src/survive_data.c | 21 +++++++++++++++------ 1 file changed, 15 insertions(+), 6 deletions(-) (limited to 'src/survive_data.c') diff --git a/src/survive_data.c b/src/survive_data.c index 3f16c7c..4bb1526 100644 --- a/src/survive_data.c +++ b/src/survive_data.c @@ -5,6 +5,10 @@ #include #include +#define USE_TURVEYBIGUATOR + +#ifdef USE_TURVEYBIGUATOR + typedef struct { unsigned int sweep_time[SENSORS_PER_OBJECT]; @@ -311,6 +315,9 @@ void handle_lightcap2( SurviveObject * so, LightcapElement * le ) } + +#endif + int32_t decode_acode(uint32_t length, int32_t main_divisor) { //+50 adds a small offset and seems to help always get it right. //Check the +50 in the future to see how well this works on a variety of hardware. @@ -325,9 +332,13 @@ int32_t decode_acode(uint32_t length, int32_t main_divisor) { void handle_lightcap( SurviveObject * so, LightcapElement * le ) { SurviveContext * ctx = so->ctx; + +#ifdef USE_TURVEYBIGUATOR handle_lightcap2(so,le); return; +#else + //int32_t deltat = (uint32_t)le->timestamp - (uint32_t)so->last_master_time; if( le->sensor_id > SENSORS_PER_OBJECT ) @@ -377,8 +388,6 @@ void handle_lightcap( SurviveObject * so, LightcapElement * le ) { int is_new_pulse = delta > so->pulselength_min_sync /*1500*/ + last_sync_length; - - so->did_handle_ootx = 0; if( is_new_pulse ) @@ -398,6 +407,7 @@ void handle_lightcap( SurviveObject * so, LightcapElement * le ) else { ssn = ++so->sync_set_number; + if( so->sync_set_number >= NUM_LIGHTHOUSES ) { SV_INFO( "Warning. Received an extra, unassociated sync pulse." ); @@ -432,8 +442,6 @@ void handle_lightcap( SurviveObject * so, LightcapElement * le ) } } - - //See if this is a valid actual pulse. else if( le->length < so->pulse_max_for_sweep && delta > so->pulse_in_clear_time && ssn >= 0 ) { @@ -496,7 +504,6 @@ void handle_lightcap( SurviveObject * so, LightcapElement * le ) so->did_handle_ootx = 1; } - if (acode > 3) { if( ssn == 0 ) { @@ -512,7 +519,7 @@ void handle_lightcap( SurviveObject * so, LightcapElement * le ) //Make sure pulse is in valid window if( offset_from < 380000 && offset_from > 70000 ) { - ctx->lightproc( so, le->sensor_id, acode, offset_from, le->timestamp, le->length, so->sync_set_number ); + ctx->lightproc( so, le->sensor_id, acode, offset_from, le->timestamp, le->length, !(acode>>2) ); } } else @@ -520,6 +527,8 @@ void handle_lightcap( SurviveObject * so, LightcapElement * le ) //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 + } -- cgit v1.2.3 From 454bb09f5ac0a72149c82a2231a9460e06657dea Mon Sep 17 00:00:00 2001 From: Joshua Allen Date: Sat, 8 Apr 2017 10:32:59 -0400 Subject: Delay picking active lighthouse until the sweep pass so we can be sure we have the longest synce pulse time and acode. HMD reads both lighthouses. --- src/survive_data.c | 80 ++++++++++++++++++++++++++++++++++++++++++------------ 1 file changed, 62 insertions(+), 18 deletions(-) (limited to 'src/survive_data.c') diff --git a/src/survive_data.c b/src/survive_data.c index 4bb1526..f61ae8b 100644 --- a/src/survive_data.c +++ b/src/survive_data.c @@ -21,8 +21,10 @@ typedef struct int activeSweepStartTime; int activeAcode; - int lh_pulse_len[NUM_LIGHTHOUSES]; +// int lh_pulse_len[NUM_LIGHTHOUSES]; int lh_start_time[NUM_LIGHTHOUSES]; + int lh_max_pulse_length[NUM_LIGHTHOUSES]; + uint16_t lh_acode[NUM_LIGHTHOUSES]; int current_lh; // used knowing which sync pulse we're looking at. } lightcap2_per_sweep_data; @@ -145,25 +147,34 @@ 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); + int acode = handle_lightcap2_getAcodeFromSyncPulse(so, le->length); //acode for this sensor reading // Process any sweep data we have handle_lightcap2_process_sweep_data(so); 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); + + 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->per_sweep.recent_sync_time = le->timestamp; // it's the same sync pulse; - so->sync_set_number = 1; +// so->sync_set_number = 1; so->recent_sync_time = 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; +// 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; + if (le->length > lcd->per_sweep.lh_max_pulse_length[lcd->per_sweep.current_lh]) { + lcd->per_sweep.lh_max_pulse_length[lcd->per_sweep.current_lh] = le->length; + lcd->per_sweep.lh_start_time[lcd->per_sweep.current_lh] = le->timestamp; + lcd->per_sweep.lh_acode[lcd->per_sweep.current_lh] = acode; + } + +/* + //this stuff should probably be happening on the sweep so that we can filter out erroneous a codes if (!(acode >> 2 & 1)) // if the skip bit is not set { lcd->per_sweep.activeLighthouse = lcd->per_sweep.current_lh; @@ -172,19 +183,26 @@ void handle_lightcap2_sync(SurviveObject * so, LightcapElement * le ) } else { + //this causes the master lighthouse to be ignored from the HMD lcd->per_sweep.activeLighthouse = -1; lcd->per_sweep.activeSweepStartTime = 0; lcd->per_sweep.activeAcode = 0; } + */ } else if (time_since_last_sync < 24000) { + lcd->per_sweep.activeLighthouse != -1; + lcd->per_sweep.recent_sync_time = le->timestamp; // I do believe we are lighthouse B lcd->per_sweep.current_lh = 1; - lcd->per_sweep.lh_pulse_len[lcd->per_sweep.current_lh] = le->length; +// 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; + lcd->per_sweep.lh_max_pulse_length[lcd->per_sweep.current_lh] = le->length; + lcd->per_sweep.lh_acode[lcd->per_sweep.current_lh] = acode; +/* if (!(acode >> 2 & 1)) // if the skip bit is not set { if (lcd->per_sweep.activeLighthouse != -1) @@ -198,10 +216,12 @@ void handle_lightcap2_sync(SurviveObject * so, LightcapElement * le ) fprintf(stderr, "WARNING: Two non-skip pulses received on the same cycle!\n"); } } + lcd->per_sweep.activeLighthouse = 1; lcd->per_sweep.activeSweepStartTime = le->timestamp; lcd->per_sweep.activeAcode = acode; } + */ } else if (time_since_last_sync > 370000) @@ -210,24 +230,24 @@ void handle_lightcap2_sync(SurviveObject * so, LightcapElement * le ) // first, send out the sync pulse data for the last round (for OOTX decoding { - if (lcd->per_sweep.lh_pulse_len[0] != 0) + if (lcd->per_sweep.lh_max_pulse_length[0] != 0) { so->ctx->lightproc( so, -1, - handle_lightcap2_getAcodeFromSyncPulse(so, lcd->per_sweep.lh_pulse_len[0]), - lcd->per_sweep.lh_pulse_len[0], + handle_lightcap2_getAcodeFromSyncPulse(so, lcd->per_sweep.lh_max_pulse_length[0]), + lcd->per_sweep.lh_max_pulse_length[0], lcd->per_sweep.lh_start_time[0], 0, 0); } - if (lcd->per_sweep.lh_pulse_len[1] != 0) + if (lcd->per_sweep.lh_max_pulse_length[1] != 0) { so->ctx->lightproc( so, -2, - handle_lightcap2_getAcodeFromSyncPulse(so, lcd->per_sweep.lh_pulse_len[1]), - lcd->per_sweep.lh_pulse_len[1], + handle_lightcap2_getAcodeFromSyncPulse(so, lcd->per_sweep.lh_max_pulse_length[1]), + lcd->per_sweep.lh_max_pulse_length[1], lcd->per_sweep.lh_start_time[1], 0, 1); @@ -239,23 +259,27 @@ void handle_lightcap2_sync(SurviveObject * so, LightcapElement * le ) memset(&lcd->per_sweep, 0, sizeof(lcd->per_sweep)); lcd->per_sweep.activeLighthouse = -1; - - lcd->per_sweep.recent_sync_time = le->timestamp; // I do believe we are lighthouse A lcd->per_sweep.current_lh = 0; - lcd->per_sweep.lh_pulse_len[lcd->per_sweep.current_lh] = le->length; +// 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; + lcd->per_sweep.lh_max_pulse_length[lcd->per_sweep.current_lh] = le->length; + lcd->per_sweep.lh_acode[lcd->per_sweep.current_lh] = acode; - int acode = handle_lightcap2_getAcodeFromSyncPulse(so, le->length); +// int acode = handle_lightcap2_getAcodeFromSyncPulse(so, le->length); +/* if (!(acode >> 2 & 1)) // if the skip bit is not set { lcd->per_sweep.activeLighthouse = 0; lcd->per_sweep.activeSweepStartTime = le->timestamp; lcd->per_sweep.activeAcode = acode; } + */ } + +// printf("%d %d\n", acode, lcd->per_sweep.activeLighthouse ); } void handle_lightcap2_sweep(SurviveObject * so, LightcapElement * le ) @@ -275,6 +299,25 @@ void handle_lightcap2_sweep(SurviveObject * so, LightcapElement * le ) //fprintf(stderr, "%2d %d\n", le->sensor_id, le->length); //fprintf(stderr, "."); + lcd->per_sweep.activeLighthouse = -1; + lcd->per_sweep.activeSweepStartTime = 0; + lcd->per_sweep.activeAcode = 0; + + for (uint8_t i=0; i< NUM_LIGHTHOUSES;++i) { + int acode = lcd->per_sweep.lh_acode[i]; + if ((acode>0) && (!(acode >> 2 & 1))) { + lcd->per_sweep.activeLighthouse = i; + lcd->per_sweep.activeSweepStartTime = lcd->per_sweep.lh_start_time[i]; + lcd->per_sweep.activeAcode = acode; + } + } + + if (lcd->per_sweep.activeLighthouse < 0) { + fprintf(stderr, "WARNING: No active lighthouse!\n"); + fprintf(stderr, " %2d %8d\n", le->sensor_id, le->length); + return; + } + if (lcd->sweep.sweep_len[le->sensor_id] < le->length) { lcd->sweep.sweep_len[le->sensor_id] = le->length; @@ -303,7 +346,8 @@ void handle_lightcap2( SurviveObject * so, LightcapElement * le ) // Should never get a reading so high. Odd. return; } - if (le->length >= 2750) +// if (le->length >= 2750) + if (le->length >= 2500) { // Looks like a sync pulse, process it! handle_lightcap2_sync(so, le); -- cgit v1.2.3 From 96cec93d3c7461587673b5dc35c6fba1e9db899f Mon Sep 17 00:00:00 2001 From: Joshua Allen Date: Sat, 8 Apr 2017 13:36:42 -0400 Subject: clear acode at beginning of pulses. only process valid acodes sweeps. makes HMD work again with single or double lighthouses. --- src/survive_data.c | 18 +++++++++++++----- 1 file changed, 13 insertions(+), 5 deletions(-) (limited to 'src/survive_data.c') diff --git a/src/survive_data.c b/src/survive_data.c index f61ae8b..989eb59 100644 --- a/src/survive_data.c +++ b/src/survive_data.c @@ -24,7 +24,7 @@ typedef struct // int lh_pulse_len[NUM_LIGHTHOUSES]; int lh_start_time[NUM_LIGHTHOUSES]; int lh_max_pulse_length[NUM_LIGHTHOUSES]; - uint16_t lh_acode[NUM_LIGHTHOUSES]; + int8_t lh_acode[NUM_LIGHTHOUSES]; int current_lh; // used knowing which sync pulse we're looking at. } lightcap2_per_sweep_data; @@ -96,7 +96,8 @@ void handle_lightcap2_process_sweep_data(SurviveObject *so) { static int counts[SENSORS_PER_OBJECT][2] = {0}; - if (lcd->per_sweep.activeLighthouse == 0 && !allZero) +// if (lcd->per_sweep.activeLighthouse == 0 && !allZero) + if (lcd->per_sweep.activeLighthouse > -1 && !allZero) { if (lcd->sweep.sweep_len[i] != 0) { @@ -226,6 +227,9 @@ void handle_lightcap2_sync(SurviveObject * so, LightcapElement * le ) } else if (time_since_last_sync > 370000) { + // XXX CAUTION: if we lose sight of a lighthouse then, the remaining lighthouse will default to master + //this should probably be fixed. Maybe some kind of timing based guess at which lighthouse. + // looks like this is the first sync pulse. Cool! // first, send out the sync pulse data for the last round (for OOTX decoding @@ -259,6 +263,10 @@ void handle_lightcap2_sync(SurviveObject * so, LightcapElement * le ) memset(&lcd->per_sweep, 0, sizeof(lcd->per_sweep)); lcd->per_sweep.activeLighthouse = -1; + for (uint8_t i=0; i < NUM_LIGHTHOUSES;++i) { + lcd->per_sweep.lh_acode[i] = -1; + } + lcd->per_sweep.recent_sync_time = le->timestamp; // I do believe we are lighthouse A lcd->per_sweep.current_lh = 0; @@ -303,9 +311,9 @@ void handle_lightcap2_sweep(SurviveObject * so, LightcapElement * le ) lcd->per_sweep.activeSweepStartTime = 0; lcd->per_sweep.activeAcode = 0; - for (uint8_t i=0; i< NUM_LIGHTHOUSES;++i) { + for (uint8_t i=0; i < NUM_LIGHTHOUSES;++i) { int acode = lcd->per_sweep.lh_acode[i]; - if ((acode>0) && (!(acode >> 2 & 1))) { + if ( (acode>=0) && !(acode >> 2 & 1)) { lcd->per_sweep.activeLighthouse = i; lcd->per_sweep.activeSweepStartTime = lcd->per_sweep.lh_start_time[i]; lcd->per_sweep.activeAcode = acode; @@ -314,7 +322,7 @@ void handle_lightcap2_sweep(SurviveObject * so, LightcapElement * le ) if (lcd->per_sweep.activeLighthouse < 0) { fprintf(stderr, "WARNING: No active lighthouse!\n"); - fprintf(stderr, " %2d %8d\n", le->sensor_id, le->length); + fprintf(stderr, " %2d %8d %d %d\n", le->sensor_id, le->length,lcd->per_sweep.lh_acode[0],lcd->per_sweep.lh_acode[1]); return; } -- cgit v1.2.3 From e2d553191e8693bb8937aac8b991d9c09e0df6cc Mon Sep 17 00:00:00 2001 From: Joshua Allen Date: Sat, 8 Apr 2017 17:42:28 -0400 Subject: try to remove outliers --- src/survive_data.c | 63 +++++++++++++++++++++++++++++++++++++++++++++++++++--- 1 file changed, 60 insertions(+), 3 deletions(-) (limited to 'src/survive_data.c') diff --git a/src/survive_data.c b/src/survive_data.c index 989eb59..647a5d7 100644 --- a/src/survive_data.c +++ b/src/survive_data.c @@ -65,10 +65,64 @@ int handle_lightcap2_getAcodeFromSyncPulse(SurviveObject * so, int pulseLen) if (pulseLen < 6250 - ACODE_OFFSET) return 6; return 7; } + +uint8_t remove_outliers(SurviveObject *so) { + lightcap2_data *lcd = so->disambiguator_data; + + uint32_t sum = 0; + uint8_t non_zero_count = 0; + uint32_t mean = 0; + + for (int i = 0; i < SENSORS_PER_OBJECT; i++) + { + if (lcd->sweep.sweep_len[i] > 0) { + sum += lcd->sweep.sweep_len[i]; + ++non_zero_count; + } + } + + if (non_zero_count==0) return 0; + + mean = sum/non_zero_count; + + float standard_deviation = 0.0f; + sum = 0; + for (int i = 0; i < SENSORS_PER_OBJECT; i++) + { + uint16_t len = lcd->sweep.sweep_len[i]; + if (len > 0) { + sum += (len - mean)*(len - mean); + } + } + standard_deviation = sqrt( ((float)sum)/((float)non_zero_count) ); + +// printf("%f\n", standard_deviation); + + float fake_tao_test = standard_deviation*2; + uint8_t removed_outliers = 0; + + for (int i = 0; i < SENSORS_PER_OBJECT; i++) + { + uint16_t len = lcd->sweep.sweep_len[i]; + if (len == 0) continue; + + if ( abs(len-mean) > fake_tao_test ) + { +// fprintf(stderr, "removing %d\n", len); + lcd->sweep.sweep_len[i] = 0; + removed_outliers = 1; + } + } + + return removed_outliers; +} + void handle_lightcap2_process_sweep_data(SurviveObject *so) { lightcap2_data *lcd = so->disambiguator_data; + while(remove_outliers(so)); + // look at all of the sensors we found, and process the ones that were hit. // TODO: find the sensor(s) with the longest pulse length, and assume // those are the "highest quality". Then, reject any pulses that are sufficiently @@ -93,6 +147,7 @@ void handle_lightcap2_process_sweep_data(SurviveObject *so) // 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}; @@ -119,10 +174,10 @@ void handle_lightcap2_process_sweep_data(SurviveObject *so) if (lcd->sweep.sweep_len[i] != 0) // if the sensor was hit, process it { - +//printf("%4d\n", lcd->sweep.sweep_len[i]); 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 (offset_from < 380000 && offset_from > 70000) { //if (longest_pulse *10 / 8 < lcd->sweep.sweep_len[i]) { @@ -134,7 +189,9 @@ void handle_lightcap2_process_sweep_data(SurviveObject *so) //if (!allZero) // printf(" ..:..\n"); +// 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. memset(&(((lightcap2_data*)so->disambiguator_data)->sweep), 0, sizeof(lightcaps_sweep_data)); @@ -156,7 +213,7 @@ void handle_lightcap2_sync(SurviveObject * so, LightcapElement * le ) 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); +// 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) { -- cgit v1.2.3 From 7686ad97d5f2542338f88e6f6e8055953609651c Mon Sep 17 00:00:00 2001 From: Josh Allen Date: Sun, 9 Apr 2017 08:38:40 -0400 Subject: untested tau test --- src/survive_data.c | 68 ++++++++++++++++++++++++++++++++++++++++++------------ 1 file changed, 53 insertions(+), 15 deletions(-) (limited to 'src/survive_data.c') diff --git a/src/survive_data.c b/src/survive_data.c index 647a5d7..8d115fd 100644 --- a/src/survive_data.c +++ b/src/survive_data.c @@ -4,16 +4,25 @@ #include "survive_internal.h" #include #include +#include #define USE_TURVEYBIGUATOR #ifdef USE_TURVEYBIGUATOR +static const float tau_table[33] = { 0, 0, 0, 1.151140982, 1.425, 1.5712213707, 1.656266074, 1.7110275587, 1.7490784054, + 1.7770229476, 1.798410005, 1.8153056661, 1.8289916275, 1.8403044103, 1.8498129961, 1.8579178211, + 1.864908883, 1.8710013691, 1.8763583296, 1.881105575, 1.885341741, 1.8891452542, 1.8925792599, + 1.8956951735, 1.8985352854, 1.9011347009, 1.9035228046, 1.9057243816, 1.9077604832, 1.9096491058, + 1.9114057255, 1.9130437248, 1.914574735 +}; + typedef struct { unsigned int sweep_time[SENSORS_PER_OBJECT]; - unsigned int sweep_len[SENSORS_PER_OBJECT]; + uint16_t sweep_len[SENSORS_PER_OBJECT]; //might want to align this to cache lines, will be hot for frequent access } lightcaps_sweep_data; + typedef struct { int recent_sync_time; @@ -73,12 +82,16 @@ uint8_t remove_outliers(SurviveObject *so) { uint8_t non_zero_count = 0; uint32_t mean = 0; - for (int i = 0; i < SENSORS_PER_OBJECT; i++) + uint16_t* min = NULL; + uint16_t* max = NULL; + uint8_t found_first = 0; + + //future: https://gcc.gnu.org/projects/tree-ssa/vectorization.html#vectorizab + + for (uint8_t i = 0; i < SENSORS_PER_OBJECT; i++) { - if (lcd->sweep.sweep_len[i] > 0) { - sum += lcd->sweep.sweep_len[i]; - ++non_zero_count; - } + sum += lcd->sweep.sweep_len[i]; + if (lcd->sweep.sweep_len[i] > 0) ++non_zero_count; } if (non_zero_count==0) return 0; @@ -87,34 +100,59 @@ uint8_t remove_outliers(SurviveObject *so) { float standard_deviation = 0.0f; sum = 0; - for (int i = 0; i < SENSORS_PER_OBJECT; i++) + for (uint8_t i = 0; i < SENSORS_PER_OBJECT; i++) { uint16_t len = lcd->sweep.sweep_len[i]; if (len > 0) { sum += (len - mean)*(len - mean); + + if (found_first==0) { + max = min = lcd->sweep.sweep_len + i; + found_first=1; + } else { + if(lcd->sweep.sweep_len[i] < *min) min=lcd->sweep.sweep_len + i; + if(lcd->sweep.sweep_len[i] > *max) max=lcd->sweep.sweep_len + i; + } } } - standard_deviation = sqrt( ((float)sum)/((float)non_zero_count) ); + standard_deviation = sqrtf( ((float)sum)/((float)non_zero_count) ); // printf("%f\n", standard_deviation); - float fake_tao_test = standard_deviation*2; - uint8_t removed_outliers = 0; + float tau_test = standard_deviation; + + if (non_zero_count > 2) tau_test = standard_deviation*tau_table[non_zero_count]; - for (int i = 0; i < SENSORS_PER_OBJECT; i++) +// uint8_t removed_outliers = 0; + + uint32_t d1 = *min - mean; + uint32_t d2 = *max - mean; + + if (d1>d2 && d1>tau_test) { + *min = 0; + return 1; + } + else if (d2>tau_test) { + *max = 0; + return 1; + } + + return 0; +/* + for (uint8_t i = 0; i < SENSORS_PER_OBJECT; i++) { uint16_t len = lcd->sweep.sweep_len[i]; if (len == 0) continue; - if ( abs(len-mean) > fake_tao_test ) + if ( abs(len-mean) > tau_test ) { // fprintf(stderr, "removing %d\n", len); lcd->sweep.sweep_len[i] = 0; removed_outliers = 1; } } - - return removed_outliers; +*/ +// return removed_outliers; } void handle_lightcap2_process_sweep_data(SurviveObject *so) @@ -250,7 +288,7 @@ void handle_lightcap2_sync(SurviveObject * so, LightcapElement * le ) } else if (time_since_last_sync < 24000) { - lcd->per_sweep.activeLighthouse != -1; + lcd->per_sweep.activeLighthouse = -1; lcd->per_sweep.recent_sync_time = le->timestamp; // I do believe we are lighthouse B -- cgit v1.2.3 From 8dee94008fec1506a3a2f58292a45e1f93b6d333 Mon Sep 17 00:00:00 2001 From: Josh Allen Date: Sun, 9 Apr 2017 08:51:29 -0400 Subject: should be absolute value if statement should not be combined --- src/survive_data.c | 12 +++++++----- 1 file changed, 7 insertions(+), 5 deletions(-) (limited to 'src/survive_data.c') diff --git a/src/survive_data.c b/src/survive_data.c index 8d115fd..22ce8c2 100644 --- a/src/survive_data.c +++ b/src/survive_data.c @@ -125,12 +125,14 @@ uint8_t remove_outliers(SurviveObject *so) { // uint8_t removed_outliers = 0; - uint32_t d1 = *min - mean; - uint32_t d2 = *max - mean; + uint32_t d1 = abs(*min - mean); + uint32_t d2 = abs(*max - mean); - if (d1>d2 && d1>tau_test) { - *min = 0; - return 1; + if (d1>d2) { + if (d1 > tau_test) { + *min = 0; + return 1; + } } else if (d2>tau_test) { *max = 0; -- cgit v1.2.3 From 4b842a1a73a2713726c3d999ab4b6886e7e9fb69 Mon Sep 17 00:00:00 2001 From: Mike Turvey Date: Tue, 11 Apr 2017 17:42:04 -0700 Subject: Disabling remove_outliers because it's removing too many non-outliers --- src/survive_data.c | 1 + 1 file changed, 1 insertion(+) (limited to 'src/survive_data.c') diff --git a/src/survive_data.c b/src/survive_data.c index 647a5d7..0273532 100644 --- a/src/survive_data.c +++ b/src/survive_data.c @@ -67,6 +67,7 @@ int handle_lightcap2_getAcodeFromSyncPulse(SurviveObject * so, int pulseLen) } uint8_t remove_outliers(SurviveObject *so) { + return 0; // disabling this for now because it seems remove almost all the points for wired watchman and wired tracker. lightcap2_data *lcd = so->disambiguator_data; uint32_t sum = 0; -- cgit v1.2.3 From a7d4ccbde10be9c14ae690d3c9a21627195cfaa5 Mon Sep 17 00:00:00 2001 From: "Dr. Orion Lawlor" Date: Mon, 17 Apr 2017 21:42:05 -0800 Subject: Fix warnings by including --- src/survive_data.c | 1 + 1 file changed, 1 insertion(+) (limited to 'src/survive_data.c') diff --git a/src/survive_data.c b/src/survive_data.c index 0273532..7b9d051 100644 --- a/src/survive_data.c +++ b/src/survive_data.c @@ -4,6 +4,7 @@ #include "survive_internal.h" #include #include +#include /* for sqrt */ #define USE_TURVEYBIGUATOR -- cgit v1.2.3 From f177f87e98637de8f07488d820390f322c45413c Mon Sep 17 00:00:00 2001 From: cnlohr Date: Sun, 30 Apr 2017 00:13:42 -0400 Subject: Update the charles disambiguator --- src/survive_data.c | 53 ++++++++++++++++++++++++++++++++++++++--------------- 1 file changed, 38 insertions(+), 15 deletions(-) (limited to 'src/survive_data.c') diff --git a/src/survive_data.c b/src/survive_data.c index 7b9d051..0e2e828 100644 --- a/src/survive_data.c +++ b/src/survive_data.c @@ -6,7 +6,7 @@ #include #include /* for sqrt */ -#define USE_TURVEYBIGUATOR +//#define USE_TURVEYBIGUATOR #ifdef USE_TURVEYBIGUATOR @@ -449,6 +449,7 @@ void handle_lightcap( SurviveObject * so, LightcapElement * le ) return; #else + //printf( "LE%3d%6d%12d\n", le->sensor_id, le->length, le->timestamp ); //int32_t deltat = (uint32_t)le->timestamp - (uint32_t)so->last_master_time; @@ -482,6 +483,9 @@ void handle_lightcap( SurviveObject * so, LightcapElement * le ) //unified driver. int ssn = so->sync_set_number; //lighthouse number if( ssn < 0 ) ssn = 0; +#ifdef DEBUG + if( ssn >= NUM_LIGHTHOUSES ) { SV_INFO( "ALGORITHMIC WARNING: ssn exceeds NUM_LIGHTHOUSES" ); } +#endif int last_sync_time = so->last_sync_time [ssn]; int last_sync_length = so->last_sync_length[ssn]; int32_t delta = le->timestamp - last_sync_time; //Handle time wrapping (be sure to be int32) @@ -498,16 +502,29 @@ void handle_lightcap( SurviveObject * so, LightcapElement * le ) if( le->length > so->pulselength_min_sync ) //Pulse longer indicates a sync pulse. { int is_new_pulse = delta > so->pulselength_min_sync /*1500*/ + last_sync_length; - + //printf( "INP: %d %d\n", is_new_pulse, so->sync_set_number ); so->did_handle_ootx = 0; if( is_new_pulse ) { int is_master_sync_pulse = delta > so->pulse_in_clear_time /*40000*/; + int is_pulse_from_same_lh_as_last_sweep; + int tp = delta % ( so->timecenter_ticks * 2); + is_pulse_from_same_lh_as_last_sweep = tp < so->pulse_synctime_slack && tp > -so->pulse_synctime_slack; - if( is_master_sync_pulse ) + if( is_master_sync_pulse ) //Could also be called by slave if no master was seen. { - ssn = so->sync_set_number = 0; + ssn = so->sync_set_number = is_pulse_from_same_lh_as_last_sweep?(so->sync_set_number):0; //If repeated lighthouse, just back off one. + if( ssn < 0 ) { SV_INFO( "SEVERE WARNING: Pulse codes for tracking not able to be backed out.\n" ); ssn = 0; } + if( ssn != 0 ) + { + //If it's the slave that is repeated, be sure to zero out its sync info. + so->last_sync_length[0] = 0; + } + else + { + so->last_sync_length[1] = 0; + } so->last_sync_time[ssn] = le->timestamp; so->last_sync_length[ssn] = le->length; } @@ -544,15 +561,19 @@ void handle_lightcap( SurviveObject * so, LightcapElement * le ) } } - //Extra tidbit for storing length-of-sync-pulses. +#if 0 + //Extra tidbit for storing length-of-sync-pulses, if you want to try to use this to determine AoI or distance to LH. { int32_t main_divisor = so->timebase_hz / 384000; //125 @ 48 MHz. int base_station = is_new_pulse; - //printf( "%s %d %d %d\n", so->codename, le->sensor_id, so->sync_set_number, le->length ); - ctx->lightproc( so, le->sensor_id, -3 - so->sync_set_number, 0, le->timestamp, le->length, base_station); + printf( "%s %d %d %d\n", so->codename, le->sensor_id, so->sync_set_number, le->length ); //XXX sync_set_number is wrong here. + ctx->lightproc( so, le->sensor_id, -3 - so->sync_set_number, 0, le->timestamp, le->length, base_station); //XXX sync_set_number is wrong here. } +#endif } + //Any else- statements below here are + //See if this is a valid actual pulse. else if( le->length < so->pulse_max_for_sweep && delta > so->pulse_in_clear_time && ssn >= 0 ) { @@ -576,9 +597,11 @@ void handle_lightcap( SurviveObject * so, LightcapElement * le ) decode_acode(so->last_sync_length[1],main_divisor) }; + //printf( "%d %d\n", acode_array[0], acode_array[1] ); + //XXX: TODO: Capture error count here. - if( acode_array[0] < 0 ) return; - if( acode_array[1] < 0 ) return; + //if( acode_array[0] < 0 ) return; + //if( acode_array[1] < 0 ) return; int acode = acode_array[0]; @@ -587,13 +610,14 @@ void handle_lightcap( SurviveObject * so, LightcapElement * le ) int32_t delta1 = so->last_sync_time[0] - so->recent_sync_time; int32_t delta2 = so->last_sync_time[1] - so->last_sync_time[0]; - ctx->lightproc( so, -1, acode_array[0], delta1, so->last_sync_time[0], so->last_sync_length[0], 0 ); - ctx->lightproc( so, -2, acode_array[1], delta2, so->last_sync_time[1], so->last_sync_length[1], 1 ); + //printf( "%p %p %d %d %d %p\n", ctx, so, so->last_sync_time[0], acode_array, so->last_sync_length[0], ctx->lightproc ); + if( acode_array[0] >= 0 ) ctx->lightproc( so, -1, acode_array[0], delta1, so->last_sync_time[0], so->last_sync_length[0], 0 ); + if( acode_array[1] >= 0 ) ctx->lightproc( so, -2, acode_array[1], delta2, so->last_sync_time[1], so->last_sync_length[1], 1 ); so->recent_sync_time = so->last_sync_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; @@ -611,7 +635,7 @@ void handle_lightcap( SurviveObject * so, LightcapElement * le ) so->sync_set_number = -1; return; } - +*/ so->did_handle_ootx = 1; } @@ -628,7 +652,7 @@ void handle_lightcap( SurviveObject * so, LightcapElement * le ) int32_t offset_from = le->timestamp - dl + le->length/2; //Make sure pulse is in valid window - if( offset_from < 380000 && offset_from > 70000 ) + if( offset_from < so->timecenter_ticks*2-so->pulse_in_clear_time && offset_from > so->pulse_in_clear_time && acode >= 0 ) { ctx->lightproc( so, le->sensor_id, acode, offset_from, le->timestamp, le->length, !(acode>>2) ); } @@ -639,7 +663,6 @@ void handle_lightcap( SurviveObject * so, LightcapElement * le ) //Runt pulse, or no sync pulses available. } #endif - } -- cgit v1.2.3 From 947dfaff73c0b758a21a410776543340e732d705 Mon Sep 17 00:00:00 2001 From: cnlohr Date: Sun, 30 Apr 2017 00:46:57 -0400 Subject: Fix charles' disambiguator to work with one lighthouse and @axlecrusher's OOTX reader. --- src/survive_data.c | 125 ++++++++++++++++++++++++++++------------------------- 1 file changed, 65 insertions(+), 60 deletions(-) (limited to 'src/survive_data.c') diff --git a/src/survive_data.c b/src/survive_data.c index 0e2e828..4f94106 100644 --- a/src/survive_data.c +++ b/src/survive_data.c @@ -432,13 +432,59 @@ void handle_lightcap2( SurviveObject * so, LightcapElement * le ) int32_t decode_acode(uint32_t length, int32_t main_divisor) { //+50 adds a small offset and seems to help always get it right. //Check the +50 in the future to see how well this works on a variety of hardware. - + if( !main_divisor ) return -1; int32_t acode = (length+main_divisor+50)/(main_divisor*2); if( acode & 1 ) return -1; return (acode>>1) - 6; } + +void HandleOOTX( SurviveContext * ctx, SurviveObject * so ) +{ + int32_t main_divisor = so->timebase_hz / 384000; //125 @ 48 MHz. + + int32_t acode_array[2] = + { + decode_acode(so->last_sync_length[0],main_divisor), + decode_acode(so->last_sync_length[1],main_divisor) + }; + + + int32_t delta1 = so->last_sync_time[0] - so->recent_sync_time; + int32_t delta2 = so->last_sync_time[1] - so->last_sync_time[0]; + + //printf( "%p %p %d %d %d %p\n", ctx, so, so->last_sync_time[0], acode_array, so->last_sync_length[0], ctx->lightproc ); + if( acode_array[0] >= 0 ) ctx->lightproc( so, -1, acode_array[0], delta1, so->last_sync_time[0], so->last_sync_length[0], 0 ); + if( acode_array[1] >= 0 ) ctx->lightproc( so, -2, acode_array[1], delta2, so->last_sync_time[1], so->last_sync_length[1], 1 ); + + so->recent_sync_time = so->last_sync_time[1]; + +/* + //Throw out everything if our sync pulses look like they're bad. + //This actually doesn't seem to hold anymore, now that we're looking for multiple LHs. + 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; +} + + //This is the disambiguator function, for taking light timing and figuring out place-in-sweep for a given photodiode. void handle_lightcap( SurviveObject * so, LightcapElement * le ) { @@ -502,9 +548,18 @@ void handle_lightcap( SurviveObject * so, LightcapElement * le ) if( le->length > so->pulselength_min_sync ) //Pulse longer indicates a sync pulse. { int is_new_pulse = delta > so->pulselength_min_sync /*1500*/ + last_sync_length; - //printf( "INP: %d %d\n", is_new_pulse, so->sync_set_number ); + + //TRICKY: If we didn't see anything from the other lighthouse, we might just not see it... But, we still have to send our sync + //information to the rest of libsurvive. This could be turned into a function and combined with the code below. + if( !so->did_handle_ootx && is_new_pulse ) + { + HandleOOTX( ctx, so ); + } so->did_handle_ootx = 0; + + //printf( "INP: %d %d\n", is_new_pulse, so->sync_set_number ); + if( is_new_pulse ) { int is_master_sync_pulse = delta > so->pulse_in_clear_time /*40000*/; @@ -563,6 +618,7 @@ void handle_lightcap( SurviveObject * so, LightcapElement * le ) #if 0 //Extra tidbit for storing length-of-sync-pulses, if you want to try to use this to determine AoI or distance to LH. + //We don't actually use this anywhere, and I doubt we ever will? Though, it could be useful at a later time to improve tracking. { int32_t main_divisor = so->timebase_hz / 384000; //125 @ 48 MHz. int base_station = is_new_pulse; @@ -590,71 +646,20 @@ void handle_lightcap( SurviveObject * so, LightcapElement * le ) //Make sure it fits nicely into a divisible-by-500 time. int32_t main_divisor = so->timebase_hz / 384000; //125 @ 48 MHz. - - int32_t acode_array[2] = - { - decode_acode(so->last_sync_length[0],main_divisor), - decode_acode(so->last_sync_length[1],main_divisor) - }; - - //printf( "%d %d\n", acode_array[0], acode_array[1] ); - - //XXX: TODO: Capture error count here. - //if( acode_array[0] < 0 ) return; - //if( acode_array[1] < 0 ) return; - - int acode = acode_array[0]; + int acode = decode_acode(so->last_sync_length[0],main_divisor); if( !so->did_handle_ootx ) - { - int32_t delta1 = so->last_sync_time[0] - so->recent_sync_time; - int32_t delta2 = so->last_sync_time[1] - so->last_sync_time[0]; - - //printf( "%p %p %d %d %d %p\n", ctx, so, so->last_sync_time[0], acode_array, so->last_sync_length[0], ctx->lightproc ); - if( acode_array[0] >= 0 ) ctx->lightproc( so, -1, acode_array[0], delta1, so->last_sync_time[0], so->last_sync_length[0], 0 ); - if( acode_array[1] >= 0 ) ctx->lightproc( so, -2, acode_array[1], delta2, so->last_sync_time[1], so->last_sync_length[1], 1 ); - - so->recent_sync_time = so->last_sync_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; - } - - if (acode > 3) { - if( ssn == 0 ) - { - //SV_INFO( "Warning: got a slave marker but only got a master sync." ); - //This happens too frequently. Consider further examination. - } - dl = so->last_sync_time[1]; - tpco = so->last_sync_length[1]; - } + HandleOOTX( ctx, so ); int32_t offset_from = le->timestamp - dl + le->length/2; //Make sure pulse is in valid window - if( offset_from < so->timecenter_ticks*2-so->pulse_in_clear_time && offset_from > so->pulse_in_clear_time && acode >= 0 ) + if( offset_from < so->timecenter_ticks*2-so->pulse_in_clear_time && offset_from > so->pulse_in_clear_time ) { - ctx->lightproc( so, le->sensor_id, acode, offset_from, le->timestamp, le->length, !(acode>>2) ); + int whichlh; + if( acode < 0 ) whichlh = 1; + else whichlh = !(acode>>2); + ctx->lightproc( so, le->sensor_id, acode, offset_from, le->timestamp, le->length, whichlh ); } } else -- cgit v1.2.3 From 78ac5da64c299bcf0f2e1c8d19c876e7723faccf Mon Sep 17 00:00:00 2001 From: cnlohr Date: Sat, 13 May 2017 03:47:35 -0400 Subject: Fix my disambiguator. Seems to work with either-or lighthouse. --- src/survive_data.c | 46 ++++++++++++++++------------------------------ 1 file changed, 16 insertions(+), 30 deletions(-) (limited to 'src/survive_data.c') diff --git a/src/survive_data.c b/src/survive_data.c index df8df8e..157650d 100644 --- a/src/survive_data.c +++ b/src/survive_data.c @@ -468,6 +468,7 @@ void handle_lightcap2( SurviveObject * so, LightcapElement * le ) #endif + int32_t decode_acode(uint32_t length, int32_t main_divisor) { //+50 adds a small offset and seems to help always get it right. //Check the +50 in the future to see how well this works on a variety of hardware. @@ -478,6 +479,10 @@ int32_t decode_acode(uint32_t length, int32_t main_divisor) { return (acode>>1) - 6; } +//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +///////The charles disambiguator. Don't use this, mostly here for debugging./////////////////////////////////////////////////////// +//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// + void HandleOOTX( SurviveContext * ctx, SurviveObject * so ) { @@ -499,30 +504,9 @@ void HandleOOTX( SurviveContext * ctx, SurviveObject * so ) so->recent_sync_time = so->last_sync_time[1]; -/* - //Throw out everything if our sync pulses look like they're bad. - //This actually doesn't seem to hold anymore, now that we're looking for multiple LHs. - 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; } - + //This is the disambiguator function, for taking light timing and figuring out place-in-sweep for a given photodiode. void handle_lightcap( SurviveObject * so, LightcapElement * le ) @@ -588,16 +572,8 @@ void handle_lightcap( SurviveObject * so, LightcapElement * le ) { int is_new_pulse = delta > so->pulselength_min_sync /*1500*/ + last_sync_length; - //TRICKY: If we didn't see anything from the other lighthouse, we might just not see it... But, we still have to send our sync - //information to the rest of libsurvive. This could be turned into a function and combined with the code below. - if( !so->did_handle_ootx && is_new_pulse ) - { - HandleOOTX( ctx, so ); - } - so->did_handle_ootx = 0; - //printf( "INP: %d %d\n", is_new_pulse, so->sync_set_number ); if( is_new_pulse ) { @@ -606,6 +582,16 @@ void handle_lightcap( SurviveObject * so, LightcapElement * le ) int tp = delta % ( so->timecenter_ticks * 2); is_pulse_from_same_lh_as_last_sweep = tp < so->pulse_synctime_slack && tp > -so->pulse_synctime_slack; + if( !so->did_handle_ootx ) + { + HandleOOTX( ctx, so ); + } + if( !is_master_sync_pulse ) + { + so->did_handle_ootx = 0; + } + + if( is_master_sync_pulse ) //Could also be called by slave if no master was seen. { ssn = so->sync_set_number = is_pulse_from_same_lh_as_last_sweep?(so->sync_set_number):0; //If repeated lighthouse, just back off one. -- cgit v1.2.3