lsm6ds33/
lib.rs

1//
2// lib.rs
3//
4// @author Natesh Narain <nnaraindev@gmail.com>
5// @date Nov 12 2021
6//
7#![no_std]
8#![allow(unused)]
9mod regs;
10
11use arrayref::array_refs;
12use core::convert::TryFrom;
13
14use regs::*;
15
16pub use regs::{
17    AccelerometerBandwidth, AccelerometerOutput, AccelerometerScale, GyroscopeFullScale,
18    GyroscopeOutput,
19};
20
21use maybe_async_cfg;
22
23#[cfg(feature = "blocking")]
24use embedded_hal::blocking::i2c::{Write, WriteRead};
25#[cfg(feature = "async")]
26use embedded_hal_async::i2c::I2c;
27
28/// Enum containing all possible types of errors when interacting with the IMU
29#[derive(Debug)]
30pub enum Error<E> {
31    CommunicationError(E),
32    ChipDetectFailed,
33    RegisterReadFailed,
34}
35
36// Allow converting the Error type from the I2C device to the error enum
37impl<E> From<E> for Error<E> {
38    fn from(e: E) -> Self {
39        Error::CommunicationError(e)
40    }
41}
42
43// Value of the WHO_AM_I register
44const CHIP_ID: u8 = 0x69;
45
46// Earth gravity constant for acceleration conversion
47const EARTH_GRAVITY: f32 = 9.80665;
48
49/// 6-DoF IMU accelerometer + gyro
50#[maybe_async_cfg::maybe(sync(feature = "blocking", keep_self), async(feature = "async"))]
51pub struct Lsm6ds33<I2C> {
52    i2c: I2C,
53    addr: u8,
54    accelerometer_scale: Option<AccelerometerScale>,
55    gyroscope_scale: Option<GyroscopeFullScale>,
56}
57
58#[maybe_async_cfg::maybe(
59    sync(feature = "blocking", keep_self),
60    async(
61        feature = "async",
62        idents(Write(async = "I2c"), WriteRead(async = "I2c"))
63    )
64)]
65impl<I2C, E> Lsm6ds33<I2C>
66where
67    I2C: Write<Error = E> + WriteRead<Error = E>,
68{
69    /// Create an instance of the Lsm6ds33 driver
70    /// If the device cannot be detected on the bus, an error will be returned
71    pub async fn new(i2c: I2C, addr: u8) -> Result<Self, (I2C, Error<E>)> {
72        let mut lsm = Lsm6ds33 {
73            i2c,
74            addr,
75            accelerometer_scale: None,
76            gyroscope_scale: None,
77        };
78
79        match lsm.check().await {
80            Ok(true) => match lsm.set_auto_increment(true).await {
81                Ok(()) => Ok(lsm),
82                Err(e) => Err((lsm.release(), e)),
83            },
84            Ok(false) => Err((lsm.release(), Error::ChipDetectFailed)),
85            Err(e) => Err((lsm.release(), e)),
86        }
87    }
88
89    /// Set the accelerometer output rate
90    pub async fn set_accelerometer_output(
91        &mut self,
92        output: AccelerometerOutput,
93    ) -> Result<(), Error<E>> {
94        self.write_register_option(Register::Ctrl1XL, output).await
95    }
96
97    /// Set the accelerometer operating range
98    pub async fn set_accelerometer_scale(
99        &mut self,
100        scale: AccelerometerScale,
101    ) -> Result<(), Error<E>> {
102        match self.write_register_option(Register::Ctrl1XL, scale).await {
103            Ok(()) => {
104                self.accelerometer_scale = Some(scale);
105                Ok(())
106            }
107            Err(e) => {
108                self.accelerometer_scale = None;
109                Err(e)
110            }
111        }
112    }
113
114    /// Set the accelerometer bandwidth filter
115    pub async fn set_accelerometer_bandwidth(
116        &mut self,
117        bandwidth: AccelerometerBandwidth,
118    ) -> Result<(), Error<E>> {
119        self.write_register_option(Register::Ctrl1XL, bandwidth)
120            .await
121    }
122
123    /// Set the gyroscope output rate
124    pub async fn set_gyroscope_output(&mut self, output: GyroscopeOutput) -> Result<(), Error<E>> {
125        self.write_register_option(Register::Ctrl2G, output).await
126    }
127
128    /// Set the gyroscope operating range
129    pub async fn set_gyroscope_scale(&mut self, scale: GyroscopeFullScale) -> Result<(), Error<E>> {
130        match self.write_register_option(Register::Ctrl2G, scale).await {
131            Ok(()) => {
132                self.gyroscope_scale = Some(scale);
133                Ok(())
134            }
135            Err(e) => {
136                self.accelerometer_scale = None;
137                Err(e)
138            }
139        }
140    }
141
142    /// Set the low power mode
143    pub async fn set_low_power_mode(&mut self, low_power: bool) -> Result<(), Error<E>> {
144        // N.B. "1" means low-power, "0" means high-performance.
145        self.write_bit(
146            Register::Ctrl6C,
147            low_power as u8,
148            Ctrl6C::AccelHighPerformanceMode as u8,
149        )
150        .await?;
151        self.write_bit(
152            Register::Ctrl7G,
153            low_power as u8,
154            Ctrl7G::HighPerformanceMode as u8,
155        )
156        .await
157    }
158
159    /// Read all three sensors in one transaction. Returns temperature, gyro, accelerometer.
160    pub async fn read_all(&mut self) -> Result<(f32, (f32, f32, f32), (f32, f32, f32)), Error<E>> {
161        let gyro_scale = self.read_gyroscope_scale().await?;
162        let accel_scale = self.read_accelerometer_scale().await?;
163        let data = self.read_registers::<14>(Register::OutTempL).await?;
164        let (temp, gyro, accel) = array_refs!(&data, 2, 6, 6);
165        Ok((
166            Self::convert_temp_data(temp),
167            Self::convert_gyro_data(gyro, gyro_scale),
168            Self::convert_accel_data(accel, accel_scale),
169        ))
170    }
171
172    /// Read the gyroscope data for each axis (RAD/s)
173    pub async fn read_gyro(&mut self) -> Result<(f32, f32, f32), Error<E>> {
174        let scale = self.read_gyroscope_scale().await?;
175        self.read_registers(Register::OutXLG)
176            .await
177            .map(|res| Self::convert_gyro_data(&res, scale))
178    }
179
180    fn convert_gyro_data(data: &[u8; 6], scale: GyroscopeFullScale) -> (f32, f32, f32) {
181        // Convert raw data to float
182        let (x, y, z) = Self::u8_to_f32(data);
183        let scale = scale.scale();
184        // Convert to RAD/s (Raw gyro data is in milli-degrees per second per bit)
185        (
186            (x * scale / 1000.0).to_radians(),
187            (y * scale / 1000.0).to_radians(),
188            (z * scale / 1000.0).to_radians(),
189        )
190    }
191
192    /// Read the accelerometer data for each axis (m/s^2)
193    pub async fn read_accelerometer(&mut self) -> Result<(f32, f32, f32), Error<E>> {
194        let scale = self.read_accelerometer_scale().await?;
195        self.read_registers(Register::OutXLXL)
196            .await
197            .map(|res| Self::convert_accel_data(&res, scale))
198    }
199
200    fn convert_accel_data(data: &[u8; 6], scale: AccelerometerScale) -> (f32, f32, f32) {
201        // Convert raw values to float
202        let (x, y, z) = Self::u8_to_f32(data);
203        let scale = scale.scale();
204
205        // Convert to m/s^2 (Raw value is in mg/bit)
206        (
207            (x * scale / 1000.0) * EARTH_GRAVITY,
208            (y * scale / 1000.0) * EARTH_GRAVITY,
209            (z * scale / 1000.0) * EARTH_GRAVITY,
210        )
211    }
212
213    /// Read the temperature (degC)
214    pub async fn read_temperature(&mut self) -> Result<f32, Error<E>> {
215        let data = self.read_registers::<2>(Register::OutTempL).await?;
216        Ok(Self::convert_temp_data(&data))
217    }
218
219    fn convert_temp_data(data: &[u8; 2]) -> f32 {
220        let (lo, hi) = (data[0], data[1]);
221
222        // Raw temperature as signal 16-bit number
223        let temperature = ((hi as i16) << 8) | (lo as i16);
224        // As float
225        let temperature = temperature as f32;
226        // Converted given the temperature sensitively value 16 bits per C
227        let temperature = (temperature / 16.0) + 25.0;
228
229        temperature
230    }
231
232    /// Check if there is new accelerometer data
233    pub async fn accel_data_available(&mut self) -> Result<bool, Error<E>> {
234        self.read_status().await.map(|status| status & 0b1 != 0)
235    }
236
237    /// Check if there is new gyro scope data
238    pub async fn gyro_data_available(&mut self) -> Result<bool, Error<E>> {
239        self.read_status().await.map(|status| status & 0b10 != 0)
240    }
241
242    /// Read the accelerometer scale value from the configuration register
243    pub async fn read_accelerometer_scale(&mut self) -> Result<AccelerometerScale, Error<E>> {
244        match self.accelerometer_scale {
245            Some(v) => Ok(v),
246            None => {
247                let scale = self.read_register_option(Register::Ctrl1XL).await?;
248                self.accelerometer_scale = Some(scale);
249                Ok(scale)
250            }
251        }
252    }
253
254    /// Read the gyroscope scale value from the configuration register
255    pub async fn read_gyroscope_scale(&mut self) -> Result<GyroscopeFullScale, Error<E>> {
256        match self.gyroscope_scale {
257            Some(v) => Ok(v),
258            None => {
259                let scale = self.read_register_option(Register::Ctrl2G).await?;
260                self.gyroscope_scale = Some(scale);
261                Ok(scale)
262            }
263        }
264    }
265
266    async fn check(&mut self) -> Result<bool, Error<E>> {
267        self.read_register(Register::WhoAmI)
268            .await
269            .map(|chip_id| chip_id == CHIP_ID)
270    }
271
272    async fn set_auto_increment(&mut self, enabled: bool) -> Result<(), Error<E>> {
273        self.write_bit(Register::Ctrl3C, enabled as u8, Ctrl3C::AutoIncrement as u8)
274            .await
275    }
276
277    async fn read_status(&mut self) -> Result<u8, Error<E>> {
278        self.read_register(Register::StatusReg).await
279    }
280
281    async fn write_register_option<RO: RegisterOption>(
282        &mut self,
283        register: Register,
284        ro: RO,
285    ) -> Result<(), Error<E>> {
286        self.write_bits(register, ro.value(), RO::mask(), RO::bit_offset())
287            .await
288    }
289
290    async fn read_register_option<RO: RegisterOption + TryFrom<u8>>(
291        &mut self,
292        register: Register,
293    ) -> Result<RO, Error<E>> {
294        let value = self.read_register(register).await?;
295        RO::try_from(value).map_err(|_| Error::RegisterReadFailed)
296    }
297
298    async fn write_bit(
299        &mut self,
300        register: Register,
301        value: u8,
302        shift: u8,
303    ) -> Result<(), Error<E>> {
304        self.write_bits(register, value, 0x01, shift).await
305    }
306
307    async fn write_bits(
308        &mut self,
309        register: Register,
310        new_value: u8,
311        mask: u8,
312        shift: u8,
313    ) -> Result<(), Error<E>> {
314        let current_value = self.read_register(register).await?;
315        let modified_value = (current_value & !(mask << shift)) | ((new_value & mask) << shift);
316        self.write_register(register, modified_value).await
317    }
318
319    fn u8_to_f32(res: &[u8; 6]) -> (f32, f32, f32) {
320        let (x, y, z) = (
321            (res[0] as i16) | ((res[1] as i16) << 8),
322            (res[2] as i16) | ((res[3] as i16) << 8),
323            (res[4] as i16) | ((res[5] as i16) << 8),
324        );
325        (x as f32, y as f32, z as f32)
326    }
327
328    // Read a byte from the given register
329    async fn read_register(&mut self, register: Register) -> Result<u8, Error<E>> {
330        let mut res = [0u8];
331        self.i2c
332            .write_read(self.addr, &[register.into()], &mut res)
333            .await?;
334        Ok(res[0])
335    }
336
337    async fn read_registers<const N: usize>(
338        &mut self,
339        start_reg: Register,
340    ) -> Result<[u8; N], Error<E>> {
341        let mut res = [0u8; N];
342        self.i2c
343            .write_read(self.addr, &[start_reg.into()], &mut res)
344            .await?;
345        Ok(res)
346    }
347
348    // Write the specified value to the given register
349    async fn write_register(&mut self, register: Register, value: u8) -> Result<(), Error<E>> {
350        self.i2c
351            .write(self.addr, &[register.into(), value])
352            .await
353            .map_err(Error::from)
354    }
355
356    /// Return the underlying I2C device
357    pub fn release(self) -> I2C {
358        self.i2c
359    }
360}
lsm6ds33/lib.rs

lsm6ds33/
lib.rs
