From f60bed509a7e416c155bcd35d5151bca65eaa190 Mon Sep 17 00:00:00 2001 From: Justin Berger Date: Wed, 21 Mar 2018 11:20:34 -0600 Subject: IMU research --- src/survive_imu.c | 89 +++++++++++++++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 89 insertions(+) create mode 100644 src/survive_imu.c (limited to 'src/survive_imu.c') diff --git a/src/survive_imu.c b/src/survive_imu.c new file mode 100644 index 0000000..6fb3076 --- /dev/null +++ b/src/survive_imu.c @@ -0,0 +1,89 @@ +#include "survive_imu.h" +#include "linmath.h" +#include "survive_internal.h" +#include + +void survive_imu_tracker_set_pose(SurviveIMUTracker *tracker, SurvivePose *pose) { tracker->pose = *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); +} +static SurvivePose iterate_position(const SurvivePose *pose, const LinmathVec3d vel, double time_diff, + const PoserDataIMU *pIMU) { + SurvivePose result = *pose; + + FLT acc_mul = time_diff * time_diff / 2; + LinmathVec3d rAcc = {0}; + RotateAccel(rAcc, pose, pIMU->accel); + 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; + gyro[i] = time_diff / 2 * pIMU->gyro[i]; + } + + LinmathEulerAngle curr, next; + quattoeuler(curr, pose->Rot); + add3d(next, curr, gyro); + quatfromeuler(result.Rot, next); + + return result; +} + +static void iterate_velocity(LinmathVec3d result, const SurvivePose *pose, const LinmathVec3d vel, double time_diff, + PoserDataIMU *pIMU) { + scale3d(result, vel, .99999); + LinmathVec3d rAcc = {0}; + RotateAccel(rAcc, pose, pIMU->accel); + scale3d(rAcc, rAcc, time_diff); + add3d(result, result, rAcc); +} + +void survive_imu_tracker_integrate(SurviveObject *so, SurviveIMUTracker *tracker, PoserDataIMU *data) { + if (tracker->last_data.timecode == 0) { + if (tracker->last_data.datamask == imu_calibration_iterations) { + tracker->last_data = *data; + return; + } + + tracker->last_data.datamask++; + + tracker->updir[0] += data->accel[0] / imu_calibration_iterations; + tracker->updir[1] += data->accel[1] / imu_calibration_iterations; + tracker->updir[2] += data->accel[2] / imu_calibration_iterations; + return; + } + + for (int i = 0; i < 3; i++) { + tracker->updir[i] = data->accel[i] * .10 + tracker->updir[i] * .90; + } + + FLT up[3] = {0, 0, 1}; + FLT pose_up[3] = {0, 0, 1}; + quatrotatevector(pose_up, tracker->pose.Rot, tracker->updir); + + FLT time_diff = (data->timecode - tracker->last_data.timecode) / (FLT)so->timebase_hz; + + SurvivePose t_next = iterate_position(&tracker->pose, tracker->current_velocity, time_diff, data); + + LinmathVec3d v_next; + iterate_velocity(v_next, &tracker->pose, tracker->current_velocity, time_diff, data); + + tracker->pose = t_next; + scale3d(tracker->current_velocity, v_next, 1); + + tracker->last_data = *data; + + FLT tmp[3]; + ApplyPoseToPoint(tmp, &tracker->pose, up); + + printf("[%f, %f, %f] [%f, %f, %f]\n", tracker->pose.Pos[0], tracker->pose.Pos[1], tracker->pose.Pos[2], tmp[0], + tmp[1], tmp[2]); +} -- cgit v1.2.3 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 From 3272ffe5245c6f39f93572d4059f35953dc38faa Mon Sep 17 00:00:00 2001 From: Justin Berger Date: Thu, 22 Mar 2018 12:42:11 -0600 Subject: Integrated IMU into SBA --- src/survive_imu.c | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) (limited to 'src/survive_imu.c') diff --git a/src/survive_imu.c b/src/survive_imu.c index 6667d2f..5197266 100644 --- a/src/survive_imu.c +++ b/src/survive_imu.c @@ -7,7 +7,8 @@ void survive_imu_tracker_set_pose(SurviveIMUTracker *tracker, uint32_t timecode, tracker->pose = *pose; for (int i = 0; i < 3; i++) - tracker->current_velocity[i] = pose->Pos[i] - tracker->lastGT.Pos[i]; + tracker->current_velocity[i] = + (pose->Pos[i] - tracker->lastGT.Pos[i]) / (timecode - tracker->lastGTTime) * 48000000.; tracker->lastGTTime = timecode; tracker->lastGT = *pose; -- cgit v1.2.3 From 50b026760144e76a2e69babe34e6dee0a6a3c9c0 Mon Sep 17 00:00:00 2001 From: Justin Berger Date: Fri, 23 Mar 2018 07:26:31 +0000 Subject: Added reset cntr for sba --- src/survive_imu.c | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) (limited to 'src/survive_imu.c') diff --git a/src/survive_imu.c b/src/survive_imu.c index 5197266..297bbac 100644 --- a/src/survive_imu.c +++ b/src/survive_imu.c @@ -30,8 +30,8 @@ static SurvivePose iterate_position(const SurvivePose *pose, const LinmathVec3d 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)); + //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); @@ -103,8 +103,8 @@ void survive_imu_tracker_integrate(SurviveObject *so, SurviveIMUTracker *tracker tracker->pose = t_next; - fprintf(stderr, "%f %f %f\n", tracker->current_velocity[0], tracker->current_velocity[1], - tracker->current_velocity[2]); + //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); -- cgit v1.2.3 From db41a20170bb7f77959b9901a31582ad2ba93db7 Mon Sep 17 00:00:00 2001 From: Justin Berger Date: Tue, 3 Apr 2018 23:30:47 -0600 Subject: Madgwick code integrated --- src/survive_imu.c | 217 ++++++++++++++++++++++++++++++++++++++++++------------ 1 file changed, 168 insertions(+), 49 deletions(-) (limited to 'src/survive_imu.c') diff --git a/src/survive_imu.c b/src/survive_imu.c index 297bbac..cdf254b 100644 --- a/src/survive_imu.c +++ b/src/survive_imu.c @@ -3,13 +3,130 @@ #include "survive_internal.h" #include +//--------------------------------------------------------------------------------------------------- +// Definitions + +#define sampleFreq 240.0f // sample frequency in Hz +#define twoKpDef (2.0f * 0.5f) // 2 * proportional gain +#define twoKiDef (2.0f * 0.0f) // 2 * integral gain + +//--------------------------------------------------------------------------------------------------- +// Function declarations + +float invSqrt(float x) { + float halfx = 0.5f * x; + float y = x; + long i = *(long *)&y; + i = 0x5f3759df - (i >> 1); + y = *(float *)&i; + y = y * (1.5f - (halfx * y * y)); + return y; +} +//--------------------------------------------------------------------------------------------------- +// IMU algorithm update +// From http://x-io.co.uk/open-source-imu-and-ahrs-algorithms/ +static void MahonyAHRSupdateIMU(SurviveIMUTracker *tracker, float gx, float gy, float gz, float ax, float ay, + float az) { + float recipNorm; + float halfvx, halfvy, halfvz; + float halfex, halfey, halfez; + float qa, qb, qc; + + const float twoKp = twoKpDef; // 2 * proportional gain (Kp) + const float twoKi = twoKiDef; // 2 * integral gain (Ki) + + float q0 = tracker->pose.Rot[0]; + float q1 = tracker->pose.Rot[1]; + float q2 = tracker->pose.Rot[2]; + float q3 = tracker->pose.Rot[3]; + + // Compute feedback only if accelerometer measurement valid (avoids NaN in accelerometer normalisation) + if (!((ax == 0.0f) && (ay == 0.0f) && (az == 0.0f))) { + + // Normalise accelerometer measurement + recipNorm = invSqrt(ax * ax + ay * ay + az * az); + ax *= recipNorm; + ay *= recipNorm; + az *= recipNorm; + + // Estimated direction of gravity and vector perpendicular to magnetic flux + halfvx = q1 * q3 - q0 * q2; + halfvy = q0 * q1 + q2 * q3; + halfvz = q0 * q0 - 0.5f + q3 * q3; + + // Error is sum of cross product between estimated and measured direction of gravity + halfex = (ay * halfvz - az * halfvy); + halfey = (az * halfvx - ax * halfvz); + halfez = (ax * halfvy - ay * halfvx); + + // Compute and apply integral feedback if enabled + if (twoKi > 0.0f) { + tracker->integralFBx += twoKi * halfex * (1.0f / sampleFreq); // tracker->integral error scaled by Ki + tracker->integralFBy += twoKi * halfey * (1.0f / sampleFreq); + tracker->integralFBz += twoKi * halfez * (1.0f / sampleFreq); + gx += tracker->integralFBx; // apply tracker->integral feedback + gy += tracker->integralFBy; + gz += tracker->integralFBz; + } else { + tracker->integralFBx = 0.0f; // prevent tracker->integral windup + tracker->integralFBy = 0.0f; + tracker->integralFBz = 0.0f; + } + + // Apply proportional feedback + gx += twoKp * halfex; + gy += twoKp * halfey; + gz += twoKp * halfez; + } + + // Integrate rate of change of quaternion + gx *= (0.5f * (1.0f / sampleFreq)); // pre-multiply common factors + gy *= (0.5f * (1.0f / sampleFreq)); + gz *= (0.5f * (1.0f / sampleFreq)); + qa = q0; + qb = q1; + qc = q2; + q0 += (-qb * gx - qc * gy - q3 * gz); + q1 += (qa * gx + qc * gz - q3 * gy); + q2 += (qa * gy - qb * gz + q3 * gx); + q3 += (qa * gz + qb * gy - qc * gx); + + // Normalise quaternion + recipNorm = invSqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3); + q0 *= recipNorm; + q1 *= recipNorm; + q2 *= recipNorm; + q3 *= recipNorm; + + tracker->pose.Rot[0] = q0; + tracker->pose.Rot[1] = q1; + tracker->pose.Rot[2] = q2; + tracker->pose.Rot[3] = q3; +} + +static inline uint32_t tick_difference(uint32_t most_recent, uint32_t least_recent) { + uint32_t diff = 0; + if (most_recent > least_recent) { + diff = most_recent - least_recent; + } else { + diff = least_recent - most_recent; + } + + if (diff > 0xFFFFFFFF / 2) + return 0x7FFFFFFF / 2 - diff; + return diff; +} + 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]) / (timecode - tracker->lastGTTime) * 48000000.; + for (int i = 0; i < 3; i++) { + tracker->current_velocity_lp[i] = tracker->current_velocity[i] = 0; + tracker->pos_lp[i] = 0; + } + //(pose->Pos[i] - tracker->lastGT.Pos[i]) / tick_difference(timecode, tracker->lastGTTime) * 48000000.; + tracker->integralFBx = tracker->integralFBy = tracker->integralFBz = 0.0; tracker->lastGTTime = timecode; tracker->lastGT = *pose; } @@ -20,53 +137,54 @@ static void RotateAccel(LinmathVec3d rAcc, const SurvivePose *pose, const Linmat quatrotatevector(rAcc, pose->Rot, accel); LinmathVec3d G = {0, 0, -1}; add3d(rAcc, rAcc, G); - scale3d(rAcc, rAcc, 9.8); + scale3d(rAcc, rAcc, 9.8066); + FLT m = magnitude3d(rAcc); } -static SurvivePose iterate_position(const SurvivePose *pose, const LinmathVec3d vel, double time_diff, - const PoserDataIMU *pIMU) { - SurvivePose result = *pose; +static void iterate_position(SurviveIMUTracker *tracker, double time_diff, const PoserDataIMU *pIMU, FLT *out) { + const SurvivePose *pose = &tracker->pose; + const FLT *vel = tracker->current_velocity; - FLT acc_mul = time_diff * time_diff / 2; - LinmathVec3d rAcc = {0}; - RotateAccel(rAcc, pose, pIMU->accel); + for (int i = 0; i < 3; i++) + out[i] = pose->Pos[i]; - //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)); + FLT acc_mul = time_diff * time_diff / 2; + LinmathVec3d acc; + scale3d(acc, pIMU->accel, tracker->accel_scale_bias); + LinmathVec3d rAcc = {0}; + RotateAccel(rAcc, pose, acc); scale3d(rAcc, rAcc, acc_mul); - LinmathVec3d gyro; - for (int i = 0; i < 3; i++) { - result.Pos[i] += time_diff * vel[i] + rAcc[i]; - gyro[i] = time_diff / 2 * pIMU->gyro[i]; + out[i] += time_diff * (vel[i] - tracker->current_velocity_lp[i]); // + rAcc[i]; } - - LinmathEulerAngle curr, next; - quattoeuler(curr, pose->Rot); - add3d(next, curr, gyro); - quatfromeuler(result.Rot, next); - - return result; } -static void iterate_velocity(LinmathVec3d result, const SurvivePose *pose, const LinmathVec3d vel, double time_diff, - PoserDataIMU *pIMU) { +static void iterate_velocity(LinmathVec3d result, SurviveIMUTracker *tracker, double time_diff, PoserDataIMU *pIMU) { + const SurvivePose *pose = &tracker->pose; + const FLT *vel = tracker->current_velocity; scale3d(result, vel, 1.); + + LinmathVec3d acc; + scale3d(acc, pIMU->accel, tracker->accel_scale_bias); + LinmathVec3d rAcc = {0}; - RotateAccel(rAcc, pose, pIMU->accel); + RotateAccel(rAcc, pose, acc); + // fprintf(stderr, "%f\n", time_diff); scale3d(rAcc, rAcc, time_diff); add3d(result, result, rAcc); } void survive_imu_tracker_integrate(SurviveObject *so, SurviveIMUTracker *tracker, PoserDataIMU *data) { 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); + tracker->accel_scale_bias = 1. / magnitude3d(tracker->updir); return; } @@ -82,37 +200,38 @@ void survive_imu_tracker_integrate(SurviveObject *so, SurviveIMUTracker *tracker tracker->updir[i] = data->accel[i] * .10 + tracker->updir[i] * .90; } - const FLT up[3] = {0, 0, 1}; - LinmathQuat upRot, wouldbeUp; - LinmathVec3d rup; - quatrotatevector(rup, tracker->pose.Rot, up); - quatfrom2vectors(upRot, rup, data->accel); + float gx = data->gyro[0], gy = data->gyro[1], gz = data->gyro[2]; + float ax = data->accel[0], ay = data->accel[1], az = data->accel[2]; - quatrotateabout(wouldbeUp, upRot, tracker->pose.Rot); - quatslerp(tracker->pose.Rot, tracker->pose.Rot, wouldbeUp, .1); + MahonyAHRSupdateIMU(tracker, gx, gy, gz, ax, ay, az); - FLT pose_up[3] = {0, 0, 1}; - quatrotatevector(pose_up, tracker->pose.Rot, tracker->updir); + FLT time_diff = tick_difference(data->timecode, tracker->last_data.timecode) / (FLT)so->timebase_hz; - FLT time_diff = (data->timecode - tracker->last_data.timecode) / (FLT)so->timebase_hz; + if (tick_difference(data->timecode, tracker->lastGTTime) < 3200000 * 3) { + FLT next[3]; + iterate_position(tracker, time_diff, data, next); - SurvivePose t_next = iterate_position(&tracker->pose, tracker->current_velocity, time_diff, data); + LinmathVec3d v_next, lp_add; + iterate_velocity(v_next, tracker, time_diff, data); - LinmathVec3d v_next; - iterate_velocity(v_next, &tracker->pose, tracker->current_velocity, time_diff, data); + scale3d(tracker->current_velocity, v_next, 1); - tracker->pose = t_next; + FLT v_alpha = 1; + FLT p_alpha = 1; + scale3d(tracker->current_velocity_lp, tracker->current_velocity_lp, v_alpha); - //fprintf(stderr, "%f %f %f\n", tracker->current_velocity[0], tracker->current_velocity[1], - //tracker->current_velocity[2]); + scale3d(lp_add, v_next, 1 - v_alpha); + add3d(tracker->current_velocity_lp, tracker->current_velocity_lp, lp_add); - scale3d(tracker->current_velocity, v_next, 1); + LinmathPoint3d p_add; + scale3d(p_add, next, 1 - p_alpha); + scale3d(tracker->pos_lp, tracker->pos_lp, p_alpha); + add3d(tracker->pos_lp, tracker->pos_lp, p_add); - tracker->last_data = *data; - - FLT tmp[3]; - ApplyPoseToPoint(tmp, &tracker->pose, up); + for (int i = 0; i < 3; i++) + tracker->pose.Pos[i] = next[i] - tracker->pos_lp[i]; + // scale3d(tracker->pose.Pos, next, 1); + } - printf("[%f, %f, %f] [%f, %f, %f]\n", tracker->pose.Pos[0], tracker->pose.Pos[1], tracker->pose.Pos[2], tmp[0], - tmp[1], tmp[2]); + tracker->last_data = *data; } -- cgit v1.2.3 From fe025b0ff6bfb440da7cec8f388fa951910a86f0 Mon Sep 17 00:00:00 2001 From: Justin Berger Date: Tue, 3 Apr 2018 23:40:29 -0600 Subject: Removed high pass filter --- src/survive_imu.c | 25 ++++--------------------- 1 file changed, 4 insertions(+), 21 deletions(-) (limited to 'src/survive_imu.c') diff --git a/src/survive_imu.c b/src/survive_imu.c index cdf254b..e49da3e 100644 --- a/src/survive_imu.c +++ b/src/survive_imu.c @@ -121,8 +121,7 @@ void survive_imu_tracker_set_pose(SurviveIMUTracker *tracker, uint32_t timecode, tracker->pose = *pose; for (int i = 0; i < 3; i++) { - tracker->current_velocity_lp[i] = tracker->current_velocity[i] = 0; - tracker->pos_lp[i] = 0; + tracker->current_velocity[i] = 0; } //(pose->Pos[i] - tracker->lastGT.Pos[i]) / tick_difference(timecode, tracker->lastGTTime) * 48000000.; @@ -156,7 +155,7 @@ static void iterate_position(SurviveIMUTracker *tracker, double time_diff, const scale3d(rAcc, rAcc, acc_mul); for (int i = 0; i < 3; i++) { - out[i] += time_diff * (vel[i] - tracker->current_velocity_lp[i]); // + rAcc[i]; + out[i] += time_diff * vel[i] + rAcc[i]; } } @@ -170,7 +169,6 @@ static void iterate_velocity(LinmathVec3d result, SurviveIMUTracker *tracker, do LinmathVec3d rAcc = {0}; RotateAccel(rAcc, pose, acc); - // fprintf(stderr, "%f\n", time_diff); scale3d(rAcc, rAcc, time_diff); add3d(result, result, rAcc); } @@ -211,26 +209,11 @@ void survive_imu_tracker_integrate(SurviveObject *so, SurviveIMUTracker *tracker FLT next[3]; iterate_position(tracker, time_diff, data, next); - LinmathVec3d v_next, lp_add; + LinmathVec3d v_next; iterate_velocity(v_next, tracker, time_diff, data); scale3d(tracker->current_velocity, v_next, 1); - - FLT v_alpha = 1; - FLT p_alpha = 1; - scale3d(tracker->current_velocity_lp, tracker->current_velocity_lp, v_alpha); - - scale3d(lp_add, v_next, 1 - v_alpha); - add3d(tracker->current_velocity_lp, tracker->current_velocity_lp, lp_add); - - LinmathPoint3d p_add; - scale3d(p_add, next, 1 - p_alpha); - scale3d(tracker->pos_lp, tracker->pos_lp, p_alpha); - add3d(tracker->pos_lp, tracker->pos_lp, p_add); - - for (int i = 0; i < 3; i++) - tracker->pose.Pos[i] = next[i] - tracker->pos_lp[i]; - // scale3d(tracker->pose.Pos, next, 1); + scale3d(tracker->pose.Pos, next, 1); } tracker->last_data = *data; -- cgit v1.2.3