veloxity_core/
sensors.rs

1pub mod health;
2pub mod ingestion;
3pub mod processors;
4
5use crate::{errors::SensorError, math::FlightFloat, packets::*};
6
7#[derive(Default)]
8pub struct SensorBus<R: FlightFloat> {
9    pub imu: Option<Result<ImuPacket<R>, SensorError>>,
10    pub mag: Option<Result<MagPacket, SensorError>>,
11    pub baro: Option<Result<BaroPacket, SensorError>>,
12    pub pitot: Option<Result<PitotPacket, SensorError>>,
13    pub range: Option<Result<RangePacket, SensorError>>,
14    pub gnss: Option<Result<GNSSPacket, SensorError>>,
15    pub battery: Option<Result<BatteryPacket, SensorError>>,
16    pub rc: Option<Result<RcPacket, SensorError>>,
17    pub attitude: Option<Result<AttitudePacket, SensorError>>,
18}
19
20impl<R: FlightFloat> SensorBus<R> {
21    pub fn clear(&mut self) {
22        self.imu = None;
23        self.mag = None;
24        self.baro = None;
25        self.pitot = None;
26        self.range = None;
27        self.gnss = None;
28        self.battery = None;
29        self.rc = None;
30        self.attitude = None;
31    }
32}
33
34#[derive(Default)]
35pub struct ProcessedSensors<R: FlightFloat> {
36    pub imu: Option<ImuPacket<R>>,
37    pub mag: Option<MagPacket>,
38    pub baro: Option<BaroPacket>,
39    pub pitot: Option<PitotPacket>,
40    pub range: Option<RangePacket>,
41    pub gnss: Option<GNSSPacket>,
42    pub battery: Option<BatteryPacket>,
43    pub rc: Option<RcPacket>,
44    pub attitude: Option<AttitudePacket>,
45}
46
47impl<R: FlightFloat> ProcessedSensors<R> {
48    pub fn clear(&mut self) {
49        self.imu = None;
50        self.mag = None;
51        self.baro = None;
52        self.pitot = None;
53        self.range = None;
54        self.gnss = None;
55        self.battery = None;
56        self.rc = None;
57        self.attitude = None;
58    }
59
60    pub fn air_density(&self) -> R {
61        self.baro
62            .map(|baro| {
63                <R as FlightFloat>::from_f32(1.225)
64                    * (<R as FlightFloat>::from_f32(baro.pressure)
65                        / <R as FlightFloat>::from_f32(101_325.0))
66                    .powf(<R as FlightFloat>::from_f64(0.809736894596450))
67            })
68            .unwrap_or_else(|| <R as FlightFloat>::from_f32(1.225))
69    }
70}
71
72#[cfg(test)]
73mod tests {
74    use super::*;
75
76    #[test]
77    fn sensor_resources_default_to_empty() {
78        let raw = SensorBus::<f64>::default();
79        let processed = ProcessedSensors::<f64>::default();
80
81        assert!(raw.imu.is_none());
82        assert!(raw.rc.is_none());
83        assert!(processed.imu.is_none());
84        assert!(processed.rc.is_none());
85    }
86
87    #[test]
88    fn processed_sensors_reports_rosflight_air_density_from_baro_pressure() {
89        let mut processed = ProcessedSensors::<f64>::default();
90        assert!((processed.air_density() - 1.225).abs() < 1e-6);
91
92        processed.baro = Some(BaroPacket {
93            pressure: 80_000.0,
94            ..Default::default()
95        });
96
97        let expected = 1.225 * (80_000.0_f64 / 101_325.0).powf(0.809736894596450);
98        assert!((processed.air_density() - expected).abs() < 1e-6);
99    }
100}
veloxity_core/sensors.rs

veloxity_core/
sensors.rs
