Progress on IMU tracking

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 <survive_imu.h>
 
-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 );
 			}