From 95eab1b8938e0033a0aa0d10d6ec53d7d2782907 Mon Sep 17 00:00:00 2001 From: Justin Berger Date: Sun, 11 Mar 2018 15:31:28 -0600 Subject: Added callback for lighthouse found; example in test.c --- src/poser_charlesslow.c | 20 ++++++++++++-------- src/poser_turveytori.c | 20 ++++++++++---------- src/survive.c | 10 +++++++++- src/survive_cal.c | 4 ---- src/survive_process.c | 13 +++++++++++++ 5 files changed, 44 insertions(+), 23 deletions(-) (limited to 'src') diff --git a/src/poser_charlesslow.c b/src/poser_charlesslow.c index 96442b3..30fc677 100644 --- a/src/poser_charlesslow.c +++ b/src/poser_charlesslow.c @@ -165,9 +165,18 @@ int PoserCharlesSlow( SurviveObject * so, PoserData * pd ) RunOpti(so, fs, lh, 1, LighthousePos, LighthouseQuat); - ctx->bsd[lh].PositionSet = 1; - copy3d( ctx->bsd[lh].Pose.Pos, LighthousePos ); - quatcopy( ctx->bsd[lh].Pose.Rot, LighthouseQuat ); + SurvivePose lighthousePose; + copy3d(lighthousePose.Pos, LighthousePos); + quatcopy(lighthousePose.Rot, LighthouseQuat); + + const FLT rt[4] = {0, 0, 1, 0}; + FLT tmp[4]; + quatrotateabout(tmp, lighthousePose.Rot, rt); + memcpy(lighthousePose.Rot, tmp, sizeof(FLT) * 4); + + if (ctx->lighthouseposeproc) { + ctx->lighthouseposeproc(ctx, lh, &lighthousePose); + } #define ALT_COORDS #ifdef ALT_COORDS @@ -189,11 +198,6 @@ int PoserCharlesSlow( SurviveObject * so, PoserData * pd ) so->FromLHPose[lh].Rot[2] = LighthouseQuat[2]; so->FromLHPose[lh].Rot[3] = LighthouseQuat[3]; #endif - - const FLT rt[4] = {0, 0, 1, 0}; - FLT tmp[4]; - quatrotateabout(tmp, so->ctx->bsd[lh].Pose.Rot, rt); - memcpy(so->ctx->bsd[lh].Pose.Rot, tmp, sizeof(FLT) * 4); } return 0; diff --git a/src/poser_turveytori.c b/src/poser_turveytori.c index dae51e6..ddc4ad2 100644 --- a/src/poser_turveytori.c +++ b/src/poser_turveytori.c @@ -1436,17 +1436,17 @@ static Point SolveForLighthouse(FLT posOut[3], FLT quatOut[4], TrackedObject *ob printf("Warning: resetting base station calibration data"); } + SurvivePose lighthousePose; FLT invRot[4]; - quatgetreciprocal(invRot, rotQuat); - - so->ctx->bsd[lh].Pose.Pos[0] = refinedEstimateGd.x; - so->ctx->bsd[lh].Pose.Pos[1] = refinedEstimateGd.y; - so->ctx->bsd[lh].Pose.Pos[2] = refinedEstimateGd.z; - so->ctx->bsd[lh].Pose.Rot[0] = invRot[0]; - so->ctx->bsd[lh].Pose.Rot[1] = invRot[1]; - so->ctx->bsd[lh].Pose.Rot[2] = invRot[2]; - so->ctx->bsd[lh].Pose.Rot[3] = invRot[3]; - so->ctx->bsd[lh].PositionSet = 1; + quatgetreciprocal(invRot, lighthousePose.Rot); + + lighthousePose.Pos[0] = refinedEstimateGd.x; + lighthousePose.Pos[1] = refinedEstimateGd.y; + lighthousePose.Pos[2] = refinedEstimateGd.z; + + if (so->ctx->lighthouseposeproc) { + so->ctx->lighthouseposeproc(so->ctx, lh, &lighthousePose); + } } diff --git a/src/survive.c b/src/survive.c index 5d0d35f..61bfc86 100755 --- a/src/survive.c +++ b/src/survive.c @@ -148,7 +148,7 @@ SurviveContext * survive_init_internal( int headless ) ctx->lightproc = survive_default_light_process; ctx->imuproc = survive_default_imu_process; ctx->angleproc = survive_default_angle_process; - + ctx->lighthouseposeproc = survive_default_lighthouse_pose_process; // initialize the button queue memset(&(ctx->buttonQueue), 0, sizeof(ctx->buttonQueue)); @@ -260,6 +260,14 @@ void survive_install_raw_pose_fn(SurviveContext * ctx, raw_pose_func fbp) else ctx->rawposeproc = survive_default_raw_pose_process; } + +void survive_install_lighthouse_pose_fn(SurviveContext *ctx, lighthouse_pose_func fbp) { + if (fbp) + ctx->lighthouseposeproc = fbp; + else + ctx->lighthouseposeproc = survive_default_lighthouse_pose_process; +} + int survive_add_object( SurviveContext * ctx, SurviveObject * obj ) { int oldct = ctx->objs_ct; diff --git a/src/survive_cal.c b/src/survive_cal.c index 696cce4..59cf68b 100755 --- a/src/survive_cal.c +++ b/src/survive_cal.c @@ -651,9 +651,6 @@ static void handle_calibration( struct SurviveCalData *cd ) fprintf( fobjp, "%f %f %f %f\n", objfromlh->Rot[0], objfromlh->Rot[1], objfromlh->Rot[2], objfromlh->Rot[3] ); if (ctx->bsd[lh].PositionSet) { - config_set_lighthouse(ctx->lh_config, &ctx->bsd[0], 0); - config_set_lighthouse(ctx->lh_config, &ctx->bsd[1], 1); - if (compute_reprojection_error) { FLT reproj_err = 0; size_t cnt = 0; @@ -690,7 +687,6 @@ static void handle_calibration( struct SurviveCalData *cd ) } } - config_save(ctx, "config.json"); } fclose( fobjp ); diff --git a/src/survive_process.c b/src/survive_process.c index d156d9e..420e771 100644 --- a/src/survive_process.c +++ b/src/survive_process.c @@ -2,6 +2,7 @@ //All MIT/x11 Licensed Code in this file may be relicensed freely under the GPL or LGPL licenses. #include "survive_cal.h" +#include "survive_config.h" //XXX TODO: Once data is avialble in the context, use the stuff here to handle converting from time codes to //proper angles, then from there perform the rest of the solution. @@ -108,6 +109,18 @@ void survive_default_raw_pose_process(SurviveObject *so, uint8_t lighthouse, Sur } +void survive_default_lighthouse_pose_process(SurviveContext *ctx, uint8_t lighthouse, SurvivePose *pose) { + if (pose) { + ctx->bsd[lighthouse].Pose = *pose; + ctx->bsd[lighthouse].PositionSet = 1; + } else { + ctx->bsd[lighthouse].PositionSet = 0; + } + + config_set_lighthouse(ctx->lh_config, &ctx->bsd[lighthouse], lighthouse); + config_save(ctx, "config.json"); +} + void survive_default_imu_process( SurviveObject * so, int mask, FLT * accelgyromag, uint32_t timecode, int id ) { if( so->PoserFn ) -- cgit v1.2.3