ph_qmi8658/data/
fifo.rs

1//! FIFO configuration and status helpers.
2
3use super::{AccelRaw, GyroRaw, MagRaw, Timestamp};
4use crate::register::{fifo_ctrl, fifo_status};
5
6/// FIFO operating mode.
7#[derive(Clone, Copy, Debug, PartialEq, Eq)]
8#[cfg_attr(feature = "defmt", derive(defmt::Format))]
9pub enum FifoMode {
10    /// FIFO disabled (bypass).
11    Bypass,
12    /// FIFO mode (stop collecting when full).
13    Fifo,
14    /// Streaming mode (overwrite oldest when full).
15    Stream,
16    /// Stream-to-FIFO mode (reserved on some revisions).
17    StreamToFifo,
18}
19
20impl FifoMode {
21    pub(crate) const fn bits(self) -> u8 {
22        match self {
23            Self::Bypass => 0b00,
24            Self::Fifo => 0b01,
25            Self::Stream => 0b10,
26            Self::StreamToFifo => 0b11,
27        }
28    }
29}
30
31/// FIFO size selection (samples per enabled sensor).
32#[derive(Clone, Copy, Debug, PartialEq, Eq)]
33#[cfg_attr(feature = "defmt", derive(defmt::Format))]
34pub enum FifoSize {
35    /// 16 samples per enabled sensor.
36    Samples16,
37    /// 32 samples per enabled sensor.
38    Samples32,
39    /// 64 samples per enabled sensor.
40    Samples64,
41    /// 128 samples per enabled sensor.
42    Samples128,
43}
44
45impl FifoSize {
46    pub(crate) const fn bits(self) -> u8 {
47        match self {
48            Self::Samples16 => 0b00,
49            Self::Samples32 => 0b01,
50            Self::Samples64 => 0b10,
51            Self::Samples128 => 0b11,
52        }
53    }
54
55    /// Returns the configured samples per enabled sensor.
56    pub const fn samples(self) -> u16 {
57        match self {
58            Self::Samples16 => 16,
59            Self::Samples32 => 32,
60            Self::Samples64 => 64,
61            Self::Samples128 => 128,
62        }
63    }
64}
65
66/// FIFO configuration (watermark + mode + size).
67#[derive(Clone, Copy, Debug, PartialEq, Eq)]
68#[cfg_attr(feature = "defmt", derive(defmt::Format))]
69pub struct FifoConfig {
70    /// FIFO watermark level (in ODRs).
71    pub watermark: u8,
72    /// FIFO mode.
73    pub mode: FifoMode,
74    /// FIFO size setting.
75    pub size: FifoSize,
76}
77
78impl FifoConfig {
79    /// Default FIFO configuration (bypass).
80    pub const DEFAULT: Self = Self {
81        watermark: 0,
82        mode: FifoMode::Bypass,
83        size: FifoSize::Samples16,
84    };
85
86    /// Creates a new FIFO configuration.
87    pub const fn new(mode: FifoMode, size: FifoSize, watermark: u8) -> Self {
88        Self {
89            watermark,
90            mode,
91            size,
92        }
93    }
94
95    /// Sets the FIFO mode.
96    #[must_use]
97    pub const fn with_mode(self, mode: FifoMode) -> Self {
98        Self { mode, ..self }
99    }
100
101    /// Sets the FIFO size.
102    #[must_use]
103    pub const fn with_size(self, size: FifoSize) -> Self {
104        Self { size, ..self }
105    }
106
107    /// Sets the FIFO watermark level (in ODRs).
108    #[must_use]
109    pub const fn with_watermark(self, watermark: u8) -> Self {
110        Self { watermark, ..self }
111    }
112
113    pub(crate) const fn ctrl_value(self) -> u8 {
114        ((self.size.bits() << fifo_ctrl::FIFO_SIZE_SHIFT) & fifo_ctrl::FIFO_SIZE_MASK)
115            | ((self.mode.bits() << fifo_ctrl::FIFO_MODE_SHIFT) & fifo_ctrl::FIFO_MODE_MASK)
116    }
117}
118
119impl Default for FifoConfig {
120    fn default() -> Self {
121        Self::DEFAULT
122    }
123}
124
125/// FIFO status decoded from FIFO_STATUS and FIFO_SMPL_CNT.
126#[derive(Clone, Copy, Debug, Default, PartialEq, Eq)]
127#[cfg_attr(feature = "defmt", derive(defmt::Format))]
128pub struct FifoStatus {
129    /// FIFO is full.
130    pub full: bool,
131    /// FIFO watermark reached.
132    pub watermark: bool,
133    /// FIFO overflow has occurred.
134    pub overflow: bool,
135    /// FIFO is not empty.
136    pub not_empty: bool,
137    /// FIFO sample count in bytes.
138    pub sample_count_bytes: u16,
139}
140
141/// Result of a FIFO readout.
142#[derive(Clone, Copy, Debug, Default, PartialEq, Eq)]
143#[cfg_attr(feature = "defmt", derive(defmt::Format))]
144pub struct FifoReadout {
145    /// Status snapshot captured before reading.
146    pub status: FifoStatus,
147    /// Number of bytes read into the caller buffer.
148    pub bytes_read: usize,
149    /// Timestamp captured after FIFO readout.
150    pub timestamp: Timestamp,
151}
152
153/// FIFO frame format derived from enabled sensors.
154///
155/// Datasheet order:
156/// - One sensor: that sensor's X/Y/Z sample order.
157/// - Two sensors: accel samples then gyro samples, repeated.
158/// - Three sensors: accel, gyro, then magnetometer samples.
159#[derive(Clone, Copy, Debug, PartialEq, Eq)]
160#[cfg_attr(feature = "defmt", derive(defmt::Format))]
161pub struct FifoFrameFormat {
162    /// Whether accelerometer data is present in each frame.
163    pub accel: bool,
164    /// Whether gyroscope data is present in each frame.
165    pub gyro: bool,
166    /// Whether magnetometer data is present in each frame.
167    pub mag: bool,
168    /// Interpret 16-bit samples as big-endian (CTRL1.BE = 1).
169    pub big_endian: bool,
170}
171
172impl FifoFrameFormat {
173    /// Creates a new FIFO frame format.
174    pub const fn new(accel: bool, gyro: bool, mag: bool) -> Self {
175        Self {
176            accel,
177            gyro,
178            mag,
179            big_endian: false,
180        }
181    }
182
183    /// Sets the endianness for frame decoding.
184    #[must_use]
185    pub const fn with_big_endian(mut self, big_endian: bool) -> Self {
186        self.big_endian = big_endian;
187        self
188    }
189
190    /// Returns the bytes per FIFO frame.
191    pub const fn bytes_per_frame(self) -> usize {
192        super::fifo_frame_bytes(self.accel, self.gyro, self.mag)
193    }
194
195    /// Returns the maximum number of complete frames in the provided byte count.
196    pub const fn frame_count(self, bytes: usize) -> usize {
197        let frame = self.bytes_per_frame();
198        if frame == 0 { 0 } else { bytes / frame }
199    }
200}
201
202pub(crate) fn fifo_read_len(
203    status: FifoStatus,
204    format: FifoFrameFormat,
205    buffer_len: usize,
206) -> usize {
207    if buffer_len == 0 {
208        return 0;
209    }
210    let mut available = status.sample_count_bytes as usize;
211    if available == 0 {
212        return 0;
213    }
214    let frame = format.bytes_per_frame();
215    if frame == 0 {
216        return 0;
217    }
218    if available > buffer_len {
219        available = buffer_len;
220    }
221    available - (available % frame)
222}
223
224/// One decoded FIFO frame.
225#[derive(Clone, Copy, Debug, Default, PartialEq, Eq)]
226#[cfg_attr(feature = "defmt", derive(defmt::Format))]
227#[allow(dead_code)]
228pub struct FifoFrame {
229    /// Accelerometer data, if present in this frame.
230    pub accel: Option<AccelRaw>,
231    /// Gyroscope data, if present in this frame.
232    pub gyro: Option<GyroRaw>,
233    /// Magnetometer data, if present in this frame.
234    pub mag: Option<MagRaw>,
235}
236
237/// Iterator over FIFO frames.
238#[allow(dead_code)]
239pub struct FifoFrameIterator<'a> {
240    data: &'a [u8],
241    format: FifoFrameFormat,
242    offset: usize,
243}
244
245#[allow(dead_code)]
246impl<'a> FifoFrameIterator<'a> {
247    /// Creates an iterator over FIFO frames.
248    pub const fn new(data: &'a [u8], format: FifoFrameFormat) -> Self {
249        Self {
250            data,
251            format,
252            offset: 0,
253        }
254    }
255
256    fn take6(&self, offset: usize) -> [u8; 6] {
257        [
258            self.data[offset],
259            self.data[offset + 1],
260            self.data[offset + 2],
261            self.data[offset + 3],
262            self.data[offset + 4],
263            self.data[offset + 5],
264        ]
265    }
266
267    /// Returns the remaining unparsed bytes.
268    pub const fn remaining(&self) -> usize {
269        if self.offset >= self.data.len() {
270            0
271        } else {
272            self.data.len() - self.offset
273        }
274    }
275}
276
277impl<'a> Iterator for FifoFrameIterator<'a> {
278    type Item = FifoFrame;
279
280    fn next(&mut self) -> Option<Self::Item> {
281        let frame_len = self.format.bytes_per_frame();
282        if frame_len == 0 || self.offset + frame_len > self.data.len() {
283            return None;
284        }
285
286        let mut offset = self.offset;
287        let accel = if self.format.accel {
288            let bytes = self.take6(offset);
289            offset += 6;
290            Some(AccelRaw::from_bytes(bytes, self.format.big_endian))
291        } else {
292            None
293        };
294
295        let gyro = if self.format.gyro {
296            let bytes = self.take6(offset);
297            offset += 6;
298            Some(GyroRaw::from_bytes(bytes, self.format.big_endian))
299        } else {
300            None
301        };
302
303        let mag = if self.format.mag {
304            let bytes = self.take6(offset);
305            offset += 6;
306            Some(MagRaw::from_bytes(bytes, self.format.big_endian))
307        } else {
308            None
309        };
310
311        self.offset = offset;
312        Some(FifoFrame { accel, gyro, mag })
313    }
314}
315
316#[allow(dead_code)]
317impl FifoFrame {
318    /// Returns the expected FIFO format for accel-only data.
319    pub const fn accel_only() -> FifoFrameFormat {
320        FifoFrameFormat::new(true, false, false)
321    }
322
323    /// Returns the expected FIFO format for gyro-only data.
324    pub const fn gyro_only() -> FifoFrameFormat {
325        FifoFrameFormat::new(false, true, false)
326    }
327
328    /// Returns the expected FIFO format for accel + gyro data.
329    pub const fn accel_gyro() -> FifoFrameFormat {
330        FifoFrameFormat::new(true, true, false)
331    }
332}
333
334impl FifoStatus {
335    pub(crate) const fn from_regs(status: u8, smpl_cnt_lsb: u8) -> Self {
336        let msb = (status & fifo_status::SMPL_CNT_MSB_MASK) as u16;
337        let lsb = smpl_cnt_lsb as u16;
338        Self {
339            full: (status & fifo_status::FIFO_FULL) != 0,
340            watermark: (status & fifo_status::FIFO_WTM) != 0,
341            overflow: (status & fifo_status::FIFO_OVFLOW) != 0,
342            not_empty: (status & fifo_status::FIFO_NOT_EMPTY) != 0,
343            sample_count_bytes: (msb << 8) | lsb,
344        }
345    }
346
347    /// Returns true if any FIFO status flag is set.
348    pub const fn any(self) -> bool {
349        self.full || self.watermark || self.overflow || self.not_empty
350    }
351
352    /// Converts the byte count to samples given a frame size.
353    pub const fn samples(self, bytes_per_sample: u16) -> u16 {
354        if bytes_per_sample == 0 {
355            0
356        } else {
357            self.sample_count_bytes / bytes_per_sample
358        }
359    }
360}
361
362#[cfg(test)]
363mod tests {
364    use super::*;
365
366    #[test]
367    fn frame_format_sizes() {
368        let none = FifoFrameFormat::new(false, false, false);
369        assert_eq!(none.bytes_per_frame(), 0);
370
371        let accel = FifoFrameFormat::new(true, false, false);
372        assert_eq!(accel.bytes_per_frame(), 6);
373        assert_eq!(accel.frame_count(12), 2);
374        assert_eq!(accel.frame_count(11), 1);
375
376        let accel_gyro = FifoFrameFormat::new(true, true, false);
377        assert_eq!(accel_gyro.bytes_per_frame(), 12);
378
379        let all = FifoFrameFormat::new(true, true, true);
380        assert_eq!(all.bytes_per_frame(), 18);
381    }
382
383    #[test]
384    fn parse_accel_gyro_little_endian() {
385        let format = FifoFrameFormat::new(true, true, false).with_big_endian(false);
386        let buffer = [
387            0x01, 0x00, 0x02, 0x00, 0x03, 0x00, // accel: 1,2,3
388            0x04, 0x00, 0x05, 0x00, 0x06, 0x00, // gyro: 4,5,6
389        ];
390
391        let mut iter = FifoFrameIterator::new(&buffer, format);
392        let frame = iter.next().expect("frame");
393        let accel = frame.accel.expect("accel");
394        let gyro = frame.gyro.expect("gyro");
395
396        assert_eq!(accel.x, 1);
397        assert_eq!(accel.y, 2);
398        assert_eq!(accel.z, 3);
399        assert_eq!(gyro.x, 4);
400        assert_eq!(gyro.y, 5);
401        assert_eq!(gyro.z, 6);
402        assert!(iter.next().is_none());
403    }
404
405    #[test]
406    fn parse_accel_big_endian() {
407        let format = FifoFrameFormat::new(true, false, false).with_big_endian(true);
408        let buffer = [0x00, 0x01, 0x00, 0x02, 0x00, 0x03];
409
410        let mut iter = FifoFrameIterator::new(&buffer, format);
411        let frame = iter.next().expect("frame");
412        let accel = frame.accel.expect("accel");
413
414        assert_eq!(accel.x, 1);
415        assert_eq!(accel.y, 2);
416        assert_eq!(accel.z, 3);
417    }
418
419    #[test]
420    fn fifo_read_len_aligns_to_frames() {
421        let status = FifoStatus {
422            sample_count_bytes: 25,
423            ..FifoStatus::default()
424        };
425        let format = FifoFrameFormat::new(true, true, false); // 12 bytes/frame
426        assert_eq!(fifo_read_len(status, format, 64), 24);
427        assert_eq!(fifo_read_len(status, format, 12), 12);
428        assert_eq!(fifo_read_len(status, format, 8), 0);
429    }
430}
ph_qmi8658/data/fifo.rs

ph_qmi8658/data/
fifo.rs
