Merge pull request #39 from peterkrull/master

Add non-fallible update function
jmagnuson · Aug 12, 2024 · ca30fcf · ca30fcf
2 parents c6b64a0 + f833f87
commit ca30fcf
Show file tree

Hide file tree

Showing 4 changed files with 90 additions and 61 deletions.
diff --git a/src/ahrs.rs b/src/ahrs.rs
@@ -1,28 +1,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
-    /// `Err(&str)` containing the reason.
+    /// `AhrsError` enum, which describes the reason.
     fn update(
         &mut self,
         gyroscope: &Vector3<N>,
         accelerometer: &Vector3<N>,
         magnetometer: &Vector3<N>,
-    ) -> Result<&UnitQuaternion<N>, &str>;
+    ) -> 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
-    /// `Err(&str)` containing the reason.
+    /// `AhrsError` enum, which describes the reason.
     fn update_imu(
         &mut self,
         gyroscope: &Vector3<N>,
         accelerometer: &Vector3<N>,
-    ) -> Result<&UnitQuaternion<N>, &str>;
+    ) -> 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>;
 }
diff --git a/src/lib.rs b/src/lib.rs
@@ -3,7 +3,7 @@
 #![cfg_attr(not(feature = "std"), no_std)]
 #![crate_name = "ahrs"]
 
-pub use crate::{ahrs::Ahrs, madgwick::Madgwick, mahony::Mahony};
+pub use crate::{ahrs::{Ahrs, AhrsError}, madgwick::Madgwick, mahony::Mahony};
 
 mod ahrs;
 mod madgwick;

diff --git a/src/madgwick.rs b/src/madgwick.rs
@@ -1,12 +1,12 @@
 #![allow(non_snake_case)]
 #![allow(clippy::many_single_char_names)]
 
-use crate::ahrs::Ahrs;
+use crate::ahrs::{Ahrs, AhrsError};
 use core::hash;
 use nalgebra::{
     Matrix4, Matrix6, Quaternion, Scalar, UnitQuaternion, Vector2, Vector3, Vector4, Vector6,
 };
-use simba::simd::{SimdRealField, SimdValue};
+use simba::{scalar::RealField, simd::{SimdRealField, SimdValue}};
 
 /// Madgwick AHRS implementation.
 ///
@@ -77,7 +77,7 @@ impl Default for Madgwick<f64> {
     /// // Madgwick {
     /// //     sample_period: 1.0f64/256.0,
     /// //     beta: 0.1f64,
-    /// //     quat: Quaternion { w: 1.0f64, i: 0.0, j: 0.0, k: 0.0 }
+    /// //     quat: Quaternion { w: 1.0f64, i: 0.0, j: 0.0, k: 0.0 },
     /// // };
     /// ```
     fn default() -> Madgwick<f64> {
@@ -158,13 +158,13 @@ impl<N: Scalar + SimdValue + Copy> Madgwick<N> {
     }
 }
 
-impl<N: simba::scalar::RealField + Copy> Ahrs<N> for Madgwick<N> {
+impl<N: RealField + Copy> Ahrs<N> for Madgwick<N> {
     fn update(
         &mut self,
         gyroscope: &Vector3<N>,
         accelerometer: &Vector3<N>,
         magnetometer: &Vector3<N>,
-    ) -> Result<&UnitQuaternion<N>, &str> {
+    ) -> Result<&UnitQuaternion<N>, AhrsError> {
         let q = self.quat.as_ref();
 
         let zero: N = nalgebra::zero();
@@ -173,17 +173,13 @@ impl<N: simba::scalar::RealField + Copy> Ahrs<N> for Madgwick<N> {
         let half: N = nalgebra::convert(0.5);
 
         // Normalize accelerometer measurement
-        let accel = match accelerometer.try_normalize(zero) {
-            Some(n) => n,
-            None => return Err("Accelerometer norm divided by zero."),
+        let Some(accel) = accelerometer.try_normalize(zero) else {
+            return Err(AhrsError::AccelerometerNormZero);
         };
 
         // Normalize magnetometer measurement
-        let mag = match magnetometer.try_normalize(zero) {
-            Some(n) => n,
-            None => {
-                return Err("Magnetometer norm divided by zero.");
-            }
+        let Some(mag) = magnetometer.try_normalize(zero) else {
+            return Err(AhrsError::MagnetometerNormZero);
         };
 
         // Reference direction of Earth's magnetic field (Quaternion should still be conj of q)
@@ -211,7 +207,10 @@ impl<N: simba::scalar::RealField + Copy> Ahrs<N> for Madgwick<N> {
              zero, zero, zero, zero, zero, zero
         );
 
-        let step = (J_t * F).normalize();
+        // Try to normalize step, falling back to gyro update if not possible
+        let Some(step) = (J_t * F).try_normalize(zero) else {
+            return Ok(self.update_gyro(gyroscope));
+        };
 
         // Compute rate of change for quaternion
         let qDot = q * Quaternion::from_parts(zero, *gyroscope) * half
@@ -227,7 +226,7 @@ impl<N: simba::scalar::RealField + Copy> Ahrs<N> for Madgwick<N> {
         &mut self,
         gyroscope: &Vector3<N>,
         accelerometer: &Vector3<N>,
-    ) -> Result<&UnitQuaternion<N>, &str> {
+    ) -> Result<&UnitQuaternion<N>, AhrsError> {
         let q = self.quat.as_ref();
 
         let zero: N = nalgebra::zero();
@@ -236,11 +235,8 @@ impl<N: simba::scalar::RealField + Copy> Ahrs<N> for Madgwick<N> {
         let half: N = nalgebra::convert(0.5);
 
         // Normalize accelerometer measurement
-        let accel = match accelerometer.try_normalize(zero) {
-            Some(n) => n,
-            None => {
-                return Err("Accelerator norm divided by zero.");
-            }
+        let Some(accel) = accelerometer.try_normalize(zero) else {
+            return Err(AhrsError::AccelerometerNormZero);
         };
 
         // Gradient descent algorithm corrective step
@@ -260,7 +256,10 @@ impl<N: simba::scalar::RealField + Copy> Ahrs<N> for Madgwick<N> {
              two*q[0], two*q[1],       zero, zero
         );
 
-        let step = (J_t * F).normalize();
+        // Try to normalize step, falling back to gyro update if not possible
+        let Some(step) = (J_t * F).try_normalize(zero) else {
+            return Ok(self.update_gyro(gyroscope));
+        };
 
         // Compute rate of change of quaternion
         let qDot = (q * Quaternion::from_parts(zero, *gyroscope)) * half
@@ -271,4 +270,22 @@ impl<N: simba::scalar::RealField + Copy> Ahrs<N> for Madgwick<N> {
 
         Ok(&self.quat)
     }
+
+    fn update_gyro(
+        &mut self, 
+        gyroscope: &Vector3<N>
+    ) -> &UnitQuaternion<N> {
+        let q = self.quat.as_ref();
+
+        let zero: N = nalgebra::zero();
+        let half: N = nalgebra::convert(0.5);
+
+        // Compute rate of change for quaternion
+        let qDot = q * Quaternion::from_parts(zero, *gyroscope) * half;
+
+        // Integrate to yield quaternion
+        self.quat = UnitQuaternion::from_quaternion(q + qDot * self.sample_period);
+
+        &self.quat
+    }
 }
diff --git a/src/mahony.rs b/src/mahony.rs
@@ -1,7 +1,7 @@
 #![allow(non_snake_case)]
 #![allow(clippy::many_single_char_names)]
 
-use crate::ahrs::Ahrs;
+use crate::ahrs::{Ahrs, AhrsError};
 use core::hash;
 use nalgebra::{Quaternion, Scalar, UnitQuaternion, Vector2, Vector3};
 use simba::simd::{SimdRealField as RealField, SimdRealField, SimdValue};
@@ -205,27 +205,21 @@ impl<N: simba::scalar::RealField + Copy> Ahrs<N> for Mahony<N> {
         gyroscope: &Vector3<N>,
         accelerometer: &Vector3<N>,
         magnetometer: &Vector3<N>,
-    ) -> Result<&UnitQuaternion<N>, &str> {
+    ) -> Result<&UnitQuaternion<N>, AhrsError> {
         let q = self.quat.as_ref();
 
         let zero: N = nalgebra::zero();
         let two: N = nalgebra::convert(2.0);
         let half: N = nalgebra::convert(0.5);
 
         // Normalize accelerometer measurement
-        let accel = match accelerometer.try_normalize(zero) {
-            Some(n) => n,
-            None => {
-                return Err("Accelerometer norm divided by zero.");
-            }
+        let Some(accel) = accelerometer.try_normalize(zero) else {
+            return Err(AhrsError::AccelerometerNormZero);
         };
 
         // Normalize magnetometer measurement
-        let mag = match magnetometer.try_normalize(zero) {
-            Some(n) => n,
-            None => {
-                return Err("Magnetometer norm divided by zero.");
-            }
+        let Some(mag) = magnetometer.try_normalize(zero) else {
+            return Err(AhrsError::MagnetometerNormZero);
         };
 
         // Reference direction of Earth's magnetic field (Quaternion should still be conj of q)
@@ -246,17 +240,11 @@ impl<N: simba::scalar::RealField + Copy> Ahrs<N> for Mahony<N> {
             two*b[0]*(q[3]*q[1] + q[0]*q[2])        + two*b[2]*(half - q[0]*q[0] - q[1]*q[1])
         );
 
+        // Error is sum of cross product between estimated direction and measured direction of fields
         let e: Vector3<N> = accel.cross(&v) + mag.cross(&w);
 
-        // Error is sum of cross product between estimated direction and measured direction of fields
-        if self.ki > zero {
-            self.e_int += e * self.sample_period;
-        } else {
-            //Vector3::new(zero, zero, zero);
-            self.e_int.x = zero;
-            self.e_int.y = zero;
-            self.e_int.z = zero;
-        }
+        // Integrate error
+        self.e_int += e * self.sample_period;
 
         // Apply feedback terms
         let gyro = *gyroscope + e * self.kp + self.e_int * self.ki;
@@ -274,19 +262,16 @@ impl<N: simba::scalar::RealField + Copy> Ahrs<N> for Mahony<N> {
         &mut self,
         gyroscope: &Vector3<N>,
         accelerometer: &Vector3<N>,
-    ) -> Result<&UnitQuaternion<N>, &str> {
+    ) -> Result<&UnitQuaternion<N>, AhrsError> {
         let q = self.quat.as_ref();
 
         let zero: N = nalgebra::zero();
         let two: N = nalgebra::convert(2.0);
         let half: N = nalgebra::convert(0.5);
 
         // Normalize accelerometer measurement
-        let accel = match accelerometer.try_normalize(zero) {
-            Some(n) => n,
-            None => {
-                return Err("Accelerometer norm divided by zero.");
-            }
+        let Some(accel) = accelerometer.try_normalize(zero) else {
+            return Err(AhrsError::AccelerometerNormZero);
         };
 
         #[rustfmt::skip]
@@ -296,16 +281,11 @@ impl<N: simba::scalar::RealField + Copy> Ahrs<N> for Mahony<N> {
             q[3]*q[3] - q[0]*q[0] - q[1]*q[1] + q[2]*q[2]
         );
 
+        // Error is estimated direction direction of fields
         let e = accel.cross(&v);
 
-        // Error is sum of cross product between estimated direction and measured direction of fields
-        if self.ki > zero {
-            self.e_int += e * self.sample_period;
-        } else {
-            self.e_int.x = zero;
-            self.e_int.y = zero;
-            self.e_int.z = zero;
-        }
+        // Integrate error
+        self.e_int += e * self.sample_period;
 
         // Apply feedback terms
         let gyro = *gyroscope + e * self.kp + self.e_int * self.ki;
@@ -318,4 +298,22 @@ impl<N: simba::scalar::RealField + Copy> Ahrs<N> for Mahony<N> {
 
         Ok(&self.quat)
     }
+
+    fn update_gyro(
+        &mut self,
+        gyroscope: &Vector3<N>
+    ) -> &UnitQuaternion<N> {
+        let q = self.quat.as_ref();
+
+        let zero: N = nalgebra::zero();
+        let half: N = nalgebra::convert(0.5);
+
+        // Compute rate of change for quaternion
+        let qDot = q * Quaternion::from_parts(zero, *gyroscope) * half;
+
+        // Integrate to yield quaternion
+        self.quat = UnitQuaternion::from_quaternion(q + qDot * self.sample_period);
+
+        &self.quat
+    }
 }