stk8ba58/
accelerometer.rs

1use core::fmt::Debug;
2
3use accelerometer_hal::{
4    vector::{F32x3, I16x3},
5    Accelerometer, Error, RawAccelerometer,
6};
7use embedded_hal::i2c::I2c;
8
9use crate::{
10    registers::{Axis, BwSel, Read},
11    Stk8ba58,
12};
13
14impl<I2C, E> Accelerometer for Stk8ba58<I2C>
15where
16    I2C: I2c<Error = E>,
17    E: Debug,
18{
19    type Error = E;
20
21    /// Get normalized ±g reading from the accelerometer.
22    ///
23    /// **Note**: it is expected that you call this when you know
24    /// data is ready and valid, like in response to an
25    /// interrupt or polling far slower than your data rate.
26    ///
27    /// Usage
28    /// ```no_run
29    /// let accel = accelerometer.accel_norm().unwrap();
30    /// let (x, y, z) = (accel.x, accel.y, accel.z)
31    /// println!("X = {x}\nY = {y},\nZ = [z]");
32    /// ```
33    ///
34    /// # Errors
35    ///  - Any I²C errors will cause the function to fail.
36    fn accel_norm(&mut self) -> Result<F32x3, Error<E>> {
37        const I12_MAX: f32 = 2048.0;
38        let raw_data: I16x3 = self.accel_raw()?;
39        let range: f32 = self.range.into();
40
41        let x = (raw_data.x as f32 / I12_MAX) * range;
42        let y = (raw_data.y as f32 / I12_MAX) * range;
43        let z = (raw_data.z as f32 / I12_MAX) * range;
44
45        Ok(F32x3::new(x, y, z))
46    }
47
48    /// Read the rample rate from the `BWSEL` register and convert it to Hz.
49    ///
50    /// **Note**: You likely want to use `.read_mode(BwSel::default())` from `registers::Read`
51    ///
52    /// # Errors
53    ///  - Any I²C errors will cause the function to fail.
54    fn sample_rate(&mut self) -> Result<f32, Error<Self::Error>> {
55        Ok(self.read_mode(BwSel::default())?.into())
56    }
57}
58
59impl<I2C, E> RawAccelerometer<I16x3> for Stk8ba58<I2C>
60where
61    I2C: I2c<Error = E>,
62    E: Debug,
63{
64    type Error = E;
65
66    /// Get raw acceleration reading from the accelerometer.
67    /// Keep in mind readings are 12 bits.
68    ///
69    /// **Note**: it is expected that you call this when you know
70    /// data is ready and valid, like in response to an
71    /// interrupt or polling far slower than your data rate.
72    ///
73    /// # Errors
74    ///  - Any I²C errors will cause the function to fail.
75    fn accel_raw(&mut self) -> Result<I16x3, Error<E>> {
76        const NEGATIVE: i16 = 0b1000_0000_0000; // Two's complement negative sign for 12 bits
77
78        let mut xyz = [0, 0, 0];
79        let axis = [Axis::X, Axis::Y, Axis::Z];
80
81        for (i, axis) in axis.iter().enumerate() {
82            // LSB should be read first due to data protection
83            let lsb = self.axis_lsb(axis)?;
84            let lsb = (lsb as i16) >> 4; // Move the bits to occupy positions 3-0 instead of 7-4
85
86            let msb = self.axis_msb(axis)?;
87            let msb = (msb as i16) << 4; // Move the bits to occupy positions 11-4 instead of 7-0
88
89            let sign = if (msb & NEGATIVE) == NEGATIVE {
90                0b1111_1111_0000_0000u16 as i16 // Correct for two's complement negative sign
91            } else {
92                0
93            };
94
95            let accel_raw = msb | lsb | sign;
96
97            xyz[i] = accel_raw;
98        }
99
100        Ok(I16x3::new(xyz[0], xyz[1], xyz[2]))
101    }
102}
stk8ba58/accelerometer.rs

stk8ba58/
accelerometer.rs
