ph_qmi8658/config/
common.rs

1use crate::register::{ctrl2, ctrl3};
2
3/// Low-pass filter bandwidth selection (percent of ODR).
4#[derive(Clone, Copy, Debug, PartialEq, Eq)]
5#[cfg_attr(feature = "defmt", derive(defmt::Format))]
6pub enum LowPassFilterMode {
7    /// 2.62% of ODR.
8    OdrPercent2_62,
9    /// 3.59% of ODR.
10    OdrPercent3_59,
11    /// 5.32% of ODR.
12    OdrPercent5_32,
13    /// 14.0% of ODR.
14    OdrPercent14_0,
15}
16
17impl LowPassFilterMode {
18    pub(crate) const fn bits(self) -> u8 {
19        match self {
20            Self::OdrPercent2_62 => 0b00,
21            Self::OdrPercent3_59 => 0b01,
22            Self::OdrPercent5_32 => 0b10,
23            Self::OdrPercent14_0 => 0b11,
24        }
25    }
26}
27
28/// Accelerometer full-scale range selection.
29#[derive(Clone, Copy, Debug, PartialEq, Eq)]
30#[cfg_attr(feature = "defmt", derive(defmt::Format))]
31pub enum AccelRange {
32    /// +/-2 g range.
33    G2,
34    /// +/-4 g range.
35    G4,
36    /// +/-8 g range.
37    G8,
38    /// +/-16 g range.
39    G16,
40}
41
42impl AccelRange {
43    /// Returns the full-scale range in g.
44    pub const fn g(self) -> u16 {
45        match self {
46            Self::G2 => 2,
47            Self::G4 => 4,
48            Self::G8 => 8,
49            Self::G16 => 16,
50        }
51    }
52
53    /// Returns the CTRL2 range bits.
54    pub(crate) const fn bits(self) -> u8 {
55        match self {
56            Self::G2 => 0b000,
57            Self::G4 => 0b001,
58            Self::G8 => 0b010,
59            Self::G16 => 0b011,
60        }
61    }
62}
63
64/// Accelerometer output data rate selection.
65#[derive(Clone, Copy, Debug, PartialEq, Eq)]
66#[cfg_attr(feature = "defmt", derive(defmt::Format))]
67pub enum AccelOutputDataRate {
68    /// 8000 Hz output data rate (normal mode).
69    Hz8000,
70    /// 4000 Hz output data rate (normal mode).
71    Hz4000,
72    /// 2000 Hz output data rate (normal mode).
73    Hz2000,
74    /// 1000 Hz output data rate (normal mode).
75    Hz1000,
76    /// 500 Hz output data rate (normal mode).
77    Hz500,
78    /// 250 Hz output data rate (normal mode).
79    Hz250,
80    /// 125 Hz output data rate (normal mode).
81    Hz125,
82    /// 62.5 Hz output data rate (normal mode).
83    Hz62_5,
84    /// 31.25 Hz output data rate (normal mode).
85    Hz31_25,
86    /// 128 Hz output data rate (low power mode).
87    LowPowerHz128,
88    /// 21 Hz output data rate (low power mode).
89    LowPowerHz21,
90    /// 11 Hz output data rate (low power mode).
91    LowPowerHz11,
92    /// 3 Hz output data rate (low power mode).
93    LowPowerHz3,
94}
95
96impl AccelOutputDataRate {
97    /// Returns the accel-only output data rate in milli-hertz.
98    pub const fn hz_milli(self) -> u32 {
99        match self {
100            Self::Hz8000 => 8_000_000,
101            Self::Hz4000 => 4_000_000,
102            Self::Hz2000 => 2_000_000,
103            Self::Hz1000 => 1_000_000,
104            Self::Hz500 => 500_000,
105            Self::Hz250 => 250_000,
106            Self::Hz125 => 125_000,
107            Self::Hz62_5 => 62_500,
108            Self::Hz31_25 => 31_250,
109            Self::LowPowerHz128 => 128_000,
110            Self::LowPowerHz21 => 21_000,
111            Self::LowPowerHz11 => 11_000,
112            Self::LowPowerHz3 => 3_000,
113        }
114    }
115
116    /// Returns the 6DOF output data rate in milli-hertz when accel+gyro are enabled.
117    /// Low-power settings are accel-only and return None.
118    pub(crate) const fn hz_milli_6dof(self) -> Option<u32> {
119        if self.is_low_power() {
120            None
121        } else {
122            Some(Self::scale_0_94(self.hz_milli()))
123        }
124    }
125
126    /// Returns the effective output data rate in milli-hertz for the current mode.
127    pub(crate) const fn effective_hz_milli(self, gyro_enabled: bool) -> u32 {
128        if gyro_enabled {
129            match self.hz_milli_6dof() {
130                Some(value) => value,
131                None => self.hz_milli(),
132            }
133        } else {
134            self.hz_milli()
135        }
136    }
137
138    /// Returns true if the ODR setting selects low-power mode.
139    pub(crate) const fn is_low_power(self) -> bool {
140        matches!(
141            self,
142            Self::LowPowerHz128 | Self::LowPowerHz21 | Self::LowPowerHz11 | Self::LowPowerHz3
143        )
144    }
145
146    const fn scale_0_94(value_milli: u32) -> u32 {
147        ((value_milli as u64) * 94 / 100) as u32
148    }
149
150    /// Returns the CTRL2 ODR bits.
151    pub(crate) const fn bits(self) -> u8 {
152        match self {
153            Self::Hz8000 => 0b0000,
154            Self::Hz4000 => 0b0001,
155            Self::Hz2000 => 0b0010,
156            Self::Hz1000 => 0b0011,
157            Self::Hz500 => 0b0100,
158            Self::Hz250 => 0b0101,
159            Self::Hz125 => 0b0110,
160            Self::Hz62_5 => 0b0111,
161            Self::Hz31_25 => 0b1000,
162            Self::LowPowerHz128 => 0b1100,
163            Self::LowPowerHz21 => 0b1101,
164            Self::LowPowerHz11 => 0b1110,
165            Self::LowPowerHz3 => 0b1111,
166        }
167    }
168
169    /// Returns the data-read delay in microseconds for sync sample mode.
170    pub(crate) const fn data_read_delay_us(self) -> u32 {
171        match self {
172            Self::Hz8000 | Self::Hz4000 => 2,
173            Self::Hz2000 => 4,
174            Self::Hz1000 => 6,
175            Self::Hz500 => 12,
176            Self::Hz250 => 24,
177            Self::Hz125 | Self::Hz62_5 | Self::Hz31_25 => 48,
178            Self::LowPowerHz128 => 40,
179            Self::LowPowerHz21 => 100,
180            Self::LowPowerHz11 => 200,
181            Self::LowPowerHz3 => 270,
182        }
183    }
184}
185
186/// Combined accelerometer configuration (range + ODR).
187#[derive(Clone, Copy, Debug, PartialEq, Eq)]
188#[cfg_attr(feature = "defmt", derive(defmt::Format))]
189pub struct AccelConfig {
190    /// Accelerometer full-scale range.
191    pub range: AccelRange,
192    /// Accelerometer output data rate.
193    pub odr: AccelOutputDataRate,
194    /// Accelerometer low-pass filter mode (None disables LPF).
195    pub lpf: Option<LowPassFilterMode>,
196}
197
198impl AccelConfig {
199    /// Default accelerometer configuration.
200    pub const DEFAULT: Self = Self {
201        range: AccelRange::G8,
202        odr: AccelOutputDataRate::Hz1000,
203        lpf: None,
204    };
205
206    /// Creates a new accelerometer configuration.
207    pub const fn new(range: AccelRange, odr: AccelOutputDataRate) -> Self {
208        Self {
209            range,
210            odr,
211            lpf: None,
212        }
213    }
214
215    /// Returns a new configuration with the provided range.
216    #[must_use]
217    pub const fn with_range(self, range: AccelRange) -> Self {
218        Self { range, ..self }
219    }
220
221    /// Returns a new configuration with the provided output data rate.
222    #[must_use]
223    pub const fn with_odr(self, odr: AccelOutputDataRate) -> Self {
224        Self { odr, ..self }
225    }
226
227    /// Returns a new configuration with the low-pass filter enabled.
228    #[must_use]
229    pub const fn with_lpf(self, mode: LowPassFilterMode) -> Self {
230        Self {
231            lpf: Some(mode),
232            ..self
233        }
234    }
235
236    /// Returns a new configuration with the low-pass filter disabled.
237    #[must_use]
238    pub const fn without_lpf(self) -> Self {
239        Self { lpf: None, ..self }
240    }
241
242    /// Builds the CTRL2 register value for this configuration.
243    pub(crate) const fn ctrl2_value(self) -> u8 {
244        ((self.range.bits() << ctrl2::A_FS_SHIFT) & ctrl2::A_FS_MASK)
245            | (self.odr.bits() & ctrl2::A_ODR_MASK)
246    }
247
248    /// Builds the CTRL2 register value including self-test when requested.
249    #[allow(dead_code)]
250    pub(crate) const fn ctrl2_value_with_self_test(self, enable_self_test: bool) -> u8 {
251        if enable_self_test {
252            ctrl2::A_ST | self.ctrl2_value()
253        } else {
254            self.ctrl2_value()
255        }
256    }
257}
258
259impl Default for AccelConfig {
260    fn default() -> Self {
261        Self::DEFAULT
262    }
263}
264
265/// Gyroscope full-scale range selection.
266#[derive(Clone, Copy, Debug, PartialEq, Eq)]
267#[cfg_attr(feature = "defmt", derive(defmt::Format))]
268pub enum GyroRange {
269    /// +/-16 deg/s.
270    Dps16,
271    /// +/-32 deg/s.
272    Dps32,
273    /// +/-64 deg/s.
274    Dps64,
275    /// +/-128 deg/s.
276    Dps128,
277    /// +/-256 deg/s.
278    Dps256,
279    /// +/-512 deg/s.
280    Dps512,
281    /// +/-1024 deg/s.
282    Dps1024,
283    /// +/-2048 deg/s.
284    Dps2048,
285}
286
287impl GyroRange {
288    /// Returns the full-scale range in degrees per second.
289    pub const fn dps(self) -> u16 {
290        match self {
291            Self::Dps16 => 16,
292            Self::Dps32 => 32,
293            Self::Dps64 => 64,
294            Self::Dps128 => 128,
295            Self::Dps256 => 256,
296            Self::Dps512 => 512,
297            Self::Dps1024 => 1024,
298            Self::Dps2048 => 2048,
299        }
300    }
301
302    /// Returns the CTRL3 range bits.
303    pub(crate) const fn bits(self) -> u8 {
304        match self {
305            Self::Dps16 => 0b000,
306            Self::Dps32 => 0b001,
307            Self::Dps64 => 0b010,
308            Self::Dps128 => 0b011,
309            Self::Dps256 => 0b100,
310            Self::Dps512 => 0b101,
311            Self::Dps1024 => 0b110,
312            Self::Dps2048 => 0b111,
313        }
314    }
315}
316
317/// Gyroscope output data rate selection.
318///
319/// Actual rates follow the gyro natural frequency (~0.94x nominal).
320#[derive(Clone, Copy, Debug, PartialEq, Eq)]
321#[cfg_attr(feature = "defmt", derive(defmt::Format))]
322pub enum GyroOutputDataRate {
323    /// 8000 Hz output data rate (normal mode).
324    Hz8000,
325    /// 4000 Hz output data rate (normal mode).
326    Hz4000,
327    /// 2000 Hz output data rate (normal mode).
328    Hz2000,
329    /// 1000 Hz output data rate (normal mode).
330    Hz1000,
331    /// 500 Hz output data rate (normal mode).
332    Hz500,
333    /// 250 Hz output data rate (normal mode).
334    Hz250,
335    /// 125 Hz output data rate (normal mode).
336    Hz125,
337    /// 62.5 Hz output data rate (normal mode).
338    Hz62_5,
339    /// 31.25 Hz output data rate (normal mode).
340    Hz31_25,
341}
342
343impl GyroOutputDataRate {
344    /// Returns the output data rate in milli-hertz.
345    pub const fn hz_milli(self) -> u32 {
346        match self {
347            Self::Hz8000 => 7_520_000,
348            Self::Hz4000 => 3_760_000,
349            Self::Hz2000 => 1_880_000,
350            Self::Hz1000 => 940_000,
351            Self::Hz500 => 470_000,
352            Self::Hz250 => 235_000,
353            Self::Hz125 => 117_500,
354            Self::Hz62_5 => 58_750,
355            Self::Hz31_25 => 29_375,
356        }
357    }
358
359    /// Returns the CTRL3 ODR bits.
360    pub(crate) const fn bits(self) -> u8 {
361        match self {
362            Self::Hz8000 => 0b0000,
363            Self::Hz4000 => 0b0001,
364            Self::Hz2000 => 0b0010,
365            Self::Hz1000 => 0b0011,
366            Self::Hz500 => 0b0100,
367            Self::Hz250 => 0b0101,
368            Self::Hz125 => 0b0110,
369            Self::Hz62_5 => 0b0111,
370            Self::Hz31_25 => 0b1000,
371        }
372    }
373
374    /// Returns the data-read delay in microseconds for sync sample mode.
375    pub(crate) const fn data_read_delay_us(self) -> u32 {
376        match self {
377            Self::Hz8000 | Self::Hz4000 => 2,
378            Self::Hz2000 => 4,
379            Self::Hz1000 => 6,
380            Self::Hz500 | Self::Hz250 | Self::Hz125 | Self::Hz62_5 | Self::Hz31_25 => 12,
381        }
382    }
383}
384
385/// Combined gyroscope configuration (range + ODR).
386#[derive(Clone, Copy, Debug, PartialEq, Eq)]
387#[cfg_attr(feature = "defmt", derive(defmt::Format))]
388pub struct GyroConfig {
389    /// Gyroscope full-scale range.
390    pub range: GyroRange,
391    /// Gyroscope output data rate.
392    pub odr: GyroOutputDataRate,
393    /// Gyroscope low-pass filter mode (None disables LPF).
394    pub lpf: Option<LowPassFilterMode>,
395}
396
397impl GyroConfig {
398    /// Default gyroscope configuration.
399    pub const DEFAULT: Self = Self {
400        range: GyroRange::Dps512,
401        odr: GyroOutputDataRate::Hz1000,
402        lpf: None,
403    };
404
405    /// Creates a new gyroscope configuration.
406    pub const fn new(range: GyroRange, odr: GyroOutputDataRate) -> Self {
407        Self {
408            range,
409            odr,
410            lpf: None,
411        }
412    }
413
414    /// Returns a new configuration with the provided range.
415    #[must_use]
416    pub const fn with_range(self, range: GyroRange) -> Self {
417        Self { range, ..self }
418    }
419
420    /// Returns a new configuration with the provided output data rate.
421    #[must_use]
422    pub const fn with_odr(self, odr: GyroOutputDataRate) -> Self {
423        Self { odr, ..self }
424    }
425
426    /// Returns a new configuration with the low-pass filter enabled.
427    #[must_use]
428    pub const fn with_lpf(self, mode: LowPassFilterMode) -> Self {
429        Self {
430            lpf: Some(mode),
431            ..self
432        }
433    }
434
435    /// Returns a new configuration with the low-pass filter disabled.
436    #[must_use]
437    pub const fn without_lpf(self) -> Self {
438        Self { lpf: None, ..self }
439    }
440
441    /// Builds the CTRL3 register value for this configuration.
442    pub(crate) const fn ctrl3_value(self) -> u8 {
443        ((self.range.bits() << ctrl3::G_FS_SHIFT) & ctrl3::G_FS_MASK)
444            | (self.odr.bits() & ctrl3::G_ODR_MASK)
445    }
446
447    /// Builds the CTRL3 register value including self-test when requested.
448    #[allow(dead_code)]
449    pub(crate) const fn ctrl3_value_with_self_test(self, enable_self_test: bool) -> u8 {
450        if enable_self_test {
451            ctrl3::G_ST | self.ctrl3_value()
452        } else {
453            self.ctrl3_value()
454        }
455    }
456}
457
458#[cfg(test)]
459mod tests {
460    use super::*;
461
462    #[test]
463    fn accel_odr_values() {
464        assert_eq!(AccelOutputDataRate::Hz1000.hz_milli(), 1_000_000);
465        assert_eq!(AccelOutputDataRate::Hz500.hz_milli(), 500_000);
466        assert_eq!(AccelOutputDataRate::LowPowerHz21.hz_milli(), 21_000);
467    }
468
469    #[test]
470    fn accel_odr_effective_for_6dof() {
471        let odr = AccelOutputDataRate::Hz1000;
472        assert_eq!(odr.hz_milli_6dof(), Some(940_000));
473        assert_eq!(odr.effective_hz_milli(true), 940_000);
474        assert_eq!(odr.effective_hz_milli(false), 1_000_000);
475
476        let low_power = AccelOutputDataRate::LowPowerHz21;
477        assert_eq!(low_power.hz_milli_6dof(), None);
478        assert_eq!(low_power.effective_hz_milli(true), 21_000);
479    }
480
481    #[test]
482    fn gyro_odr_values() {
483        assert_eq!(GyroOutputDataRate::Hz1000.hz_milli(), 940_000);
484        assert_eq!(GyroOutputDataRate::Hz500.hz_milli(), 470_000);
485    }
486
487    #[test]
488    fn data_read_delay_table() {
489        assert_eq!(AccelOutputDataRate::Hz1000.data_read_delay_us(), 6);
490        assert_eq!(AccelOutputDataRate::LowPowerHz3.data_read_delay_us(), 270);
491        assert_eq!(GyroOutputDataRate::Hz8000.data_read_delay_us(), 2);
492        assert_eq!(GyroOutputDataRate::Hz31_25.data_read_delay_us(), 12);
493    }
494}
495
496impl Default for GyroConfig {
497    fn default() -> Self {
498        Self::DEFAULT
499    }
500}
ph_qmi8658/config/common.rs

ph_qmi8658/config/
common.rs
