icm20948_async/
lib.rs

1#![no_std]
2
3use core::{future::Future, marker::PhantomData};
4use embedded_hal_async::{
5    delay::DelayNs,
6    i2c::{self, I2c},
7    spi::{self},
8};
9
10mod reg;
11use reg::*;
12
13mod cfg;
14pub use cfg::*;
15
16const MAGNET_ADDR: u8 = 0x0C;
17const IMU_WHOAMI: u8 = 0xEA;
18const MAG_WHOAMI: u8 = 0x09;
19
20#[derive(Debug, Clone, PartialEq)]
21pub enum SetupError<E> {
22    /// An error occured with the I2C/SPI connection during setup
23    Transport(E),
24    /// An incorrect 'Who Am I' value was returned from the imu, expected 0xEA (233)
25    ImuWhoAmI(u8),
26    /// An incorrect 'Who Am I' value was returned from the mag, expected 0x09 (9)
27    MagWhoAmI(u8),
28}
29
30impl<E> From<E> for SetupError<E> {
31    fn from(error: E) -> Self {
32        SetupError::Transport(error)
33    }
34}
35
36#[derive(Debug, Clone, PartialEq)]
37#[cfg_attr(feature = "serde", derive(serde::Serialize, serde::Deserialize))]
38/// Container for accelerometer and gyroscope measurements
39pub struct Data6Dof<T> {
40    pub acc: [T; 3],
41    pub gyr: [T; 3],
42    pub tmp: T,
43}
44
45#[derive(Debug, Clone, PartialEq)]
46#[cfg_attr(feature = "serde", derive(serde::Serialize, serde::Deserialize))]
47/// Container for accelerometer, gyroscope and magnetometer measurements
48pub struct Data9Dof<T> {
49    pub acc: [T; 3],
50    pub gyr: [T; 3],
51    pub mag: [T; 3],
52    pub tmp: T,
53}
54
55// Compile-time MAG states
56pub struct MagEnabled;
57pub struct MagDisabled;
58
59// Type to hold bus information for I2c
60pub struct I2cDevice<I2C> {
61    bus_inner: I2C,
62    address: I2cAddress,
63}
64
65// Trait to allow for generic behavior across I2c or Spi usage
66pub trait Icm20948Transport {
67    type Error: Into<SetupError<Self::Error>>;
68    fn read_registers(
69        &mut self,
70        reg_addr: u8,
71        read: &mut [u8],
72    ) -> impl Future<Output = Result<(), Self::Error>>;
73    fn write_registers(
74        &mut self,
75        reg_addr: u8,
76        write: &[u8],
77    ) -> impl Future<Output = Result<(), Self::Error>>;
78}
79
80// Implementation of transport trait for I2c
81impl<I2C: I2c> Icm20948Transport for I2cDevice<I2C> {
82    type Error = I2C::Error;
83    async fn read_registers(&mut self, reg_addr: u8, read: &mut [u8]) -> Result<(), Self::Error> {
84        let addr = self.address.get();
85        self.bus_inner.write_read(addr, &[reg_addr], read).await
86    }
87
88    async fn write_registers(&mut self, reg_addr: u8, write: &[u8]) -> Result<(), Self::Error> {
89        let addr = self.address.get();
90        self.bus_inner
91            .transaction(
92                addr,
93                &mut [
94                    i2c::Operation::Write(&[reg_addr]),
95                    i2c::Operation::Write(write),
96                ],
97            )
98            .await
99    }
100}
101
102mod spi_helpers {
103    const SPI_READ_NWRITE: u8 = 0x80;
104    pub(super) const fn write_addr(addr: u8) -> [u8; 1] {
105        [addr]
106    }
107    // Read operation demands to firstly write a register address with MSB set
108    pub(super) const fn read_addr(addr: u8) -> [u8; 1] {
109        [addr | SPI_READ_NWRITE]
110    }
111}
112
113pub struct SpiDevice<SPI> {
114    inner: SPI,
115}
116
117// Implementation of transport trait for Spi
118impl<SPI: spi::SpiDevice> Icm20948Transport for SpiDevice<SPI> {
119    type Error = SPI::Error;
120    async fn read_registers(&mut self, reg_addr: u8, read: &mut [u8]) -> Result<(), Self::Error> {
121        self.inner
122            .transaction(&mut [
123                spi::Operation::Write(&spi_helpers::read_addr(reg_addr)),
124                spi::Operation::Read(read),
125            ])
126            .await
127    }
128
129    async fn write_registers(&mut self, reg_addr: u8, write: &[u8]) -> Result<(), Self::Error> {
130        self.inner
131            .transaction(&mut [
132                spi::Operation::Write(&spi_helpers::write_addr(reg_addr)),
133                spi::Operation::Write(write),
134            ])
135            .await
136    }
137}
138
139/// Configure and initialize a new instance of [`Icm20948`]. Use either of the
140/// [`Icm20948::new_i2c`] or [`Icm20948::new_spi`] methods to begin.
141///
142/// # Example
143///
144/// ```rust
145/// let mut imu = IcmBuilder::new_i2c(i2c, delay)
146///     .gyr_unit(GyrUnit::Rps)
147///     .gyr_dlp(GyrDlp::Hz196)
148///     .initialize_9dof().await?;
149/// ```
150pub struct IcmBuilder<TRANSPORT, DELAY> {
151    transport: TRANSPORT,
152    delay: DELAY,
153    config: Config,
154}
155
156/// An initialized IMU. Use the [`IcmBuilder`] to construct a new instance.
157pub struct Icm20948<TRANSPORT, MAG> {
158    transport: TRANSPORT,
159    config: Config,
160    user_bank: UserBank,
161    mag_state: PhantomData<MAG>,
162}
163
164impl<BUS, DELAY> IcmBuilder<I2cDevice<BUS>, DELAY>
165where
166    BUS: I2c,
167    DELAY: DelayNs,
168{
169    /// Creates an uninitialized IMU struct with a default config.
170    #[must_use]
171    pub fn new_i2c(bus: BUS, delay: DELAY) -> IcmBuilder<I2cDevice<BUS>, DELAY> {
172        Self {
173            transport: I2cDevice {
174                bus_inner: bus,
175                address: I2cAddress::default(),
176            },
177            delay,
178            config: Config::default(),
179        }
180    }
181
182    /// Set I2C address of ICM module. See `I2cAddress` for defaults, otherwise `u8` implements `Into<I2cAddress>`
183    ///
184    /// **Note:** This will not change the actual address, only the address used by this driver.
185    #[must_use]
186    pub fn set_address(self, address: impl Into<I2cAddress>) -> IcmBuilder<I2cDevice<BUS>, DELAY> {
187        IcmBuilder {
188            transport: I2cDevice {
189                address: address.into(),
190                ..self.transport
191            },
192            ..self
193        }
194    }
195}
196
197impl<SPI, DELAY> IcmBuilder<SpiDevice<SPI>, DELAY>
198where
199    SPI: spi::SpiDevice,
200    DELAY: DelayNs,
201{
202    /// Creates an uninitialized IMU struct with a default config.
203    #[must_use]
204    pub fn new_spi(spi: SPI, delay: DELAY) -> IcmBuilder<SpiDevice<SPI>, DELAY> {
205        Self {
206            transport: SpiDevice { inner: spi },
207            delay,
208            config: Config::default(),
209        }
210    }
211}
212
213impl<TRANPORT, DELAY> IcmBuilder<TRANPORT, DELAY>
214where
215    TRANPORT: Icm20948Transport,
216    DELAY: DelayNs,
217{
218    /// Set the whole IMU config at once
219    #[must_use]
220    pub fn with_config(self, config: Config) -> IcmBuilder<TRANPORT, DELAY> {
221        IcmBuilder { config, ..self }
222    }
223
224    /// Set accelerometer measuring range, choises are 2G, 4G, 8G or 16G
225    #[must_use]
226    pub fn acc_range(self, acc_range: AccRange) -> IcmBuilder<TRANPORT, DELAY> {
227        IcmBuilder {
228            config: Config {
229                acc_range,
230                ..self.config
231            },
232            ..self
233        }
234    }
235
236    /// Set accelerometer digital lowpass filter frequency
237    #[must_use]
238    pub fn acc_dlp(self, acc_dlp: AccDlp) -> IcmBuilder<TRANPORT, DELAY> {
239        IcmBuilder {
240            config: Config {
241                acc_dlp,
242                ..self.config
243            },
244            ..self
245        }
246    }
247
248    /// Set returned unit of accelerometer measurement, choises are Gs or m/s^2
249    #[must_use]
250    pub fn acc_unit(self, acc_unit: AccUnit) -> IcmBuilder<TRANPORT, DELAY> {
251        IcmBuilder {
252            config: Config {
253                acc_unit,
254                ..self.config
255            },
256            ..self
257        }
258    }
259
260    /// Set accelerometer output data rate
261    #[must_use]
262    pub fn acc_odr(self, acc_odr: u16) -> IcmBuilder<TRANPORT, DELAY> {
263        IcmBuilder {
264            config: Config {
265                acc_odr,
266                ..self.config
267            },
268            ..self
269        }
270    }
271
272    /// Set gyroscope measuring range, choises are 250Dps, 500Dps, 1000Dps and 2000Dps
273    #[must_use]
274    pub fn gyr_range(self, gyr_range: GyrRange) -> IcmBuilder<TRANPORT, DELAY> {
275        IcmBuilder {
276            config: Config {
277                gyr_range,
278                ..self.config
279            },
280            ..self
281        }
282    }
283
284    /// Set gyroscope digital low pass filter frequency
285    #[must_use]
286    pub fn gyr_dlp(self, gyr_dlp: GyrDlp) -> IcmBuilder<TRANPORT, DELAY> {
287        IcmBuilder {
288            config: Config {
289                gyr_dlp,
290                ..self.config
291            },
292            ..self
293        }
294    }
295
296    /// Set returned unit of gyroscope measurement, choises are degrees/s or radians/s
297    #[must_use]
298    pub fn gyr_unit(self, gyr_unit: GyrUnit) -> IcmBuilder<TRANPORT, DELAY> {
299        IcmBuilder {
300            config: Config {
301                gyr_unit,
302                ..self.config
303            },
304            ..self
305        }
306    }
307
308    /// Set gyroscope output data rate
309    #[must_use]
310    pub fn gyr_odr(self, gyr_odr: u8) -> IcmBuilder<TRANPORT, DELAY> {
311        IcmBuilder {
312            config: Config {
313                gyr_odr,
314                ..self.config
315            },
316            ..self
317        }
318    }
319
320    /// Initializes the IMU with accelerometer and gyroscope
321    pub async fn initialize_6dof(
322        mut self,
323    ) -> Result<Icm20948<TRANPORT, MagDisabled>, SetupError<TRANPORT::Error>> {
324        let mut imu = Icm20948 {
325            transport: self.transport,
326            config: self.config,
327            user_bank: UserBank::Bank0,
328            mag_state: PhantomData,
329        };
330
331        imu.setup_acc_gyr(&mut self.delay).await?;
332
333        Ok(imu)
334    }
335
336    /// Initializes the IMU with accelerometer, gyroscope and magnetometer
337    pub async fn initialize_9dof(
338        mut self,
339    ) -> Result<Icm20948<TRANPORT, MagEnabled>, SetupError<TRANPORT::Error>> {
340        let mut imu = Icm20948 {
341            transport: self.transport,
342            config: self.config,
343            user_bank: UserBank::Bank0,
344            mag_state: PhantomData,
345        };
346
347        imu.setup_acc_gyr(&mut self.delay).await?;
348        imu.setup_mag(&mut self.delay).await?;
349
350        Ok(imu)
351    }
352}
353
354impl<TRANSPORT, MAG> Icm20948<TRANSPORT, MAG>
355where
356    TRANSPORT: Icm20948Transport,
357{
358    /// Setup accelerometer and gyroscope according to config
359    async fn setup_acc_gyr(
360        &mut self,
361        delay: &mut impl DelayNs,
362    ) -> Result<(), SetupError<TRANSPORT::Error>> {
363        // Ensure known-good state
364        self.device_reset(delay).await?;
365
366        // Initially set user bank by force, and check identity
367        self.set_user_bank::<Bank0>(true).await?;
368        let [imu_whoami] = self.read_from(Bank0::WhoAmI).await?;
369
370        if imu_whoami != IMU_WHOAMI {
371            return Err(SetupError::ImuWhoAmI(imu_whoami));
372        }
373
374        // Disable sleep mode and set to auto select clock source
375        self.write_to(Bank0::PwrMgmt1, 0x01).await?;
376
377        // Set gyro and accel ranges
378        self.set_acc_range(self.config.acc_range).await?;
379        self.set_gyr_range(self.config.gyr_range).await?;
380
381        // Apply digital lowpass filter settings
382        self.set_gyr_dlp(self.config.gyr_dlp).await?;
383        self.set_acc_dlp(self.config.acc_dlp).await?;
384
385        // Set gyro and accel output data rate
386        self.set_acc_odr(self.config.acc_odr).await?;
387        self.set_gyr_odr(self.config.gyr_odr).await?;
388
389        Ok(())
390    }
391
392    /// Reset accelerometer / gyroscope module
393    pub async fn device_reset(&mut self, delay: &mut impl DelayNs) -> Result<(), TRANSPORT::Error> {
394        delay.delay_ms(20).await;
395        self.write_to_flag(Bank0::PwrMgmt1, 1 << 7, 1 << 7).await?;
396        delay.delay_ms(50).await;
397        Ok(())
398    }
399
400    /// Enables main ICM module to act as I2C master (eg. for magnetometer)
401    async fn enable_i2c_master(&mut self, enable: bool) -> Result<(), TRANSPORT::Error> {
402        self.write_to_flag(Bank0::UserCtrl, u8::from(enable) << 5, 1 << 5)
403            .await
404    }
405
406    /// Resets I2C master module
407    async fn reset_i2c_master(&mut self) -> Result<(), TRANSPORT::Error> {
408        self.write_to_flag(Bank0::UserCtrl, 1 << 1, 1 << 1).await
409    }
410
411    /// Ensure correct user bank for given register
412    async fn set_user_bank<R: Register>(&mut self, force: bool) -> Result<(), TRANSPORT::Error> {
413        if (self.user_bank != R::bank()) || force {
414            self.transport
415                .write_registers(REG_BANK_SEL, &[(R::bank() as u8) << 4])
416                .await?;
417            self.user_bank = R::bank();
418        }
419        Ok(())
420    }
421
422    /// Read a const number `N` of bytes from the requested register
423    async fn read_from<const N: usize, R: Register>(
424        &mut self,
425        reg: R,
426    ) -> Result<[u8; N], TRANSPORT::Error> {
427        let mut buf = [0u8; N];
428        self.set_user_bank::<R>(false).await?;
429        self.transport.read_registers(reg.addr(), &mut buf).await?;
430        Ok(buf)
431    }
432
433    /// Write a single byte to the requeste register
434    async fn write_to<R: Register>(&mut self, reg: R, data: u8) -> Result<(), TRANSPORT::Error> {
435        self.set_user_bank::<R>(false).await?;
436        self.transport.write_registers(reg.addr(), &[data]).await
437    }
438
439    /// Write to a register, but only overwrite the parts corresponding to the flag byte
440    async fn write_to_flag<R: Register>(
441        &mut self,
442        reg: R,
443        data: u8,
444        flag: u8,
445    ) -> Result<(), TRANSPORT::Error> {
446        let [mut register] = self.read_from(reg).await?;
447        register = (register & !flag) | (data & flag);
448        self.write_to(reg, register).await
449    }
450
451    /// Put the magnetometer into read mode
452    async fn set_mag_read(&mut self) -> Result<(), TRANSPORT::Error> {
453        let [mut reg] = self.read_from(Bank3::I2cSlv0Addr).await?;
454        reg &= 0b0111_1111;
455        reg |= 1 << 7;
456        self.write_to(Bank3::I2cSlv0Addr, reg).await
457    }
458
459    /// Put the magnetometer into write mode
460    async fn set_mag_write(&mut self) -> Result<(), TRANSPORT::Error> {
461        let [mut reg] = self.read_from(Bank3::I2cSlv0Addr).await?;
462        reg &= 0b0111_1111;
463        self.write_to(Bank3::I2cSlv0Addr, reg).await
464    }
465
466    /// Write `data` to the magnetometer module in `reg` (20 ms non-blocking delays)
467    async fn mag_write_to(
468        &mut self,
469        reg: MagBank,
470        data: u8,
471        delay: &mut impl DelayNs,
472    ) -> Result<(), TRANSPORT::Error> {
473        self.set_mag_write().await?;
474        delay.delay_ms(10).await;
475        self.write_to(Bank3::I2cSlv0Reg, reg.reg()).await?;
476        self.write_to(Bank3::I2cSlv0Do, data).await?;
477        self.write_to(Bank3::I2cSlv0Ctrl, (1 << 7) | 1).await?;
478        delay.delay_ms(10).await;
479        self.set_mag_read().await
480    }
481
482    /// Read a `N` bytes from the magnetometer in `reg` (20 ms non-blocking delays)
483    async fn mag_read_from<const N: usize>(
484        &mut self,
485        reg: MagBank,
486        delay: &mut impl DelayNs,
487    ) -> Result<[u8; N], TRANSPORT::Error> {
488        self.set_mag_read().await?;
489        delay.delay_ms(10).await;
490        self.write_to(Bank3::I2cSlv0Reg, reg.reg()).await?;
491        let data = (1 << 7) | (N as u8).min(16);
492        self.write_to(Bank3::I2cSlv0Ctrl, data).await?;
493        delay.delay_ms(10).await;
494        self.read_from(Bank0::ExtSlvSensData00).await
495    }
496
497    /// Configure acceleromter to measure with given range
498    pub async fn set_acc_range(&mut self, range: AccRange) -> Result<(), TRANSPORT::Error> {
499        self.write_to_flag(Bank2::AccelConfig, (range as u8) << 1, 0b0110)
500            .await?;
501        self.config.acc_range = range;
502        Ok(())
503    }
504
505    /// Configure gyroscope to measure with given range
506    pub async fn set_gyr_range(&mut self, range: GyrRange) -> Result<(), TRANSPORT::Error> {
507        self.write_to_flag(Bank2::GyroConfig1, (range as u8) << 1, 0b0110)
508            .await?;
509        self.config.gyr_range = range;
510        Ok(())
511    }
512
513    /// Set returned unit of accelerometer
514    pub fn set_acc_unit(&mut self, unit: AccUnit) {
515        self.config.acc_unit = unit;
516    }
517
518    /// Set returned unit of gyroscope
519    pub fn set_gyr_unit(&mut self, unit: GyrUnit) {
520        self.config.gyr_unit = unit;
521    }
522
523    /// Set (or disable) accelerometer digital low-pass filter
524    pub async fn set_acc_dlp(&mut self, acc_dlp: AccDlp) -> Result<(), TRANSPORT::Error> {
525        let flag = 0b0011_1001;
526        let data = if AccDlp::Disabled != acc_dlp {
527            ((acc_dlp as u8) << 3) | 1
528        } else {
529            0u8
530        };
531        self.write_to_flag(Bank2::AccelConfig, data, flag).await
532    }
533
534    /// Set (or disable) gyroscope digital low-pass filter
535    pub async fn set_gyr_dlp(&mut self, gyr_dlp: GyrDlp) -> Result<(), TRANSPORT::Error> {
536        let flag = 0b0011_1001;
537        if GyrDlp::Disabled == gyr_dlp {
538            self.write_to_flag(Bank2::GyroConfig1, 0u8, flag).await
539        } else {
540            let data = ((gyr_dlp as u8) << 3) | 1;
541            self.write_to_flag(Bank2::GyroConfig1, data, flag).await
542        }
543    }
544
545    /// Set accelerometer output data rate. Value will be clamped above 4095.
546    pub async fn set_acc_odr(&mut self, acc_odr: u16) -> Result<(), TRANSPORT::Error> {
547        let [msb, lsb] = acc_odr.clamp(0, 0xFFF).to_be_bytes();
548        self.write_to(Bank2::AccelSmplrtDiv1, msb).await?;
549        self.write_to(Bank2::AccelSmplrtDiv2, lsb).await
550    }
551
552    /// Set gyroscope output data rate.
553    pub async fn set_gyr_odr(&mut self, gyr_odr: u8) -> Result<(), TRANSPORT::Error> {
554        self.write_to(Bank2::GyroSmplrtDiv, gyr_odr).await
555    }
556}
557
558impl<TRANSPORT> Icm20948<TRANSPORT, MagEnabled>
559where
560    TRANSPORT: Icm20948Transport,
561{
562    /// Setup magnetometer in continuous mode
563    async fn setup_mag(
564        &mut self,
565        delay: &mut impl DelayNs,
566    ) -> Result<(), SetupError<TRANSPORT::Error>> {
567        // Ensure known-good state
568        self.mag_reset(delay).await?;
569
570        // Setup magnetometer (i2c slave) clock (default 400 kHz)
571        self.write_to(Bank3::I2cMstCtrl, 0x07).await?;
572
573        // Enable I2C master mode for magnetometer
574        self.enable_i2c_master(true).await?;
575
576        // Configure slave address as magnetometer
577        self.write_to(Bank3::I2cSlv0Addr, MAGNET_ADDR).await?;
578
579        // Verify magnetometer identifier
580        let [mag_whoami] = self.mag_read_from(MagBank::DeviceId, delay).await?;
581
582        if mag_whoami != MAG_WHOAMI {
583            return Err(SetupError::MagWhoAmI(mag_whoami));
584        }
585
586        // Reset magnetometer
587        self.mag_reset(delay).await?;
588
589        // Set magnetometer to continuous mode 4 (100 Hz)
590        self.mag_write_to(MagBank::Control2, 0b01000, delay).await?;
591
592        // Set slave register to read from
593        self.write_to(Bank3::I2cSlv0Reg, MagBank::XDataLow.reg())
594            .await?;
595
596        // Set expected read size
597        self.write_to(Bank3::I2cSlv0Ctrl, (1 << 7) | 8).await?;
598
599        Ok(())
600    }
601
602    /// Reset magnetometer module ( 120 ms non-blocking delays)
603    async fn mag_reset(&mut self, delay: &mut impl DelayNs) -> Result<(), TRANSPORT::Error> {
604        // Control 3 register bit 1 resets magnetometer unit
605        self.mag_write_to(MagBank::Control3, 1, delay).await?;
606        delay.delay_ms(100).await;
607
608        // Reset i2c master module
609        self.reset_i2c_master().await
610    }
611
612    /// Get vector of scaled magnetometer values
613    pub async fn read_mag(&mut self) -> Result<[f32; 3], TRANSPORT::Error> {
614        let mag = self.read_mag_unscaled().await?;
615        let mag = mag.map(|x| 0.15 * x as f32);
616
617        Ok(mag)
618    }
619
620    /// Get array of unscaled accelerometer values
621    pub async fn read_mag_unscaled(&mut self) -> Result<[i16; 3], TRANSPORT::Error> {
622        let raw: [u8; 6] = self.read_from(Bank0::ExtSlvSensData00).await?;
623        let mag = collect_3xi16_mag(raw);
624
625        Ok(mag)
626    }
627
628    /// Get scaled measurement for accelerometer, gyroscope and magnetometer, and temperature
629    pub async fn read_9dof(&mut self) -> Result<Data9Dof<f32>, TRANSPORT::Error> {
630        let raw: [u8; 20] = self.read_from(Bank0::AccelXoutH).await?;
631        let [axh, axl, ayh, ayl, azh, azl, gxh, gxl, gyh, gyl, gzh, gzl, tph, tpl, mxl, mxh, myl, myh, mzl, mzh] =
632            raw;
633
634        let acc = self.scaled_acc_from_bytes([axh, axl, ayh, ayl, azh, azl]);
635        let gyr = self.scaled_gyr_from_bytes([gxh, gxl, gyh, gyl, gzh, gzl]);
636        let mag = self.scaled_mag_from_bytes([mxl, mxh, myl, myh, mzl, mzh]);
637
638        let tmp = self.scaled_tmp_from_bytes([tph, tpl]);
639
640        Ok(Data9Dof { acc, gyr, mag, tmp })
641    }
642
643    /// Get unscaled measurements for accelerometer and gyroscope, and temperature
644    pub async fn read_9dof_unscaled(&mut self) -> Result<Data9Dof<i16>, TRANSPORT::Error> {
645        let raw: [u8; 20] = self.read_from(Bank0::AccelXoutH).await?;
646        let [axh, axl, ayh, ayl, azh, azl, gxh, gxl, gyh, gyl, gzh, gzl, tph, tpl, mxl, mxh, myl, myh, mzl, mzh] =
647            raw;
648
649        let acc = collect_3xi16([axh, axl, ayh, ayl, azh, azl]);
650        let gyr = collect_3xi16([gxh, gxl, gyh, gyl, gzh, gzl]);
651        let mag = collect_3xi16_mag([mxl, mxh, myl, myh, mzl, mzh]);
652
653        let tmp = i16::from_be_bytes([tph, tpl]);
654
655        Ok(Data9Dof { acc, gyr, mag, tmp })
656    }
657
658    /// Takes 6 bytes converts them into a Vector3 of floats, unit is micro tesla
659    fn scaled_mag_from_bytes(&self, bytes: [u8; 6]) -> [f32; 3] {
660        collect_3xi16_mag(bytes).map(|x| 0.15 * x as f32)
661    }
662}
663
664impl<TRANSPORT, MAG> Icm20948<TRANSPORT, MAG>
665where
666    TRANSPORT: Icm20948Transport,
667{
668    /// Takes 6 bytes converts them into a Vector3 of floats
669    fn scaled_acc_from_bytes(&self, bytes: [u8; 6]) -> [f32; 3] {
670        collect_3xi16(bytes).map(|x| f32::from(x) * self.acc_scalar())
671    }
672
673    /// Takes 6 bytes converts them into a Vector3 of floats
674    fn scaled_gyr_from_bytes(&self, bytes: [u8; 6]) -> [f32; 3] {
675        collect_3xi16(bytes).map(|x| f32::from(x) * self.gyr_scalar())
676    }
677
678    /// Takes 2 bytes converts them into a temerature as a float
679    fn scaled_tmp_from_bytes(&self, bytes: [u8; 2]) -> f32 {
680        f32::from(i16::from_be_bytes(bytes)) / 333.87 + 21.
681    }
682
683    /// Get array of unscaled accelerometer values
684    pub async fn read_acc_unscaled(&mut self) -> Result<[i16; 3], TRANSPORT::Error> {
685        let raw = self.read_from(Bank0::AccelXoutH).await?;
686        Ok(collect_3xi16(raw))
687    }
688
689    /// Get array of scaled accelerometer values
690    pub async fn read_acc(&mut self) -> Result<[f32; 3], TRANSPORT::Error> {
691        let acc = self
692            .read_acc_unscaled()
693            .await?
694            .map(|x| f32::from(x) * self.acc_scalar());
695        Ok(acc)
696    }
697
698    /// Get array of unscaled gyroscope values
699    pub async fn read_gyr_unscaled(&mut self) -> Result<[i16; 3], TRANSPORT::Error> {
700        let raw = self.read_from(Bank0::GyroXoutH).await?;
701        Ok(collect_3xi16(raw))
702    }
703
704    /// Get array of scaled gyroscope values
705    pub async fn read_gyr(&mut self) -> Result<[f32; 3], TRANSPORT::Error> {
706        let gyr = self
707            .read_gyr_unscaled()
708            .await?
709            .map(|x| f32::from(x) * self.gyr_scalar());
710        Ok(gyr)
711    }
712
713    /// Get scaled measurements for accelerometer and gyroscope, and temperature
714    pub async fn read_6dof(&mut self) -> Result<Data6Dof<f32>, TRANSPORT::Error> {
715        let raw: [u8; 14] = self.read_from(Bank0::AccelXoutH).await?;
716        let [axh, axl, ayh, ayl, azh, azl, gxh, gxl, gyh, gyl, gzh, gzl, tph, tpl] = raw;
717
718        let acc = self.scaled_acc_from_bytes([axh, axl, ayh, ayl, azh, azl]);
719        let gyr = self.scaled_gyr_from_bytes([gxh, gxl, gyh, gyl, gzh, gzl]);
720
721        let tmp = self.scaled_tmp_from_bytes([tph, tpl]);
722
723        Ok(Data6Dof { acc, gyr, tmp })
724    }
725
726    /// Get unscaled measurements for accelerometer and gyroscope, and temperature
727    pub async fn read_6dof_unscaled(&mut self) -> Result<Data6Dof<i16>, TRANSPORT::Error> {
728        let raw: [u8; 14] = self.read_from(Bank0::AccelXoutH).await?;
729        let [axh, axl, ayh, ayl, azh, azl, gxh, gxl, gyh, gyl, gzh, gzl, tph, tpl] = raw;
730
731        let acc = collect_3xi16([axh, axl, ayh, ayl, azh, azl]);
732        let gyr = collect_3xi16([gxh, gxl, gyh, gyl, gzh, gzl]);
733
734        let tmp = i16::from_be_bytes([tph, tpl]);
735
736        Ok(Data6Dof { acc, gyr, tmp })
737    }
738
739    /// Set gyroscope calibration offsets by writing them to the IMU
740    pub async fn set_gyr_offsets(&mut self, offsets: [i16; 3]) -> Result<(), TRANSPORT::Error> {
741        let [[xh, xl], [yh, yl], [zh, zl]]: [[u8; 2]; 3] = offsets.map(|x| (-x).to_be_bytes());
742
743        self.set_user_bank::<Bank2>(false).await?;
744
745        self.transport
746            .write_registers(Bank2::XgOffsH.addr(), &[xh, xl])
747            .await?;
748        self.transport
749            .write_registers(Bank2::YgOffsH.addr(), &[yh, yl])
750            .await?;
751        self.transport
752            .write_registers(Bank2::ZgOffsH.addr(), &[zh, zl])
753            .await?;
754
755        Ok(())
756    }
757
758    /// Set accelerometer calibration offsets by writing them to the IMU
759    pub async fn set_acc_offsets(&mut self, offsets: [i16; 3]) -> Result<(), TRANSPORT::Error> {
760        let [[xh, xl], [yh, yl], [zh, zl]]: [[u8; 2]; 3] = offsets.map(|x| (-x).to_be_bytes());
761
762        self.set_user_bank::<Bank1>(false).await?;
763
764        self.transport
765            .write_registers(Bank1::XaOffsH.addr(), &[xh, xl])
766            .await?;
767        self.transport
768            .write_registers(Bank1::YaOffsH.addr(), &[yh, yl])
769            .await?;
770        self.transport
771            .write_registers(Bank1::ZaOffsH.addr(), &[zh, zl])
772            .await?;
773
774        Ok(())
775    }
776
777    /// Returns the number of new readings in FIFO buffer
778    pub async fn new_data_ready(&mut self) -> u8 {
779        self.read_from(Bank0::DataRdyStatus)
780            .await
781            .map_or(0, |[b]| b & 0b1111)
782    }
783
784    /// Returns the scalar corresponding to the unit and range configured
785    pub const fn acc_scalar(&self) -> f32 {
786        self.config.acc_unit.scalar() / self.config.acc_range.divisor()
787    }
788
789    /// Returns the scalar corresponding to the unit and range configured
790    pub const fn gyr_scalar(&self) -> f32 {
791        self.config.gyr_unit.scalar() / self.config.gyr_range.divisor()
792    }
793}
794
795/// Collects 6 bytes into a vector of i16 values (acc/gyr only)
796const fn collect_3xi16(values: [u8; 6]) -> [i16; 3] {
797    let [xh, xl, yh, yl, zh, zl] = values;
798    [
799        i16::from_be_bytes([xh, xl]),
800        i16::from_be_bytes([yh, yl]),
801        i16::from_be_bytes([zh, zl]),
802    ]
803}
804
805/// Collects 6 bytes into a vector of i16 values (mag only)
806const fn collect_3xi16_mag(values: [u8; 6]) -> [i16; 3] {
807    let [xl, xh, yl, yh, zl, zh] = values;
808
809    #[cfg(feature = "align-mag")]
810    let mag = [
811        i16::from_be_bytes([xh, xl]),
812        -i16::from_be_bytes([yh, yl]),
813        -i16::from_be_bytes([zh, zl]),
814    ];
815
816    #[cfg(not(feature = "align-mag"))]
817    let mag = [
818        i16::from_be_bytes([xh, xl]),
819        i16::from_be_bytes([yh, yl]),
820        i16::from_be_bytes([zh, zl]),
821    ];
822
823    mag
824}
icm20948_async/lib.rs

icm20948_async/
lib.rs
