marg_orientation/
gyroscope_reading.rs

1use crate::impl_standard_traits;
2use core::fmt::{Debug, Formatter};
3use core::ops::{Mul, Sub};
4use uniform_array_derive::UniformArray;
5
6#[derive(UniformArray)]
7#[cfg_attr(test, ensure_uniform_type::ensure_uniform_type)]
8#[repr(C)]
9pub struct GyroscopeReading<T> {
10    /// The angular rate around the x-axis, in radians per second.
11    pub omega_x: T,
12    /// The angular rate around the y-axis, in radians per second.
13    pub omega_y: T,
14    /// The angular rate around the z-axis, in radians per second.
15    pub omega_z: T,
16}
17
18impl<T> GyroscopeReading<T> {
19    /// Initializes a new [`GyroscopeReading`] instance.
20    #[inline(always)]
21    pub const fn new(omega_x: T, omega_y: T, omega_z: T) -> Self {
22        Self {
23            omega_x,
24            omega_y,
25            omega_z,
26        }
27    }
28
29    /// Constructs a new [`GyroscopeReading`] instance from a reading in a given coordinate frame.
30    #[cfg(feature = "coordinate-frame")]
31    #[cfg_attr(docsrs, doc(cfg(feature = "coordinate-frame")))]
32    pub fn north_east_down<C>(coordinate: C) -> Self
33    where
34        C: Into<coordinate_frame::NorthEastDown<T>>,
35        T: Clone,
36    {
37        let coordinate = coordinate.into();
38        Self {
39            omega_x: coordinate.x(),
40            omega_y: coordinate.y(),
41            omega_z: coordinate.z(),
42        }
43    }
44}
45
46impl<T> Default for GyroscopeReading<T>
47where
48    T: Default,
49{
50    #[inline]
51    fn default() -> Self {
52        Self::new(Default::default(), Default::default(), Default::default())
53    }
54}
55
56impl<T> Clone for GyroscopeReading<T>
57where
58    T: Clone,
59{
60    fn clone(&self) -> Self {
61        Self {
62            omega_x: self.omega_x.clone(),
63            omega_y: self.omega_y.clone(),
64            omega_z: self.omega_z.clone(),
65        }
66    }
67}
68
69impl<T> Debug for GyroscopeReading<T>
70where
71    T: Debug,
72{
73    fn fmt(&self, f: &mut Formatter<'_>) -> core::fmt::Result {
74        f.debug_tuple("GyroscopeReading")
75            .field(&self.omega_x)
76            .field(&self.omega_y)
77            .field(&self.omega_z)
78            .finish()
79    }
80}
81
82impl<T> Mul<T> for GyroscopeReading<T>
83where
84    T: Mul<T, Output = T> + Clone,
85{
86    type Output = GyroscopeReading<T>;
87
88    fn mul(self, rhs: T) -> Self::Output {
89        Self {
90            omega_x: self.omega_x * rhs.clone(),
91            omega_y: self.omega_y * rhs.clone(),
92            omega_z: self.omega_z * rhs.clone(),
93        }
94    }
95}
96
97impl<T> Sub<T> for GyroscopeReading<T>
98where
99    T: Sub<T, Output = T> + Clone,
100{
101    type Output = GyroscopeReading<T>;
102
103    fn sub(self, rhs: T) -> Self::Output {
104        Self {
105            omega_x: self.omega_x - rhs.clone(),
106            omega_y: self.omega_y - rhs.clone(),
107            omega_z: self.omega_z - rhs.clone(),
108        }
109    }
110}
111
112#[cfg(feature = "coordinate-frame")]
113#[cfg_attr(docsrs, doc(cfg(feature = "coordinate-frame")))]
114impl<T, C> From<C> for GyroscopeReading<T>
115where
116    C: coordinate_frame::CoordinateFrame<Type = T>,
117    T: Copy + coordinate_frame::SaturatingNeg<Output = T>,
118{
119    fn from(value: C) -> Self {
120        Self::north_east_down(value.to_ned())
121    }
122}
123
124impl_standard_traits!(GyroscopeReading, T);
125
126#[cfg(test)]
127mod test {
128    use super::*;
129
130    #[test]
131    fn test_len() {
132        let reading = GyroscopeReading::<f32>::default();
133        assert_eq!(reading.len(), 3);
134    }
135
136    #[test]
137    fn test_index() {
138        let reading = GyroscopeReading::<f32> {
139            omega_x: 1.0,
140            omega_y: 2.0,
141            omega_z: 3.0,
142        };
143
144        assert_eq!(reading[0], 1.0);
145        assert_eq!(reading[1], 2.0);
146        assert_eq!(reading[2], 3.0);
147    }
148}
marg_orientation/gyroscope_reading.rs

marg_orientation/
gyroscope_reading.rs
