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    pub now_ms: u32,
201}
202
203pub fn process_sensor_bus<R, Processors>(ctx: SensorIngestionCtx<'_, R, Processors>)
204where
205    R: FlightFloat,
206    Processors: SensorProcessorAccess<R>,
207{
208    let SensorIngestionCtx {
209        raw,
210        processed,
211        processors,
212        flags,
213        params,
214        now_ms,
215    } = ctx;
216
217    if raw.imu.is_some() {
218        processed.imu = processors.imu().process(&mut raw.imu, flags, params);
219    } else {
220        processed.imu = None;
221    }
222    if raw.mag.is_some() {
223        processed.mag = processors.mag().process(&mut raw.mag, flags, params);
224    } else {
225        processed.mag = None;
226    }
227    if raw.baro.is_some() {
228        processed.baro = processors
229            .baro()
230            .process_at(&mut raw.baro, flags, params, now_ms);
231    } else {
232        processed.baro = None;
233    }
234    if raw.pitot.is_some() {
235        processed.pitot = processors
236            .pitot()
237            .process_at(&mut raw.pitot, flags, params, now_ms);
238    } else {
239        processed.pitot = None;
240    }
241    if raw.range.is_some() {
242        processed.range = processors.range().process(&mut raw.range, flags, params);
243    } else {
244        processed.range = None;
245    }
246    if raw.gnss.is_some() {
247        processed.gnss = processors.gnss().process(&mut raw.gnss, flags, params);
248    } else {
249        processed.gnss = None;
250    }
251    if raw.battery.is_some() {
252        processed.battery = processors
253            .battery()
254            .process(&mut raw.battery, flags, params);
255    }
256    if raw.rc.is_some() {
257        processed.rc = processors.rc().process(&mut raw.rc, flags, params);
258    } else {
259        processed.rc = None;
260    }
261    if raw.attitude.is_some() {
262        processed.attitude = processors
263            .attitude()
264            .process(&mut raw.attitude, flags, params);
265    } else {
266        processed.attitude = None;
267    }
268}
269
270pub fn process_imu_sensor<R, Processors>(ctx: SensorIngestionCtx<'_, R, Processors>)
271where
272    R: FlightFloat,
273    Processors: SensorProcessorAccess<R>,
274{
275    let SensorIngestionCtx {
276        raw,
277        processed,
278        processors,
279        flags,
280        params,
281        now_ms: _,
282    } = ctx;
283
284    if raw.imu.is_some() {
285        processed.imu = processors.imu().process(&mut raw.imu, flags, params);
286    } else {
287        processed.imu = None;
288    }
289}
290
291#[cfg(test)]
292mod tests {
293    use super::*;
294    use crate::{
295        packets::RosflightPacketHeader,
296        sensors::processors::{
297            PassthroughBaroProcessor, PassthroughBatteryProcessor, PassthroughImuProcessor,
298            PassthroughMagProcessor, PassthroughPitotProcessor,
299        },
300    };
301
302    #[test]
303    fn process_sensor_bus_moves_raw_packets_into_named_processed_fields() {
304        let mut raw = SensorBus::<f64>::default();
305        let mut processed = ProcessedSensors::<f64>::default();
306        let mut processors = SensorProcessorSet::<
307            f64,
308            PassthroughImuProcessor,
309            PassthroughMagProcessor,
310            PassthroughBaroProcessor,
311            PassthroughPitotProcessor,
312            PassthroughRangeProcessor,
313            PassthroughGNSSProcessor,
314            PassthroughBatteryProcessor,
315            PassthroughRcProcessor,
316            PassthroughAttitudeProcessor,
317        >::default();
318        let mut flags = CalibrationFlags::empty();
319        let mut params = Params::new();
320
321        raw.rc = Some(Ok(RcPacket {
322            header: RosflightPacketHeader {
323                timestamp: 123,
324                status: 0,
325            },
326            n_chan: 1,
327            chan: [0.0; RC_PACKET_CHANNELS],
328            lol: false,
329        }));
330
331        process_sensor_bus(SensorIngestionCtx {
332            raw: &mut raw,
333            processed: &mut processed,
334            processors: &mut processors,
335            flags: &mut flags,
336            params: &mut params,
337            now_ms: 0,
338        });
339
340        assert!(raw.rc.is_none());
341        assert_eq!(processed.rc.unwrap().header.timestamp, 123);
342    }
343
344    #[test]
345    fn process_sensor_bus_clears_stale_one_shot_packets_when_no_raw_packet_arrives() {
346        let mut raw = SensorBus::<f64>::default();
347        let mut processed = ProcessedSensors::<f64> {
348            imu: Some(ImuPacket {
349                header: RosflightPacketHeader {
350                    timestamp: 100,
351                    status: 0,
352                },
353                ..Default::default()
354            }),
355            rc: Some(RcPacket {
356                header: RosflightPacketHeader {
357                    timestamp: 100,
358                    status: 0,
359                },
360                n_chan: 1,
361                chan: [0.0; RC_PACKET_CHANNELS],
362                lol: false,
363            }),
364            ..Default::default()
365        };
366        let mut processors = SensorProcessorSet::<
367            f64,
368            PassthroughImuProcessor,
369            PassthroughMagProcessor,
370            PassthroughBaroProcessor,
371            PassthroughPitotProcessor,
372            PassthroughRangeProcessor,
373            PassthroughGNSSProcessor,
374            PassthroughBatteryProcessor,
375            PassthroughRcProcessor,
376            PassthroughAttitudeProcessor,
377        >::default();
378        let mut flags = CalibrationFlags::empty();
379        let mut params = Params::new();
380
381        process_sensor_bus(SensorIngestionCtx {
382            raw: &mut raw,
383            processed: &mut processed,
384            processors: &mut processors,
385            flags: &mut flags,
386            params: &mut params,
387            now_ms: 0,
388        });
389
390        assert!(processed.imu.is_none());
391        assert!(processed.rc.is_none());
392    }
393
394    #[test]
395    fn process_imu_sensor_leaves_service_sensor_state_intact() {
396        let mut raw = SensorBus::<f64>::default();
397        let mut processed = ProcessedSensors::<f64> {
398            rc: Some(RcPacket {
399                header: RosflightPacketHeader {
400                    timestamp: 100,
401                    status: 0,
402                },
403                n_chan: 1,
404                chan: [0.0; RC_PACKET_CHANNELS],
405                lol: false,
406            }),
407            ..Default::default()
408        };
409        let mut processors = SensorProcessorSet::<
410            f64,
411            PassthroughImuProcessor,
412            PassthroughMagProcessor,
413            PassthroughBaroProcessor,
414            PassthroughPitotProcessor,
415            PassthroughRangeProcessor,
416            PassthroughGNSSProcessor,
417            PassthroughBatteryProcessor,
418            PassthroughRcProcessor,
419            PassthroughAttitudeProcessor,
420        >::default();
421        let mut flags = CalibrationFlags::empty();
422        let mut params = Params::new();
423
424        raw.imu = Some(Ok(ImuPacket {
425            header: RosflightPacketHeader {
426                timestamp: 200,
427                status: 0,
428            },
429            ..Default::default()
430        }));
431
432        process_imu_sensor(SensorIngestionCtx {
433            raw: &mut raw,
434            processed: &mut processed,
435            processors: &mut processors,
436            flags: &mut flags,
437            params: &mut params,
438            now_ms: 0,
439        });
440
441        assert!(raw.imu.is_none());
442        assert_eq!(processed.imu.unwrap().header.timestamp, 200);
443        assert_eq!(processed.rc.unwrap().header.timestamp, 100);
444    }
445}
veloxity_core/sensors/ingestion.rs

veloxity_core/sensors/
ingestion.rs
