Add explicit micromath support for types

Signed-off-by: Markus Mayer <[email protected]>

Cargo.toml

@@ -15,9 +15,11 @@ default = ["coordinate-frame"]
 std = ["minikalman/std", "num-traits/std"]
 unsafe = []
 coordinate-frame = ["dep:coordinate-frame"]
+micromath = ["dep:micromath", "coordinate-frame/micromath"]
 
 [dependencies]
 coordinate-frame = { version = "0.4.0", optional = true, features = ["num-traits", "nalgebra"] }
+micromath = { version = "2.1.0", features = ["vector", "quaternion", "num-traits"], optional = true }
 minikalman = { version = "0.6.0", default-features = false }
 num-traits = { version = "0.2.19", default-features = false }
 uniform-array-derive = "0.1.0"

src/gyro_free/filter.rs

@@ -117,7 +117,7 @@ impl<T> OwnedOrientationEstimator<T> {
 
         // Update the Jacobian.
         let two = T::one() + T::one();
-        let (q0, q1, q2, q3) = self.estimated_quaternion();
+        let (q0, q1, q2, q3) = self.estimated_quaternion_internal();
 
         // This applies a simplified version of the Jacobian of the rotated vector with
         // respect to the rotation quaternion. In general, all vector components influence
@@ -189,7 +189,7 @@ impl<T> OwnedOrientationEstimator<T> {
         // Update the Jacobian.
         let one = T::one();
         let two = one + one;
-        let (q0, q1, q2, q3) = self.estimated_quaternion();
+        let (q0, q1, q2, q3) = self.estimated_quaternion_internal();
         let (mx, my, mz) = self.magnetic_field_ref.into();
 
         // This applies a simplified version of the Jacobian of the rotated vector with
@@ -336,7 +336,7 @@ impl<T> OwnedOrientationEstimator<T> {
     }
 
     /// Gets the estimated quaternion in (w, x, y, z) order.
-    fn estimated_quaternion(&self) -> (T, T, T, T)
+    fn estimated_quaternion_internal(&self) -> (T, T, T, T)
     where
         T: Copy,
     {
@@ -369,7 +369,7 @@ impl<T> OwnedOrientationEstimator<T> {
     where
         T: MatrixDataType + ArcTan<T, Output = T> + NormalizeAngle<Output = T>,
     {
-        let (w, x, y, z) = self.estimated_quaternion();
+        let (w, x, y, z) = self.estimated_quaternion_internal();
         let one = T::one();
         let two = one + one;
         let sinr_cosp = two * (w * x + y * z);
@@ -399,7 +399,7 @@ impl<T> OwnedOrientationEstimator<T> {
     where
         T: MatrixDataType + Abs<T, Output = T> + ArcSin<T, Output = T> + NormalizeAngle<Output = T>,
     {
-        let (w, x, y, z) = self.estimated_quaternion();
+        let (w, x, y, z) = self.estimated_quaternion_internal();
         let one = T::one();
         let two = one + one;
         let sinp = two * (w * y - z * x);
@@ -429,7 +429,7 @@ impl<T> OwnedOrientationEstimator<T> {
     where
         T: MatrixDataType + ArcTan<T, Output = T> + NormalizeAngle<Output = T>,
     {
-        let (w, x, y, z) = self.estimated_quaternion();
+        let (w, x, y, z) = self.estimated_quaternion_internal();
         let one = T::one();
         let two = one + one;
         let siny_cosp = two * (w * z + x * y);
@@ -453,6 +453,16 @@ impl<T> OwnedOrientationEstimator<T> {
     }
 }
 
+impl OwnedOrientationEstimator<f32> {
+    /// Provides the estimated orientation as a [`Quaternion`](micromath::Quaternion).
+    #[cfg(feature = "micromath")]
+    #[cfg_attr(docsrs, doc(cfg(feature = "micromath")))]
+    pub fn estimated_quaternion(&self) -> micromath::Quaternion {
+        let (w, x, y, z) = self.estimated_quaternion_internal();
+        micromath::Quaternion::new(w, x, y, z)
+    }
+}
+
 impl<T> OwnedOrientationEstimator<T> {
     /// Builds the Kalman filter used for prediction.
     fn build_filter(process_noise_value: T) -> OwnedKalmanFilter<T>

src/lib.rs

@@ -1,9 +1,6 @@
-// Enable no_std mode.
 #![cfg_attr(not(feature = "std"), no_std)]
-// Ensure allow(unsafe_code) / forbid(unsafe_code) markers.
 #![cfg_attr(feature = "unsafe", allow(unsafe_code))]
 #![cfg_attr(not(feature = "unsafe"), forbid(unsafe_code))]
-// Only enables the `doc_cfg` feature when the `docsrs` configuration attribute is defined.
 #![cfg_attr(docsrs, feature(doc_cfg))]
 
 pub mod gyro_drift;

src/macros.rs

@@ -2,5 +2,20 @@
 macro_rules! impl_standard_traits {
     ($type_name:ident, $type_param:ident) => {
         impl<$type_param> Copy for $type_name<$type_param> where $type_param: Copy {}
+
+        #[cfg(feature = "micromath")]
+        #[cfg_attr(docsrs, doc(cfg(feature = "micromath")))]
+        impl<$type_param> From<$type_name<$type_param>> for micromath::vector::Vector3d<$type_param>
+        where
+            $type_param: micromath::vector::Component,
+        {
+            fn from(value: $type_name<$type_param>) -> micromath::vector::Vector3d<$type_param> {
+                micromath::vector::Vector3d {
+                    x: value[0],
+                    y: value[1],
+                    z: value[2],
+                }
+            }
+        }
     };
 }