From dcf5d7a482e022e762a656253017ebbc721d8a83 Mon Sep 17 00:00:00 2001 From: Justin Berger Date: Thu, 22 Mar 2018 12:06:41 -0600 Subject: Progress on IMU tracking --- src/survive_imu.c | 38 +++++++++++++++++++++++++++++++++----- 1 file changed, 33 insertions(+), 5 deletions(-) (limited to 'src/survive_imu.c') diff --git a/src/survive_imu.c b/src/survive_imu.c index 6fb3076..6667d2f 100644 --- a/src/survive_imu.c +++ b/src/survive_imu.c @@ -3,15 +3,23 @@ #include "survive_internal.h" #include -void survive_imu_tracker_set_pose(SurviveIMUTracker *tracker, SurvivePose *pose) { tracker->pose = *pose; } +void survive_imu_tracker_set_pose(SurviveIMUTracker *tracker, uint32_t timecode, SurvivePose *pose) { + tracker->pose = *pose; + + for (int i = 0; i < 3; i++) + tracker->current_velocity[i] = pose->Pos[i] - tracker->lastGT.Pos[i]; + + tracker->lastGTTime = timecode; + tracker->lastGT = *pose; +} static const int imu_calibration_iterations = 100; static void RotateAccel(LinmathVec3d rAcc, const SurvivePose *pose, const LinmathVec3d accel) { quatrotatevector(rAcc, pose->Rot, accel); - scale3d(rAcc, rAcc, 2.); LinmathVec3d G = {0, 0, -1}; add3d(rAcc, rAcc, G); + scale3d(rAcc, rAcc, 9.8); } static SurvivePose iterate_position(const SurvivePose *pose, const LinmathVec3d vel, double time_diff, const PoserDataIMU *pIMU) { @@ -20,12 +28,16 @@ static SurvivePose iterate_position(const SurvivePose *pose, const LinmathVec3d FLT acc_mul = time_diff * time_diff / 2; LinmathVec3d rAcc = {0}; RotateAccel(rAcc, pose, pIMU->accel); + + fprintf(stderr, "r %f %f %f %f\n", pIMU->accel[0], pIMU->accel[1], pIMU->accel[2], quatmagnitude(pIMU->accel)); + fprintf(stderr, "i %f %f %f %f\n", rAcc[0], rAcc[1], rAcc[2], quatmagnitude(rAcc)); + scale3d(rAcc, rAcc, acc_mul); LinmathVec3d gyro; for (int i = 0; i < 3; i++) { - result.Pos[i] += time_diff * vel[i] + rAcc[i] * 9.8; + result.Pos[i] += time_diff * vel[i] + rAcc[i]; gyro[i] = time_diff / 2 * pIMU->gyro[i]; } @@ -39,7 +51,7 @@ static SurvivePose iterate_position(const SurvivePose *pose, const LinmathVec3d static void iterate_velocity(LinmathVec3d result, const SurvivePose *pose, const LinmathVec3d vel, double time_diff, PoserDataIMU *pIMU) { - scale3d(result, vel, .99999); + scale3d(result, vel, 1.); LinmathVec3d rAcc = {0}; RotateAccel(rAcc, pose, pIMU->accel); scale3d(rAcc, rAcc, time_diff); @@ -50,6 +62,10 @@ void survive_imu_tracker_integrate(SurviveObject *so, SurviveIMUTracker *tracker if (tracker->last_data.timecode == 0) { if (tracker->last_data.datamask == imu_calibration_iterations) { tracker->last_data = *data; + tracker->pose.Rot[0] = 1.; + + const FLT up[3] = {0, 0, 1}; + quatfrom2vectors(tracker->pose.Rot, tracker->updir, up); return; } @@ -65,7 +81,15 @@ void survive_imu_tracker_integrate(SurviveObject *so, SurviveIMUTracker *tracker tracker->updir[i] = data->accel[i] * .10 + tracker->updir[i] * .90; } - FLT up[3] = {0, 0, 1}; + const FLT up[3] = {0, 0, 1}; + LinmathQuat upRot, wouldbeUp; + LinmathVec3d rup; + quatrotatevector(rup, tracker->pose.Rot, up); + quatfrom2vectors(upRot, rup, data->accel); + + quatrotateabout(wouldbeUp, upRot, tracker->pose.Rot); + quatslerp(tracker->pose.Rot, tracker->pose.Rot, wouldbeUp, .1); + FLT pose_up[3] = {0, 0, 1}; quatrotatevector(pose_up, tracker->pose.Rot, tracker->updir); @@ -77,6 +101,10 @@ void survive_imu_tracker_integrate(SurviveObject *so, SurviveIMUTracker *tracker iterate_velocity(v_next, &tracker->pose, tracker->current_velocity, time_diff, data); tracker->pose = t_next; + + fprintf(stderr, "%f %f %f\n", tracker->current_velocity[0], tracker->current_velocity[1], + tracker->current_velocity[2]); + scale3d(tracker->current_velocity, v_next, 1); tracker->last_data = *data; -- cgit v1.2.3