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 --- include/libsurvive/survive_imu.h | 6 ++++-- src/poser_imu.c | 2 ++ src/survive_default_devices.c | 9 ++++----- src/survive_imu.c | 38 +++++++++++++++++++++++++++++++++----- src/survive_vive.c | 39 +++++++++++++++++++++++---------------- 5 files changed, 66 insertions(+), 28 deletions(-) diff --git a/include/libsurvive/survive_imu.h b/include/libsurvive/survive_imu.h index 323cb6a..a37a4df 100644 --- a/include/libsurvive/survive_imu.h +++ b/include/libsurvive/survive_imu.h @@ -11,13 +11,15 @@ extern "C" { typedef struct { FLT updir[3]; - LinmathVec3d current_velocity; + LinmathVec3d current_velocity; // Velocity in world frame PoserDataIMU last_data; SurvivePose pose; + SurvivePose lastGT; + uint32_t lastGTTime; } SurviveIMUTracker; -void survive_imu_tracker_set_pose(SurviveIMUTracker *tracker, SurvivePose *pose); +void survive_imu_tracker_set_pose(SurviveIMUTracker *tracker, uint32_t timecode, SurvivePose *pose); void survive_imu_tracker_integrate(SurviveObject *so, SurviveIMUTracker *tracker, PoserDataIMU *data); #ifdef __cplusplus diff --git a/src/poser_imu.c b/src/poser_imu.c index 8ca716b..02ff8e9 100644 --- a/src/poser_imu.c +++ b/src/poser_imu.c @@ -22,6 +22,8 @@ int PoserIMU(SurviveObject *so, PoserData *pd) { PoserData_poser_raw_pose_func(pd, so, -1, &dd->pose); + // if(magnitude3d(dd->pose.Pos) > 1) + // SV_ERROR("IMU drift"); return 0; } } diff --git a/src/survive_default_devices.c b/src/survive_default_devices.c index 6b65752..2e47b9e 100644 --- a/src/survive_default_devices.c +++ b/src/survive_default_devices.c @@ -144,11 +144,10 @@ int survive_load_htc_config_format(SurviveObject *so, char *ct0conf, int len) { FLT *values = NULL; if (parse_float_array(ct0conf, tk + 2, &values, count) > 0) { so->acc_bias = values; - so->acc_bias[0] *= .125; // XXX Wat? Observed by CNL. Biasing - // by more than this seems to hose - // things. - so->acc_bias[1] *= .125; - so->acc_bias[2] *= .125; + const FLT bias_units = 1. / 1000.; // I deeply suspect bias is in milligravities -JB + so->acc_bias[0] *= bias_units; + so->acc_bias[1] *= bias_units; + so->acc_bias[2] *= bias_units; } } if (jsoneq(ct0conf, tk, "acc_scale") == 0) { 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; diff --git a/src/survive_vive.c b/src/survive_vive.c index d431207..91f4b1f 100755 --- a/src/survive_vive.c +++ b/src/survive_vive.c @@ -1485,26 +1485,33 @@ void survive_data_cb( SurviveUSBInterface * si ) obj->oldcode = code; //XXX XXX BIG TODO!!! Actually recal gyro data. - FLT agm[9] = { acceldata[0].v, acceldata[1].v, acceldata[2].v, - acceldata[3].v, acceldata[4].v, acceldata[5].v, - 0,0,0 }; - - agm[0]*=(float)(1./8192.0); - agm[1]*=(float)(1./8192.0); - agm[2]*=(float)(1./8192.0); - calibrate_acc(obj, agm); + FLT agm[9] = {acceldata[0].v, + acceldata[1].v, + acceldata[2].v, + acceldata[3].v, + acceldata[4].v, + acceldata[5].v, + 0, + 0, + 0}; //1G for accelerometer, from MPU6500 datasheet //this can change if the firmware changes the sensitivity. + // When coming off of USB, these values are in units of .5g -JB + agm[0] *= (float)(2. / 8192.0); + agm[1] *= (float)(2. / 8192.0); + agm[2] *= (float)(2. / 8192.0); + calibrate_acc(obj, agm); - - agm[3]*=(float)((1./32.768)*(3.14159/180.)); - agm[4]*=(float)((1./32.768)*(3.14159/180.)); - agm[5]*=(float)((1./32.768)*(3.14159/180.)); - calibrate_gyro(obj, agm+3); - - //65.5 deg/s for gyroscope, from MPU6500 datasheet - //1000 deg/s for gyroscope, from MPU6500 datasheet + // From datasheet, can be 250, 500, 1000, 2000 deg/s range over 16 bits + // FLT deg_per_sec = 250; + // FLT conv = (float)((1./deg_per_sec)*(3.14159/180.)) / 8192.; + FLT DEGREES_TO_RADS = 3.14159 / 180.; + FLT conv = 1. / 10. * DEGREES_TO_RADS; + // calibrate_gyro(obj, agm+3); + agm[3] *= conv; + agm[4] *= conv; + agm[5] *= conv; ctx->imuproc( obj, 3, agm, timecode, code ); } -- cgit v1.2.3