From a2ba45f43ae02b1e39b1816fe9c1c70c54a7f046 Mon Sep 17 00:00:00 2001 From: cnlohr Date: Sat, 10 Mar 2018 23:57:58 -0500 Subject: Switch from pos,quat to pose. Also change initialization order. --- src/poser_turveytori.c | 6 +++--- src/survive.c | 40 ++++++++++++++++++++++------------------ src/survive_process.c | 3 ++- src/survive_vive.c | 5 +++++ 4 files changed, 32 insertions(+), 22 deletions(-) (limited to 'src') diff --git a/src/poser_turveytori.c b/src/poser_turveytori.c index 94d572e..4398b66 100644 --- a/src/poser_turveytori.c +++ b/src/poser_turveytori.c @@ -1616,7 +1616,7 @@ static void QuickPose(SurviveObject *so, int lh) if (sensorCount > 4) { - FLT pos[3], quat[4]; + FLT pose[7]; // TODO: This countdown stuff is a total hack! // it basically ignores all the logic to find the most reliable data points @@ -1637,12 +1637,12 @@ static void QuickPose(SurviveObject *so, int lh) - SolveForLighthouse(pos, quat, to, so, 0, lh, 0); + SolveForLighthouse(&pose[0], &pose[3], 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]); if (so->ctx->rawposeproc) { - so->ctx->rawposeproc(so, lh, pos, quat); + so->ctx->rawposeproc(so, lh, pose); } if (ttDebug) printf("!\n"); diff --git a/src/survive.c b/src/survive.c index 76bf8e4..21e1f0e 100755 --- a/src/survive.c +++ b/src/survive.c @@ -138,7 +138,6 @@ SurviveContext * survive_init_internal( int headless ) init_config_group(&ctx->lh_config[1],10); config_read(ctx, "config.json"); - ctx->activeLighthouses = config_read_uint32(ctx->global_config_values, "LighthouseCount", 2); config_read_lighthouse(ctx->lh_config, &(ctx->bsd[0]), 0); config_read_lighthouse(ctx->lh_config, &(ctx->bsd[1]), 1); @@ -150,16 +149,19 @@ SurviveContext * survive_init_internal( int headless ) ctx->imuproc = survive_default_imu_process; ctx->angleproc = survive_default_angle_process; - const char * DriverName; - while( ( DriverName = GetDriverNameMatching( "DriverReg", i++ ) ) ) - { - DeviceDriver dd = GetDriver( DriverName ); - printf( "Loading driver %s (%p) (%d)\n", DriverName, dd, i ); - r = dd( ctx ); - printf( "Driver %s reports status %d\n", DriverName, r ); - } + + // initialize the button queue + memset(&(ctx->buttonQueue), 0, sizeof(ctx->buttonQueue)); + ctx->buttonQueue.buttonservicesem = OGCreateSema(); + + // 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); i = 0; + const char * DriverName; + //const char * PreferredPoser = config_read_str(ctx->global_config_values, "DefaultPoser", "PoserDummy"); const char * PreferredPoser = config_read_str(ctx->global_config_values, "DefaultPoser", "PoserTurveyTori"); PoserCB PreferredPoserCB = 0; @@ -179,6 +181,17 @@ SurviveContext * survive_init_internal( int headless ) SV_ERROR( "Error. Cannot find any valid poser." ); } + i = 0; + while( ( DriverName = GetDriverNameMatching( "DriverReg", i++ ) ) ) + { + DeviceDriver dd = GetDriver( DriverName ); + printf( "Loading driver %s (%p) (%d)\n", DriverName, dd, i ); + r = dd( ctx ); + printf( "Driver %s reports status %d\n", DriverName, r ); + } +printf( "REGISTERING DRIVERS\n" ); + + //Apply poser to objects. for( i = 0; i < ctx->objs_ct; i++ ) { ctx->objs[i]->PoserFn = PreferredPoserCB; @@ -187,15 +200,6 @@ SurviveContext * survive_init_internal( int headless ) // saving the config extra to make sure that the user has a config file they can change. config_save(ctx, "config.json"); - // initialize the button queue - memset(&(ctx->buttonQueue), 0, sizeof(ctx->buttonQueue)); - - ctx->buttonQueue.buttonservicesem = OGCreateSema(); - - // 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; } diff --git a/src/survive_process.c b/src/survive_process.c index 0f19007..1ea3061 100644 --- a/src/survive_process.c +++ b/src/survive_process.c @@ -59,6 +59,7 @@ void survive_default_angle_process( SurviveObject * so, int sensor_id, int acode .angle = angle, .lh = lh, }; + so->PoserFn( so, (PoserData *)&l ); } } @@ -101,7 +102,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) +void survive_default_raw_pose_process(SurviveObject * so, uint8_t lighthouse, FLT *pose) { // 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]); diff --git a/src/survive_vive.c b/src/survive_vive.c index b3d990a..5001efd 100755 --- a/src/survive_vive.c +++ b/src/survive_vive.c @@ -1655,6 +1655,11 @@ static int LoadConfig( SurviveViveData * sv, SurviveObject * so, int devno, int int len = survive_get_config( &ct0conf, sv, devno, iface, extra_magic ); printf( "Loading config: %d\n", len ); + if( len < 0 ) + { + return len; + } + { char raw_fname[100]; sprintf( raw_fname, "%s_config.json", so->codename ); -- cgit v1.2.3