From ed85661121c3ed8769a69d059fd3162aec33c6fe Mon Sep 17 00:00:00 2001 From: Mike Turvey Date: Fri, 5 Jan 2018 04:09:54 -0700 Subject: Update test.c to do calibration --- src/poser_turveytori.c | 12 ++++++++++++ src/survive_process.c | 2 +- test.c | 2 ++ 3 files changed, 15 insertions(+), 1 deletion(-) 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(); -- cgit v1.2.3