From 999c8d881299bdefd4eb123244df225260a7e302 Mon Sep 17 00:00:00 2001 From: cnlohr Date: Sun, 18 Mar 2018 22:00:55 -0400 Subject: Fix configuration system and fix race condition in survive_vive. --- src/survive.c | 17 +++++++--- src/survive_cal.c | 30 ++++++++++++----- src/survive_config.c | 5 ++- src/survive_data.c | 93 +++++++++++++++++++++++++++++++++++++++++++++++++--- src/survive_vive.c | 46 +++++++++++++------------- 5 files changed, 151 insertions(+), 40 deletions(-) (limited to 'src') diff --git a/src/survive.c b/src/survive.c index 24bb7d3..05c66d7 100755 --- a/src/survive.c +++ b/src/survive.c @@ -164,13 +164,22 @@ SurviveContext * survive_init_internal( int argc, char * const * argv ) if( vartoupdate ) { - survive_configs( ctx, *av, SC_OVERRIDE, *(av+1) ); - av++; + if( av+1 == argvend ) + { + fprintf( stderr, "Error: expected parameter after %s\n", *av ); + showhelp = 1; + } + else + { + survive_configs( ctx, *av+2, SC_OVERRIDE|SC_SET, *(av+1) ); + av++; + } } } } if( showhelp ) { + //Can't use SV_ERROR here since we don't have a context to send to yet. fprintf( stderr, "libsurvive - usage:\n" ); fprintf( stderr, " --[parameter] [value] - sets parameter\n" ); fprintf( stderr, " -h - shows help.\n" ); @@ -178,8 +187,6 @@ SurviveContext * survive_init_internal( int argc, char * const * argv ) fprintf( stderr, " -l [lighthouse count] - use a specific number of lighthoses.\n" ); fprintf( stderr, " -c [config file] - set config file\n" ); fprintf( stderr, " -p [lighthouse count] - use a specific number of lighthoses.\n" ); - - //XXX: TODO: Should this just exit(-1)? return 0; } @@ -435,7 +442,7 @@ void survive_close( SurviveContext * ctx ) int survive_poll( struct SurviveContext * ctx ) { int i, r; - if( ctx->state = SURVIVE_STOPPED ) + if( ctx->state == SURVIVE_STOPPED ) { r = survive_startup( ctx ); if( r ) diff --git a/src/survive_cal.c b/src/survive_cal.c index 4b1df89..cb242ae 100755 --- a/src/survive_cal.c +++ b/src/survive_cal.c @@ -119,6 +119,11 @@ void survive_cal_install( struct SurviveContext * ctx ) int i; struct SurviveCalData * cd = ctx->calptr = calloc( 1, sizeof( struct SurviveCalData ) ); + if( ctx->state != SURVIVE_RUNNING ) + { + SV_ERROR( "Error: You cannot install a calibrator until the system is running." ); + } + for( i = 0; i < NUM_LIGHTHOUSES; i++ ) { ootx_init_decoder_context(&cd->ootx_decoders[i]); @@ -182,19 +187,27 @@ void survive_cal_install( struct SurviveContext * ctx ) // const char * PreferredPoser = survive_configs(ctx, "configposer", "PoserCharlesSlow"); const char * PreferredPoser = survive_configs(ctx, "configposer", SC_SETCONFIG, "PoserTurveyTori"); - PoserCB PreferredPoserCB = 0; - const char * FirstPoser = 0; - printf( "Available posers:\n" ); + SV_INFO( "Trying to load poser %s for cal.", PreferredPoser ); + PoserCB SelectedPoserCB = 0; + const char * SelectedPoserName = 0; i = 0; while( ( DriverName = GetDriverNameMatching( "Poser", i++ ) ) ) { PoserCB p = GetDriver( DriverName ); - if( !PreferredPoserCB ) PreferredPoserCB = p; + if( !SelectedPoserCB ) + { + SelectedPoserCB = p; + SelectedPoserName = DriverName; + } int ThisPoser = strcmp( DriverName, PreferredPoser ) == 0; - if( ThisPoser ) PreferredPoserCB = p; + if( ThisPoser ) + { + SelectedPoserCB = p; + SelectedPoserName = DriverName; + } } - cd->ConfigPoserFn = PreferredPoserCB; - printf( "Got config poser: %p\n", cd->ConfigPoserFn ); + cd->ConfigPoserFn = SelectedPoserCB; + SV_INFO( "Got config poser: %s (%p)", SelectedPoserName, cd->ConfigPoserFn ); ootx_packet_clbk = ootx_packet_clbk_d; ctx->calptr = cd; @@ -207,7 +220,7 @@ void survive_cal_light( struct SurviveObject * so, int sensor_id, int acode, int struct SurviveCalData * cd = ctx->calptr; if( !cd ) return; - + switch( cd->stage ) { default: @@ -222,6 +235,7 @@ void survive_cal_light( struct SurviveObject * so, int sensor_id, int acode, int //fprintf(stderr, "%s\n", so->codename); int lhid = -sensor_id-1; // Take the OOTX data from the first device. (if using HMD, WM0, WM1 only, this will be HMD) + if( lhid < NUM_LIGHTHOUSES && so == cd->poseobjects[0] ) { uint8_t dbit = (acode & 2)>>1; diff --git a/src/survive_config.c b/src/survive_config.c index 1a64828..44933fa 100644 --- a/src/survive_config.c +++ b/src/survive_config.c @@ -449,7 +449,10 @@ void config_read(SurviveContext* sctx, const char* path) { static config_entry * sc_search(SurviveContext * ctx, const char *tag ) { config_entry *cv = find_config_entry(ctx->temporary_config_values, tag); - if( !cv ) cv = find_config_entry(ctx->global_config_values, tag); + if( !cv ) + { + cv = find_config_entry(ctx->global_config_values, tag); + } return cv; } diff --git a/src/survive_data.c b/src/survive_data.c index 0427659..cb6340b 100644 --- a/src/survive_data.c +++ b/src/survive_data.c @@ -6,9 +6,83 @@ #include #include /* for sqrt */ -#define USE_TURVEYBIGUATOR +//#define USE_TURVEYBIGUATOR +#define CHARLES_STATIBUATOR -#ifdef USE_TURVEYBIGUATOR +#if defined(USE_CHARLESSTATIBGUATOR) + +/* + The operating principle of + +*/ + + + +typedef struct +{ + uint32_t next_expected_time; +} statbiguator_beamtracker; + +typedef struct +{ + statbiguator_beamtracker * beams; + + //NUM_LIGHTHOUSES first, then so->nr_locations + +} statbiguator_data; + +void handle_lightcap2( SurviveObject * so, LightcapElement * le ) +{ + SurviveContext * ctx = so->ctx; + + 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)); + } + + + if( disambiguator_data +} + +/* + + ctx->lightproc( so, le->sensor_id, acode, offset_from, le->timestamp, le->length, whichlh ); + // first, send out the sync pulse data for the last round (for OOTX decoding + { + if (lcd->per_sweep.lh_max_pulse_length[0] != 0) + { + so->ctx->lightproc( + so, + -1, + 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_max_pulse_length[1] != 0) + { + so->ctx->lightproc( + so, + -2, + 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); + } + } +*/ + + + + +// handle_lightcap2(so,le); + + +#elif defined( 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, @@ -469,6 +543,11 @@ 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. @@ -513,7 +592,7 @@ void handle_lightcap( SurviveObject * so, LightcapElement * le ) { SurviveContext * ctx = so->ctx; -#ifdef USE_TURVEYBIGUATOR +#if defined (USE_TURVEYBIGUATOR) || defined(USE_CHARLESSTATIBGUATOR) handle_lightcap2(so,le); return; @@ -605,7 +684,10 @@ void handle_lightcap( SurviveObject * so, LightcapElement * le ) { so->last_sync_length[1] = 0; } + + //This is triggered on the first full sync pulse. so->last_sync_time[ssn] = le->timestamp; + //printf( "A: %d\n", so->last_sync_time[ssn] ); so->last_sync_length[ssn] = le->length; } else if( so->sync_set_number == -1 ) @@ -623,7 +705,9 @@ void handle_lightcap( SurviveObject * so, LightcapElement * le ) } else { + //This is triggered on the slave base station's sync pulse. so->last_sync_time[ssn] = le->timestamp; + //printf( "B: %d\n", so->last_sync_time[ssn] ); so->last_sync_length[ssn] = le->length; } } @@ -635,6 +719,7 @@ void handle_lightcap( SurviveObject * so, LightcapElement * le ) { if( so->last_sync_time[ssn] > le->timestamp ) { + //printf( "C: %d\n", so->last_sync_time[ssn] ); so->last_sync_time[ssn] = le->timestamp; so->last_sync_length[ssn] = le->length; } @@ -683,7 +768,7 @@ void handle_lightcap( SurviveObject * so, LightcapElement * le ) { int whichlh; if( acode < 0 ) whichlh = 1; - else whichlh = !(acode>>2); + else whichlh = (acode>>2); ctx->lightproc( so, le->sensor_id, acode, offset_from, le->timestamp, le->length, whichlh ); } } diff --git a/src/survive_vive.c b/src/survive_vive.c index 47af701..cf068a3 100755 --- a/src/survive_vive.c +++ b/src/survive_vive.c @@ -339,6 +339,7 @@ int survive_usb_init( SurviveViveData * sv, SurviveObject * hmd, SurviveObject * { SurviveContext * ctx = sv->ctx; #ifdef HIDAPI + SV_INFO( "Vive starting in HIDAPI mode." ); if( !GlobalRXUSBMutx ) { GlobalRXUSBMutx = OGCreateMutex(); @@ -403,6 +404,8 @@ int survive_usb_init( SurviveViveData * sv, SurviveObject * hmd, SurviveObject * } #else + SV_INFO( "Vive starting in libusb mode." ); + int r = libusb_init( &sv->usbctx ); if( r ) { @@ -495,6 +498,13 @@ int survive_usb_init( SurviveViveData * sv, SurviveObject * hmd, SurviveObject * libusb_free_device_list( devs, 1 ); #endif + //Add the drivers - this must happen BEFORE we actually attach interfaces. + if( sv->udev[USB_DEV_HMD_IMU_LH] ) { survive_add_object( ctx, hmd ); } + if( sv->udev[USB_DEV_WATCHMAN1] ) { survive_add_object( ctx, wm0 ); } + if( sv->udev[USB_DEV_WATCHMAN2] ) { survive_add_object( ctx, wm1 ); } + if( sv->udev[USB_DEV_TRACKER0] ) { survive_add_object( ctx, tr0 ); } + if( sv->udev[USB_DEV_W_WATCHMAN1] ) { survive_add_object( ctx, ww0 ); } + if( sv->udev[USB_DEV_HMD] && AttachInterface( sv, hmd, USB_IF_HMD, sv->udev[USB_DEV_HMD], 0x81, survive_data_cb, "Mainboard" ) ) { return -6; } if( sv->udev[USB_DEV_HMD_IMU_LH] && AttachInterface( sv, hmd, USB_IF_HMD_IMU_LH, sv->udev[USB_DEV_HMD_IMU_LH], 0x81, survive_data_cb, "Lighthouse" ) ) { return -7; } if( sv->udev[USB_DEV_WATCHMAN1] && AttachInterface( sv, wm0, USB_IF_WATCHMAN1, sv->udev[USB_DEV_WATCHMAN1], 0x81, survive_data_cb, "Watchman 1" ) ) { return -8; } @@ -532,7 +542,6 @@ int survive_vive_send_magic(SurviveContext * ctx, void * drv, int magic_code, vo { int r; SurviveViveData * sv = drv; - printf( "*CALLING %p %p\n", ctx, sv ); //XXX TODO: Handle haptics, etc. int turnon = magic_code; @@ -700,6 +709,7 @@ int survive_vive_send_magic(SurviveContext * ctx, void * drv, int magic_code, vo int survive_vive_send_haptic(SurviveObject * so, uint8_t reserved, uint16_t pulseHigh, uint16_t pulseLow, uint16_t repeatCount) { SurviveViveData *sv = (SurviveViveData*)so->driver; + SurviveContext * ctx = so->ctx; if (NULL == sv) { @@ -719,7 +729,7 @@ int survive_vive_send_haptic(SurviveObject * so, uint8_t reserved, uint16_t puls //SV_INFO("UCR: %d", r); if (r != sizeof(vive_controller_haptic_pulse)) { - printf("HAPTIC FAILED **************************\n"); + SV_ERROR("HAPTIC FAILED **************************\n"); return -1; } @@ -1135,7 +1145,6 @@ SurviveObject *so; static void handle_watchman( SurviveObject * w, uint8_t * readdata ) { - uint8_t startread[29]; memcpy( startread, readdata, 29 ); @@ -1438,7 +1447,6 @@ void survive_data_cb( SurviveUSBInterface * si ) } #endif - switch( si->which_interface_am_i ) { case USB_IF_HMD: @@ -1673,7 +1681,7 @@ static int LoadConfig( SurviveViveData * sv, SurviveObject * so, int devno, int SurviveContext * ctx = sv->ctx; char * ct0conf = 0; int len = survive_get_config( &ct0conf, sv, devno, iface, extra_magic ); - printf( "Loading config: %d\n", len ); + SV_INFO( "Loading config: %d", len ); if( len < 0 ) { @@ -1736,11 +1744,21 @@ int DriverRegHTCVive( SurviveContext * ctx ) #endif //USB must happen last. - if (survive_usb_init(sv, hmd, wm0, wm1, tr0, ww0)) { + if (survive_usb_init(sv, hmd, wm0, wm1, tr0, ww0)) { // TODO: Cleanup any libUSB stuff sitting around. goto fail_gracefully; } + if( sv->udev[USB_DEV_HMD_IMU_LH] || + sv->udev[USB_DEV_WATCHMAN1] || + sv->udev[USB_DEV_WATCHMAN2] || + sv->udev[USB_DEV_TRACKER0] || + sv->udev[USB_DEV_W_WATCHMAN1] ) { + survive_add_driver( ctx, sv, survive_vive_usb_poll, survive_vive_close, survive_vive_send_magic ); + } else { + SV_ERROR("No USB devices detected"); + } + //Next, pull out the config stuff. if (sv->udev[USB_DEV_HMD_IMU_LH] && LoadConfig(sv, hmd, 1, 0, 0)) { SV_INFO("HMD config issue."); @@ -1750,22 +1768,6 @@ int DriverRegHTCVive( SurviveContext * ctx ) if( sv->udev[USB_DEV_TRACKER0] && LoadConfig( sv, tr0, 4, 0, 0 )) { SV_INFO( "Tracker 0 config issue." ); } if( sv->udev[USB_DEV_W_WATCHMAN1] && LoadConfig( sv, ww0, 5, 0, 0 )) { SV_INFO( "Wired Watchman 0 config issue." ); } - //Add the drivers. - if( sv->udev[USB_DEV_HMD_IMU_LH] ) { survive_add_object( ctx, hmd ); } - if( sv->udev[USB_DEV_WATCHMAN1] ) { survive_add_object( ctx, wm0 ); } - if( sv->udev[USB_DEV_WATCHMAN2] ) { survive_add_object( ctx, wm1 ); } - if( sv->udev[USB_DEV_TRACKER0] ) { survive_add_object( ctx, tr0 ); } - if( sv->udev[USB_DEV_W_WATCHMAN1] ) { survive_add_object( ctx, ww0 ); } - - if( sv->udev[USB_DEV_HMD_IMU_LH] || - sv->udev[USB_DEV_WATCHMAN1] || - sv->udev[USB_DEV_WATCHMAN2] || - sv->udev[USB_DEV_TRACKER0] || - sv->udev[USB_DEV_W_WATCHMAN1] ) { - survive_add_driver( ctx, sv, survive_vive_usb_poll, survive_vive_close, survive_vive_send_magic ); - } else { - fprintf(stderr, "No USB devices detected\n"); - } return 0; fail_gracefully: -- cgit v1.2.3