blackbox_log/
units.rs

1use alloc::vec::Vec;
2
3pub use uom::si;
4pub use uom::si::f64::{
5    Acceleration, AngularVelocity, ElectricCurrent, ElectricPotential, Length, Time, Velocity,
6};
7
8use crate::Headers;
9
10#[allow(unreachable_pub, unused_imports)]
11pub(crate) mod prelude {
12    pub use super::si::acceleration::{meter_per_second_squared as mps2, standard_gravity};
13    pub use super::si::angular_velocity::degree_per_second;
14    pub use super::si::electric_current::{ampere, milliampere};
15    pub use super::si::electric_potential::{millivolt, volt};
16    pub use super::si::length::meter;
17    pub use super::si::time::{microsecond, second};
18    pub use super::si::velocity::meter_per_second;
19    pub use super::{
20        Acceleration, AngularVelocity, ElectricCurrent, ElectricPotential, Length, Time, Velocity,
21    };
22}
23
24include_generated!("failsafe_phase");
25include_generated!("flight_mode");
26include_generated!("state");
27
28pub(crate) mod new {
29    use super::*;
30
31    pub(crate) fn time(raw: u64) -> Time {
32        Time::new::<prelude::microsecond>(raw as f64)
33    }
34
35    pub(crate) fn acceleration(raw: i32, headers: &Headers) -> Acceleration {
36        let gs = f64::from(raw) / f64::from(headers.acceleration_1g.unwrap());
37        Acceleration::new::<prelude::standard_gravity>(gs)
38    }
39
40    pub(crate) fn angular_velocity(raw: i32, headers: &Headers) -> AngularVelocity {
41        let scale = headers.gyro_scale.unwrap();
42        let rad = f64::from(scale) * f64::from(raw);
43
44        AngularVelocity::new::<si::angular_velocity::radian_per_second>(rad)
45    }
46
47    pub(crate) fn current(raw: i32) -> ElectricCurrent {
48        // Correct from BF 3.1.7 (3.1.0?), INAV 2.0.0
49        ElectricCurrent::new::<si::electric_current::centiampere>(raw.into())
50    }
51
52    pub(crate) fn vbat(raw: u32) -> ElectricPotential {
53        // Correct from BF 4.0.0, INAV 3.0.0?
54        ElectricPotential::new::<si::electric_potential::centivolt>(raw.into())
55    }
56
57    pub(crate) fn velocity(raw: u32) -> Velocity {
58        Velocity::new::<si::velocity::centimeter_per_second>(raw.into())
59    }
60}
61
62pub trait FlagSet {
63    type Flag: Flag;
64
65    /// Checks if a given flag is enabled.
66    fn is_set(&self, flag: Self::Flag) -> bool;
67
68    /// Returns the names of all enabled flags.
69    fn as_names(&self) -> Vec<&'static str>;
70}
71
72pub trait Flag {
73    /// Returns the name of this flag.
74    fn as_name(&self) -> &'static str;
75}
76
77#[cfg(test)]
78mod tests {
79    use super::*;
80
81    macro_rules! float_eq {
82        ($left:expr, $right:expr) => {
83            let epsilon = 0.0001;
84            let diff = ($left - $right).abs();
85            assert!(
86                diff < epsilon,
87                "{left} and {right} are greater than {epsilon} apart: {diff}",
88                left = $left,
89                right = $right
90            );
91        };
92    }
93
94    #[test]
95    fn electric_current() {
96        float_eq!(1.39, new::current(139).get::<prelude::ampere>());
97    }
98
99    #[test]
100    fn electric_potential() {
101        float_eq!(16.32, new::vbat(1632).get::<prelude::volt>());
102    }
103
104    mod resolution {
105        use super::*;
106
107        #[test]
108        fn time() {
109            use si::time::{day, microsecond};
110
111            let ms = Time::new::<microsecond>(1.);
112            float_eq!(1., ms.get::<microsecond>());
113
114            let d = Time::new::<day>(1.);
115            float_eq!(1., d.get::<day>());
116
117            float_eq!(
118                ms.get::<microsecond>() + d.get::<microsecond>(),
119                (ms + d).get::<microsecond>()
120            );
121        }
122
123        #[test]
124        fn acceleration() {
125            use si::acceleration::{millimeter_per_second_squared as mmps2, standard_gravity};
126
127            let milli_gs = Acceleration::new::<standard_gravity>(0.001);
128            float_eq!(0.001, milli_gs.get::<standard_gravity>());
129
130            let hecto_gs = Acceleration::new::<standard_gravity>(100.);
131            float_eq!(100., hecto_gs.get::<standard_gravity>());
132
133            float_eq!(
134                milli_gs.get::<mmps2>() + hecto_gs.get::<mmps2>(),
135                (milli_gs + hecto_gs).get::<mmps2>()
136            );
137        }
138
139        #[test]
140        fn angular_velocity() {
141            use si::angular_velocity::degree_per_second as dps;
142
143            let slow = AngularVelocity::new::<dps>(0.01);
144            float_eq!(0.01, slow.get::<dps>());
145
146            let fast = AngularVelocity::new::<dps>(5_000.);
147            float_eq!(5_000., fast.get::<dps>());
148
149            float_eq!(5_000.01, (slow + fast).get::<dps>());
150        }
151
152        #[test]
153        fn electric_current() {
154            use si::electric_current::{kiloampere, milliampere};
155
156            let ma = ElectricCurrent::new::<milliampere>(1.);
157            float_eq!(1., ma.get::<milliampere>());
158
159            let ka = ElectricCurrent::new::<kiloampere>(1.);
160            float_eq!(1., ka.get::<kiloampere>());
161
162            float_eq!(
163                ma.get::<milliampere>() + ka.get::<milliampere>(),
164                (ma + ka).get::<milliampere>()
165            );
166        }
167
168        #[test]
169        fn electric_potential() {
170            use si::electric_potential::{kilovolt, millivolt};
171
172            let mv = ElectricPotential::new::<millivolt>(1.);
173            float_eq!(1., mv.get::<millivolt>());
174
175            let kv = ElectricPotential::new::<kilovolt>(1.);
176            float_eq!(1., kv.get::<kilovolt>());
177
178            float_eq!(
179                mv.get::<millivolt>() + kv.get::<millivolt>(),
180                (mv + kv).get::<millivolt>()
181            );
182        }
183    }
184}