Commit ea758056 authored by Ryan Pavlik's avatar Ryan Pavlik

Start converting IMU stuff to timestamps.

parent a308930e
Pipeline #78030 failed with stages
in 3 minutes and 43 seconds
...@@ -16,11 +16,21 @@ ...@@ -16,11 +16,21 @@
struct imu_fusion struct imu_fusion
{ {
uint64_t time_ns{}; uint64_t time_ns{0};
xrt_fusion::SimpleIMUFusion simple_fusion; 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 * API functions
...@@ -45,10 +55,9 @@ imu_fusion_destroy(struct imu_fusion *fusion) ...@@ -45,10 +55,9 @@ imu_fusion_destroy(struct imu_fusion *fusion)
assert(false && "Caught exception on destroy"); assert(false && "Caught exception on destroy");
} }
} }
int int
imu_fusion_incorporate_gyros(struct imu_fusion *fusion, imu_fusion_incorporate_gyros(struct imu_fusion *fusion,
float dt, uint64_t timestamp_ns,
struct xrt_vec3 const *ang_vel, struct xrt_vec3 const *ang_vel,
struct xrt_vec3 const *variance) struct xrt_vec3 const *variance)
{ {
...@@ -57,6 +66,7 @@ imu_fusion_incorporate_gyros(struct imu_fusion *fusion, ...@@ -57,6 +66,7 @@ imu_fusion_incorporate_gyros(struct imu_fusion *fusion,
assert(ang_vel); assert(ang_vel);
assert(variance); assert(variance);
float dt = imu_fusion_get_dt(*fusion, timestamp_ns);
fusion->simple_fusion.handleGyro( fusion->simple_fusion.handleGyro(
map_vec3(*ang_vel).cast<double>(), dt); map_vec3(*ang_vel).cast<double>(), dt);
return 0; return 0;
...@@ -68,7 +78,7 @@ imu_fusion_incorporate_gyros(struct imu_fusion *fusion, ...@@ -68,7 +78,7 @@ imu_fusion_incorporate_gyros(struct imu_fusion *fusion,
int int
imu_fusion_incorporate_accelerometer(struct imu_fusion *fusion, imu_fusion_incorporate_accelerometer(struct imu_fusion *fusion,
float dt, uint64_t timestamp_ns,
struct xrt_vec3 const *accel, struct xrt_vec3 const *accel,
struct xrt_vec3 const *variance) struct xrt_vec3 const *variance)
{ {
...@@ -77,6 +87,7 @@ imu_fusion_incorporate_accelerometer(struct imu_fusion *fusion, ...@@ -77,6 +87,7 @@ imu_fusion_incorporate_accelerometer(struct imu_fusion *fusion,
assert(accel); assert(accel);
assert(variance); assert(variance);
float dt = imu_fusion_get_dt(*fusion, timestamp_ns);
fusion->simple_fusion.handleAccel( fusion->simple_fusion.handleAccel(
map_vec3(*accel).cast<double>(), dt); map_vec3(*accel).cast<double>(), dt);
return 0; return 0;
...@@ -89,7 +100,7 @@ imu_fusion_incorporate_accelerometer(struct imu_fusion *fusion, ...@@ -89,7 +100,7 @@ imu_fusion_incorporate_accelerometer(struct imu_fusion *fusion,
int int
imu_fusion_get_prediction(struct imu_fusion const *fusion, imu_fusion_get_prediction(struct imu_fusion const *fusion,
float dt, uint64_t timestamp_ns,
struct xrt_quat *out_quat, struct xrt_quat *out_quat,
struct xrt_vec3 *out_ang_vel) struct xrt_vec3 *out_ang_vel)
{ {
...@@ -103,12 +114,14 @@ imu_fusion_get_prediction(struct imu_fusion const *fusion, ...@@ -103,12 +114,14 @@ imu_fusion_get_prediction(struct imu_fusion const *fusion,
map_vec3(*out_ang_vel) = map_vec3(*out_ang_vel) =
fusion->simple_fusion.getAngVel().cast<float>(); fusion->simple_fusion.getAngVel().cast<float>();
if (dt == 0) {
if (timestamp_ns == fusion->time_ns) {
// No need to predict here. // No need to predict here.
map_quat(*out_quat) = map_quat(*out_quat) =
fusion->simple_fusion.getQuat().cast<float>(); fusion->simple_fusion.getQuat().cast<float>();
return 0; return 0;
} }
float dt = (timestamp_ns - fusion->time_ns) * 1.e-9;
Eigen::Quaterniond predicted_quat = Eigen::Quaterniond predicted_quat =
fusion->simple_fusion.getPredictedQuat(dt); fusion->simple_fusion.getPredictedQuat(dt);
map_quat(*out_quat) = predicted_quat.cast<float>(); map_quat(*out_quat) = predicted_quat.cast<float>();
...@@ -122,7 +135,7 @@ imu_fusion_get_prediction(struct imu_fusion const *fusion, ...@@ -122,7 +135,7 @@ imu_fusion_get_prediction(struct imu_fusion const *fusion,
int int
imu_fusion_get_prediction_rotation_vec(struct imu_fusion const *fusion, imu_fusion_get_prediction_rotation_vec(struct imu_fusion const *fusion,
float dt, uint64_t timestamp_ns,
struct xrt_vec3 *out_rotation_vec) struct xrt_vec3 *out_rotation_vec)
{ {
try { try {
...@@ -132,12 +145,13 @@ imu_fusion_get_prediction_rotation_vec(struct imu_fusion const *fusion, ...@@ -132,12 +145,13 @@ imu_fusion_get_prediction_rotation_vec(struct imu_fusion const *fusion,
if (!fusion->simple_fusion.valid()) { if (!fusion->simple_fusion.valid()) {
return -2; return -2;
} }
if (dt == 0) { if (timestamp_ns == fusion->time_ns) {
// No need to predict here. // No need to predict here.
map_vec3(*out_rotation_vec) = map_vec3(*out_rotation_vec) =
fusion->simple_fusion.getRotationVec() fusion->simple_fusion.getRotationVec()
.cast<float>(); .cast<float>();
} else { } else {
float dt = (timestamp_ns - fusion->time_ns) * 1.e-9;
Eigen::Quaterniond predicted_quat = Eigen::Quaterniond predicted_quat =
fusion->simple_fusion.getPredictedQuat(dt); fusion->simple_fusion.getPredictedQuat(dt);
map_vec3(*out_rotation_vec) = map_vec3(*out_rotation_vec) =
...@@ -154,7 +168,7 @@ imu_fusion_get_prediction_rotation_vec(struct imu_fusion const *fusion, ...@@ -154,7 +168,7 @@ imu_fusion_get_prediction_rotation_vec(struct imu_fusion const *fusion,
int int
imu_fusion_incorporate_gyros_and_accelerometer( imu_fusion_incorporate_gyros_and_accelerometer(
struct imu_fusion *fusion, struct imu_fusion *fusion,
float dt, uint64_t timestamp_ns,
struct xrt_vec3 const *ang_vel, struct xrt_vec3 const *ang_vel,
struct xrt_vec3 const *ang_vel_variance, struct xrt_vec3 const *ang_vel_variance,
struct xrt_vec3 const *accel, struct xrt_vec3 const *accel,
...@@ -167,6 +181,7 @@ imu_fusion_incorporate_gyros_and_accelerometer( ...@@ -167,6 +181,7 @@ imu_fusion_incorporate_gyros_and_accelerometer(
assert(accel); assert(accel);
assert(accel_variance); assert(accel_variance);
float dt = imu_fusion_get_dt(*fusion, timestamp_ns);
Eigen::Vector3d accelVec = map_vec3(*accel).cast<double>(); Eigen::Vector3d accelVec = map_vec3(*accel).cast<double>();
Eigen::Vector3d angVelVec = map_vec3(*ang_vel).cast<double>(); Eigen::Vector3d angVelVec = map_vec3(*ang_vel).cast<double>();
fusion->simple_fusion.handleAccel(accelVec, dt); fusion->simple_fusion.handleAccel(accelVec, dt);
......
...@@ -54,7 +54,7 @@ imu_fusion_destroy(struct imu_fusion *fusion); ...@@ -54,7 +54,7 @@ imu_fusion_destroy(struct imu_fusion *fusion);
*/ */
int int
imu_fusion_incorporate_gyros(struct imu_fusion *fusion, imu_fusion_incorporate_gyros(struct imu_fusion *fusion,
float dt, uint64_t timestamp_ns,
struct xrt_vec3 const *ang_vel, struct xrt_vec3 const *ang_vel,
struct xrt_vec3 const *variance); struct xrt_vec3 const *variance);
...@@ -73,7 +73,7 @@ imu_fusion_incorporate_gyros(struct imu_fusion *fusion, ...@@ -73,7 +73,7 @@ imu_fusion_incorporate_gyros(struct imu_fusion *fusion,
*/ */
int int
imu_fusion_incorporate_accelerometer(struct imu_fusion *fusion, imu_fusion_incorporate_accelerometer(struct imu_fusion *fusion,
float dt, uint64_t timestamp_ns,
struct xrt_vec3 const *accel, struct xrt_vec3 const *accel,
struct xrt_vec3 const *variance); struct xrt_vec3 const *variance);
...@@ -91,7 +91,7 @@ imu_fusion_incorporate_accelerometer(struct imu_fusion *fusion, ...@@ -91,7 +91,7 @@ imu_fusion_incorporate_accelerometer(struct imu_fusion *fusion,
int int
imu_fusion_incorporate_gyros_and_accelerometer( imu_fusion_incorporate_gyros_and_accelerometer(
struct imu_fusion *fusion, struct imu_fusion *fusion,
float dt, uint64_t timestamp_ns,
struct xrt_vec3 const *ang_vel, struct xrt_vec3 const *ang_vel,
struct xrt_vec3 const *ang_vel_variance, struct xrt_vec3 const *ang_vel_variance,
struct xrt_vec3 const *accel, struct xrt_vec3 const *accel,
...@@ -107,7 +107,7 @@ imu_fusion_incorporate_gyros_and_accelerometer( ...@@ -107,7 +107,7 @@ imu_fusion_incorporate_gyros_and_accelerometer(
*/ */
int int
imu_fusion_get_prediction(struct imu_fusion const *fusion, imu_fusion_get_prediction(struct imu_fusion const *fusion,
float dt, uint64_t timestamp_ns,
struct xrt_quat *out_quat, struct xrt_quat *out_quat,
struct xrt_vec3 *out_ang_vel); struct xrt_vec3 *out_ang_vel);
...@@ -123,7 +123,7 @@ imu_fusion_get_prediction(struct imu_fusion const *fusion, ...@@ -123,7 +123,7 @@ imu_fusion_get_prediction(struct imu_fusion const *fusion,
*/ */
int int
imu_fusion_get_prediction_rotation_vec(struct imu_fusion const *fusion, imu_fusion_get_prediction_rotation_vec(struct imu_fusion const *fusion,
float dt, uint64_t timestamp_ns,
struct xrt_vec3 *out_rotation_vec); struct xrt_vec3 *out_rotation_vec);
#ifdef __cplusplus #ifdef __cplusplus
} }
......
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment