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

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

#[derive(UniformArray)]
#[cfg_attr(test, ensure_uniform_type::ensure_uniform_type)]
#[repr(C)]
pub struct EulerAngles<T> {
    /// The roll angle, in radians.
    pub roll_phi: T,
    /// The pitch angle, in radians.
    pub pitch_theta: T,
    /// The yaw angle, in radians.
    pub yaw_psi: T,
}

impl<T> EulerAngles<T> {
    /// Initializes a new [`EulerAngles`] instance.
    #[inline(always)]
    pub const fn new(roll_phi: T, pitch_theta: T, yaw_psi: T) -> Self {
        Self {
            roll_phi,
            pitch_theta,
            yaw_psi,
        }
    }
}

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

impl<T> Clone for EulerAngles<T>
where
    T: Clone,
{
    fn clone(&self) -> Self {
        Self {
            roll_phi: self.roll_phi.clone(),
            pitch_theta: self.pitch_theta.clone(),
            yaw_psi: self.yaw_psi.clone(),
        }
    }
}

impl<T> Debug for EulerAngles<T>
where
    T: Debug,
{
    fn fmt(&self, f: &mut Formatter<'_>) -> core::fmt::Result {
        f.debug_tuple("EulerAngles")
            .field(&self.roll_phi)
            .field(&self.pitch_theta)
            .field(&self.yaw_psi)
            .finish()
    }
}

impl_standard_traits!(EulerAngles, T);

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

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

    #[test]
    fn test_index() {
        let reading = EulerAngles::<f32> {
            roll_phi: 1.0,
            pitch_theta: 2.0,
            yaw_psi: 3.0,
        };

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