veloxity_core/sensors/
ingestion.rs

1use crate::{
2    math::FlightFloat,
3    packets::*,
4    params::Params,
5    sensors::processors::{
6        BaroProcessor, BatteryProcessor, CalibrationFlags, ImuProcessor, MagProcessor,
7        PassthroughAttitudeProcessor, PassthroughGNSSProcessor, PassthroughRangeProcessor,
8        PassthroughRcProcessor, PitotProcessor, SensorPacketProcessor,
9    },
10    sensors::{ProcessedSensors, SensorBus},
11};
12
13pub struct SensorProcessorSet<
14    R: FlightFloat,
15    ImuProc = ImuProcessor<R>,
16    MagProc = MagProcessor,
17    BaroProc = BaroProcessor,
18    PitotProc = PitotProcessor,
19    RangeProc = PassthroughRangeProcessor,
20    GnssProc = PassthroughGNSSProcessor,
21    BatteryProc = BatteryProcessor,
22    RcProc = PassthroughRcProcessor,
23    AttitudeProc = PassthroughAttitudeProcessor,
24> {
25    _real: core::marker::PhantomData<R>,
26    pub imu: ImuProc,
27    pub mag: MagProc,
28    pub baro: BaroProc,
29    pub pitot: PitotProc,
30    pub range: RangeProc,
31    pub gnss: GnssProc,
32    pub battery: BatteryProc,
33    pub rc: RcProc,
34    pub attitude: AttitudeProc,
35}
36
37impl<
38    R,
39    ImuProc,
40    MagProc,
41    BaroProc,
42    PitotProc,
43    RangeProc,
44    GnssProc,
45    BatteryProc,
46    RcProc,
47    AttitudeProc,
48> Default
49    for SensorProcessorSet<
50        R,
51        ImuProc,
52        MagProc,
53        BaroProc,
54        PitotProc,
55        RangeProc,
56        GnssProc,
57        BatteryProc,
58        RcProc,
59        AttitudeProc,
60    >
61where
62    R: FlightFloat,
63    ImuProc: Default,
64    MagProc: Default,
65    BaroProc: Default,
66    PitotProc: Default,
67    RangeProc: Default,
68    GnssProc: Default,
69    BatteryProc: Default,
70    RcProc: Default,
71    AttitudeProc: Default,
72{
73    fn default() -> Self {
74        Self {
75            _real: core::marker::PhantomData,
76            imu: ImuProc::default(),
77            mag: MagProc::default(),
78            baro: BaroProc::default(),
79            pitot: PitotProc::default(),
80            range: RangeProc::default(),
81            gnss: GnssProc::default(),
82            battery: BatteryProc::default(),
83            rc: RcProc::default(),
84            attitude: AttitudeProc::default(),
85        }
86    }
87}
88
89pub trait SensorProcessorAccess<R: FlightFloat> {
90    type Imu: SensorPacketProcessor<ImuPacket<R>>;
91    type Mag: SensorPacketProcessor<MagPacket>;
92    type Baro: SensorPacketProcessor<BaroPacket>;
93    type Pitot: SensorPacketProcessor<PitotPacket>;
94    type Range: SensorPacketProcessor<RangePacket>;
95    type Gnss: SensorPacketProcessor<GNSSPacket>;
96    type Battery: SensorPacketProcessor<BatteryPacket>;
97    type Rc: SensorPacketProcessor<RcPacket>;
98    type Attitude: SensorPacketProcessor<AttitudePacket>;
99
100    fn imu(&mut self) -> &mut Self::Imu;
101    fn mag(&mut self) -> &mut Self::Mag;
102    fn baro(&mut self) -> &mut Self::Baro;
103    fn pitot(&mut self) -> &mut Self::Pitot;
104    fn range(&mut self) -> &mut Self::Range;
105    fn gnss(&mut self) -> &mut Self::Gnss;
106    fn battery(&mut self) -> &mut Self::Battery;
107    fn rc(&mut self) -> &mut Self::Rc;
108    fn attitude(&mut self) -> &mut Self::Attitude;
109}
110
111impl<
112    R,
113    ImuProc,
114    MagProc,
115    BaroProc,
116    PitotProc,
117    RangeProc,
118    GnssProc,
119    BatteryProc,
120    RcProc,
121    AttitudeProc,
122> SensorProcessorAccess<R>
123    for SensorProcessorSet<
124        R,
125        ImuProc,
126        MagProc,
127        BaroProc,
128        PitotProc,
129        RangeProc,
130        GnssProc,
131        BatteryProc,
132        RcProc,
133        AttitudeProc,
134    >
135where
136    R: FlightFloat,
137    ImuProc: SensorPacketProcessor<ImuPacket<R>>,
138    MagProc: SensorPacketProcessor<MagPacket>,
139    BaroProc: SensorPacketProcessor<BaroPacket>,
140    PitotProc: SensorPacketProcessor<PitotPacket>,
141    RangeProc: SensorPacketProcessor<RangePacket>,
142    GnssProc: SensorPacketProcessor<GNSSPacket>,
143    BatteryProc: SensorPacketProcessor<BatteryPacket>,
144    RcProc: SensorPacketProcessor<RcPacket>,
145    AttitudeProc: SensorPacketProcessor<AttitudePacket>,
146{
147    type Imu = ImuProc;
148    type Mag = MagProc;
149    type Baro = BaroProc;
150    type Pitot = PitotProc;
151    type Range = RangeProc;
152    type Gnss = GnssProc;
153    type Battery = BatteryProc;
154    type Rc = RcProc;
155    type Attitude = AttitudeProc;
156
157    fn imu(&mut self) -> &mut Self::Imu {
158        &mut self.imu
159    }
160
161    fn mag(&mut self) -> &mut Self::Mag {
162        &mut self.mag
163    }
164
165    fn baro(&mut self) -> &mut Self::Baro {
166        &mut self.baro
167    }
168
169    fn pitot(&mut self) -> &mut Self::Pitot {
170        &mut self.pitot
171    }
172
173    fn range(&mut self) -> &mut Self::Range {
174        &mut self.range
175    }
176
177    fn gnss(&mut self) -> &mut Self::Gnss {
178        &mut self.gnss
179    }
180
181    fn battery(&mut self) -> &mut Self::Battery {
182        &mut self.battery
183    }
184
185    fn rc(&mut self) -> &mut Self::Rc {
186        &mut self.rc
187    }
188
189    fn attitude(&mut self) -> &mut Self::Attitude {
190        &mut self.attitude
191    }
192}
193
194pub struct SensorIngestionCtx<'a, R: FlightFloat, Processors = SensorProcessorSet<R>> {
195    pub raw: &'a mut SensorBus<R>,
196    pub processed: &'a mut ProcessedSensors<R>,
197    pub processors: &'a mut Processors,
198    pub flags: &'a mut CalibrationFlags,
199    pub params: &'a mut Params,
200}
201
202pub fn process_sensor_bus<R, Processors>(ctx: SensorIngestionCtx<'_, R, Processors>)
203where
204    R: FlightFloat,
205    Processors: SensorProcessorAccess<R>,
206{
207    let SensorIngestionCtx {
208        raw,
209        processed,
210        processors,
211        flags,
212        params,
213    } = ctx;
214
215    if raw.imu.is_some() {
216        processed.imu = processors.imu().process(&mut raw.imu, flags, params);
217    } else {
218        processed.imu = None;
219    }
220    if raw.mag.is_some() {
221        processed.mag = processors.mag().process(&mut raw.mag, flags, params);
222    } else {
223        processed.mag = None;
224    }
225    if raw.baro.is_some() {
226        processed.baro = processors.baro().process(&mut raw.baro, flags, params);
227    } else {
228        processed.baro = None;
229    }
230    if raw.pitot.is_some() {
231        processed.pitot = processors.pitot().process(&mut raw.pitot, flags, params);
232    } else {
233        processed.pitot = None;
234    }
235    if raw.range.is_some() {
236        processed.range = processors.range().process(&mut raw.range, flags, params);
237    } else {
238        processed.range = None;
239    }
240    if raw.gnss.is_some() {
241        processed.gnss = processors.gnss().process(&mut raw.gnss, flags, params);
242    } else {
243        processed.gnss = None;
244    }
245    if raw.battery.is_some() {
246        processed.battery = processors
247            .battery()
248            .process(&mut raw.battery, flags, params);
249    }
250    if raw.rc.is_some() {
251        processed.rc = processors.rc().process(&mut raw.rc, flags, params);
252    } else {
253        processed.rc = None;
254    }
255    if raw.attitude.is_some() {
256        processed.attitude = processors
257            .attitude()
258            .process(&mut raw.attitude, flags, params);
259    } else {
260        processed.attitude = None;
261    }
262}
263
264pub fn process_imu_sensor<R, Processors>(ctx: SensorIngestionCtx<'_, R, Processors>)
265where
266    R: FlightFloat,
267    Processors: SensorProcessorAccess<R>,
268{
269    let SensorIngestionCtx {
270        raw,
271        processed,
272        processors,
273        flags,
274        params,
275    } = ctx;
276
277    if raw.imu.is_some() {
278        processed.imu = processors.imu().process(&mut raw.imu, flags, params);
279    } else {
280        processed.imu = None;
281    }
282}
283
284#[cfg(test)]
285mod tests {
286    use super::*;
287    use crate::{
288        packets::RosflightPacketHeader,
289        sensors::processors::{
290            PassthroughBaroProcessor, PassthroughBatteryProcessor, PassthroughImuProcessor,
291            PassthroughMagProcessor, PassthroughPitotProcessor,
292        },
293    };
294
295    #[test]
296    fn process_sensor_bus_moves_raw_packets_into_named_processed_fields() {
297        let mut raw = SensorBus::<f64>::default();
298        let mut processed = ProcessedSensors::<f64>::default();
299        let mut processors = SensorProcessorSet::<
300            f64,
301            PassthroughImuProcessor,
302            PassthroughMagProcessor,
303            PassthroughBaroProcessor,
304            PassthroughPitotProcessor,
305            PassthroughRangeProcessor,
306            PassthroughGNSSProcessor,
307            PassthroughBatteryProcessor,
308            PassthroughRcProcessor,
309            PassthroughAttitudeProcessor,
310        >::default();
311        let mut flags = CalibrationFlags::empty();
312        let mut params = Params::new();
313
314        raw.rc = Some(Ok(RcPacket {
315            header: RosflightPacketHeader {
316                timestamp: 123,
317                status: 0,
318            },
319            n_chan: 1,
320            chan: [0.0; RC_PACKET_CHANNELS],
321            lol: false,
322        }));
323
324        process_sensor_bus(SensorIngestionCtx {
325            raw: &mut raw,
326            processed: &mut processed,
327            processors: &mut processors,
328            flags: &mut flags,
329            params: &mut params,
330        });
331
332        assert!(raw.rc.is_none());
333        assert_eq!(processed.rc.unwrap().header.timestamp, 123);
334    }
335
336    #[test]
337    fn process_sensor_bus_clears_stale_one_shot_packets_when_no_raw_packet_arrives() {
338        let mut raw = SensorBus::<f64>::default();
339        let mut processed = ProcessedSensors::<f64> {
340            imu: Some(ImuPacket {
341                header: RosflightPacketHeader {
342                    timestamp: 100,
343                    status: 0,
344                },
345                ..Default::default()
346            }),
347            rc: Some(RcPacket {
348                header: RosflightPacketHeader {
349                    timestamp: 100,
350                    status: 0,
351                },
352                n_chan: 1,
353                chan: [0.0; RC_PACKET_CHANNELS],
354                lol: false,
355            }),
356            ..Default::default()
357        };
358        let mut processors = SensorProcessorSet::<
359            f64,
360            PassthroughImuProcessor,
361            PassthroughMagProcessor,
362            PassthroughBaroProcessor,
363            PassthroughPitotProcessor,
364            PassthroughRangeProcessor,
365            PassthroughGNSSProcessor,
366            PassthroughBatteryProcessor,
367            PassthroughRcProcessor,
368            PassthroughAttitudeProcessor,
369        >::default();
370        let mut flags = CalibrationFlags::empty();
371        let mut params = Params::new();
372
373        process_sensor_bus(SensorIngestionCtx {
374            raw: &mut raw,
375            processed: &mut processed,
376            processors: &mut processors,
377            flags: &mut flags,
378            params: &mut params,
379        });
380
381        assert!(processed.imu.is_none());
382        assert!(processed.rc.is_none());
383    }
384
385    #[test]
386    fn process_imu_sensor_leaves_service_sensor_state_intact() {
387        let mut raw = SensorBus::<f64>::default();
388        let mut processed = ProcessedSensors::<f64> {
389            rc: Some(RcPacket {
390                header: RosflightPacketHeader {
391                    timestamp: 100,
392                    status: 0,
393                },
394                n_chan: 1,
395                chan: [0.0; RC_PACKET_CHANNELS],
396                lol: false,
397            }),
398            ..Default::default()
399        };
400        let mut processors = SensorProcessorSet::<
401            f64,
402            PassthroughImuProcessor,
403            PassthroughMagProcessor,
404            PassthroughBaroProcessor,
405            PassthroughPitotProcessor,
406            PassthroughRangeProcessor,
407            PassthroughGNSSProcessor,
408            PassthroughBatteryProcessor,
409            PassthroughRcProcessor,
410            PassthroughAttitudeProcessor,
411        >::default();
412        let mut flags = CalibrationFlags::empty();
413        let mut params = Params::new();
414
415        raw.imu = Some(Ok(ImuPacket {
416            header: RosflightPacketHeader {
417                timestamp: 200,
418                status: 0,
419            },
420            ..Default::default()
421        }));
422
423        process_imu_sensor(SensorIngestionCtx {
424            raw: &mut raw,
425            processed: &mut processed,
426            processors: &mut processors,
427            flags: &mut flags,
428            params: &mut params,
429        });
430
431        assert!(raw.imu.is_none());
432        assert_eq!(processed.imu.unwrap().header.timestamp, 200);
433        assert_eq!(processed.rc.unwrap().header.timestamp, 100);
434    }
435}
veloxity_core/sensors/ingestion.rs

veloxity_core/sensors/
ingestion.rs
