Update test.c to do calibration

3 files changed, 15 insertions, 1 deletions

diff --git a/src/poser_turveytori.c b/src/poser_turveytori.c
index 28e533e..ad30f6c 100644
--- a/src/poser_turveytori.c
+++ b/src/poser_turveytori.c
@@ -1856,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_process.c b/src/survive_process.c
index 42d2897..1df24e9 100644
--- a/src/survive_process.c
+++ b/src/survive_process.c
@@ -104,7 +104,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)
 {
 	// print the pose;
-	printf("Pose: [%2.2x][% 08.8f,% 08.8f,% 08.8f] [% 08.8f,% 08.8f,% 08.8f,% 08.8f]\n", lighthouse, pos[0], pos[1], pos[2], quat[0], quat[1], quat[2], quat[3]);
+	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/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();