Madgwick code integrated

4 files changed, 188 insertions, 60 deletions

diff --git a/include/libsurvive/survive_imu.h b/include/libsurvive/survive_imu.h
index a37a4df..e7a3d90 100644
--- a/include/libsurvive/survive_imu.h
+++ b/include/libsurvive/survive_imu.h
@@ -9,14 +9,23 @@
 extern "C" {
 #endif
 
+struct SurviveIMUTracker_p;
+
 typedef struct {
 	FLT updir[3];
-	LinmathVec3d current_velocity; // Velocity in world frame
+	FLT accel_scale_bias;
+
+	LinmathVec3d current_velocity;	// Velocity in world frame
+	LinmathVec3d current_velocity_lp; // Velocity in world frame
 	PoserDataIMU last_data;
 	SurvivePose pose;
+	LinmathPoint3d pos_lp;
 
 	SurvivePose lastGT;
 	uint32_t lastGTTime;
+
+	float integralFBx, integralFBy, integralFBz; // integral error terms scaled by Ki
+
 } SurviveIMUTracker;
 
 void survive_imu_tracker_set_pose(SurviveIMUTracker *tracker, uint32_t timecode, SurvivePose *pose);
diff --git a/src/poser_sba.c b/src/poser_sba.c
index 1dbc820..a23eb0f 100644
--- a/src/poser_sba.c
+++ b/src/poser_sba.c
@@ -272,12 +272,12 @@ static double run_sba_find_3d_structure(SBAData *d, PoserDataLight *pdl, Survive
 								info);			// info
 
 	if (currentPositionValid) {
-		FLT distp[3];
-		sub3d(distp, so->OutPose.Pos, soLocation.Pos);
-		FLT distance = magnitude3d(distp);
-		;
-		if (distance > 1.)
-			status = -1;
+		// FLT distp[3];
+		// sub3d(distp, so->OutPose.Pos, soLocation.Pos);
+		// FLT distance = magnitude3d(distp);
+
+		// if (distance > 1.)
+		//	status = -1;
 	}
 	if (status > 0 && (info[1] / meas_size * 2) < d->max_error) {
 		d->failures_to_reset_cntr = d->failures_to_reset;
@@ -296,7 +296,7 @@ static double run_sba_find_3d_structure(SBAData *d, PoserDataLight *pdl, Survive
 		}
 	}
 
-	return info[1] / meas_size * 2;
+	return status; // info[1] / meas_size * 2;
 }
 
 // Optimizes for LH position assuming object is posed at 0
@@ -396,12 +396,12 @@ int PoserSBA(SurviveObject *so, PoserData *pd) {
 		d->failures_to_reset = survive_configi(ctx, "sba-failures-to-reset", SC_GET, 1);
 		d->successes_to_reset_cntr = 0;
 		d->successes_to_reset = survive_configi(ctx, "sba-successes-to-reset", SC_GET, 100);
-
+		d->useIMU = true;
 		d->required_meas = survive_configi(ctx, "sba-required-meas", SC_GET, 8);
 		d->max_error = survive_configf(ctx, "sba-max-error", SC_GET, .0001);
 		d->sensor_time_window =
 			survive_configi(ctx, "sba-time-window", SC_GET, SurviveSensorActivations_default_tolerance * 2);
-		d->sensor_variance_per_second = survive_configf(ctx, "sba-sensor-variance-per-sec", SC_GET, 0.001);
+		d->sensor_variance_per_second = survive_configf(ctx, "sba-sensor-variance-per-sec", SC_GET, 10.0);
 		d->sensor_variance = survive_configf(ctx, "sba-sensor-variance", SC_GET, 1.0);
 		d->so = so;
 
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 <survive_imu.h>
 
+//---------------------------------------------------------------------------------------------------
+// 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;
 }
diff --git a/src/survive_vive.c b/src/survive_vive.c
index 53f6d42..1ffb737 100755
--- a/src/survive_vive.c
+++ b/src/survive_vive.c
@@ -1508,7 +1508,7 @@ void survive_data_cb( SurviveUSBInterface * si )
 				// 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);
+				calibrate_gyro(obj, agm + 3);
 				agm[3] *= conv;
 				agm[4] *= conv;
 				agm[5] *= conv;