ph_qmi8658/data/
mod.rs

1//! Sensor data readout helpers.
2
3pub(crate) mod fifo;
4pub(crate) mod scale;
5#[cfg(feature = "fixed")]
6pub(crate) mod fixed;
7
8pub(crate) use fifo::fifo_read_len;
9pub use fifo::{
10    FifoConfig, FifoFrame, FifoFrameFormat, FifoFrameIterator, FifoMode, FifoReadout, FifoSize,
11    FifoStatus,
12};
13pub use scale::{
14    ScaleFactor,
15    accel_lsb_per_g,
16    accel_mg_per_lsb,
17    gyro_lsb_per_dps,
18    gyro_mdps_per_lsb,
19    temperature_lsb_per_celsius,
20    temperature_mdegc_per_lsb,
21};
22
23use crate::register::Register;
24
25pub(crate) const RAW_BLOCK_START: Register = Register::TimestampLow;
26pub(crate) const RAW_BLOCK_LEN: usize = 17;
27pub(crate) const RAW_BLOCK_TIMESTAMP_OFFSET: usize = 0;
28pub(crate) const RAW_BLOCK_TEMPERATURE_OFFSET: usize = 3;
29pub(crate) const RAW_BLOCK_ACCEL_OFFSET: usize = 5;
30pub(crate) const RAW_BLOCK_GYRO_OFFSET: usize = 11;
31
32pub(crate) const FIFO_AXIS_BYTES: usize = 6;
33
34#[derive(Clone, Copy, Debug, PartialEq, Eq)]
35pub(crate) struct FifoFrameLayout {
36    pub accel: bool,
37    pub gyro: bool,
38    pub mag: bool,
39    pub bytes_per_frame: usize,
40}
41
42pub(crate) const FIFO_FRAME_LAYOUTS: [FifoFrameLayout; 8] = [
43    FifoFrameLayout {
44        accel: false,
45        gyro: false,
46        mag: false,
47        bytes_per_frame: 0,
48    },
49    FifoFrameLayout {
50        accel: true,
51        gyro: false,
52        mag: false,
53        bytes_per_frame: FIFO_AXIS_BYTES,
54    },
55    FifoFrameLayout {
56        accel: false,
57        gyro: true,
58        mag: false,
59        bytes_per_frame: FIFO_AXIS_BYTES,
60    },
61    FifoFrameLayout {
62        accel: true,
63        gyro: true,
64        mag: false,
65        bytes_per_frame: FIFO_AXIS_BYTES * 2,
66    },
67    FifoFrameLayout {
68        accel: false,
69        gyro: false,
70        mag: true,
71        bytes_per_frame: FIFO_AXIS_BYTES,
72    },
73    FifoFrameLayout {
74        accel: true,
75        gyro: false,
76        mag: true,
77        bytes_per_frame: FIFO_AXIS_BYTES * 2,
78    },
79    FifoFrameLayout {
80        accel: false,
81        gyro: true,
82        mag: true,
83        bytes_per_frame: FIFO_AXIS_BYTES * 2,
84    },
85    FifoFrameLayout {
86        accel: true,
87        gyro: true,
88        mag: true,
89        bytes_per_frame: FIFO_AXIS_BYTES * 3,
90    },
91];
92
93pub(crate) const fn fifo_frame_bytes(accel: bool, gyro: bool, mag: bool) -> usize {
94    let idx = (accel as usize) | ((gyro as usize) << 1) | ((mag as usize) << 2);
95    FIFO_FRAME_LAYOUTS[idx].bytes_per_frame
96}
97
98/// Sample timestamp (24-bit counter).
99#[derive(Clone, Copy, Debug, PartialEq, Eq, Default)]
100#[cfg_attr(feature = "defmt", derive(defmt::Format))]
101pub struct Timestamp {
102    /// Raw 24-bit counter value.
103    pub ticks: u32,
104}
105
106impl Timestamp {
107    pub(crate) const fn from_bytes(bytes: [u8; 3]) -> Self {
108        let ticks = ((bytes[2] as u32) << 16) | ((bytes[1] as u32) << 8) | (bytes[0] as u32);
109        Self { ticks }
110    }
111}
112
113/// Timestamped sample wrapper.
114#[derive(Clone, Copy, Debug, PartialEq, Eq, Default)]
115#[cfg_attr(feature = "defmt", derive(defmt::Format))]
116pub struct Sample<T> {
117    /// Sample timestamp.
118    pub timestamp: Timestamp,
119    /// Sample payload.
120    pub data: T,
121}
122
123/// Raw block of sensor data sampled at a single timestamp.
124#[derive(Clone, Copy, Debug, PartialEq, Eq, Default)]
125#[cfg_attr(feature = "defmt", derive(defmt::Format))]
126pub struct RawBlock {
127    /// Sample timestamp.
128    pub timestamp: Timestamp,
129    /// Raw temperature reading.
130    pub temperature: TemperatureRaw,
131    /// Raw accelerometer reading, if enabled.
132    pub accel: Option<AccelRaw>,
133    /// Raw gyroscope reading, if enabled.
134    pub gyro: Option<GyroRaw>,
135}
136
137/// Decodes a raw block buffer into a structured sample.
138pub(crate) fn decode_raw_block(
139    buffer: &[u8; RAW_BLOCK_LEN],
140    big_endian: bool,
141    accel_enabled: bool,
142    gyro_enabled: bool,
143) -> RawBlock {
144    let timestamp = Timestamp::from_bytes([
145        buffer[RAW_BLOCK_TIMESTAMP_OFFSET],
146        buffer[RAW_BLOCK_TIMESTAMP_OFFSET + 1],
147        buffer[RAW_BLOCK_TIMESTAMP_OFFSET + 2],
148    ]);
149    let temperature = TemperatureRaw::from_bytes(
150        [
151            buffer[RAW_BLOCK_TEMPERATURE_OFFSET],
152            buffer[RAW_BLOCK_TEMPERATURE_OFFSET + 1],
153        ],
154        big_endian,
155    );
156
157    let accel = if accel_enabled {
158        Some(AccelRaw::from_bytes(
159            [
160                buffer[RAW_BLOCK_ACCEL_OFFSET],
161                buffer[RAW_BLOCK_ACCEL_OFFSET + 1],
162                buffer[RAW_BLOCK_ACCEL_OFFSET + 2],
163                buffer[RAW_BLOCK_ACCEL_OFFSET + 3],
164                buffer[RAW_BLOCK_ACCEL_OFFSET + 4],
165                buffer[RAW_BLOCK_ACCEL_OFFSET + 5],
166            ],
167            big_endian,
168        ))
169    } else {
170        None
171    };
172
173    let gyro = if gyro_enabled {
174        Some(GyroRaw::from_bytes(
175            [
176                buffer[RAW_BLOCK_GYRO_OFFSET],
177                buffer[RAW_BLOCK_GYRO_OFFSET + 1],
178                buffer[RAW_BLOCK_GYRO_OFFSET + 2],
179                buffer[RAW_BLOCK_GYRO_OFFSET + 3],
180                buffer[RAW_BLOCK_GYRO_OFFSET + 4],
181                buffer[RAW_BLOCK_GYRO_OFFSET + 5],
182            ],
183            big_endian,
184        ))
185    } else {
186        None
187    };
188
189    RawBlock {
190        timestamp,
191        temperature,
192        accel,
193        gyro,
194    }
195}
196
197/// Raw accelerometer sample with timestamp, flattened for stable layout.
198#[repr(C)]
199#[derive(Clone, Copy, Debug, PartialEq, Eq, Default)]
200#[cfg_attr(feature = "defmt", derive(defmt::Format))]
201#[allow(dead_code)]
202pub struct AccelSampleRaw {
203    /// Raw timestamp counter.
204    pub timestamp_ticks: u32,
205    /// Accelerometer X-axis.
206    pub accel_x: i16,
207    /// Accelerometer Y-axis.
208    pub accel_y: i16,
209    /// Accelerometer Z-axis.
210    pub accel_z: i16,
211}
212
213/// Raw accelerometer + gyroscope sample with timestamp, flattened for stable layout.
214#[repr(C)]
215#[derive(Clone, Copy, Debug, PartialEq, Eq, Default)]
216#[cfg_attr(feature = "defmt", derive(defmt::Format))]
217#[allow(dead_code)]
218pub struct AccelGyroSampleRaw {
219    /// Raw timestamp counter.
220    pub timestamp_ticks: u32,
221    /// Accelerometer X-axis.
222    pub accel_x: i16,
223    /// Accelerometer Y-axis.
224    pub accel_y: i16,
225    /// Accelerometer Z-axis.
226    pub accel_z: i16,
227    /// Gyroscope X-axis.
228    pub gyro_x: i16,
229    /// Gyroscope Y-axis.
230    pub gyro_y: i16,
231    /// Gyroscope Z-axis.
232    pub gyro_z: i16,
233}
234
235/// Raw accelerometer + gyroscope + magnetometer sample with timestamp, flattened for stable layout.
236#[repr(C)]
237#[derive(Clone, Copy, Debug, PartialEq, Eq, Default)]
238#[cfg_attr(feature = "defmt", derive(defmt::Format))]
239#[allow(dead_code)]
240pub struct AccelGyroMagSampleRaw {
241    /// Raw timestamp counter.
242    pub timestamp_ticks: u32,
243    /// Accelerometer X-axis.
244    pub accel_x: i16,
245    /// Accelerometer Y-axis.
246    pub accel_y: i16,
247    /// Accelerometer Z-axis.
248    pub accel_z: i16,
249    /// Gyroscope X-axis.
250    pub gyro_x: i16,
251    /// Gyroscope Y-axis.
252    pub gyro_y: i16,
253    /// Gyroscope Z-axis.
254    pub gyro_z: i16,
255    /// Magnetometer X-axis.
256    pub mag_x: i16,
257    /// Magnetometer Y-axis.
258    pub mag_y: i16,
259    /// Magnetometer Z-axis.
260    pub mag_z: i16,
261}
262
263/// Raw accelerometer sample.
264#[derive(Clone, Copy, Debug, PartialEq, Eq, Default)]
265#[cfg_attr(feature = "defmt", derive(defmt::Format))]
266pub struct AccelRaw {
267    /// X-axis raw count.
268    pub x: i16,
269    /// Y-axis raw count.
270    pub y: i16,
271    /// Z-axis raw count.
272    pub z: i16,
273}
274
275impl AccelRaw {
276    pub(crate) fn from_bytes(bytes: [u8; 6], big_endian: bool) -> Self {
277        let (x, y, z) = if big_endian {
278            (
279                i16::from_be_bytes([bytes[0], bytes[1]]),
280                i16::from_be_bytes([bytes[2], bytes[3]]),
281                i16::from_be_bytes([bytes[4], bytes[5]]),
282            )
283        } else {
284            (
285                i16::from_le_bytes([bytes[0], bytes[1]]),
286                i16::from_le_bytes([bytes[2], bytes[3]]),
287                i16::from_le_bytes([bytes[4], bytes[5]]),
288            )
289        };
290        Self { x, y, z }
291    }
292
293    #[allow(dead_code)]
294    pub(crate) fn from_le_bytes(bytes: [u8; 6]) -> Self {
295        Self::from_bytes(bytes, false)
296    }
297}
298
299/// Raw gyroscope sample.
300#[derive(Clone, Copy, Debug, PartialEq, Eq, Default)]
301#[cfg_attr(feature = "defmt", derive(defmt::Format))]
302pub struct GyroRaw {
303    /// X-axis raw count.
304    pub x: i16,
305    /// Y-axis raw count.
306    pub y: i16,
307    /// Z-axis raw count.
308    pub z: i16,
309}
310
311impl GyroRaw {
312    pub(crate) fn from_bytes(bytes: [u8; 6], big_endian: bool) -> Self {
313        let (x, y, z) = if big_endian {
314            (
315                i16::from_be_bytes([bytes[0], bytes[1]]),
316                i16::from_be_bytes([bytes[2], bytes[3]]),
317                i16::from_be_bytes([bytes[4], bytes[5]]),
318            )
319        } else {
320            (
321                i16::from_le_bytes([bytes[0], bytes[1]]),
322                i16::from_le_bytes([bytes[2], bytes[3]]),
323                i16::from_le_bytes([bytes[4], bytes[5]]),
324            )
325        };
326        Self { x, y, z }
327    }
328
329    #[allow(dead_code)]
330    pub(crate) fn from_le_bytes(bytes: [u8; 6]) -> Self {
331        Self::from_bytes(bytes, false)
332    }
333}
334
335/// Raw temperature sample.
336#[derive(Clone, Copy, Debug, PartialEq, Eq, Default)]
337#[cfg_attr(feature = "defmt", derive(defmt::Format))]
338pub struct TemperatureRaw {
339    /// Raw temperature count.
340    pub value: i16,
341}
342
343impl TemperatureRaw {
344    pub(crate) fn from_bytes(bytes: [u8; 2], big_endian: bool) -> Self {
345        let value = if big_endian {
346            i16::from_be_bytes(bytes)
347        } else {
348            i16::from_le_bytes(bytes)
349        };
350        Self { value }
351    }
352
353    #[allow(dead_code)]
354    pub(crate) fn from_le_bytes(bytes: [u8; 2]) -> Self {
355        Self::from_bytes(bytes, false)
356    }
357}
358
359/// Raw magnetometer sample.
360#[derive(Clone, Copy, Debug, PartialEq, Eq, Default)]
361#[cfg_attr(feature = "defmt", derive(defmt::Format))]
362#[allow(dead_code)]
363pub struct MagRaw {
364    /// X-axis raw count.
365    pub x: i16,
366    /// Y-axis raw count.
367    pub y: i16,
368    /// Z-axis raw count.
369    pub z: i16,
370}
371
372#[allow(dead_code)]
373impl MagRaw {
374    pub(crate) fn from_bytes(bytes: [u8; 6], big_endian: bool) -> Self {
375        let (x, y, z) = if big_endian {
376            (
377                i16::from_be_bytes([bytes[0], bytes[1]]),
378                i16::from_be_bytes([bytes[2], bytes[3]]),
379                i16::from_be_bytes([bytes[4], bytes[5]]),
380            )
381        } else {
382            (
383                i16::from_le_bytes([bytes[0], bytes[1]]),
384                i16::from_le_bytes([bytes[2], bytes[3]]),
385                i16::from_le_bytes([bytes[4], bytes[5]]),
386            )
387        };
388        Self { x, y, z }
389    }
390
391    pub(crate) fn from_le_bytes(bytes: [u8; 6]) -> Self {
392        Self::from_bytes(bytes, false)
393    }
394}
395
396#[cfg(test)]
397mod tests {
398    use super::*;
399
400    #[test]
401    fn decode_raw_block_little_endian() {
402        let mut buffer = [0u8; RAW_BLOCK_LEN];
403        buffer[RAW_BLOCK_TIMESTAMP_OFFSET] = 0x01;
404        buffer[RAW_BLOCK_TIMESTAMP_OFFSET + 1] = 0x02;
405        buffer[RAW_BLOCK_TIMESTAMP_OFFSET + 2] = 0x03;
406        buffer[RAW_BLOCK_TEMPERATURE_OFFSET] = 0x10;
407        buffer[RAW_BLOCK_TEMPERATURE_OFFSET + 1] = 0x00;
408
409        buffer[RAW_BLOCK_ACCEL_OFFSET] = 0x01;
410        buffer[RAW_BLOCK_ACCEL_OFFSET + 2] = 0x02;
411        buffer[RAW_BLOCK_ACCEL_OFFSET + 4] = 0x03;
412
413        buffer[RAW_BLOCK_GYRO_OFFSET] = 0x04;
414        buffer[RAW_BLOCK_GYRO_OFFSET + 2] = 0x05;
415        buffer[RAW_BLOCK_GYRO_OFFSET + 4] = 0x06;
416
417        let block = decode_raw_block(&buffer, false, true, true);
418        assert_eq!(block.timestamp.ticks, 0x030201);
419        assert_eq!(block.temperature.value, 0x0010);
420
421        let accel = block.accel.expect("accel");
422        let gyro = block.gyro.expect("gyro");
423        assert_eq!(accel.x, 1);
424        assert_eq!(accel.y, 2);
425        assert_eq!(accel.z, 3);
426        assert_eq!(gyro.x, 4);
427        assert_eq!(gyro.y, 5);
428        assert_eq!(gyro.z, 6);
429    }
430
431    #[test]
432    fn decode_raw_block_big_endian_no_sensors() {
433        let mut buffer = [0u8; RAW_BLOCK_LEN];
434        buffer[RAW_BLOCK_TIMESTAMP_OFFSET] = 0xAA;
435        buffer[RAW_BLOCK_TIMESTAMP_OFFSET + 1] = 0xBB;
436        buffer[RAW_BLOCK_TIMESTAMP_OFFSET + 2] = 0xCC;
437        buffer[RAW_BLOCK_TEMPERATURE_OFFSET] = 0x00;
438        buffer[RAW_BLOCK_TEMPERATURE_OFFSET + 1] = 0x10;
439
440        let block = decode_raw_block(&buffer, true, false, false);
441        assert_eq!(block.timestamp.ticks, 0xCCBBAA);
442        assert_eq!(block.temperature.value, 0x0010);
443        assert!(block.accel.is_none());
444        assert!(block.gyro.is_none());
445    }
446}
ph_qmi8658/data/mod.rs

ph_qmi8658/data/
mod.rs
