From ea758056f60e254377e7d5d51b170a2b31d8e691 Mon Sep 17 00:00:00 2001 From: Ryan Pavlik Date: Mon, 11 Nov 2019 11:55:39 -0600 Subject: [PATCH] Start converting IMU stuff to timestamps. --- src/xrt/auxiliary/tracking/t_imu.cpp | 33 ++++++++++++++++++++-------- src/xrt/auxiliary/tracking/t_imu.h | 10 ++++----- 2 files changed, 29 insertions(+), 14 deletions(-) diff --git a/src/xrt/auxiliary/tracking/t_imu.cpp b/src/xrt/auxiliary/tracking/t_imu.cpp index 8a518225b..d4beaf6a3 100644 --- a/src/xrt/auxiliary/tracking/t_imu.cpp +++ b/src/xrt/auxiliary/tracking/t_imu.cpp @@ -16,11 +16,21 @@ struct imu_fusion { - uint64_t time_ns{}; + uint64_t time_ns{0}; xrt_fusion::SimpleIMUFusion simple_fusion; }; +static float +imu_fusion_get_dt(struct imu_fusion &fusion, uint64_t timestamp_ns) +{ + float dt = 0; + if (fusion.time_ns != 0) { + dt = (timestamp_ns - fusion.time_ns) * 1.e-9; + } + fusion.time_ns = timestamp_ns; + return dt; +} /* * API functions @@ -45,10 +55,9 @@ imu_fusion_destroy(struct imu_fusion *fusion) assert(false && "Caught exception on destroy"); } } - int imu_fusion_incorporate_gyros(struct imu_fusion *fusion, - float dt, + uint64_t timestamp_ns, struct xrt_vec3 const *ang_vel, struct xrt_vec3 const *variance) { @@ -57,6 +66,7 @@ imu_fusion_incorporate_gyros(struct imu_fusion *fusion, assert(ang_vel); assert(variance); + float dt = imu_fusion_get_dt(*fusion, timestamp_ns); fusion->simple_fusion.handleGyro( map_vec3(*ang_vel).cast(), dt); return 0; @@ -68,7 +78,7 @@ imu_fusion_incorporate_gyros(struct imu_fusion *fusion, int imu_fusion_incorporate_accelerometer(struct imu_fusion *fusion, - float dt, + uint64_t timestamp_ns, struct xrt_vec3 const *accel, struct xrt_vec3 const *variance) { @@ -77,6 +87,7 @@ imu_fusion_incorporate_accelerometer(struct imu_fusion *fusion, assert(accel); assert(variance); + float dt = imu_fusion_get_dt(*fusion, timestamp_ns); fusion->simple_fusion.handleAccel( map_vec3(*accel).cast(), dt); return 0; @@ -89,7 +100,7 @@ imu_fusion_incorporate_accelerometer(struct imu_fusion *fusion, int imu_fusion_get_prediction(struct imu_fusion const *fusion, - float dt, + uint64_t timestamp_ns, struct xrt_quat *out_quat, struct xrt_vec3 *out_ang_vel) { @@ -103,12 +114,14 @@ imu_fusion_get_prediction(struct imu_fusion const *fusion, map_vec3(*out_ang_vel) = fusion->simple_fusion.getAngVel().cast(); - if (dt == 0) { + + if (timestamp_ns == fusion->time_ns) { // No need to predict here. map_quat(*out_quat) = fusion->simple_fusion.getQuat().cast(); return 0; } + float dt = (timestamp_ns - fusion->time_ns) * 1.e-9; Eigen::Quaterniond predicted_quat = fusion->simple_fusion.getPredictedQuat(dt); map_quat(*out_quat) = predicted_quat.cast(); @@ -122,7 +135,7 @@ imu_fusion_get_prediction(struct imu_fusion const *fusion, int imu_fusion_get_prediction_rotation_vec(struct imu_fusion const *fusion, - float dt, + uint64_t timestamp_ns, struct xrt_vec3 *out_rotation_vec) { try { @@ -132,12 +145,13 @@ imu_fusion_get_prediction_rotation_vec(struct imu_fusion const *fusion, if (!fusion->simple_fusion.valid()) { return -2; } - if (dt == 0) { + if (timestamp_ns == fusion->time_ns) { // No need to predict here. map_vec3(*out_rotation_vec) = fusion->simple_fusion.getRotationVec() .cast(); } else { + float dt = (timestamp_ns - fusion->time_ns) * 1.e-9; Eigen::Quaterniond predicted_quat = fusion->simple_fusion.getPredictedQuat(dt); map_vec3(*out_rotation_vec) = @@ -154,7 +168,7 @@ imu_fusion_get_prediction_rotation_vec(struct imu_fusion const *fusion, int imu_fusion_incorporate_gyros_and_accelerometer( struct imu_fusion *fusion, - float dt, + uint64_t timestamp_ns, struct xrt_vec3 const *ang_vel, struct xrt_vec3 const *ang_vel_variance, struct xrt_vec3 const *accel, @@ -167,6 +181,7 @@ imu_fusion_incorporate_gyros_and_accelerometer( assert(accel); assert(accel_variance); + float dt = imu_fusion_get_dt(*fusion, timestamp_ns); Eigen::Vector3d accelVec = map_vec3(*accel).cast(); Eigen::Vector3d angVelVec = map_vec3(*ang_vel).cast(); fusion->simple_fusion.handleAccel(accelVec, dt); diff --git a/src/xrt/auxiliary/tracking/t_imu.h b/src/xrt/auxiliary/tracking/t_imu.h index ff10808db..57c57d187 100644 --- a/src/xrt/auxiliary/tracking/t_imu.h +++ b/src/xrt/auxiliary/tracking/t_imu.h @@ -54,7 +54,7 @@ imu_fusion_destroy(struct imu_fusion *fusion); */ int imu_fusion_incorporate_gyros(struct imu_fusion *fusion, - float dt, + uint64_t timestamp_ns, struct xrt_vec3 const *ang_vel, struct xrt_vec3 const *variance); @@ -73,7 +73,7 @@ imu_fusion_incorporate_gyros(struct imu_fusion *fusion, */ int imu_fusion_incorporate_accelerometer(struct imu_fusion *fusion, - float dt, + uint64_t timestamp_ns, struct xrt_vec3 const *accel, struct xrt_vec3 const *variance); @@ -91,7 +91,7 @@ imu_fusion_incorporate_accelerometer(struct imu_fusion *fusion, int imu_fusion_incorporate_gyros_and_accelerometer( struct imu_fusion *fusion, - float dt, + uint64_t timestamp_ns, struct xrt_vec3 const *ang_vel, struct xrt_vec3 const *ang_vel_variance, struct xrt_vec3 const *accel, @@ -107,7 +107,7 @@ imu_fusion_incorporate_gyros_and_accelerometer( */ int imu_fusion_get_prediction(struct imu_fusion const *fusion, - float dt, + uint64_t timestamp_ns, struct xrt_quat *out_quat, struct xrt_vec3 *out_ang_vel); @@ -123,7 +123,7 @@ imu_fusion_get_prediction(struct imu_fusion const *fusion, */ int imu_fusion_get_prediction_rotation_vec(struct imu_fusion const *fusion, - float dt, + uint64_t timestamp_ns, struct xrt_vec3 *out_rotation_vec); #ifdef __cplusplus } -- GitLab