sensor_fusion/
sensor_fusion.rs

1//#![allow(unused)]
2
3use vqm::{Quaternion, Quaternionf32, Vector3d, Vector3df32};
4
5/// A generic interface for any sensor fusion filter (Madgwick, Mahony, etc.)
6/// ```
7/// use vqm::{Vector3df32,Quaternionf32};
8/// use sensor_fusion::{MadgwickFilterf32,SensorFusion};
9///
10/// let mut madgwick_filter = MadgwickFilterf32::default();
11///
12/// let delta_t: f32 = 0.0;
13/// let acc = Vector3df32::default();
14/// let gyro_rps = Vector3df32::default();
15///
16/// let orientation = madgwick_filter.fuse_acc_gyro(acc, gyro_rps, delta_t);
17/// assert_eq!(orientation, Quaternionf32 { w: 1.0, x: 0.0, y: 0.0, z: 0.0 });
18/// ```
19pub trait SensorFusion<T> {
20    fn set_free_parameters(&mut self, parameter0: T, parameter1: T);
21    fn requires_initialization() -> bool;
22
23    fn fuse_acc_gyro(&mut self, acc: Vector3d<T>, gyro_rps: Vector3d<T>, delta_t: T) -> Quaternion<T>;
24    fn fuse_acc_gyro_mag(&mut self, acc: Vector3d<T>, gyro: Vector3d<T>, mag: Vector3d<T>, delta_t: T)
25    -> Quaternion<T>;
26}
27
28#[allow(unused)]
29pub trait SensorFusionf32 {
30    fn set_free_parameters(&mut self, parameter0: f32, parameter1: f32);
31    fn requires_initialization() -> bool;
32
33    fn fuse_acc_gyro(&mut self, acc: Vector3df32, gyro_rps: Vector3df32, delta_t: f32) -> Quaternionf32;
34    fn fuse_acc_gyro_mag(
35        &mut self,
36        acc: Vector3df32,
37        gyro: Vector3df32,
38        mag: Vector3df32,
39        delta_t: f32,
40    ) -> Quaternionf32;
41}
42
43/// Implements method call syntax for a sensor fusion filter.
44/// ```
45/// use vqm::{Vector3df32,Quaternionf32};
46/// use sensor_fusion::{MadgwickFilterf32,SensorFusion,FuseAccGyro};
47///
48/// let mut madgwick_filter = MadgwickFilterf32::default();
49///
50/// let delta_t: f32 = 0.0;
51/// let acc = Vector3df32::default();
52/// let gyro_rps = Vector3df32::default();
53///
54/// let orientation = (acc, gyro_rps).fuse_acc_gyro_using(&mut madgwick_filter, delta_t);
55/// assert_eq!(orientation, Quaternionf32 { w: 1.0, x: 0.0, y: 0.0, z: 0.0 });
56/// ```
57pub trait FuseAccGyro<T> {
58    fn fuse_acc_gyro_using<F: SensorFusion<T>>(self, filter: &mut F, delta_t: T) -> Quaternion<T>;
59}
60
61impl<T> FuseAccGyro<T> for (Vector3d<T>, Vector3d<T>) {
62    fn fuse_acc_gyro_using<F: SensorFusion<T>>(self, filter: &mut F, delta_t: T) -> Quaternion<T> {
63        let (acc, gyro) = self;
64        filter.fuse_acc_gyro(acc, gyro, delta_t)
65    }
66}
67
68pub trait FuseAccGyroMag<T> {
69    fn fuse_acc_gyro_mag_using<F: SensorFusion<T>>(self, sensor_fusion_filter: &mut F, delta_t: T) -> Quaternion<T>;
70}
71
72impl<T> FuseAccGyroMag<T> for (Vector3d<T>, Vector3d<T>, Vector3d<T>) {
73    fn fuse_acc_gyro_mag_using<F: SensorFusion<T>>(self, sensor_fusion_filter: &mut F, delta_t: T) -> Quaternion<T> {
74        let (acc, gyro, mag) = self;
75        sensor_fusion_filter.fuse_acc_gyro_mag(acc, gyro, mag, delta_t)
76    }
77}
78
79/*
80/// Calculate quaternion derivative (dq/dt aka q_dot) from angular rate <https://ahrs.readthedocs.io/en/latest/filters/angular.html#quaternion-derivative>
81pub fn q_dot<T>(q: Quaternion<T>, gyro_rps: Vector3d<T>) -> Quaternion<T>
82where
83    T: Copy + One + Neg<Output = T> + Add<Output = T> + Sub<Output = T> + Mul<Output = T> + Div<Output = T>,
84{
85    let half = T::one() / (T::one() + T::one());
86    Quaternion {
87        w: (-q.x * gyro_rps.x - q.y * gyro_rps.y - q.z * gyro_rps.z) * half,
88        x: (q.w * gyro_rps.x + q.y * gyro_rps.z - q.z * gyro_rps.y) * half,
89        y: (q.w * gyro_rps.y - q.x * gyro_rps.z + q.z * gyro_rps.x) * half,
90        z: (q.w * gyro_rps.z + q.x * gyro_rps.y - q.y * gyro_rps.x) * half,
91    }
92}*/
93
94#[cfg(any(debug_assertions, test))]
95mod tests {
96    #![allow(clippy::wildcard_imports)]
97    use super::*;
98    use vqm::Vector3df32;
99
100    #[allow(dead_code)]
101    pub struct TestStruct;
102    impl SensorFusion<f32> for TestStruct {
103        fn set_free_parameters(&mut self, _parameter0: f32, _parameter1: f32) {}
104        fn requires_initialization() -> bool {
105            true
106        }
107        fn fuse_acc_gyro(&mut self, _acc: Vector3df32, _gyro_rps: Vector3df32, _delta_t: f32) -> Quaternionf32 {
108            Quaternionf32::default()
109        }
110        fn fuse_acc_gyro_mag(
111            &mut self,
112            acc: Vector3df32,
113            gyro_rps: Vector3df32,
114            _mag: Vector3df32,
115            delta_t: f32,
116        ) -> Quaternionf32 {
117            self.fuse_acc_gyro(acc, gyro_rps, delta_t)
118        }
119    }
120
121    //#[allow(dead_code)]
122    #[test]
123    fn sensor_fusion() {
124        let mut test_struct: TestStruct = TestStruct {};
125        _ = TestStruct::requires_initialization();
126        //assert_eq!(TestStruct::requires_initialization(), true);
127
128        test_struct.set_free_parameters(0.0, 0.0);
129
130        let delta_t: f32 = 0.0;
131        let acc = Vector3df32::default();
132        let gyro_rps = Vector3df32::default();
133
134        let orientation = test_struct.fuse_acc_gyro(acc, gyro_rps, delta_t);
135        assert_eq!(orientation, Quaternion::default());
136    }
137
138    #[test]
139    fn fuse_using() {
140        use crate::MadgwickFilterf32;
141
142        let mut madgwick_filter = MadgwickFilterf32::default();
143        let requires_initialization = MadgwickFilterf32::requires_initialization();
144        assert!(requires_initialization);
145
146        madgwick_filter.set_beta(1.0);
147
148        let delta_t: f32 = 0.0;
149        let acc = Vector3df32::default();
150        let gyro_rps = Vector3df32::default();
151
152        //let orientation = madgwick_filter.fuse_acc_gyro(acc, gyro_rps, delta_t);
153        let orientation = (acc, gyro_rps).fuse_acc_gyro_using(&mut madgwick_filter, delta_t);
154        assert_eq!(orientation, Quaternion { w: 1.0, x: 0.0, y: 0.0, z: 0.0 });
155    }
156}
sensor_fusion/sensor_fusion.rs

sensor_fusion/
sensor_fusion.rs
