diff options
| -rw-r--r-- | Makefile | 10 | ||||
| -rw-r--r-- | README.md | 16 | ||||
| -rw-r--r-- | include/libsurvive/survive.h | 2 | ||||
| -rw-r--r-- | redist/json_helpers.c | 28 | ||||
| -rw-r--r-- | src/survive_data.c | 154 | ||||
| -rwxr-xr-x | src/survive_vive.c | 4 | ||||
| -rw-r--r--[-rwxr-xr-x] | test.c | 0 |
7 files changed, 131 insertions, 83 deletions
@@ -25,12 +25,9 @@ GRAPHICS_LOFI:=redist/CNFGFunctions.o redist/CNFGXDriver.o endif -POSERS:=src/poser_dummy.o src/poser_daveortho.o src/poser_charlesslow.o +POSERS:=src/poser_dummy.o src/poser_daveortho.o src/poser_charlesslow.o src/poser_octavioradii.o src/poser_turveytori.o REDISTS:=redist/json_helpers.o redist/linmath.o redist/jsmn.o redist/os_generic.o LIBSURVIVE_CORE:=src/survive.o src/survive_usb.o src/survive_data.o src/survive_process.o src/ootx_decoder.o src/survive_driverman.o src/survive_vive.o src/survive_config.o src/survive_cal.o -LIBSURVIVE_CORE:=$(LIBSURVIVE_CORE) -LIBSURVIVE_O:=$(POSERS) $(REDISTS) $(LIBSURVIVE_CORE) -LIBSURVIVE_C:=$(LIBSURVIVE_O:.o=.c) #If you want to use HIDAPI on Linux. @@ -48,6 +45,11 @@ LIBSURVIVE_C:=$(LIBSURVIVE_O:.o=.c) + +LIBSURVIVE_CORE:=$(LIBSURVIVE_CORE) +LIBSURVIVE_O:=$(POSERS) $(REDISTS) $(LIBSURVIVE_CORE) +LIBSURVIVE_C:=$(LIBSURVIVE_O:.o=.c) + # unused: redist/crc32.c test : test.c ./lib/libsurvive.so redist/os_generic.o @@ -26,6 +26,22 @@ Please see the issues for what help needs to be done now! HackADay article and video with Dr. Yates on how they made the Vive a thing. http://hackaday.com/2016/12/21/alan-yates-why-valves-lighthouse-cant-work/ + +## Nomenclature + +* WRT = With Respect To +* PoV / POV = Point of View (typically WRT to a LH, sometimes (though rarely) a sensor) +* LH = Lighthouse = Base Station = A device that produces a 1.8 MHz modulated sync pulse in IR and then sweeps the scene with laser planes. +* Sync Pulse = A pulse of modulated IR data sent from a ligthhouse, typically by the floodlight aspect of a lighthouse. +* Sweep Pulse = The evenlope created by a laser sweeping over a light sensor. +* OOTX = Omnidirectional Optical Transmitter = Data encoded in the sync pulses of the LHs. +* HMD = Headset = Main sensor receiver with a visual display for a human. +* WM = Watchman = Controller = The HTC Vive controller. +* TR = Tracker = Official HTC Tracker. +* LightcapElement = A single pulse of light, including a timestamp, source sensor and length of pulse. +* Disambiguator = System that accepts lightcap elements and pulls out OOTX data and relative sweep times of sweep pulses. +* Poser = Device to convert series of angles from a LH's PoV + ## Getting things working There are two things you should consider doing to your system before running libsurvive. diff --git a/include/libsurvive/survive.h b/include/libsurvive/survive.h index 3c35bd2..e85fe48 100644 --- a/include/libsurvive/survive.h +++ b/include/libsurvive/survive.h @@ -42,7 +42,7 @@ struct SurviveObject //Timing sensitive data (mostly for disambiguation) int32_t timebase_hz; //48,000,000 for normal vive hardware. (checked) - int32_t timecenter_ticks; //200,000 for normal vive hardware. (checked) + int32_t timecenter_ticks; //200,000 for normal vive hardware. (checked) (This doubles-up as 2x this = full sweep length) int32_t pulsedist_max_ticks; //500,000 for normal vive hardware. (guessed) int32_t pulselength_min_sync; //2,200 for normal vive hardware. (guessed) int32_t pulse_in_clear_time; //35,000 for normal vive hardware. (guessed) diff --git a/redist/json_helpers.c b/redist/json_helpers.c index 29d48bd..c52301d 100644 --- a/redist/json_helpers.c +++ b/redist/json_helpers.c @@ -50,44 +50,46 @@ void json_write_float_array(FILE* f, const char* tag, float* v, uint8_t count) { uint8_t i = 0; char * str1 = NULL; char * str2 = NULL; - asprintf(&str1,"\"%s\":[", tag); + if( asprintf(&str1,"\"%s\":[", tag) < 0 ) goto giveup; for (i=0;i<count;++i) { if ( (i+1) < count) { - asprintf(&str2, "%s\"%f\"", str1,v[i]); + if( asprintf(&str2, "%s\"%f\"", str1,v[i]) < 0 ) goto giveup; } else { - asprintf(&str2, "%s\"%f\",", str1,v[i]); + if( asprintf(&str2, "%s\"%f\",", str1,v[i]) < 0 ) goto giveup; } free(str1); str1=str2; str2=NULL; } - asprintf(&str2, "%s]", str1); + if( asprintf(&str2, "%s]", str1) < 0 ) goto giveup; fputs(str2,f); - free(str1); - free(str2); +giveup: + if( str1 ) free(str1); + if( str2 ) free(str2); } void json_write_double_array(FILE* f, const char* tag, double* v, uint8_t count) { uint8_t i = 0; char * str1 = NULL; char * str2 = NULL; - asprintf(&str1,"\"%s\":[", tag); + if( asprintf(&str1,"\"%s\":[", tag) < 0 ) goto giveup; for (i=0;i<count;++i) { if (i<(count-1)) { - asprintf(&str2, "%s\"%f\",", str1,v[i]); + if( asprintf(&str2, "%s\"%f\",", str1,v[i]) < 0 ) goto giveup; } else { - asprintf(&str2, "%s\"%f\"", str1,v[i]); + if( asprintf(&str2, "%s\"%f\"", str1,v[i]) < 0 ) goto giveup; } free(str1); str1=str2; str2=NULL; } - asprintf(&str2, "%s]", str1); + if( asprintf(&str2, "%s]", str1) < 0 ) goto giveup; fputs(str2,f); - free(str1); - free(str2); +giveup: + if( str1 ) free(str1); + if( str2 ) free(str2); } void json_write_uint32(FILE* f, const char* tag, uint32_t v) { @@ -119,7 +121,7 @@ char* load_file_to_mem(const char* path) { fseek( f, 0, SEEK_SET ); char * JSON_STRING = malloc( len + 1); memset(JSON_STRING,0,len+1); - fread( JSON_STRING, len, 1, f ); + int i = fread( JSON_STRING, len, 1, f ); i = i; //Ignore return value. fclose( f ); return JSON_STRING; } 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; |