diff options
-rw-r--r-- | Makefile | 10 | ||||
-rw-r--r-- | README.md | 16 | ||||
-rw-r--r-- | include/libsurvive/poser.h | 9 | ||||
-rw-r--r-- | include/libsurvive/survive.h | 15 | ||||
-rw-r--r-- | include/libsurvive/survive_types.h | 8 | ||||
-rw-r--r-- | redist/json_helpers.c | 28 | ||||
-rw-r--r-- | redist/linmath.c | 12 | ||||
-rw-r--r-- | redist/os_generic.c | 17 | ||||
-rw-r--r-- | src/poser_turveytori.c | 46 | ||||
-rw-r--r-- | src/survive_data.c | 157 | ||||
-rwxr-xr-x | src/survive_vive.c | 10 | ||||
-rw-r--r--[-rwxr-xr-x] | test.c | 0 |
12 files changed, 222 insertions, 106 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/poser.h b/include/libsurvive/poser.h index 497b009..582590e 100644 --- a/include/libsurvive/poser.h +++ b/include/libsurvive/poser.h @@ -3,6 +3,11 @@ #include "survive_types.h" + +#ifdef __cplusplus +extern "C" { +#endif + typedef enum PoserType_t { POSERDATA_NONE = 0, @@ -55,4 +60,8 @@ typedef struct typedef int (*PoserCB)( SurviveObject * so, PoserData * pd ); +#ifdef __cplusplus +}; +#endif + #endif diff --git a/include/libsurvive/survive.h b/include/libsurvive/survive.h index 278bbca..e85fe48 100644 --- a/include/libsurvive/survive.h +++ b/include/libsurvive/survive.h @@ -5,6 +5,10 @@ #include "survive_types.h" #include "poser.h" +#ifdef __cplusplus +extern "C" { +#endif + //DANGER: This structure may be redefined. Note that it is logically split into 64-bit chunks //for optimization on 32- and 64-bit systems. @@ -38,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) @@ -116,7 +120,7 @@ void survive_install_imu_fn( SurviveContext * ctx, imu_process_func fbp ); void survive_install_angle_fn( SurviveContext * ctx, angle_process_func fbp ); void survive_close( SurviveContext * ctx ); -int survive_poll(); +int survive_poll( SurviveContext * ctx ); SurviveObject * survive_get_so_by_name( SurviveContext * ctx, const char * name ); @@ -128,6 +132,9 @@ int survive_send_magic( SurviveContext * ctx, int magic_code, void * data, int d //Install the calibrator. void survive_cal_install( SurviveContext * ctx ); //XXX This will be removed if not already done so. +// Read back a human-readable string description of the calibration status +int survive_cal_get_status( struct SurviveContext * ctx, char * description, int description_length ); + //Call these from your callback if overridden. //Accept higher-level data. void survive_default_light_process( SurviveObject * so, int sensor_id, int acode, int timeinsweep, uint32_t timecode, uint32_t length , uint32_t lh); @@ -174,5 +181,9 @@ void handle_lightcap( SurviveObject * so, LightcapElement * le ); #define SV_ERROR( ... ) { char stbuff[1024]; sprintf( stbuff, __VA_ARGS__ ); ctx->faultfunction( ctx, stbuff ); } #define SV_KILL() exit(0) //XXX This should likely be re-defined. +#ifdef __cplusplus +}; +#endif + #endif diff --git a/include/libsurvive/survive_types.h b/include/libsurvive/survive_types.h index bfd0b1d..224719e 100644 --- a/include/libsurvive/survive_types.h +++ b/include/libsurvive/survive_types.h @@ -1,6 +1,11 @@ #ifndef _SURVIVE_TYPES_H #define _SURVIVE_TYPES_H +#ifdef __cplusplus +extern "C" { +#endif + + #ifndef FLT #ifdef USE_DOUBLE #define FLT double @@ -39,6 +44,9 @@ typedef int (*DeviceDriver)( SurviveContext * ctx ); typedef int (*DeviceDriverCb)( struct SurviveContext * ctx, void * driver ); typedef int (*DeviceDriverMagicCb)( struct SurviveContext * ctx, void * driver, int magic_code, void * data, int datalen ); +#ifdef __cplusplus +}; +#endif #endif 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/redist/linmath.c b/redist/linmath.c index caff1de..5fefe1e 100644 --- a/redist/linmath.c +++ b/redist/linmath.c @@ -520,17 +520,15 @@ void quatfrom2vectors(FLT *q, const FLT *src, const FLT *dest) FLT invs = 1 / s; FLT c[3]; - //cross3d(c, v0, v1); - cross3d(c, v1, v0); + cross3d(c, v0, v1); - q[0] = c[0] * invs; - q[1] = c[1] * invs; - q[2] = c[2] * invs; - q[3] = s * 0.5f; + q[0] = s * 0.5f; + q[1] = c[0] * invs; + q[2] = c[1] * invs; + q[3] = c[2] * invs; quatnormalize(q, q); } - } void matrix44copy(FLT * mout, const FLT * minm ) diff --git a/redist/os_generic.c b/redist/os_generic.c index 1ab4863..3191357 100644 --- a/redist/os_generic.c +++ b/redist/os_generic.c @@ -151,8 +151,9 @@ void OGDeleteSema( og_sema_t os ) #else -#define _GNU_SOURCE - +#ifndef _GNU_SOURCE +# define _GNU_SOURCE +#endif #include <sys/stat.h> #include <stdlib.h> @@ -198,7 +199,7 @@ double OGGetFileTime( const char * file ) og_thread_t OGCreateThread( void * (routine)( void * ), void * parameter ) { - pthread_t * ret = malloc( sizeof( pthread_t ) ); + pthread_t * ret = (pthread_t *)malloc( sizeof( pthread_t ) ); int r = pthread_create( ret, 0, routine, parameter ); if( r ) { @@ -277,7 +278,7 @@ void OGDeleteMutex( og_mutex_t om ) og_sema_t OGCreateSema() { - sem_t * sem = malloc( sizeof( sem_t ) ); + sem_t * sem = (sem_t *)malloc( sizeof( sem_t ) ); sem_init( sem, 0, 0 ); return (og_sema_t)sem; } @@ -285,24 +286,24 @@ og_sema_t OGCreateSema() int OGGetSema( og_sema_t os ) { int valp; - sem_getvalue( os, &valp ); + sem_getvalue( (sem_t *)os, &valp ); return valp; } void OGLockSema( og_sema_t os ) { - sem_wait( os ); + sem_wait( (sem_t *)os ); } void OGUnlockSema( og_sema_t os ) { - sem_post( os ); + sem_post( (sem_t *)os ); } void OGDeleteSema( og_sema_t os ) { - sem_destroy( os ); + sem_destroy( (sem_t *)os ); free(os); } diff --git a/src/poser_turveytori.c b/src/poser_turveytori.c index c251040..7abf5d0 100644 --- a/src/poser_turveytori.c +++ b/src/poser_turveytori.c @@ -83,6 +83,9 @@ typedef struct FLT oldAngles[SENSORS_PER_OBJECT][2][NUM_LIGHTHOUSES][OLD_ANGLES_BUFF_LEN]; // sensor, sweep axis, lighthouse, instance int angleIndex[NUM_LIGHTHOUSES][2]; // index into circular buffer ahead. separate index for each axis. int lastAxis[NUM_LIGHTHOUSES]; + + Point lastLhPos[NUM_LIGHTHOUSES]; + FLT lastLhRotAxisAngle[NUM_LIGHTHOUSES][4]; } ToriData; @@ -433,23 +436,25 @@ Point getGradient(Point pointIn, PointsAndAngle *pna, size_t pnaCount, FLT preci { Point result; + FLT baseFitness = getPointFitness(pointIn, pna, pnaCount, 0); + Point tmpXplus = pointIn; Point tmpXminus = pointIn; tmpXplus.x = pointIn.x + precision; tmpXminus.x = pointIn.x - precision; - result.x = getPointFitness(tmpXplus, pna, pnaCount, 0) - getPointFitness(tmpXminus, pna, pnaCount, 0); + result.x = baseFitness - getPointFitness(tmpXminus, pna, pnaCount, 0); Point tmpYplus = pointIn; Point tmpYminus = pointIn; tmpYplus.y = pointIn.y + precision; tmpYminus.y = pointIn.y - precision; - result.y = getPointFitness(tmpYplus, pna, pnaCount, 0) - getPointFitness(tmpYminus, pna, pnaCount, 0); + result.y = baseFitness - getPointFitness(tmpYminus, pna, pnaCount, 0); Point tmpZplus = pointIn; Point tmpZminus = pointIn; tmpZplus.z = pointIn.z + precision; tmpZminus.z = pointIn.z - precision; - result.z = getPointFitness(tmpZplus, pna, pnaCount, 0) - getPointFitness(tmpZminus, pna, pnaCount, 0); + result.z = baseFitness - getPointFitness(tmpZminus, pna, pnaCount, 0); return result; } @@ -1144,6 +1149,8 @@ void SolveForRotation(FLT rotOut[4], TrackedObject *obj, Point lh) static Point SolveForLighthouse(FLT posOut[3], FLT quatOut[4], TrackedObject *obj, SurviveObject *so, char doLogOutput, int lh, int setLhCalibration) { + ToriData *toriData = so->PoserData; + //printf("Solving for Lighthouse\n"); //printf("obj->numSensors = %d;\n", obj->numSensors); @@ -1234,6 +1241,14 @@ static Point SolveForLighthouse(FLT posOut[3], FLT quatOut[4], TrackedObject *ob // back into the search for the correct point (see "if (a1 > M_PI / 2)" below) Point p1 = getNormalizedAndScaledVector(avgNorm, 8); + // if the last lighthouse position has been populated (extremely rare it would be 0) + if (toriData->lastLhPos[lh].x != 0) + { + p1.x = toriData->lastLhPos[lh].x; + p1.y = toriData->lastLhPos[lh].y; + p1.z = toriData->lastLhPos[lh].z; + } + Point refinedEstimateGd = RefineEstimateUsingModifiedGradientDescent1(p1, pna, pnaCount, logFile); FLT pf1[3] = { refinedEstimateGd.x, refinedEstimateGd.y, refinedEstimateGd.z }; @@ -1258,11 +1273,29 @@ static Point SolveForLighthouse(FLT posOut[3], FLT quatOut[4], TrackedObject *ob //printf("Distance is %f, Fitness is %f\n", distance, fitGd); FLT rot[4]; // this is axis/ angle rotation, not a quaternion! + + if (toriData->lastLhRotAxisAngle[lh][0] != 0) + { + rot[0] = toriData->lastLhRotAxisAngle[lh][0]; + rot[1] = toriData->lastLhRotAxisAngle[lh][1]; + rot[2] = toriData->lastLhRotAxisAngle[lh][2]; + rot[3] = toriData->lastLhRotAxisAngle[lh][3]; + } + + SolveForRotation(rot, obj, refinedEstimateGd); FLT objPos[3]; + { + toriData->lastLhRotAxisAngle[lh][0] = rot[0]; + toriData->lastLhRotAxisAngle[lh][1] = rot[1]; + toriData->lastLhRotAxisAngle[lh][2] = rot[2]; + toriData->lastLhRotAxisAngle[lh][3] = rot[3]; + } + WhereIsTheTrackedObjectAxisAngle(objPos, rot, refinedEstimateGd); + FLT rotQuat[4]; quatfromaxisangle(rotQuat, rot, rot[3]); @@ -1325,6 +1358,11 @@ static Point SolveForLighthouse(FLT posOut[3], FLT quatOut[4], TrackedObject *ob fclose(logFile); } + + toriData->lastLhPos[lh].x = refinedEstimateGd.x; + toriData->lastLhPos[lh].y = refinedEstimateGd.y; + toriData->lastLhPos[lh].z = refinedEstimateGd.z; + return refinedEstimateGd; } @@ -1482,7 +1520,7 @@ int PoserTurveyTori( SurviveObject * so, PoserData * poserData ) counter++; // let's just do this occasionally for now... - if (counter % 2 == 0) + if (counter % 4 == 0) QuickPose(so, 0); } // axis changed, time to increment the circular buffer index. diff --git a/src/survive_data.c b/src/survive_data.c index 22ce8c2..df8df8e 100644 --- a/src/survive_data.c +++ b/src/survive_data.c @@ -4,9 +4,9 @@ #include "survive_internal.h" #include <stdint.h> #include <string.h> -#include <math.h> +#include <math.h> /* for sqrt */ -#define USE_TURVEYBIGUATOR +//#define USE_TURVEYBIGUATOR #ifdef USE_TURVEYBIGUATOR @@ -76,6 +76,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; @@ -470,13 +471,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 ) { @@ -487,6 +534,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; @@ -520,6 +568,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) @@ -537,15 +588,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; } @@ -582,15 +655,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 ) { @@ -607,68 +685,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 @@ -677,7 +707,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 8e22ea1..030db8a 100755 --- a/src/survive_vive.c +++ b/src/survive_vive.c @@ -54,7 +54,7 @@ const short vidpids[] = { const char * devnames[] = { "HMD", - "Lighthouse", + "HMD IMU & LH", "Watchman 1", "Watchman 2", "Tracker 0", @@ -344,7 +344,8 @@ int survive_usb_init( SurviveViveData * sv, SurviveObject * hmd, SurviveObject * if (cur_dev->vendor_id == vendor_id && cur_dev->product_id == product_id) { - if( cur_dev->interface_number == enumid ) + if( cur_dev->interface_number == enumid || + cur_dev->interface_number == -1 && menum == enumid) { path_to_open = cur_dev->path; break; @@ -370,6 +371,7 @@ int survive_usb_init( SurviveViveData * sv, SurviveObject * hmd, SurviveObject * wchar_t wstr[255]; res = hid_get_serial_number_string(handle, wstr, 255); + printf("Found %s. ", devnames[i]); wprintf(L"Serial Number String: (%d) %s for %04x:%04x@%d (Dev: %p)\n", wstr[0], wstr,vendor_id, product_id, menum, handle); sv->udev[i] = handle; @@ -1164,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; @@ -1182,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; |