From 7fadeeff29074520224be385ecd6c50271a07220 Mon Sep 17 00:00:00 2001 From: Justin Berger Date: Sun, 11 Mar 2018 23:31:47 -0600 Subject: FLT* -> SurvivePose* --- include/libsurvive/survive.h | 3 +-- include/libsurvive/survive_types.h | 2 +- simple_pose_test.c | 14 +++++++------- src/poser_turveytori.c | 9 +++------ src/survive_process.c | 3 +-- test.c | 6 +++--- 6 files changed, 16 insertions(+), 21 deletions(-) diff --git a/include/libsurvive/survive.h b/include/libsurvive/survive.h index 9840908..58462b3 100644 --- a/include/libsurvive/survive.h +++ b/include/libsurvive/survive.h @@ -201,8 +201,7 @@ void survive_default_light_process( SurviveObject * so, int sensor_id, int acode void survive_default_imu_process( SurviveObject * so, int mode, FLT * accelgyro, uint32_t timecode, int id ); void survive_default_angle_process( SurviveObject * so, int sensor_id, int acode, uint32_t timecode, FLT length, FLT angle, uint32_t lh ); void survive_default_button_process(SurviveObject * so, uint8_t eventType, uint8_t buttonId, uint8_t axis1Id, uint16_t axis1Val, uint8_t axis2Id, uint16_t axis2Val); -void survive_default_raw_pose_process(SurviveObject * so, uint8_t lighthouse, FLT *pose); - +void survive_default_raw_pose_process(SurviveObject *so, uint8_t lighthouse, SurvivePose *pose); ////////////////////// Survive Drivers //////////////////////////// diff --git a/include/libsurvive/survive_types.h b/include/libsurvive/survive_types.h index 37a1303..4db76b4 100644 --- a/include/libsurvive/survive_types.h +++ b/include/libsurvive/survive_types.h @@ -49,7 +49,7 @@ typedef void (*light_process_func)( SurviveObject * so, int sensor_id, int acode typedef void (*imu_process_func)( SurviveObject * so, int mask, FLT * accelgyro, uint32_t timecode, int id ); typedef void (*angle_process_func)( SurviveObject * so, int sensor_id, int acode, uint32_t timecode, FLT length, FLT angle, uint32_t lh); typedef void(*button_process_func)(SurviveObject * so, uint8_t eventType, uint8_t buttonId, uint8_t axis1Id, uint16_t axis1Val, uint8_t axis2Id, uint16_t axis2Val); -typedef void(*raw_pose_func)(SurviveObject * so, uint8_t lighthouse, FLT *pose); +typedef void (*raw_pose_func)(SurviveObject *so, uint8_t lighthouse, SurvivePose *pose); typedef int(*haptic_func)(SurviveObject * so, uint8_t reserved, uint16_t pulseHigh , uint16_t pulseLow, uint16_t repeatCount); diff --git a/simple_pose_test.c b/simple_pose_test.c index 71b6ae9..b39c932 100644 --- a/simple_pose_test.c +++ b/simple_pose_test.c @@ -42,8 +42,7 @@ void HandleDestroy() FLT hpos[3]; FLT hpos2[3]; -void testprog_raw_pose_process(SurviveObject * so, uint8_t lighthouse, FLT *pose) -{ +void testprog_raw_pose_process(SurviveObject *so, uint8_t lighthouse, SurvivePose *pose) { survive_default_raw_pose_process(so, lighthouse, pose ); if( lighthouse != 0 || strcmp( so->codename, "HMD" ) != 0 ) @@ -65,13 +64,14 @@ void testprog_raw_pose_process(SurviveObject * so, uint8_t lighthouse, FLT *pose hy = pos[1]; hz = pos[2];*/ - printf("Pose: [%1.1x][%s][% 08.8f,% 08.8f,% 08.8f] [ang:%08.2f %08.2f %08.2f %08.2f]\n", lighthouse, so->codename, pose[0], pose[1], pose[2], pose[3], pose[4], pose[5], pose[6] ); + printf("Pose: [%1.1x][%s][% 08.8f,% 08.8f,% 08.8f] [ang:%08.2f %08.2f %08.2f %08.2f]\n", lighthouse, so->codename, + pose->Pos[0], pose->Pos[1], pose->Pos[2], pose->Rot[0], pose->Rot[1], pose->Rot[2], pose->Rot[3]); - hpos[0] = pose[0]; - hpos[1] = pose[1]; - hpos[2] = pose[2]; + hpos[0] = pose->Pos[0]; + hpos[1] = pose->Pos[1]; + hpos[2] = pose->Pos[2]; FLT hposin[3] = { 0, 0, 1 }; - ApplyPoseToPoint(hpos2, pose, hposin); + ApplyPoseToPoint(hpos2, &pose->Pos[0], hposin); fflush(stdout); } diff --git a/src/poser_turveytori.c b/src/poser_turveytori.c index 4398b66..dae51e6 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 pose[7]; + SurvivePose pose; // TODO: This countdown stuff is a total hack! // it basically ignores all the logic to find the most reliable data points @@ -1633,16 +1633,13 @@ 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); + SolveForLighthouse(&pose.Pos[0], &pose.Rot[0], 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, pose); + so->ctx->rawposeproc(so, lh, &pose); } if (ttDebug) printf("!\n"); diff --git a/src/survive_process.c b/src/survive_process.c index 1ea3061..d156d9e 100644 --- a/src/survive_process.c +++ b/src/survive_process.c @@ -102,8 +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 *pose) -{ +void survive_default_raw_pose_process(SurviveObject *so, uint8_t lighthouse, SurvivePose *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/test.c b/test.c index ef7ae45..bc9677a 100644 --- a/test.c +++ b/test.c @@ -84,12 +84,12 @@ void testprog_button_process(SurviveObject * so, uint8_t eventType, uint8_t butt } } -void testprog_raw_pose_process(SurviveObject * so, uint8_t lighthouse, FLT *pose) -{ +void testprog_raw_pose_process(SurviveObject *so, uint8_t lighthouse, SurvivePose *pose) { survive_default_raw_pose_process(so, lighthouse, 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, pose[0], pose[1], pose[2], pose[3], pose[4], pose[5], pose[6]); + printf("Pose: [%1.1x][%s][% 08.8f,% 08.8f,% 08.8f] [% 08.8f,% 08.8f,% 08.8f,% 08.8f]\n", lighthouse, so->codename, + pose->Pos[0], pose->Pos[1], pose->Pos[2], pose->Rot[0], pose->Rot[1], pose->Rot[2], pose->Rot[3]); } void testprog_imu_process(SurviveObject * so, int mask, FLT * accelgyromag, uint32_t timecode, int id) -- cgit v1.2.3