aboutsummaryrefslogtreecommitdiff
path: root/src
diff options
context:
space:
mode:
Diffstat (limited to 'src')
-rw-r--r--src/survive_data.c154
-rwxr-xr-xsrc/survive_vive.c4
2 files changed, 93 insertions, 65 deletions
diff --git a/src/survive_data.c b/src/survive_data.c
index 7b9d051..4f94106 100644
--- a/src/survive_data.c
+++ b/src/survive_data.c
@@ -6,7 +6,7 @@
#include <string.h>
#include <math.h> /* for sqrt */
-#define USE_TURVEYBIGUATOR
+//#define USE_TURVEYBIGUATOR
#ifdef USE_TURVEYBIGUATOR
@@ -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 )
{
@@ -449,6 +495,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 +529,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)
@@ -499,15 +549,37 @@ 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 )
{
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 +616,20 @@ 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.
+ //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;
- //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 )
{
@@ -569,68 +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)
- };
-
- //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];
-
- 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];
-
- //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 < 380000 && offset_from > 70000 )
+ 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
@@ -639,7 +668,6 @@ void handle_lightcap( SurviveObject * so, LightcapElement * le )
//Runt pulse, or no sync pulses available.
}
#endif
-
}
diff --git a/src/survive_vive.c b/src/survive_vive.c
index 8f2a42e..030db8a 100755
--- a/src/survive_vive.c
+++ b/src/survive_vive.c
@@ -1166,7 +1166,7 @@ void survive_data_cb( SurviveUSBInterface * si )
le.sensor_id = POP1;
le.length = POP2;
le.timestamp = POP4;
- if( le.sensor_id == 0xff ) break;
+ if( le.sensor_id > 0xfd ) continue;
handle_lightcap( obj, &le );
}
break;
@@ -1184,7 +1184,7 @@ void survive_data_cb( SurviveUSBInterface * si )
le.sensor_id = (uint8_t)POP2;
le.length = POP2;
le.timestamp = POP4;
- if( le.sensor_id == 0xff ) break;
+ if( le.sensor_id > 0xfd ) continue; //
handle_lightcap( obj, &le );
}
break;