Merge pull request #91 from mwturvey/MakeTrackingGood

Provide standard way for posers to report pose output

7 files changed, 66 insertions, 16 deletions

diff --git a/include/libsurvive/survive.h b/include/libsurvive/survive.h
index 0cfab1f..4821e63 100644
--- a/include/libsurvive/survive.h
+++ b/include/libsurvive/survive.h
@@ -129,6 +129,7 @@ struct SurviveContext
 	imu_process_func imuproc;
 	angle_process_func angleproc;
 	button_process_func buttonproc;
+	raw_pose_func rawposeproc;
 
 	struct config_group* global_config_values;
 	struct config_group* lh_config; //lighthouse configs
@@ -163,7 +164,8 @@ void survive_install_error_fn( SurviveContext * ctx,  text_feedback_func fbp );
 void survive_install_light_fn( SurviveContext * ctx,  light_process_func fbp );
 void survive_install_imu_fn( SurviveContext * ctx,  imu_process_func fbp );
 void survive_install_angle_fn( SurviveContext * ctx,  angle_process_func fbp );
-void survive_install_button_fn( SurviveContext * ctx, button_process_func fbp );
+void survive_install_button_fn(SurviveContext * ctx, button_process_func fbp);
+void survive_install_raw_pose_fn(SurviveContext * ctx, raw_pose_func fbp);
 
 void survive_close( SurviveContext * ctx );
 int survive_poll( SurviveContext * ctx );
@@ -190,6 +192,7 @@ void survive_default_light_process( SurviveObject * so, int sensor_id, int acode
 void survive_default_imu_process( SurviveObject * so, int mode, FLT * accelgyro, uint32_t timecode, int id );
 void survive_default_angle_process( SurviveObject * so, int sensor_id, int acode, uint32_t timecode, FLT length, FLT angle, uint32_t lh );
 void survive_default_button_process(SurviveObject * so, uint8_t eventType, uint8_t buttonId, uint8_t axis1Id, uint16_t axis1Val, uint8_t axis2Id, uint16_t axis2Val);
+void survive_default_raw_pose_process(SurviveObject * so, uint8_t lighthouse, FLT *position, FLT *quaternion);
 
 
 ////////////////////// Survive Drivers ////////////////////////////
diff --git a/include/libsurvive/survive_types.h b/include/libsurvive/survive_types.h
index 9a6e148..5384345 100644
--- a/include/libsurvive/survive_types.h
+++ b/include/libsurvive/survive_types.h
@@ -42,7 +42,8 @@ typedef void (*text_feedback_func)( SurviveContext * ctx, const char * fault );
 typedef void (*light_process_func)( SurviveObject * so, int sensor_id, int acode, int timeinsweep, uint32_t timecode, uint32_t length, uint32_t lighthouse);
 typedef void (*imu_process_func)( SurviveObject * so, int mask, FLT * accelgyro, uint32_t timecode, int id );
 typedef void (*angle_process_func)( SurviveObject * so, int sensor_id, int acode, uint32_t timecode, FLT length, FLT angle, uint32_t lh);
-typedef void (*button_process_func)(SurviveObject * so, uint8_t eventType, uint8_t buttonId, uint8_t axis1Id, uint16_t axis1Val, uint8_t axis2Id, uint16_t axis2Val);
+typedef void(*button_process_func)(SurviveObject * so, uint8_t eventType, uint8_t buttonId, uint8_t axis1Id, uint16_t axis1Val, uint8_t axis2Id, uint16_t axis2Val);
+typedef void(*raw_pose_func)(SurviveObject * so, uint8_t lighthouse, FLT *position, FLT *quaternion);
 
 typedef int(*haptic_func)(SurviveObject * so, uint8_t reserved, uint16_t pulseHigh , uint16_t pulseLow, uint16_t repeatCount);
 
diff --git a/src/poser_turveytori.c b/src/poser_turveytori.c
index ac979c7..ad30f6c 100644
--- a/src/poser_turveytori.c
+++ b/src/poser_turveytori.c
@@ -1177,7 +1177,7 @@ static void RefineRotationEstimateQuaternion(FLT *rotOut, Point lhPoint, FLT *in
 
 
 	}
-	printf("Ri=%3d  Fitness=%3f ", i, lastMatchFitness);
+	if (ttDebug) printf("Ri=%3d  Fitness=%3f ", i, lastMatchFitness);
 }
 
 
@@ -1638,7 +1638,11 @@ static void QuickPose(SurviveObject *so, int lh)
 
 			SolveForLighthouse(pos, quat, 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]);
+			//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);
+			}
 
 			if (ttDebug) printf("!\n");
 		}
@@ -1852,6 +1856,18 @@ int PoserTurveyTori( SurviveObject * so, PoserData * poserData )
 		//	}
 		//}
 
+		for (int i=0; i < ctx->activeLighthouses; i++)
+		{ 
+			printf("Lighthouse Pose: [%1.1x][% 08.8f,% 08.8f,% 08.8f] [% 08.8f,% 08.8f,% 08.8f,% 08.8f]\n",
+				i,
+				ctx->bsd[i].Pose.Pos[0],
+				ctx->bsd[i].Pose.Pos[1],
+				ctx->bsd[i].Pose.Pos[2],
+				ctx->bsd[i].Pose.Rot[0],
+				ctx->bsd[i].Pose.Rot[1],
+				ctx->bsd[i].Pose.Rot[2],
+				ctx->bsd[i].Pose.Rot[3]);
+		}
 		config_set_lighthouse(ctx->lh_config, ctx->bsd[0], 0);
 		config_set_lighthouse(ctx->lh_config, ctx->bsd[1], 1);
 
diff --git a/src/survive.c b/src/survive.c
index 75c07bf..1c406e8 100755
--- a/src/survive.c
+++ b/src/survive.c
@@ -184,6 +184,7 @@ SurviveContext * survive_init( int headless )
 	// 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;
 }
@@ -235,9 +236,15 @@ void survive_install_button_fn(SurviveContext * ctx, button_process_func fbp)
 		ctx->buttonproc = fbp;
 	else
 		ctx->buttonproc = survive_default_button_process;
-
 }
 
+void survive_install_raw_pose_fn(SurviveContext * ctx, raw_pose_func fbp)
+{
+	if (fbp)
+		ctx->rawposeproc = fbp;
+	else
+		ctx->rawposeproc = survive_default_raw_pose_process;
+}
 int survive_add_object( SurviveContext * ctx, SurviveObject * obj )
 {
 	int oldct = ctx->objs_ct;
diff --git a/src/survive_data.c b/src/survive_data.c
index 1b80269..0427659 100644
--- a/src/survive_data.c
+++ b/src/survive_data.c
@@ -419,8 +419,8 @@ void handle_lightcap2_sweep(SurviveObject * so, LightcapElement * le )
 	}
 
 	if (lcd->per_sweep.activeLighthouse < 0) {
-		fprintf(stderr, "WARNING: No active lighthouse!\n");
-		fprintf(stderr, "            %2d %8d %d %d\n", le->sensor_id, le->length,lcd->per_sweep.lh_acode[0],lcd->per_sweep.lh_acode[1]);
+		//fprintf(stderr, "WARNING: No active lighthouse!\n");
+		//fprintf(stderr, "            %2d %8d %d %d\n", le->sensor_id, le->length,lcd->per_sweep.lh_acode[0],lcd->per_sweep.lh_acode[1]);
 		return;
 	}
 
diff --git a/src/survive_process.c b/src/survive_process.c
index 4b86144..1df24e9 100644
--- a/src/survive_process.c
+++ b/src/survive_process.c
@@ -66,20 +66,34 @@ void survive_default_angle_process( SurviveObject * so, int sensor_id, int acode
 void survive_default_button_process(SurviveObject * so, uint8_t eventType, uint8_t buttonId, uint8_t axis1Id, uint16_t axis1Val, uint8_t axis2Id, uint16_t axis2Val)
 {
 	// do nothing.
-	//printf("ButtonEntry: eventType:%x, buttonId:%d, axis1:%d, axis1Val:%8.8x, axis2:%d, axis2Val:%8.8x\n",
-	//	eventType,
-	//	buttonId,
-	//	axis1Id,
-	//	axis1Val,
-	//	axis2Id,
-	//	axis2Val);
+	printf("ButtonEntry: eventType:%x, buttonId:%d, axis1:%d, axis1Val:%8.8x, axis2:%d, axis2Val:%8.8x\n",
+		eventType,
+		buttonId,
+		axis1Id,
+		axis1Val,
+		axis2Id,
+		axis2Val);
 	if (buttonId == 24 && eventType == 1) // trigger engage
 	{
-		for (int j = 0; j < 40; j++)
+		for (int j = 0; j < 6; j++)
 		{
-			for (int i = 0; i < 0x1; i++)
+			for (int i = 0; i < 0x5; i++)
 			{
 				survive_haptic(so, 0, 0xf401, 0xb5a2, 0x0100);
+				//survive_haptic(so, 0, 0xf401, 0xb5a2, 0x0100);
+				OGUSleep(1000);
+			}
+			OGUSleep(20000);
+		}
+	}
+	if (buttonId == 2 && eventType == 1) // trigger engage
+	{
+		for (int j = 0; j < 6; j++)
+		{
+			for (int i = 0; i < 0x1; i++)
+			{
+				survive_haptic(so, 0, 0xf401, 0x05a2, 0xf100);
+				//survive_haptic(so, 0, 0xf401, 0xb5a2, 0x0100);
 				OGUSleep(5000);
 			}
 			OGUSleep(20000);
@@ -87,6 +101,13 @@ 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)
+{
+	// 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]);
+
+}
+
 void survive_default_imu_process( SurviveObject * so, int mask, FLT * accelgyromag, uint32_t timecode, int id )
 {
 	if( so->PoserFn )
diff --git a/test.c b/test.c
index 4909d50..c9f03d7 100644
--- a/test.c
+++ b/test.c
@@ -85,6 +85,8 @@ int main()
 	dump_iface( survive_get_so_by_name( ctx, "TR0" ), "TR0" );
 	dump_iface( survive_get_so_by_name( ctx, "WW0" ), "WW0" );
 
+	survive_cal_install(ctx);
+
 	while(survive_poll(ctx) == 0)
 	{
 		double Now = OGGetAbsoluteTime();