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 @@
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<double>(), 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<double>(), 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<float>();
if (dt == 0) {
if (timestamp_ns == fusion->time_ns) {
// No need to predict here.
map_quat(*out_quat) =
fusion->simple_fusion.getQuat().cast<float>();
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<float>();
......@@ -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<float>();
} 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<double>();
Eigen::Vector3d angVelVec = map_vec3(*ang_vel).cast<double>();
fusion->simple_fusion.handleAccel(accelVec, dt);
......
......@@ -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
}
......
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