From f60bed509a7e416c155bcd35d5151bca65eaa190 Mon Sep 17 00:00:00 2001 From: Justin Berger Date: Wed, 21 Mar 2018 11:20:34 -0600 Subject: IMU research --- Makefile | 3 +- include/libsurvive/survive_imu.h | 27 ++++++++++++ redist/linmath.h | 2 +- src/poser_imu.c | 31 ++++++++++++++ src/survive_imu.c | 89 ++++++++++++++++++++++++++++++++++++++++ 5 files changed, 150 insertions(+), 2 deletions(-) create mode 100644 include/libsurvive/survive_imu.h create mode 100644 src/poser_imu.c create mode 100644 src/survive_imu.c 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 + +#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 +#include + +#include +#include + +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 + +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