Start converting IMU stuff to timestamps.

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<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);

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
 }