Fix configuration system and fix race condition in survive_vive.

5 files changed, 151 insertions, 40 deletions

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 <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_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: