t_imu.h 3.3 KB
Newer Older
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15
// Copyright 2019, Collabora, Ltd.
// SPDX-License-Identifier: BSL-1.0
/*!
 * @file
 * @brief  C interface to basic IMU fusion.
 * @author Ryan Pavlik <ryan.pavlik@collabora.com>
 * @ingroup aux_tracking
 */

#include "math/m_api.h"

#ifdef __cplusplus
extern "C" {
#endif

Ryan Pavlik's avatar
Ryan Pavlik committed
16 17 18
/*!
 * Opaque type for fusing IMU reports.
 */
19
struct imu_fusion;
Ryan Pavlik's avatar
Ryan Pavlik committed
20

21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56
/*!
 * Create a struct imu_fusion.
 *
 * @public @memberof imu_fusion
 * @ingroup aux_tracking
 */
struct imu_fusion *
imu_fusion_create();


/*!
 * Destroy a struct imu_fusion.
 *
 * Should not be called simultaneously with any other imu_fusion function.
 *
 * @public @memberof imu_fusion
 * @ingroup aux_tracking
 */
void
imu_fusion_destroy(struct imu_fusion *fusion);

/*!
 * Predict and correct fusion with a gyroscope reading.
 *
 * dt should not be zero: If you're receiving accel and gyro data at the same
 * time, call imu_fusion_incorporate_gyros_and_accelerometer() instead.
 *
 * Should not be called simultaneously with any other imu_fusion function.
 *
 * Non-zero return means error.
 *
 * @public @memberof imu_fusion
 * @ingroup aux_tracking
 */
int
imu_fusion_incorporate_gyros(struct imu_fusion *fusion,
57
                             uint64_t timestamp_ns,
58 59 60 61 62 63 64 65 66 67 68 69 70 71 72 73 74 75
                             struct xrt_vec3 const *ang_vel,
                             struct xrt_vec3 const *variance);

/*!
 * Predict and correct fusion with an accelerometer reading.
 *
 * dt should not be zero: If you're receiving accel and gyro data at the same
 * time, call imu_fusion_incorporate_gyros_and_accelerometer() instead.
 *
 * Should not be called simultaneously with any other imu_fusion function.
 *
 * Non-zero return means error.
 *
 * @public @memberof imu_fusion
 * @ingroup aux_tracking
 */
int
imu_fusion_incorporate_accelerometer(struct imu_fusion *fusion,
76
                                     uint64_t timestamp_ns,
77 78 79 80 81 82 83 84 85 86 87 88 89 90 91 92 93
                                     struct xrt_vec3 const *accel,
                                     struct xrt_vec3 const *variance);

/*!
 * Predict and correct fusion with a simultaneous accelerometer and gyroscope
 * reading.
 *
 * Should not be called simultaneously with any other imu_fusion function.
 *
 * Non-zero return means error.
 *
 * @public @memberof imu_fusion
 * @ingroup aux_tracking
 */
int
imu_fusion_incorporate_gyros_and_accelerometer(
    struct imu_fusion *fusion,
94
    uint64_t timestamp_ns,
95 96 97 98 99 100 101 102 103 104 105 106 107 108 109
    struct xrt_vec3 const *ang_vel,
    struct xrt_vec3 const *ang_vel_variance,
    struct xrt_vec3 const *accel,
    struct xrt_vec3 const *accel_variance);

/*!
 * Get the predicted state. Does not advance the internal state clock.
 *
 * Non-zero return means error.
 *
 * @public @memberof imu_fusion
 * @ingroup aux_tracking
 */
int
imu_fusion_get_prediction(struct imu_fusion const *fusion,
110
                          uint64_t timestamp_ns,
111 112 113 114 115 116 117 118 119 120 121 122 123 124 125
                          struct xrt_quat *out_quat,
                          struct xrt_vec3 *out_ang_vel);


/*!
 * Get the predicted state as a rotation vector. Does not advance the internal
 * state clock.
 *
 * Non-zero return means error.
 *
 * @public @memberof imu_fusion
 * @ingroup aux_tracking
 */
int
imu_fusion_get_prediction_rotation_vec(struct imu_fusion const *fusion,
126
                                       uint64_t timestamp_ns,
127 128 129 130
                                       struct xrt_vec3 *out_rotation_vec);
#ifdef __cplusplus
}
#endif