marg-orientation 0.1.0-alpha.2

A MARG (Magnetic, Angular Rate, and Gravity) orientation estimator
Documentation 
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120

use crate::impl_standard_traits;
use core::fmt::{Debug, Formatter};
use core::ops::{Mul, Sub};
use uniform_array_derive::UniformArray;

#[derive(UniformArray)]
#[cfg_attr(test, ensure_uniform_type::ensure_uniform_type)]
#[repr(C)]
pub struct GyroscopeBias<T> {
    /// The angular rate around the x-axis, in radians per second.
    pub omega_x: T,
    /// The angular rate around the y-axis, in radians per second.
    pub omega_y: T,
    /// The angular rate around the z-axis, in radians per second.
    pub omega_z: T,
}

impl<T> GyroscopeBias<T> {
    /// Initializes a new [`GyroscopeBias`] instance.
    #[inline(always)]
    pub const fn new(omega_x: T, omega_y: T, omega_z: T) -> Self {
        Self {
            omega_x,
            omega_y,
            omega_z,
        }
    }
}

impl<T> Default for GyroscopeBias<T>
where
    T: Default,
{
    #[inline]
    fn default() -> Self {
        Self::new(Default::default(), Default::default(), Default::default())
    }
}

impl<T> Clone for GyroscopeBias<T>
where
    T: Clone,
{
    fn clone(&self) -> Self {
        Self {
            omega_x: self.omega_x.clone(),
            omega_y: self.omega_y.clone(),
            omega_z: self.omega_z.clone(),
        }
    }
}

impl<T> Debug for GyroscopeBias<T>
where
    T: Debug,
{
    fn fmt(&self, f: &mut Formatter<'_>) -> core::fmt::Result {
        f.debug_tuple("GyroscopeBias")
            .field(&self.omega_x)
            .field(&self.omega_y)
            .field(&self.omega_z)
            .finish()
    }
}

impl<T> Mul<T> for GyroscopeBias<T>
where
    T: Mul<T, Output = T> + Clone,
{
    type Output = GyroscopeBias<T>;

    fn mul(self, rhs: T) -> Self::Output {
        Self {
            omega_x: self.omega_x * rhs.clone(),
            omega_y: self.omega_y * rhs.clone(),
            omega_z: self.omega_z * rhs.clone(),
        }
    }
}

impl<T> Sub<T> for GyroscopeBias<T>
where
    T: Sub<T, Output = T> + Clone,
{
    type Output = GyroscopeBias<T>;

    fn sub(self, rhs: T) -> Self::Output {
        Self {
            omega_x: self.omega_x - rhs.clone(),
            omega_y: self.omega_y - rhs.clone(),
            omega_z: self.omega_z - rhs.clone(),
        }
    }
}

impl_standard_traits!(GyroscopeBias, T);

#[cfg(test)]
mod test {
    use super::*;

    #[test]
    fn test_len() {
        let reading = GyroscopeBias::<f32>::default();
        assert_eq!(reading.len(), 3);
    }

    #[test]
    fn test_index() {
        let reading = GyroscopeBias::<f32> {
            omega_x: 1.0,
            omega_y: 2.0,
            omega_z: 3.0,
        };

        assert_eq!(reading[0], 1.0);
        assert_eq!(reading[1], 2.0);
        assert_eq!(reading[2], 3.0);
    }
}