Merge pull request #115 from cnlohr/charles_statbiguator

Charles statbiguator

7 files changed, 320 insertions, 61 deletions

diff --git a/src/survive.c b/src/survive.c
index bafacad..05c66d7 100755
--- a/src/survive.c
+++ b/src/survive.c
@@ -50,7 +50,7 @@ static void *button_servicer(void * context)
 	{
 		OGLockSema(ctx->buttonQueue.buttonservicesem);
 
-		if (ctx->isClosing)
+		if (ctx->state != SURVIVE_RUNNING)
 		{
 			// we're shutting down.  Close.
 			return NULL;
@@ -104,7 +104,8 @@ void survive_verify_FLT_size(uint32_t user_size) {
   }
 }
 
-SurviveContext *survive_init_internal(int headless, htc_config_func configFunc) {
+SurviveContext * survive_init_internal( int argc, char * const * argv )
+{
 #ifdef RUNTIME_SYMNUM
 	if( !did_runtime_symnum )
 	{
@@ -121,35 +122,94 @@ SurviveContext *survive_init_internal(int headless, htc_config_func configFunc)
 	MANUAL_DRIVER_REGISTRATION(PoserDaveOrtho)
 	MANUAL_DRIVER_REGISTRATION(PoserDummy)
 	MANUAL_DRIVER_REGISTRATION(DriverRegHTCVive)
-
 #endif
 
-	int r = 0;
-	int i = 0;
 	SurviveContext * ctx = calloc( 1, sizeof( SurviveContext ) );
 
-	ctx->isClosing = 0;
+	ctx->state = SURVIVE_STOPPED;
 
 	ctx->global_config_values = malloc( sizeof(config_group) );
+	ctx->temporary_config_values = malloc( sizeof(config_group) );
 	ctx->lh_config = malloc( sizeof(config_group) * NUM_LIGHTHOUSES);
 
+	//initdata
 	init_config_group(ctx->global_config_values,10);
+	init_config_group(ctx->temporary_config_values,20);
 	init_config_group(&ctx->lh_config[0],10);
 	init_config_group(&ctx->lh_config[1],10);
 
-	config_read(ctx, "config.json");
-	ctx->activeLighthouses = config_read_uint32(ctx->global_config_values, "LighthouseCount", 2);
+	//Process command-line parameters.
+	char * const * argvend = &argv[argc];
+	char * const * av = argv+1;
+	int showhelp = 0;
+	for( ; av != argvend; av++ )
+	{
+		if( (*av)[0] != '-' )
+			showhelp = 1;
+		else
+		{
+			const char * vartoupdate = 0;
+
+			switch( (*av)[1] )
+			{
+			case '-': vartoupdate = &(*av)[2];    break;
+			case 'h': showhelp = 1; break;
+			case 'p': vartoupdate = "defaultposer";	break;
+			case 'l': vartoupdate = "lighthousecount";  break;
+			case 'c': vartoupdate = "configfile";   break;
+			default:
+				fprintf( stderr, "Error: unknown parameter %s\n", *av );
+				showhelp = 1;
+			}
+
+			if( vartoupdate )
+			{
+				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" );
+		fprintf( stderr, " -p [poser]              - use a specific defaultposer.\n" );
+		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" );
+		return 0;
+	}
+
+	config_read(ctx, survive_configs( ctx, "configfile", SC_GET, "config.json" ) );
+	ctx->activeLighthouses = survive_configi( ctx, "lighthousecount", SC_SETCONFIG, 2 );
+
 	config_read_lighthouse(ctx->lh_config, &(ctx->bsd[0]), 0);
 	config_read_lighthouse(ctx->lh_config, &(ctx->bsd[1]), 1);
 
 	ctx->faultfunction = survivefault;
 	ctx->notefunction = survivenote;
-
 	ctx->lightproc = survive_default_light_process;
 	ctx->imuproc = survive_default_imu_process;
 	ctx->angleproc = survive_default_angle_process;
 	ctx->lighthouseposeproc = survive_default_lighthouse_pose_process;
-	ctx->configfunction = configFunc ? configFunc : survive_default_htc_config_process;
+	ctx->configfunction = survive_default_htc_config_process;
+	return ctx;
+}
+
+int survive_startup( SurviveContext * ctx )
+{
+	int r = 0;
+	int i = 0;
 
 	// initialize the button queue
 	memset(&(ctx->buttonQueue), 0, sizeof(ctx->buttonQueue));
@@ -160,11 +220,10 @@ SurviveContext *survive_init_internal(int headless, htc_config_func configFunc)
 	survive_install_button_fn(ctx, NULL);
 	survive_install_raw_pose_fn(ctx, NULL);
 
-	i = 0;
 	const char * DriverName;
 
-	//const char * PreferredPoser = config_read_str(ctx->global_config_values, "DefaultPoser", "PoserDummy");
-	const char * PreferredPoser = config_read_str(ctx->global_config_values, "DefaultPoser", "PoserTurveyTori");
+	//const char * PreferredPoser = survive_config_reads(ctx->global_config_values, "defaultposer", "PoserDummy");
+	const char * PreferredPoser = survive_configs( ctx, "defaultposer", SC_SETCONFIG, "PoserTurveyTori");
 	PoserCB PreferredPoserCB = 0;
 	const char * FirstPoser = 0;
 	printf( "Available posers:\n" );
@@ -190,8 +249,6 @@ SurviveContext *survive_init_internal(int headless, htc_config_func configFunc)
 		r = dd( ctx );
 		printf( "Driver %s reports status %d\n", DriverName, r );
 	}
-printf( "REGISTERING DRIVERS\n" );
-
 	//Apply poser to objects.
 	for( i = 0; i < ctx->objs_ct; i++ )
 	{
@@ -199,12 +256,14 @@ printf( "REGISTERING DRIVERS\n" );
 	}
 
 	// saving the config extra to make sure that the user has a config file they can change.
-	config_save(ctx, "config.json");
+	config_save(ctx, survive_configs( ctx, "configfile", SC_GET, "config.json" ) );
 
+	ctx->state = SURVIVE_RUNNING;
 
-	return ctx;
+	return 0;
 }
 
+
 void survive_install_info_fn( SurviveContext * ctx,  text_feedback_func fbp )
 {
 	if( fbp )
@@ -328,7 +387,7 @@ void survive_close( SurviveContext * ctx )
 	const char * DriverName;
 	int r = 0;
 
-	ctx->isClosing = 1;
+	ctx->state = SURVIVE_CLOSING;
 
 	// unlock/ post to button service semaphore so the thread can kill itself
 	OGUnlockSema(ctx->buttonQueue.buttonservicesem);
@@ -358,9 +417,10 @@ void survive_close( SurviveContext * ctx )
 	}
 
 
-	config_save(ctx, "config.json");
+	config_save(ctx, survive_configs( ctx, "configfile", SC_GET, "config.json" ) );
 
 	destroy_config_group(ctx->global_config_values);
+	destroy_config_group(ctx->temporary_config_values);
 	destroy_config_group(ctx->lh_config);
 
 	for (i = 0; i < ctx->objs_ct; i++) {
@@ -373,6 +433,7 @@ void survive_close( SurviveContext * ctx )
 	free( ctx->drivermagics );
 	free( ctx->drivercloses );
 	free( ctx->global_config_values );
+	free( ctx->temporary_config_values );
 	free( ctx->lh_config );
 
 	free( ctx );
@@ -380,8 +441,15 @@ void survive_close( SurviveContext * ctx )
 
 int survive_poll( struct SurviveContext * ctx )
 {
-	int oldct = ctx->driver_ct;
 	int i, r;
+	if( ctx->state == SURVIVE_STOPPED )
+	{
+		r = survive_startup( ctx );
+		if( r )
+			return r;
+	}
+
+	int oldct = ctx->driver_ct;
 
 	for( i = 0; i < oldct; i++ )
 	{
diff --git a/src/survive_cal.c b/src/survive_cal.c
index 2fd96ef..cb242ae 100755
--- a/src/survive_cal.c
+++ b/src/survive_cal.c
@@ -8,6 +8,7 @@
 // to not include it at all on any stripped-down versions of libsurvive.
 //
 
+
 #include "survive_cal.h"
 #include "survive_internal.h"
 #include "survive_reproject.h"
@@ -118,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]);
@@ -178,21 +184,30 @@ void survive_cal_install( struct SurviveContext * ctx )
 	}
 
 	const char * DriverName;
-//	const char * PreferredPoser = config_read_str(ctx->global_config_values, "ConfigPoser", "PoserCharlesSlow");
-	const char * PreferredPoser = config_read_str(ctx->global_config_values, "ConfigPoser", "PoserTurveyTori");
-	PoserCB PreferredPoserCB = 0;
-	const char * FirstPoser = 0;
-	printf( "Available posers:\n" );
+//	const char * PreferredPoser = survive_configs(ctx, "configposer", "PoserCharlesSlow");
+	const char * PreferredPoser = survive_configs(ctx, "configposer", SC_SETCONFIG, "PoserTurveyTori");
+
+	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;
@@ -205,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:
@@ -220,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 d923ba4..44933fa 100644
--- a/src/survive_config.c
+++ b/src/survive_config.c
@@ -181,6 +181,8 @@ FLT config_read_float(config_group *cg, const char *tag, const FLT def) {
 	return config_set_float(cg, tag, def);
 }
 
+
+
 // TODO: Do something better than this:
 #define CFG_MIN(x,y) ((x) < (y)? (x): (y))
 
@@ -441,3 +443,87 @@ void config_read(SurviveContext* sctx, const char* path) {
 	json_tag_value = NULL;
 }
 
+
+
+
+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);
+	}
+	return cv;
+}
+
+
+
+FLT survive_configf( SurviveContext * ctx, const char *tag, char flags, FLT def )
+{
+	if( !(flags & SC_OVERRIDE) )
+	{
+		config_entry * cv = sc_search( ctx, tag );
+		if( cv )
+			return cv->numeric.f;
+	}
+
+	//If override is flagged, or, we can't find the variable, ,continue on.
+	if( flags & SC_SETCONFIG )
+	{
+		config_set_float( ctx->temporary_config_values, tag, def );
+		config_set_float( ctx->global_config_values, tag, def );
+	}
+	else if( flags & SC_SET )
+	{
+		config_set_float( ctx->temporary_config_values, tag, def );
+	}
+
+	return def;
+}
+
+uint32_t survive_configi( SurviveContext * ctx, const char *tag, char flags, uint32_t def )
+{
+	if( !(flags & SC_OVERRIDE) )
+	{
+		config_entry * cv = sc_search( ctx, tag );
+		if( cv )
+			return cv->numeric.i;
+	}
+
+	//If override is flagged, or, we can't find the variable, ,continue on.
+	if( flags & SC_SETCONFIG )
+	{
+		config_set_uint32( ctx->temporary_config_values, tag, def );
+		config_set_uint32( ctx->global_config_values, tag, def );
+	}
+	else if( flags & SC_SET )
+	{
+		config_set_uint32( ctx->temporary_config_values, tag, def );
+	}
+
+	return def;
+}
+
+const char * survive_configs( SurviveContext * ctx, const char *tag, char flags, const char *def )
+{
+	if( !(flags & SC_OVERRIDE) )
+	{
+		config_entry * cv = sc_search( ctx, tag );
+		if( cv )
+			return cv->data;
+	}
+
+	//If override is flagged, or, we can't find the variable, ,continue on.
+	if( flags & SC_SETCONFIG )
+	{
+		config_set_str( ctx->temporary_config_values, tag, def );
+		config_set_str( ctx->global_config_values, tag, def );
+	}
+	else if( flags & SC_SET )
+	{
+		config_set_str( ctx->temporary_config_values, tag, def );
+	}
+
+	return def;
+}
+
diff --git a/src/survive_config.h b/src/survive_config.h
index e2686e9..23d80c8 100644
--- a/src/survive_config.h
+++ b/src/survive_config.h
@@ -1,5 +1,7 @@
 // (C) 2017 <>< Joshua Allen, Under MIT/x11 License.
-
+//
+// This header is for handling internal parameter values.  Most accesses should be done through functions like survive_config
+//
 
 #ifndef _SURVIVE_CONFIG_H
 #define _SURVIVE_CONFIG_H
@@ -52,6 +54,7 @@ const FLT config_set_float(config_group *cg, const char *tag, const FLT value);
 const uint32_t config_set_uint32(config_group *cg, const char *tag, const uint32_t value);
 const char* config_set_str(config_group *cg, const char *tag, const char* value);
 
+//These functions look for a parameter in a specific group, and then chose the best to return. If the parameter does not exist, default will be written.
 FLT config_read_float(config_group *cg, const char *tag, const FLT def);
 uint16_t config_read_float_array(config_group *cg, const char *tag, FLT* values, const FLT* def, uint8_t count);
 uint32_t config_read_uint32(config_group *cg, const char *tag, const uint32_t def);
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 <string.h>
 #include <math.h> /* 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_playback.c b/src/survive_playback.c
index df9fcaa..014542b 100644
--- a/src/survive_playback.c
+++ b/src/survive_playback.c
@@ -203,7 +203,7 @@ static int LoadConfig(SurvivePlaybackData *sv, SurviveObject *so) {
 }
 
 int DriverRegPlayback(SurviveContext *ctx) {
-	const char *playback_file = config_read_str(ctx->global_config_values, "PlaybackFile", "");
+	const char *playback_file = survive_configs(ctx, "playbackfile", SC_SETCONFIG, "");
 
 	if (strlen(playback_file) == 0) {
 		return 0;
@@ -212,8 +212,7 @@ int DriverRegPlayback(SurviveContext *ctx) {
 	SurvivePlaybackData *sp = calloc(1, sizeof(SurvivePlaybackData));
 	sp->ctx = ctx;
 	sp->playback_dir = playback_file;
-	sp->time_factor =
-		config_read_float(ctx->global_config_values, "PlaybackFactor", 1.);
+	sp->time_factor = survive_configf(ctx, "playbackfactor", SC_SETCONFIG, 1.f);
 
 	printf("%s\n", playback_file);
 
diff --git a/src/survive_vive.c b/src/survive_vive.c
index 4411efb..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 )
 	{
@@ -1712,7 +1720,7 @@ void init_SurviveObject(SurviveObject* so) {
 
 int DriverRegHTCVive( SurviveContext * ctx )
 {
-	const char *playback_dir = config_read_str(ctx->global_config_values, "PlaybackFile", "");
+	const char *playback_dir = survive_configs(ctx, "playbackfile", SC_SETCONFIG, "");
 	if(strlen(playback_dir) != 0) {
 	  SV_INFO("Playback is active; disabling USB driver");
 	  return 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: