Switch from pos,quat to pose. Also change initialization order.

4 files changed, 32 insertions, 22 deletions

diff --git a/src/poser_turveytori.c b/src/poser_turveytori.c
index 94d572e..4398b66 100644
--- a/src/poser_turveytori.c
+++ b/src/poser_turveytori.c
@@ -1616,7 +1616,7 @@ static void QuickPose(SurviveObject *so, int lh)
 
 		if (sensorCount > 4)
 		{
-			FLT pos[3], quat[4];
+			FLT pose[7];
 
 			// TODO: This countdown stuff is a total hack!
 			// it basically ignores all the logic to find the most reliable data points
@@ -1637,12 +1637,12 @@ static void QuickPose(SurviveObject *so, int lh)
 
 
 
-			SolveForLighthouse(pos, quat, to, so, 0, lh, 0);
+			SolveForLighthouse(&pose[0], &pose[3], to, so, 0, lh, 0);
 
 			//printf("P&O: [% 08.8f,% 08.8f,% 08.8f] [% 08.8f,% 08.8f,% 08.8f,% 08.8f]\n", pos[0], pos[1], pos[2], quat[0], quat[1], quat[2], quat[3]);
 			if (so->ctx->rawposeproc)
 			{
-				so->ctx->rawposeproc(so, lh, pos, quat);
+				so->ctx->rawposeproc(so, lh, pose);
 			}
 
 			if (ttDebug) printf("!\n");
diff --git a/src/survive.c b/src/survive.c
index 76bf8e4..21e1f0e 100755
--- a/src/survive.c
+++ b/src/survive.c
@@ -138,7 +138,6 @@ SurviveContext * survive_init_internal( int headless )
 	init_config_group(&ctx->lh_config[1],10);
 
 	config_read(ctx, "config.json");
-
 	ctx->activeLighthouses = config_read_uint32(ctx->global_config_values, "LighthouseCount", 2);
 	config_read_lighthouse(ctx->lh_config, &(ctx->bsd[0]), 0);
 	config_read_lighthouse(ctx->lh_config, &(ctx->bsd[1]), 1);
@@ -150,16 +149,19 @@ SurviveContext * survive_init_internal( int headless )
 	ctx->imuproc = survive_default_imu_process;
 	ctx->angleproc = survive_default_angle_process;
 
-	const char * DriverName;
-	while( ( DriverName = GetDriverNameMatching( "DriverReg", i++ ) ) )
-	{
-		DeviceDriver dd = GetDriver( DriverName );
-		printf( "Loading driver %s (%p) (%d)\n", DriverName, dd, i );
-		r = dd( ctx );
-		printf( "Driver %s reports status %d\n", DriverName, r );
-	}
+
+	// initialize the button queue
+	memset(&(ctx->buttonQueue), 0, sizeof(ctx->buttonQueue));
+	ctx->buttonQueue.buttonservicesem = OGCreateSema();	
+
+	// start the thread to process button data
+	ctx->buttonservicethread = OGCreateThread(button_servicer, ctx);
+	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");
 	PoserCB PreferredPoserCB = 0;
@@ -179,6 +181,17 @@ SurviveContext * survive_init_internal( int headless )
 		SV_ERROR( "Error.  Cannot find any valid poser." );
 	}
 
+	i = 0;
+	while( ( DriverName = GetDriverNameMatching( "DriverReg", i++ ) ) )
+	{
+		DeviceDriver dd = GetDriver( DriverName );
+		printf( "Loading driver %s (%p) (%d)\n", DriverName, dd, i );
+		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++ )
 	{
 		ctx->objs[i]->PoserFn = PreferredPoserCB;
@@ -187,15 +200,6 @@ SurviveContext * survive_init_internal( int headless )
 	// saving the config extra to make sure that the user has a config file they can change.
 	config_save(ctx, "config.json");
 
-	// initialize the button queue
-	memset(&(ctx->buttonQueue), 0, sizeof(ctx->buttonQueue));
-
-	ctx->buttonQueue.buttonservicesem = OGCreateSema();	
-
-	// start the thread to process button data
-	ctx->buttonservicethread = OGCreateThread(button_servicer, ctx);
-	survive_install_button_fn(ctx, NULL);
-	survive_install_raw_pose_fn(ctx, NULL);
 
 	return ctx;
 }
diff --git a/src/survive_process.c b/src/survive_process.c
index 0f19007..1ea3061 100644
--- a/src/survive_process.c
+++ b/src/survive_process.c
@@ -59,6 +59,7 @@ void survive_default_angle_process( SurviveObject * so, int sensor_id, int acode
 			.angle = angle,
 			.lh = lh,
 		};
+
 		so->PoserFn( so, (PoserData *)&l );
 	}
 }	
@@ -101,7 +102,7 @@ void survive_default_button_process(SurviveObject * so, uint8_t eventType, uint8
 	//}
 }
 
-void survive_default_raw_pose_process(SurviveObject * so, uint8_t lighthouse, FLT *pos, FLT *quat)
+void survive_default_raw_pose_process(SurviveObject * so, uint8_t lighthouse, FLT *pose)
 {
 	// print the pose;
 	//printf("Pose: [%1.1x][%s][% 08.8f,% 08.8f,% 08.8f] [% 08.8f,% 08.8f,% 08.8f,% 08.8f]\n", lighthouse, so->codename, pos[0], pos[1], pos[2], quat[0], quat[1], quat[2], quat[3]);
diff --git a/src/survive_vive.c b/src/survive_vive.c
index b3d990a..5001efd 100755
--- a/src/survive_vive.c
+++ b/src/survive_vive.c
@@ -1655,6 +1655,11 @@ static int LoadConfig( SurviveViveData * sv, SurviveObject * so, int devno, int
 	int len = survive_get_config( &ct0conf, sv, devno, iface, extra_magic );
 	printf( "Loading config: %d\n", len );
 
+	if( len < 0 )
+	{
+		return len;
+	}
+
 	{
 	  char raw_fname[100];
 	  sprintf( raw_fname, "%s_config.json", so->codename );