ph_qmi8658/
self_test.rs

1//! Self-test helpers for accelerometer and gyroscope.
2
3use crate::error::Error;
4use crate::interrupt::InterruptWaitError;
5
6/// Self-test axis readings.
7#[derive(Clone, Copy, Debug, PartialEq, Eq, Default)]
8#[cfg_attr(feature = "defmt", derive(defmt::Format))]
9pub struct SelfTestAxis {
10    /// X-axis self-test reading.
11    pub x: i16,
12    /// Y-axis self-test reading.
13    pub y: i16,
14    /// Z-axis self-test reading.
15    pub z: i16,
16}
17
18impl SelfTestAxis {
19    pub(crate) fn from_bytes(bytes: [u8; 6], big_endian: bool) -> Self {
20        let (x, y, z) = if big_endian {
21            (
22                i16::from_be_bytes([bytes[0], bytes[1]]),
23                i16::from_be_bytes([bytes[2], bytes[3]]),
24                i16::from_be_bytes([bytes[4], bytes[5]]),
25            )
26        } else {
27            (
28                i16::from_le_bytes([bytes[0], bytes[1]]),
29                i16::from_le_bytes([bytes[2], bytes[3]]),
30                i16::from_le_bytes([bytes[4], bytes[5]]),
31            )
32        };
33        Self { x, y, z }
34    }
35
36    fn abs_exceeds(self, threshold_lsb: i16) -> bool {
37        abs_i16(self.x) >= threshold_lsb
38            && abs_i16(self.y) >= threshold_lsb
39            && abs_i16(self.z) >= threshold_lsb
40    }
41}
42
43/// Self-test report with raw data and pass/fail evaluation.
44#[derive(Clone, Copy, Debug, PartialEq, Eq)]
45#[cfg_attr(feature = "defmt", derive(defmt::Format))]
46pub struct SelfTestReport {
47    /// Raw self-test axis readings.
48    pub raw: SelfTestAxis,
49    /// Pass/fail threshold in LSB.
50    pub threshold_lsb: i16,
51    /// Whether all axes exceeded the threshold.
52    pub passed: bool,
53}
54
55impl SelfTestReport {
56    pub(crate) fn accel(raw: SelfTestAxis) -> Self {
57        let threshold = accel_threshold_lsb();
58        Self {
59            raw,
60            threshold_lsb: threshold,
61            passed: raw.abs_exceeds(threshold),
62        }
63    }
64
65    pub(crate) fn gyro(raw: SelfTestAxis) -> Self {
66        let threshold = gyro_threshold_lsb();
67        Self {
68            raw,
69            threshold_lsb: threshold,
70            passed: raw.abs_exceeds(threshold),
71        }
72    }
73}
74
75/// Self-test error wrapper (driver errors + interrupt wait errors).
76#[derive(Debug)]
77#[cfg_attr(feature = "defmt", derive(defmt::Format))]
78pub enum SelfTestError<E> {
79    /// A driver-level error occurred.
80    Driver(Error),
81    /// An interrupt wait error occurred.
82    Interrupt(InterruptWaitError<E>),
83}
84
85impl<E> From<Error> for SelfTestError<E> {
86    fn from(err: Error) -> Self {
87        Self::Driver(err)
88    }
89}
90
91impl<E> From<InterruptWaitError<E>> for SelfTestError<E> {
92    fn from(err: InterruptWaitError<E>) -> Self {
93        Self::Interrupt(err)
94    }
95}
96
97const fn accel_threshold_lsb() -> i16 {
98    // 200 mg threshold, data format is signed Q5.11 (1 g = 2048 LSB).
99    // 200 mg = 0.2 g => 0.2 * 2048 = 409.6 -> 410 LSB.
100    ((200i32 * 2048 + 999) / 1000) as i16
101}
102
103const fn gyro_threshold_lsb() -> i16 {
104    // 300 dps threshold, data format is signed Q12.4 (1 dps = 16 LSB).
105    (300i32 * 16) as i16
106}
107
108const fn abs_i16(value: i16) -> i16 {
109    if value < 0 {
110        (-(value as i32)) as i16
111    } else {
112        value
113    }
114}
115
116#[cfg(test)]
117mod tests {
118    use super::*;
119
120    #[test]
121    fn accel_threshold_is_expected() {
122        assert_eq!(accel_threshold_lsb(), 410);
123    }
124
125    #[test]
126    fn gyro_threshold_is_expected() {
127        assert_eq!(gyro_threshold_lsb(), 4800);
128    }
129
130    #[test]
131    fn report_passes_when_above_threshold() {
132        let raw = SelfTestAxis {
133            x: 500,
134            y: -500,
135            z: 600,
136        };
137        let report = SelfTestReport::accel(raw);
138        assert!(report.passed);
139    }
140}
ph_qmi8658/self_test.rs

ph_qmi8658/
self_test.rs
