diff --git a/src/ahrs.rs b/src/ahrs.rs index 5dcdd65..5d1186e 100644 --- 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 { /// 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, accelerometer: &Vector3, magnetometer: &Vector3, - ) -> Result<&UnitQuaternion, &str>; + ) -> Result<&UnitQuaternion, 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, accelerometer: &Vector3, - ) -> Result<&UnitQuaternion, &str>; + ) -> Result<&UnitQuaternion, 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, + ) -> &UnitQuaternion; } diff --git a/src/lib.rs b/src/lib.rs index 9fddc4a..37f3c1d 100644 --- 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 index 2b39a30..41be3e7 100644 --- 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 { /// // 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 { @@ -158,13 +158,13 @@ impl Madgwick { } } -impl Ahrs for Madgwick { +impl Ahrs for Madgwick { fn update( &mut self, gyroscope: &Vector3, accelerometer: &Vector3, magnetometer: &Vector3, - ) -> Result<&UnitQuaternion, &str> { + ) -> Result<&UnitQuaternion, AhrsError> { let q = self.quat.as_ref(); let zero: N = nalgebra::zero(); @@ -173,17 +173,13 @@ impl Ahrs for Madgwick { 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 Ahrs for Madgwick { 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 Ahrs for Madgwick { &mut self, gyroscope: &Vector3, accelerometer: &Vector3, - ) -> Result<&UnitQuaternion, &str> { + ) -> Result<&UnitQuaternion, AhrsError> { let q = self.quat.as_ref(); let zero: N = nalgebra::zero(); @@ -236,11 +235,8 @@ impl Ahrs for Madgwick { 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 Ahrs for Madgwick { 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 Ahrs for Madgwick { Ok(&self.quat) } + + fn update_gyro( + &mut self, + gyroscope: &Vector3 + ) -> &UnitQuaternion { + 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 index c2ef3d8..fc697c1 100644 --- 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,7 +205,7 @@ impl Ahrs for Mahony { gyroscope: &Vector3, accelerometer: &Vector3, magnetometer: &Vector3, - ) -> Result<&UnitQuaternion, &str> { + ) -> Result<&UnitQuaternion, AhrsError> { let q = self.quat.as_ref(); let zero: N = nalgebra::zero(); @@ -213,19 +213,13 @@ impl Ahrs for Mahony { 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 Ahrs for Mahony { 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 = 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,7 +262,7 @@ impl Ahrs for Mahony { &mut self, gyroscope: &Vector3, accelerometer: &Vector3, - ) -> Result<&UnitQuaternion, &str> { + ) -> Result<&UnitQuaternion, AhrsError> { let q = self.quat.as_ref(); let zero: N = nalgebra::zero(); @@ -282,11 +270,8 @@ impl Ahrs for Mahony { 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 Ahrs for Mahony { 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 Ahrs for Mahony { Ok(&self.quat) } + + fn update_gyro( + &mut self, + gyroscope: &Vector3 + ) -> &UnitQuaternion { + 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 + } }