Architect the way the posers receive data.

4 files changed, 85 insertions, 19 deletions

diff --git a/src/survive.c b/src/survive.c
index e1ab943..efa5d82 100644
--- a/src/survive.c
+++ b/src/survive.c
@@ -20,11 +20,11 @@ static void survivenote( struct SurviveContext * ctx, const char * fault )
 }
 
 
-struct SurviveContext * survive_init( int headless )
+SurviveContext * survive_init( int headless )
 {
 	int r = 0;
 	int i = 0;
-	struct SurviveContext * ctx = calloc( 1, sizeof( struct SurviveContext ) );
+	SurviveContext * ctx = calloc( 1, sizeof( SurviveContext ) );
 
 	ctx->faultfunction = survivefault;
 	ctx->notefunction = survivenote;
@@ -35,7 +35,6 @@ struct SurviveContext * survive_init( int headless )
 
 	const char * DriverName;
 	while( ( DriverName = GetDriverNameMatching( "DriverReg", i++ ) ) )
-
 	{
 		DeviceDriver dd = GetDriver( DriverName );
 		printf( "Loading driver %s (%p) (%d)\n", DriverName, dd, i );
@@ -43,10 +42,34 @@ struct SurviveContext * survive_init( int headless )
 		printf( "Driver %s reports status %d\n", DriverName, r );
 	}
 
+	i = 0;
+	const char * PreferredPoser = "PoserDummy"; //config_read_str( cg, "DefualtPoser", "PoserDummy" ); /XXX Axlecrusher, can you add config stuff for this?
+	PoserCB PreferredPoserCB = 0;
+	const char * FirstPoser = 0;
+	printf( "Available posers:\n" );
+	while( ( DriverName = GetDriverNameMatching( "Poser", i++ ) ) )
+	{
+		PoserCB p = GetDriver( DriverName );
+		if( !PreferredPoserCB ) PreferredPoserCB = p;
+		int ThisPoser = strcmp( DriverName, PreferredPoser ) == 0;
+		printf( "\t%c%s\n", ThisPoser?'*':' ', DriverName );
+		if( ThisPoser ) PreferredPoserCB = p;
+	}
+	printf( "Totals %d posers.  Using selected poser (or first!).\n", i-1 );
+	if( !PreferredPoserCB )
+	{
+		SV_ERROR( "Error.  Cannot find any valid poser." );
+	}
+
+	for( i = 0; i < ctx->objs_ct; i++ )
+	{
+		ctx->objs[i]->PoserFn = PreferredPoserCB;
+	}
+
 	return ctx;
 }
 
-void survive_install_info_fn( struct SurviveContext * ctx,  text_feedback_func fbp )
+void survive_install_info_fn( SurviveContext * ctx,  text_feedback_func fbp )
 {
 	if( fbp )
 		ctx->notefunction = fbp;
@@ -54,7 +77,7 @@ void survive_install_info_fn( struct SurviveContext * ctx,  text_feedback_func f
 		ctx->notefunction = survivenote;
 }
 
-void survive_install_error_fn( struct SurviveContext * ctx,  text_feedback_func fbp )
+void survive_install_error_fn( SurviveContext * ctx,  text_feedback_func fbp )
 {
 	if( fbp )
 		ctx->faultfunction = fbp;
@@ -62,7 +85,7 @@ void survive_install_error_fn( struct SurviveContext * ctx,  text_feedback_func
 		ctx->faultfunction = survivefault;
 }
 
-void survive_install_light_fn( struct SurviveContext * ctx, light_process_func fbp )
+void survive_install_light_fn( SurviveContext * ctx, light_process_func fbp )
 {
 	if( fbp )
 		ctx->lightproc = fbp;
@@ -70,7 +93,7 @@ void survive_install_light_fn( struct SurviveContext * ctx, light_process_func f
 		ctx->lightproc = survive_default_light_process;
 }
 
-void survive_install_imu_fn( struct SurviveContext * ctx,  imu_process_func fbp )
+void survive_install_imu_fn( SurviveContext * ctx,  imu_process_func fbp )
 {
 	if( fbp )
 		ctx->imuproc = fbp;
@@ -79,7 +102,7 @@ void survive_install_imu_fn( struct SurviveContext * ctx,  imu_process_func fbp
 }
 
 
-void survive_install_angle_fn( struct SurviveContext * ctx,  angle_process_func fbp )
+void survive_install_angle_fn( SurviveContext * ctx,  angle_process_func fbp )
 {
 	if( fbp )
 		ctx->angleproc = fbp;
@@ -87,15 +110,15 @@ void survive_install_angle_fn( struct SurviveContext * ctx,  angle_process_func
 		ctx->angleproc = survive_default_angle_process;
 }
 
-int survive_add_object( struct SurviveContext * ctx, struct SurviveObject * obj )
+int survive_add_object( SurviveContext * ctx, SurviveObject * obj )
 {
 	int oldct = ctx->objs_ct;
-	ctx->objs = realloc( ctx->objs, sizeof( struct SurviveObject * ) * (oldct+1) );
+	ctx->objs = realloc( ctx->objs, sizeof( SurviveObject * ) * (oldct+1) );
 	ctx->objs[oldct] = obj;
 	ctx->objs_ct = oldct+1;
 }
 
-void survive_add_driver( struct SurviveContext * ctx, void * payload, DeviceDriverCb poll, DeviceDriverCb close, DeviceDriverMagicCb magic )
+void survive_add_driver( SurviveContext * ctx, void * payload, DeviceDriverCb poll, DeviceDriverCb close, DeviceDriverMagicCb magic )
 {
 	int oldct = ctx->driver_ct;
 	ctx->drivers = realloc( ctx->drivers, sizeof( void * ) * (oldct+1) );
@@ -109,7 +132,7 @@ void survive_add_driver( struct SurviveContext * ctx, void * payload, DeviceDriv
 	ctx->driver_ct = oldct+1;
 }
 
-int survive_send_magic( struct SurviveContext * ctx, int magic_code, void * data, int datalen )
+int survive_send_magic( SurviveContext * ctx, int magic_code, void * data, int datalen )
 {
 	int oldct = ctx->driver_ct;
 	int i;
@@ -119,7 +142,7 @@ int survive_send_magic( struct SurviveContext * ctx, int magic_code, void * data
 	}	
 }
 
-void survive_close( struct SurviveContext * ctx )
+void survive_close( SurviveContext * ctx )
 {
 	const char * DriverName;
 	int r = 0;
@@ -133,6 +156,14 @@ void survive_close( struct SurviveContext * ctx )
 
 	int oldct = ctx->driver_ct;
 	int i;
+
+	for( i = 0; i < ctx->objs_ct; i++ )
+	{
+		PoserData pd;
+		pd.pt = POSERDATA_DISASSOCIATE;
+		if( ctx->objs[i]->PoserFn ) ctx->objs[i]->PoserFn( ctx->objs[i], &pd );
+	}
+
 	for( i = 0; i < oldct; i++ )
 	{
 		ctx->drivercloses[i]( ctx, ctx->drivers[i] );
diff --git a/src/survive_cal.h b/src/survive_cal.h
index a5e372e..dd2a1e2 100644
--- a/src/survive_cal.h
+++ b/src/survive_cal.h
@@ -2,6 +2,8 @@
 
 // All OOTX code was written by J. Allen. Rest of the code is probably mostly CNLohr.
 
+//XXX XXX XXX Warning:  This subsystem will likely be mostly re-written.
+
 #ifndef _SURVIVE_CAL_H
 #define _SURVIVE_CAL_H
 
diff --git a/src/survive_process.c b/src/survive_process.c
index edabfff..2fea99d 100644
--- a/src/survive_process.c
+++ b/src/survive_process.c
@@ -46,13 +46,34 @@ void survive_default_angle_process( SurviveObject * so, int sensor_id, int acode
 	{
 		survive_cal_angle( so, sensor_id, acode, timecode, length, angle );
 	}
-
-	//TODO: Writeme!
+	if( so->PoserFn )
+	{
+		PoserDataLight l = {
+			.pt = POSERDATA_LIGHT,
+			.sensor_id = sensor_id,
+			.acode = acode,
+			.timecode = timecode,
+			.length = length,
+			.angle = angle,
+		};
+		so->PoserFn( so, (PoserData *)&l );
+	}
 }	
 
 
-void survive_default_imu_process( SurviveObject * so, int16_t * accelgyro, uint32_t timecode, int id )
+void survive_default_imu_process( SurviveObject * so, int mask, FLT * accelgyromag, uint32_t timecode, int id )
 {
-	//TODO: Writeme!
+	if( so->PoserFn )
+	{
+		PoserDataIMU imu = {
+			.pt = POSERDATA_IMU,
+			.datamask = mask,
+			.accel = { accelgyromag[0], accelgyromag[1], accelgyromag[2] },
+			.gyro = {  accelgyromag[3], accelgyromag[4], accelgyromag[5] },
+			.mag = {   accelgyromag[6], accelgyromag[7], accelgyromag[8] },
+			.timecode = timecode,
+		};
+		so->PoserFn( so, (PoserData *)&imu );
+	}
 }
 
diff --git a/src/survive_vive.c b/src/survive_vive.c
index 61716d3..7da2897 100644
--- a/src/survive_vive.c
+++ b/src/survive_vive.c
@@ -595,7 +595,13 @@ static void handle_watchman( struct SurviveObject * w, uint8_t * readdata )
 	if( ( ( type & 0xe8 ) == 0xe8 ) || doimu ) //Hmm, this looks kind of yucky... we can get e8's that are accelgyro's but, cleared by first propset.
 	{
 		propset |= 2;
-		w->ctx->imuproc( w, (int16_t *)&readdata[1], (time1<<24)|(time2<<16)|readdata[0], 0 );
+		//XXX XXX BIG TODO!!! Actually recal gyro data.
+		FLT agm[9] = { readdata[1], readdata[2], readdata[3],
+						readdata[4], readdata[5], readdata[6],
+						0,0,0 };
+
+		w->ctx->imuproc( w, 3, agm, (time1<<24)|(time2<<16)|readdata[0], 0 );
+
 		int16_t * k = (int16_t *)readdata+1;
 		//printf( "Match8 %d %d %d %d %d %3d %3d\n", qty, k[0], k[1], k[2], k[3], k[4], k[5] );
 		readdata += 13; qty -= 13;
@@ -810,7 +816,13 @@ void survive_data_cb( SurviveUSBInterface * si )
 			if( cd > 0 )
 			{
 				obj->oldcode = code;
-				ctx->imuproc( obj, acceldata, timecode, code );
+
+				//XXX XXX BIG TODO!!! Actually recal gyro data.
+				FLT agm[9] = { acceldata[0], acceldata[1], acceldata[2],
+								acceldata[3], acceldata[4], acceldata[5],
+								0,0,0 };
+
+				ctx->imuproc( obj, 3, agm, timecode, code );
 			}
 		}