sensor_fusion/
sensor_fusion.rs

1//#![allow(unused)]
2
3use vqm::{Quaternion, Quaternionf32, Vector3d, Vector3df32};
4
5/// Common interface for the sensor fusion filters (Madgwick, Mahony, complementary).
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_gains(&mut self, gain0: T, gain1: 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_gains(&mut self, gain0: f32, gain1: 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#[allow(clippy::doc_paragraphs_missing_punctuation)]
44/// Trait to allow sensor fusion filters to be used with method-call syntax, ie:<br>
45/// `let orientation = (acc, gyro_rps).fuse_acc_gyro_using(&mut madgwick, dt);`
46/// ```
47/// use vqm::{Vector3df32,Quaternionf32};
48/// use sensor_fusion::{MadgwickFilterf32,SensorFusion,FuseAccGyro};
49///
50/// let mut madgwick_filter = MadgwickFilterf32::default();
51///
52/// let delta_t: f32 = 0.0;
53/// let acc = Vector3df32::default();
54/// let gyro_rps = Vector3df32::default();
55///
56/// let orientation = (acc, gyro_rps).fuse_acc_gyro_using(&mut madgwick_filter, delta_t);
57/// assert_eq!(orientation, Quaternionf32 { w: 1.0, x: 0.0, y: 0.0, z: 0.0 });
58/// ```
59pub trait FuseAccGyro<T> {
60    fn fuse_acc_gyro_using<F: SensorFusion<T>>(self, filter: &mut F, delta_t: T) -> Quaternion<T>;
61}
62
63impl<T> FuseAccGyro<T> for (Vector3d<T>, Vector3d<T>) {
64    fn fuse_acc_gyro_using<F: SensorFusion<T>>(self, filter: &mut F, delta_t: T) -> Quaternion<T> {
65        let (acc, gyro) = self;
66        filter.fuse_acc_gyro(acc, gyro, delta_t)
67    }
68}
69
70#[allow(clippy::doc_paragraphs_missing_punctuation)]
71/// Trait to allow sensor fusion filters to be used with method-call syntax, ie:<br>
72/// `let orientation = (acc, gyro_rps).fuse_acc_gyro_using(&mut madgwick, dt);`
73/// ```
74/// use vqm::{Vector3df32,Quaternionf32};
75/// use sensor_fusion::{MadgwickFilterf32,SensorFusion,FuseAccGyroMag};
76///
77/// let mut madgwick_filter = MadgwickFilterf32::default();
78///
79/// let delta_t: f32 = 0.0;
80/// let acc = Vector3df32::default();
81/// let gyro_rps = Vector3df32::default();
82/// let mag = Vector3df32::default();
83///
84/// let orientation = (acc, gyro_rps, mag).fuse_acc_gyro_mag_using(&mut madgwick_filter, delta_t);
85/// assert_eq!(orientation, Quaternionf32 { w: 1.0, x: 0.0, y: 0.0, z: 0.0 });
86/// ```
87pub trait FuseAccGyroMag<T> {
88    fn fuse_acc_gyro_mag_using<F: SensorFusion<T>>(self, sensor_fusion_filter: &mut F, delta_t: T) -> Quaternion<T>;
89}
90
91impl<T> FuseAccGyroMag<T> for (Vector3d<T>, Vector3d<T>, Vector3d<T>) {
92    fn fuse_acc_gyro_mag_using<F: SensorFusion<T>>(self, sensor_fusion_filter: &mut F, delta_t: T) -> Quaternion<T> {
93        let (acc, gyro, mag) = self;
94        sensor_fusion_filter.fuse_acc_gyro_mag(acc, gyro, mag, delta_t)
95    }
96}
97
98/*
99/// Calculate quaternion derivative (dq/dt aka q_dot) from angular rate <https://ahrs.readthedocs.io/en/latest/filters/angular.html#quaternion-derivative>
100pub fn q_dot<T>(q: Quaternion<T>, gyro_rps: Vector3d<T>) -> Quaternion<T>
101where
102    T: Copy + One + Neg<Output = T> + Add<Output = T> + Sub<Output = T> + Mul<Output = T> + Div<Output = T>,
103{
104    let half = T::one() / (T::one() + T::one());
105    Quaternion {
106        w: (-q.x * gyro_rps.x - q.y * gyro_rps.y - q.z * gyro_rps.z) * half,
107        x: (q.w * gyro_rps.x + q.y * gyro_rps.z - q.z * gyro_rps.y) * half,
108        y: (q.w * gyro_rps.y - q.x * gyro_rps.z + q.z * gyro_rps.x) * half,
109        z: (q.w * gyro_rps.z + q.x * gyro_rps.y - q.y * gyro_rps.x) * half,
110    }
111}*/
112
113#[cfg(any(debug_assertions, test))]
114mod tests {
115    #![allow(clippy::wildcard_imports)]
116    use super::*;
117    use vqm::Vector3df32;
118
119    #[allow(dead_code)]
120    pub struct TestStruct;
121    impl SensorFusion<f32> for TestStruct {
122        fn set_gains(&mut self, _gain0: f32, _gain1: f32) {}
123        fn requires_initialization() -> bool {
124            true
125        }
126        fn fuse_acc_gyro(&mut self, _acc: Vector3df32, _gyro_rps: Vector3df32, _delta_t: f32) -> Quaternionf32 {
127            Quaternionf32::default()
128        }
129        fn fuse_acc_gyro_mag(
130            &mut self,
131            acc: Vector3df32,
132            gyro_rps: Vector3df32,
133            _mag: Vector3df32,
134            delta_t: f32,
135        ) -> Quaternionf32 {
136            self.fuse_acc_gyro(acc, gyro_rps, delta_t)
137        }
138    }
139
140    //#[allow(dead_code)]
141    #[test]
142    fn sensor_fusion() {
143        let mut test_struct: TestStruct = TestStruct {};
144        _ = TestStruct::requires_initialization();
145        //assert_eq!(TestStruct::requires_initialization(), true);
146
147        test_struct.set_gains(0.0, 0.0);
148
149        let delta_t: f32 = 0.0;
150        let acc = Vector3df32::default();
151        let gyro_rps = Vector3df32::default();
152
153        let orientation = test_struct.fuse_acc_gyro(acc, gyro_rps, delta_t);
154        assert_eq!(orientation, Quaternion::default());
155    }
156
157    #[test]
158    fn fuse_using() {
159        use crate::MadgwickFilterf32;
160
161        let mut madgwick_filter = MadgwickFilterf32::default();
162        let requires_initialization = MadgwickFilterf32::requires_initialization();
163        assert!(requires_initialization);
164
165        madgwick_filter.set_beta(1.0);
166
167        let delta_t: f32 = 0.0;
168        let acc = Vector3df32::default();
169        let gyro_rps = Vector3df32::default();
170
171        //let orientation = madgwick_filter.fuse_acc_gyro(acc, gyro_rps, delta_t);
172        let orientation = (acc, gyro_rps).fuse_acc_gyro_using(&mut madgwick_filter, delta_t);
173        assert_eq!(orientation, Quaternion { w: 1.0, x: 0.0, y: 0.0, z: 0.0 });
174    }
175}
sensor_fusion/sensor_fusion.rs

sensor_fusion/
sensor_fusion.rs
