1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42
use nalgebra::{Scalar, UnitQuaternion, Vector3};
use simba::simd::SimdValue;
#[derive(Debug)]
pub enum AhrsError {
AccelerometerNormZero,
MagnetometerNormZero,
}
/// Trait for implementing an AHRS filter.
pub trait Ahrs<N: Scalar + SimdValue> {
/// Attempts to update the current state quaternion using 9dof IMU values, made up by `gyroscope`,
/// `accelerometer`, and `magnetometer`.
///
/// Returns a reference to the updated quaternion on success, or in the case of failure, an
/// `AhrsError` enum, which describes the reason.
fn update(
&mut self,
gyroscope: &Vector3<N>,
accelerometer: &Vector3<N>,
magnetometer: &Vector3<N>,
) -> Result<&UnitQuaternion<N>, AhrsError>;
/// Attempts to update the current state quaternion using 6dof IMU values, made up by `gyroscope` &
/// `accelerometer`.
///
/// Returns a reference to the updated quaternion on success, or in the case of failure, an
/// `AhrsError` enum, which describes the reason.
fn update_imu(
&mut self,
gyroscope: &Vector3<N>,
accelerometer: &Vector3<N>,
) -> Result<&UnitQuaternion<N>, AhrsError>;
/// Updates the current state quaternion using only 3dof IMU values, made up by `gyroscope`.
///
/// Returns a reference to the updated quaternion.
fn update_gyro(
&mut self,
gyroscope: &Vector3<N>,
) -> &UnitQuaternion<N>;
}