diff options
Diffstat (limited to 'src/survive_process.c')
| -rw-r--r-- | src/survive_process.c | 35 |
1 files changed, 19 insertions, 16 deletions
diff --git a/src/survive_process.c b/src/survive_process.c index b697f4a..a49b632 100644 --- a/src/survive_process.c +++ b/src/survive_process.c @@ -115,9 +115,10 @@ void survive_default_raw_pose_process(SurviveObject *so, uint8_t lighthouse, Sur so->FromLHPose[lighthouse] = *pose; } -void survive_default_lighthouse_pose_process(SurviveContext *ctx, uint8_t lighthouse, SurvivePose *pose) { - if (pose) { - ctx->bsd[lighthouse].Pose = *pose; +void survive_default_lighthouse_pose_process(SurviveContext *ctx, uint8_t lighthouse, SurvivePose *lighthouse_pose, + SurvivePose *object_pose) { + if (lighthouse_pose) { + ctx->bsd[lighthouse].Pose = *lighthouse_pose; ctx->bsd[lighthouse].PositionSet = 1; } else { ctx->bsd[lighthouse].PositionSet = 0; @@ -132,19 +133,21 @@ int survive_default_htc_config_process(SurviveObject *so, char *ct0conf, int len } void survive_default_imu_process( SurviveObject * so, int mask, FLT * accelgyromag, uint32_t timecode, int id ) { - if( so->PoserFn ) - { - PoserDataIMU imu = { - .hdr = - { - .pt = POSERDATA_IMU, - }, - .datamask = mask, - .accel = {accelgyromag[0], accelgyromag[1], accelgyromag[2]}, - .gyro = {accelgyromag[3], accelgyromag[4], accelgyromag[5]}, - .mag = {accelgyromag[6], accelgyromag[7], accelgyromag[8]}, - .timecode = timecode, - }; + PoserDataIMU imu = { + .hdr = + { + .pt = POSERDATA_IMU, + }, + .datamask = mask, + .accel = {accelgyromag[0], accelgyromag[1], accelgyromag[2]}, + .gyro = {accelgyromag[3], accelgyromag[4], accelgyromag[5]}, + .mag = {accelgyromag[6], accelgyromag[7], accelgyromag[8]}, + .timecode = timecode, + }; + + SurviveSensorActivations_add_imu(&so->activations, &imu); + + if (so->PoserFn) { so->PoserFn( so, (PoserData *)&imu ); } } |