From f60bed509a7e416c155bcd35d5151bca65eaa190 Mon Sep 17 00:00:00 2001 From: Justin Berger Date: Wed, 21 Mar 2018 11:20:34 -0600 Subject: IMU research --- include/libsurvive/survive_imu.h | 27 +++++++++++++++++++++++++++ 1 file changed, 27 insertions(+) create mode 100644 include/libsurvive/survive_imu.h (limited to 'include/libsurvive') 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 -- cgit v1.2.3 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 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) (limited to 'include/libsurvive') 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 -- cgit v1.2.3 From db41a20170bb7f77959b9901a31582ad2ba93db7 Mon Sep 17 00:00:00 2001 From: Justin Berger Date: Tue, 3 Apr 2018 23:30:47 -0600 Subject: Madgwick code integrated --- include/libsurvive/survive_imu.h | 11 ++++++++++- 1 file changed, 10 insertions(+), 1 deletion(-) (limited to 'include/libsurvive') 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); -- cgit v1.2.3 From fe025b0ff6bfb440da7cec8f388fa951910a86f0 Mon Sep 17 00:00:00 2001 From: Justin Berger Date: Tue, 3 Apr 2018 23:40:29 -0600 Subject: Removed high pass filter --- include/libsurvive/survive_imu.h | 2 -- 1 file changed, 2 deletions(-) (limited to 'include/libsurvive') diff --git a/include/libsurvive/survive_imu.h b/include/libsurvive/survive_imu.h index e7a3d90..124ad7e 100644 --- a/include/libsurvive/survive_imu.h +++ b/include/libsurvive/survive_imu.h @@ -16,10 +16,8 @@ typedef struct { 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; -- cgit v1.2.3