IMU research

5 files changed, 150 insertions, 2 deletions

diff --git a/Makefile b/Makefile
index a6d139b..de74589 100644
--- a/Makefile
+++ b/Makefile
@@ -39,7 +39,8 @@ REDISTS:=redist/json_helpers.o redist/linmath.o redist/jsmn.o redist/os_generic.
 ifeq ($(UNAME), Darwin)
 REDISTS:=$(REDISTS) redist/hid-osx.c
 endif
-LIBSURVIVE_CORE:=src/survive.o src/survive_usb.o src/survive_charlesbiguator.o src/survive_process.o src/ootx_decoder.o src/survive_driverman.o src/survive_default_devices.o src/survive_vive.o src/survive_playback.o src/survive_config.o src/survive_cal.o src/survive_reproject.o src/poser.o src/epnp/epnp.c src/survive_sensor_activations.o src/survive_turveybiguator.o src/survive_disambiguator.o
+
+LIBSURVIVE_CORE:=src/survive.o src/survive_usb.o src/survive_charlesbiguator.o src/survive_process.o src/ootx_decoder.o src/survive_driverman.o src/survive_default_devices.o src/survive_vive.o src/survive_playback.o src/survive_config.o src/survive_cal.o src/survive_reproject.o src/poser.o src/epnp/epnp.c src/survive_sensor_activations.o src/survive_turveybiguator.o src/survive_disambiguator.o  src/survive_imu.o src/poser_imu.o
 
 #If you want to use HIDAPI on Linux.
 #CFLAGS:=$(CFLAGS) -DHIDAPI
diff --git a/include/libsurvive/survive_imu.h b/include/libsurvive/survive_imu.h
new file mode 100644
index 0000000..323cb6a
--- /dev/null
+++ b/include/libsurvive/survive_imu.h
@@ -0,0 +1,27 @@
+#ifndef _SURVIVE_IMU_H
+#define _SURVIVE_IMU_H
+
+#include "poser.h"
+#include "survive_types.h"
+#include <stdint.h>
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+typedef struct {
+	FLT updir[3];
+	LinmathVec3d current_velocity;
+	PoserDataIMU last_data;
+	SurvivePose pose;
+
+} SurviveIMUTracker;
+
+void survive_imu_tracker_set_pose(SurviveIMUTracker *tracker, SurvivePose *pose);
+void survive_imu_tracker_integrate(SurviveObject *so, SurviveIMUTracker *tracker, PoserDataIMU *data);
+
+#ifdef __cplusplus
+};
+#endif
+
+#endif
diff --git a/redist/linmath.h b/redist/linmath.h
index cacb1c6..5d5bed2 100644
--- a/redist/linmath.h
+++ b/redist/linmath.h
@@ -49,7 +49,7 @@
 
 typedef FLT LinmathQuat[4]; // This is the [wxyz] quaternion, in wxyz format.
 typedef FLT LinmathPoint3d[3];
-typedef FLT linmathVec3d[3];
+typedef FLT LinmathVec3d[3];
 
 typedef struct LinmathPose {
 	LinmathPoint3d Pos;
diff --git a/src/poser_imu.c b/src/poser_imu.c
new file mode 100644
index 0000000..8ca716b
--- /dev/null
+++ b/src/poser_imu.c
@@ -0,0 +1,31 @@
+#include <survive.h>
+#include <survive_imu.h>
+
+#include <stdio.h>
+#include <stdlib.h>
+
+int PoserIMU(SurviveObject *so, PoserData *pd) {
+	PoserType pt = pd->pt;
+	SurviveContext *ctx = so->ctx;
+	SurviveIMUTracker *dd = so->PoserData;
+
+	if (!dd) {
+		so->PoserData = dd = malloc(sizeof(SurviveIMUTracker));
+		*dd = (SurviveIMUTracker){};
+	}
+
+	switch (pt) {
+	case POSERDATA_IMU: {
+		PoserDataIMU *imu = (PoserDataIMU *)pd;
+
+		survive_imu_tracker_integrate(so, dd, imu);
+
+		PoserData_poser_raw_pose_func(pd, so, -1, &dd->pose);
+
+		return 0;
+	}
+	}
+	return -1;
+}
+
+REGISTER_LINKTIME(PoserIMU);
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 <survive_imu.h>
+
+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]);
+}