icm20948_async/
lib.rs

1#![no_std]
2
3use core::marker::PhantomData;
4use embedded_hal_async::{delay::DelayNs, i2c::I2c, spi::SpiDevice};
5use nalgebra::Vector3;
6
7mod reg;
8use crate::reg::*;
9
10const MAGNET_ADDR: u8 = 0x0C; // I2C address of magnetometer
11
12#[derive(Clone, Copy)]
13/// Container for accelerometer and gyroscope measurements
14pub struct Data6Dof<T> {
15    pub acc: Vector3<T>,
16    pub gyr: Vector3<T>,
17    pub tmp: T,
18}
19
20#[derive(Clone, Copy)]
21/// Container for accelerometer, gyroscope and magnetometer measurements
22pub struct Data9Dof<T> {
23    pub acc: Vector3<T>,
24    pub gyr: Vector3<T>,
25    pub mag: Vector3<T>,
26    pub tmp: T,
27}
28
29// Compile-time MAG states
30pub struct MagEnabled {
31    is_calibrated: bool,
32    offset: Vector3<f32>,
33    scale: Vector3<f32>,
34}
35
36impl MagEnabled {
37    fn uncralibrated() -> Self {
38        Self {
39            is_calibrated: false,
40            offset: Vector3::zeros(),
41            scale: Vector3::from_element(1.),
42        }
43    }
44}
45pub struct MagDisabled;
46
47// Compile-time init states
48pub struct Init;
49pub struct NotInit;
50
51// Type to hold bus information for I2c
52pub struct IcmBusI2c<I2C> {
53    bus_inner: I2C,
54    address: I2cAddress,
55}
56// Type to hold bus information for Spi
57pub struct IcmBusSpi<SPI> {
58    bus_inner: SPI,
59}
60
61// Trait to allow for generic behavior across I2c or Spi usage
62#[allow(async_fn_in_trait)]
63pub trait BusTransfer<E>
64where
65    E: Into<IcmError<E>>,
66{
67    async fn bus_transfer(&mut self, write: &[u8], read: &mut [u8]) -> Result<(), E>;
68    async fn bus_write(&mut self, write: &[u8]) -> Result<(), E>;
69}
70
71// Implementation of bus trait for I2c
72impl<I2C, E> BusTransfer<E> for IcmBusI2c<I2C>
73where
74    I2C: I2c<Error = E>,
75    E: Into<IcmError<E>>,
76{
77    async fn bus_transfer(&mut self, write: &[u8], read: &mut [u8]) -> Result<(), E> {
78        self.bus_inner
79            .write_read(self.address.get(), write, read)
80            .await
81    }
82
83    async fn bus_write(&mut self, write: &[u8]) -> Result<(), E> {
84        self.bus_inner.write(self.address.get(), write).await
85    }
86}
87
88// Implementation of bus trait for Spi
89impl<SPI, E> BusTransfer<E> for IcmBusSpi<SPI>
90where
91    SPI: SpiDevice<Error = E>,
92    E: Into<IcmError<E>>,
93{
94    async fn bus_transfer(&mut self, write: &[u8], read: &mut [u8]) -> Result<(), E> {
95        self.bus_inner.transfer(read, write).await
96    }
97
98    async fn bus_write(&mut self, write: &[u8]) -> Result<(), E> {
99        self.bus_inner.write(write).await
100    }
101}
102
103pub struct Icm20948<BUS, MAG, INIT, DELAY, E> {
104    bus: BUS,
105    config: Icm20948Config,
106    user_bank: UserBank,
107    delay: DELAY,
108    mag_state: MAG,
109    init_state: PhantomData<INIT>,
110    bus_error: PhantomData<E>,
111}
112
113impl<BUS, DELAY, E> Icm20948<IcmBusI2c<BUS>, MagDisabled, NotInit, DELAY, E>
114where
115    BUS: I2c<Error = E>,
116    E: Into<IcmError<E>>,
117    DELAY: DelayNs,
118{
119    /// Creates an uninitialized IMU struct with the given config.
120    #[must_use]
121    pub fn new_i2c_from_cfg(
122        bus: BUS,
123        cfg: Icm20948Config,
124        delay: DELAY,
125    ) -> Icm20948<IcmBusI2c<BUS>, MagDisabled, NotInit, DELAY, E> {
126        Self {
127            bus: IcmBusI2c {
128                bus_inner: bus,
129                address: I2cAddress::default(),
130            },
131            config: cfg,
132            user_bank: UserBank::Bank0,
133            delay,
134            mag_state: MagDisabled,
135            init_state: PhantomData::<NotInit>,
136            bus_error: PhantomData::<E>,
137        }
138    }
139
140    /// Creates an uninitialized IMU struct with a default config.
141    #[must_use]
142    pub fn new_i2c(
143        bus: BUS,
144        delay: DELAY,
145    ) -> Icm20948<IcmBusI2c<BUS>, MagDisabled, NotInit, DELAY, E> {
146        Self::new_i2c_from_cfg(bus, Icm20948Config::default(), delay)
147    }
148}
149
150impl<BUS, DELAY, E> Icm20948<IcmBusSpi<BUS>, MagDisabled, NotInit, DELAY, E>
151where
152    BUS: SpiDevice<Error = E>,
153    E: Into<IcmError<E>>,
154    DELAY: DelayNs,
155{
156    /// Creates an uninitialized IMU struct with the given config.
157    #[must_use]
158    pub fn new_spi_from_cfg(
159        bus: BUS,
160        cfg: Icm20948Config,
161        delay: DELAY,
162    ) -> Icm20948<IcmBusSpi<BUS>, MagDisabled, NotInit, DELAY, E> {
163        Self {
164            bus: IcmBusSpi { bus_inner: bus },
165            config: cfg,
166            user_bank: UserBank::Bank0,
167            mag_state: MagDisabled,
168            delay,
169            init_state: PhantomData::<NotInit>,
170            bus_error: PhantomData::<E>,
171        }
172    }
173
174    /// Creates an uninitialized IMU struct with a default config.
175    #[must_use]
176    pub fn new_spi(
177        bus: BUS,
178        delay: DELAY,
179    ) -> Icm20948<IcmBusSpi<BUS>, MagDisabled, NotInit, DELAY, E> {
180        Self::new_spi_from_cfg(bus, Icm20948Config::default(), delay)
181    }
182}
183
184impl<BUS, DELAY, E> Icm20948<IcmBusI2c<BUS>, MagDisabled, NotInit, DELAY, E>
185where
186    BUS: I2c<Error = E>,
187    E: Into<IcmError<E>>,
188    DELAY: DelayNs,
189{
190    /// Set I2C address of ICM module. See `I2cAddress` for defaults, otherwise `u8` implements `Into<I2cAddress>`
191    #[must_use]
192    pub fn set_address(
193        self,
194        address: impl Into<I2cAddress>,
195    ) -> Icm20948<IcmBusI2c<BUS>, MagDisabled, NotInit, DELAY, E> {
196        Icm20948 {
197            bus: IcmBusI2c {
198                address: address.into(),
199                ..self.bus
200            },
201            ..self
202        }
203    }
204}
205
206impl<BUS, DELAY, E> Icm20948<BUS, MagDisabled, NotInit, DELAY, E>
207where
208    BUS: BusTransfer<E>,
209    DELAY: DelayNs,
210{
211    /*
212        Configuration methods
213    */
214
215    /// Set accelerometer measuring range, choises are 2G, 4G, 8G or 16G
216    #[must_use]
217    pub fn acc_range(self, acc_range: AccRange) -> Icm20948<BUS, MagDisabled, NotInit, DELAY, E> {
218        Icm20948 {
219            config: Icm20948Config {
220                acc_range,
221                ..self.config
222            },
223            ..self
224        }
225    }
226
227    /// Set accelerometer digital lowpass filter frequency
228    #[must_use]
229    pub fn acc_dlp(self, acc_dlp: AccDlp) -> Icm20948<BUS, MagDisabled, NotInit, DELAY, E> {
230        Icm20948 {
231            config: Icm20948Config {
232                acc_dlp,
233                ..self.config
234            },
235            ..self
236        }
237    }
238
239    /// Set returned unit of accelerometer measurement, choises are Gs or m/s^2
240    #[must_use]
241    pub fn acc_unit(self, acc_unit: AccUnit) -> Icm20948<BUS, MagDisabled, NotInit, DELAY, E> {
242        Icm20948 {
243            config: Icm20948Config {
244                acc_unit,
245                ..self.config
246            },
247            ..self
248        }
249    }
250
251    /// Set accelerometer output data rate
252    #[must_use]
253    pub fn acc_odr(self, acc_odr: u16) -> Icm20948<BUS, MagDisabled, NotInit, DELAY, E> {
254        Icm20948 {
255            config: Icm20948Config {
256                acc_odr,
257                ..self.config
258            },
259            ..self
260        }
261    }
262
263    /// Set gyroscope measuring range, choises are 250Dps, 500Dps, 1000Dps and 2000Dps
264    #[must_use]
265    pub fn gyr_range(self, gyr_range: GyrRange) -> Icm20948<BUS, MagDisabled, NotInit, DELAY, E> {
266        Icm20948 {
267            config: Icm20948Config {
268                gyr_range,
269                ..self.config
270            },
271            ..self
272        }
273    }
274
275    /// Set gyroscope digital low pass filter frequency
276    #[must_use]
277    pub fn gyr_dlp(self, gyr_dlp: GyrDlp) -> Icm20948<BUS, MagDisabled, NotInit, DELAY, E> {
278        Icm20948 {
279            config: Icm20948Config {
280                gyr_dlp,
281                ..self.config
282            },
283            ..self
284        }
285    }
286
287    /// Set returned unit of gyroscope measurement, choises are degrees/s or radians/s
288    #[must_use]
289    pub fn gyr_unit(self, gyr_unit: GyrUnit) -> Icm20948<BUS, MagDisabled, NotInit, DELAY, E> {
290        Icm20948 {
291            config: Icm20948Config {
292                gyr_unit,
293                ..self.config
294            },
295            ..self
296        }
297    }
298
299    /// Set gyroscope output data rate
300    #[must_use]
301    pub fn gyr_odr(self, gyr_odr: u8) -> Icm20948<BUS, MagDisabled, NotInit, DELAY, E> {
302        Icm20948 {
303            config: Icm20948Config {
304                gyr_odr,
305                ..self.config
306            },
307            ..self
308        }
309    }
310
311    /*
312        Initialization methods
313    */
314
315    /// Initializes the IMU with accelerometer and gyroscope
316    pub async fn initialize_6dof(
317        mut self,
318    ) -> Result<Icm20948<BUS, MagDisabled, Init, DELAY, E>, IcmError<E>> {
319        self.setup_acc_gyr().await?;
320
321        Ok(Icm20948 {
322            mag_state: MagDisabled,
323            init_state: PhantomData::<Init>,
324            bus: self.bus,
325            config: self.config,
326            user_bank: self.user_bank,
327            delay: self.delay,
328            bus_error: PhantomData,
329        })
330    }
331
332    /// Initializes the IMU with accelerometer, gyroscope and magnetometer
333    pub async fn initialize_9dof(
334        mut self,
335    ) -> Result<Icm20948<BUS, MagEnabled, Init, DELAY, E>, IcmError<E>> {
336        self.setup_acc_gyr().await?;
337        self.setup_mag().await?;
338
339        Ok(Icm20948 {
340            mag_state: MagEnabled::uncralibrated(),
341            init_state: PhantomData::<Init>,
342            bus: self.bus,
343            config: self.config,
344            user_bank: self.user_bank,
345            delay: self.delay,
346            bus_error: self.bus_error,
347        })
348    }
349
350    /// Setup accelerometer and gyroscope according to config
351    async fn setup_acc_gyr(&mut self) -> Result<(), IcmError<E>> {
352        // Ensure known-good state
353        self.device_reset().await?;
354
355        // Initially set user bank by force, and check identity
356        self.set_user_bank(&Bank0::WhoAmI, true).await?;
357        let [whoami] = self.read_from(Bank0::WhoAmI).await?;
358
359        if whoami != 0xEA {
360            return Err(IcmError::ImuSetupError);
361        }
362
363        // Disable sleep mode and set to auto select clock source
364        self.write_to(Bank0::PwrMgmt1, 0x01).await?;
365
366        // Set gyro and accel ranges
367        self.set_acc_range(self.config.acc_range).await?;
368        self.set_gyr_range(self.config.gyr_range).await?;
369
370        // Apply digital lowpass filter settings
371        self.set_gyr_dlp(self.config.gyr_dlp).await?;
372        self.set_acc_dlp(self.config.acc_dlp).await?;
373
374        // Set gyro and accel output data rate
375        self.set_acc_odr(self.config.acc_odr).await?;
376        self.set_gyr_odr(self.config.gyr_odr).await?;
377
378        Ok(())
379    }
380
381    /// Setup magnetometer in continuous mode
382    async fn setup_mag(&mut self) -> Result<(), IcmError<E>> {
383        // Ensure known-good state
384        self.mag_reset().await?;
385
386        // Setup magnetometer (i2c slave) clock (default 400 kHz)
387        self.write_to(Bank3::I2cMstCtrl, 0x07).await?;
388
389        // Enable I2C master mode for magnetometer
390        self.enable_i2c_master(true).await?;
391
392        // Configure slave address as magnetometer
393        self.write_to(Bank3::I2cSlv0Addr, MAGNET_ADDR).await?;
394
395        // Verify magnetometer identifier
396        let [whoami] = self.mag_read_from(MagBank::DeviceId).await?;
397
398        if whoami != 9 {
399            return Err(IcmError::MagSetupError);
400        }
401
402        // Reset magnetometer
403        self.mag_reset().await?;
404
405        // Set magnetometer to continuous mode 4 (100 Hz)
406        self.mag_write_to(MagBank::Control2.reg(), 0b01000).await?;
407
408        // Set slave register to read from
409        self.write_to(Bank3::I2cSlv0Reg, MagBank::XDataLow.reg())
410            .await?;
411
412        // Set expected read size
413        self.write_to(Bank3::I2cSlv0Ctrl, (1 << 7) | 8).await?;
414
415        Ok(())
416    }
417}
418
419impl<BUS, E, MAG, INIT, DELAY> Icm20948<BUS, MAG, INIT, DELAY, E>
420where
421    BUS: BusTransfer<E>,
422    DELAY: DelayNs,
423{
424    /// Reset accelerometer / gyroscope module
425    pub async fn device_reset(&mut self) -> Result<(), E> {
426        self.delay.delay_ms(20).await;
427        self.write_to_flag(Bank0::PwrMgmt1, 1 << 7, 1 << 7).await?;
428        self.delay.delay_ms(50).await;
429        Ok(())
430    }
431
432    /// Enables main ICM module to act as I2C master (eg. for magnetometer)
433    async fn enable_i2c_master(&mut self, enable: bool) -> Result<(), E> {
434        self.write_to_flag(Bank0::UserCtrl, u8::from(enable) << 5, 1 << 5)
435            .await
436    }
437
438    /// Resets I2C master module
439    async fn reset_i2c_master(&mut self) -> Result<(), E> {
440        self.write_to_flag(Bank0::UserCtrl, 1 << 1, 1 << 1).await
441    }
442
443    /// Ensure correct user bank for given register
444    async fn set_user_bank<R: Register + Copy>(&mut self, bank: &R, force: bool) -> Result<(), E> {
445        if (self.user_bank != bank.bank()) || force {
446            self.bus
447                .bus_write(&[REG_BANK_SEL, (bank.bank() as u8) << 4])
448                .await?;
449            self.user_bank = bank.bank();
450        }
451        Ok(())
452    }
453
454    /// Read a const number `N` of bytes from the requested register
455    async fn read_from<const N: usize, R: Register + Copy>(
456        &mut self,
457        cmd: R,
458    ) -> Result<[u8; N], E> {
459        let mut buf = [0u8; N];
460        self.set_user_bank(&cmd, false).await?;
461        self.bus.bus_transfer(&[cmd.reg()], &mut buf).await?;
462        Ok(buf)
463    }
464
465    /// Write a single byte to the requeste register
466    async fn write_to<R: Register + Copy>(&mut self, cmd: R, data: u8) -> Result<(), E> {
467        self.set_user_bank(&cmd, false).await?;
468        self.bus.bus_write(&[cmd.reg(), data]).await
469    }
470
471    /// Write to a register, but only overwrite the parts corresponding to the flag byte
472    async fn write_to_flag<R>(&mut self, cmd: R, data: u8, flag: u8) -> Result<(), E>
473    where
474        R: Register + Copy + Clone,
475    {
476        let [mut register] = self.read_from(cmd).await?;
477        register = (register & !flag) | (data & flag);
478        self.write_to(cmd, register).await
479    }
480
481    /// Put the magnetometer into read mode
482    async fn set_mag_read(&mut self) -> Result<(), E> {
483        let [mut reg] = self.read_from(Bank3::I2cSlv0Addr).await?;
484        reg &= 0b0111_1111;
485        reg |= 1 << 7;
486        self.write_to(Bank3::I2cSlv0Addr, reg).await
487    }
488
489    /// Put the magnetometer into write mode
490    async fn set_mag_write(&mut self) -> Result<(), E> {
491        let [mut reg] = self.read_from(Bank3::I2cSlv0Addr).await?;
492        reg &= 0b0111_1111;
493        self.write_to(Bank3::I2cSlv0Addr, reg).await
494    }
495
496    /// Write `data` to the magnetometer module in `reg` (20 ms non-blocking delays)
497    async fn mag_write_to(&mut self, reg: u8, data: u8) -> Result<(), E> {
498        self.set_mag_write().await?;
499        self.delay.delay_ms(10).await;
500        self.write_to(Bank3::I2cSlv0Reg, reg).await?;
501        self.write_to(Bank3::I2cSlv0Do, data).await?;
502        self.write_to(Bank3::I2cSlv0Ctrl, (1 << 7) | 1).await?;
503        self.delay.delay_ms(10).await;
504        self.set_mag_read().await
505    }
506
507    /// Read a `N` bytes from the magnetometer in `reg` (20 ms non-blocking delays)
508    async fn mag_read_from<const N: usize>(&mut self, reg: MagBank) -> Result<[u8; N], E> {
509        self.set_mag_read().await?;
510        self.delay.delay_ms(10).await;
511        self.write_to(Bank3::I2cSlv0Reg, reg.reg()).await?;
512        self.write_to(Bank3::I2cSlv0Ctrl, (1 << 7) | N as u8)
513            .await?;
514        self.delay.delay_ms(10).await;
515        self.read_from(Bank0::ExtSlvSensData00).await
516    }
517
518    /// Reset magnetometer module ( 120 ms non-blocking delays)
519    async fn mag_reset(&mut self) -> Result<(), E> {
520        // Control 3 register bit 1 resets magnetometer unit
521        self.mag_write_to(MagBank::Control3.reg(), 1).await?;
522        self.delay.delay_ms(100).await;
523
524        // Reset i2c master module
525        self.reset_i2c_master().await
526    }
527
528    /// Configure acceleromter to measure with given range
529    pub async fn set_acc_range(&mut self, range: AccRange) -> Result<(), E> {
530        self.write_to_flag(Bank2::AccelConfig, (range as u8) << 1, 0b0110)
531            .await?;
532        self.config.acc_range = range;
533        Ok(())
534    }
535
536    /// Configure gyroscope to measure with given range
537    pub async fn set_gyr_range(&mut self, range: GyrRange) -> Result<(), E> {
538        self.write_to_flag(Bank2::GyroConfig1, (range as u8) << 1, 0b0110)
539            .await?;
540        self.config.gyr_range = range;
541        Ok(())
542    }
543
544    /// Set returned unit of accelerometer
545    pub fn set_acc_unit(&mut self, unit: AccUnit) {
546        self.config.acc_unit = unit;
547    }
548
549    /// Set returned unit of gyroscope
550    pub fn set_gyr_unit(&mut self, unit: GyrUnit) {
551        self.config.gyr_unit = unit;
552    }
553
554    /// Set (or disable) accelerometer digital low-pass filter
555    pub async fn set_acc_dlp(&mut self, acc_dlp: AccDlp) -> Result<(), E> {
556        if AccDlp::Disabled == acc_dlp {
557            self.write_to_flag(Bank2::AccelConfig, 0u8, 0b0011_1001)
558                .await
559        } else {
560            self.write_to_flag(Bank2::AccelConfig, ((acc_dlp as u8) << 3) | 1, 0b0011_1001)
561                .await
562        }
563    }
564
565    /// Set (or disable) gyroscope digital low-pass filter
566    pub async fn set_gyr_dlp(&mut self, gyr_dlp: GyrDlp) -> Result<(), E> {
567        if GyrDlp::Disabled == gyr_dlp {
568            self.write_to_flag(Bank2::GyroConfig1, 0u8, 0b0011_1001)
569                .await
570        } else {
571            self.write_to_flag(Bank2::GyroConfig1, ((gyr_dlp as u8) << 3) | 1, 0b0011_1001)
572                .await
573        }
574    }
575
576    /// Set accelerometer output data rate. Value will be clamped above 4095.
577    pub async fn set_acc_odr(&mut self, acc_odr: u16) -> Result<(), E> {
578        let [msb, lsb] = acc_odr.clamp(0, 0xFFF).to_be_bytes();
579        self.write_to(Bank2::AccelSmplrtDiv1, msb).await?;
580        self.write_to(Bank2::AccelSmplrtDiv2, lsb).await
581    }
582
583    /// Set gyroscope output data rate.
584    pub async fn set_gyr_odr(&mut self, gyr_odr: u8) -> Result<(), E> {
585        self.write_to(Bank2::GyroSmplrtDiv, gyr_odr).await
586    }
587}
588
589impl<BUS, DELAY, E> Icm20948<BUS, MagEnabled, Init, DELAY, E>
590where
591    BUS: BusTransfer<E>,
592    DELAY: DelayNs,
593{
594    /// Apply the saved calibration offset+scale to measurement vector
595    fn apply_mag_calibration(&self, mag: &mut Vector3<f32>) {
596        if self.mag_state.is_calibrated {
597            *mag = mag.zip_zip_map(&self.mag_state.offset, &self.mag_state.scale, |m, o, s| {
598                (m - o) / s
599            });
600        }
601    }
602
603    /// Set magnetometer calibration data (offset,scale)
604    pub fn set_mag_calibration(&mut self, offset: [f32; 3], scale: [f32; 3]) {
605        self.mag_state.is_calibrated = true;
606        self.mag_state.offset = offset.into();
607        self.mag_state.scale = scale.into();
608    }
609
610    /// Resets (disables) magnetometer calibration data
611    pub fn reset_mag_calibration(&mut self) {
612        self.mag_state.is_calibrated = false;
613        self.mag_state.offset = Vector3::zeros();
614        self.mag_state.scale = Vector3::from_element(1.);
615    }
616
617    /// Get vector of scaled magnetometer values
618    pub async fn read_mag(&mut self) -> Result<Vector3<f32>, E> {
619        let mut mag = self.read_mag_unscaled().await?.map(|x| (x as f32)).into();
620        self.apply_mag_calibration(&mut mag);
621
622        Ok(mag)
623    }
624
625    /// Get array of unscaled accelerometer values
626    pub async fn read_mag_unscaled(&mut self) -> Result<[i16; 3], E> {
627        let raw: [u8; 6] = self.read_from(Bank0::ExtSlvSensData00).await?;
628        let mag = collect_3xi16_mag(raw);
629
630        Ok(mag)
631    }
632
633    /// Get scaled measurement for accelerometer, gyroscope and magnetometer, and temperature
634    pub async fn read_9dof(&mut self) -> Result<Data9Dof<f32>, E> {
635        let raw: [u8; 20] = self.read_from(Bank0::AccelXoutH).await?;
636        let [axh, axl, ayh, ayl, azh, azl, gxh, gxl, gyh, gyl, gzh, gzl, tph, tpl, mxl, mxh, myl, myh, mzl, mzh] =
637            raw;
638
639        let acc = self.scaled_acc_from_bytes([axh, axl, ayh, ayl, azh, azl]);
640        let gyr = self.scaled_gyr_from_bytes([gxh, gxl, gyh, gyl, gzh, gzl]);
641        let mag = self.scaled_mag_from_bytes([mxl, mxh, myl, myh, mzl, mzh]);
642
643        let tmp = self.scaled_tmp_from_bytes([tph, tpl]);
644
645        Ok(Data9Dof { acc, gyr, mag, tmp })
646    }
647
648    /// Get unscaled measurements for accelerometer and gyroscope, and temperature
649    pub async fn read_9dof_unscaled(&mut self) -> Result<Data9Dof<i16>, E> {
650        let raw: [u8; 20] = self.read_from(Bank0::AccelXoutH).await?;
651        let [axh, axl, ayh, ayl, azh, azl, gxh, gxl, gyh, gyl, gzh, gzl, tph, tpl, mxl, mxh, myl, myh, mzl, mzh] =
652            raw;
653
654        let acc = collect_3xi16([axh, axl, ayh, ayl, azh, azl]).into();
655        let gyr = collect_3xi16([gxh, gxl, gyh, gyl, gzh, gzl]).into();
656        let mag = collect_3xi16_mag([mxl, mxh, myl, myh, mzl, mzh]).into();
657
658        let tmp = i16::from_be_bytes([tph, tpl]);
659
660        Ok(Data9Dof { acc, gyr, mag, tmp })
661    }
662
663    /// Takes 6 bytes converts them into a Vector3 of floats, unit is micro tesla
664    fn scaled_mag_from_bytes(&self, bytes: [u8; 6]) -> Vector3<f32> {
665        let mut mag = collect_3xi16_mag(bytes).map(|x| (0.15 * x as f32)).into();
666        self.apply_mag_calibration(&mut mag);
667        mag
668    }
669}
670
671impl<BUS, E, MAG, DELAY> Icm20948<BUS, MAG, Init, DELAY, E>
672where
673    BUS: BusTransfer<E>,
674    E: Into<IcmError<E>>,
675    DELAY: DelayNs,
676{
677    /// Takes 6 bytes converts them into a Vector3 of floats
678    fn scaled_acc_from_bytes(&self, bytes: [u8; 6]) -> Vector3<f32> {
679        let acc = collect_3xi16(bytes).map(|x| f32::from(x) * self.acc_scalar());
680        Vector3::from(acc)
681    }
682
683    /// Takes 6 bytes converts them into a Vector3 of floats
684    fn scaled_gyr_from_bytes(&self, bytes: [u8; 6]) -> Vector3<f32> {
685        let gyr = collect_3xi16(bytes).map(|x| f32::from(x) * self.gyr_scalar());
686        Vector3::from(gyr)
687    }
688
689    /// Takes 2 bytes converts them into a temerature as a float
690    fn scaled_tmp_from_bytes(&self, bytes: [u8; 2]) -> f32 {
691        f32::from(i16::from_be_bytes(bytes)) / 333.87 + 21.
692    }
693
694    /// Get array of unscaled accelerometer values
695    pub async fn read_acc_unscaled(&mut self) -> Result<Vector3<i16>, E> {
696        let raw = self.read_from(Bank0::AccelXoutH).await?;
697        Ok(collect_3xi16(raw).into())
698    }
699
700    /// Get array of scaled accelerometer values
701    pub async fn read_acc(&mut self) -> Result<Vector3<f32>, E> {
702        let acc = self
703            .read_acc_unscaled()
704            .await?
705            .map(|x| f32::from(x) * self.acc_scalar());
706        Ok(acc)
707    }
708
709    /// Get array of unscaled gyroscope values
710    pub async fn read_gyr_unscaled(&mut self) -> Result<Vector3<i16>, E> {
711        let raw = self.read_from(Bank0::GyroXoutH).await?;
712        Ok(collect_3xi16(raw).into())
713    }
714
715    /// Get array of scaled gyroscope values
716    pub async fn read_gyr(&mut self) -> Result<Vector3<f32>, E> {
717        let gyr = self
718            .read_gyr_unscaled()
719            .await?
720            .map(|x| f32::from(x) * self.gyr_scalar());
721        Ok(gyr)
722    }
723
724    /// Get scaled measurements for accelerometer and gyroscope, and temperature
725    pub async fn read_6dof(&mut self) -> Result<Data6Dof<f32>, E> {
726        let raw: [u8; 14] = self.read_from(Bank0::AccelXoutH).await?;
727        let [axh, axl, ayh, ayl, azh, azl, gxh, gxl, gyh, gyl, gzh, gzl, tph, tpl] = raw;
728
729        let acc = self.scaled_acc_from_bytes([axh, axl, ayh, ayl, azh, azl]);
730        let gyr = self.scaled_gyr_from_bytes([gxh, gxl, gyh, gyl, gzh, gzl]);
731
732        let tmp = self.scaled_tmp_from_bytes([tph, tpl]);
733
734        Ok(Data6Dof { acc, gyr, tmp })
735    }
736
737    /// Get unscaled measurements for accelerometer and gyroscope, and temperature
738    pub async fn read_6dof_unscaled(&mut self) -> Result<Data6Dof<i16>, E> {
739        let raw: [u8; 14] = self.read_from(Bank0::AccelXoutH).await?;
740        let [axh, axl, ayh, ayl, azh, azl, gxh, gxl, gyh, gyl, gzh, gzl, tph, tpl] = raw;
741
742        let acc = collect_3xi16([axh, axl, ayh, ayl, azh, azl]).into();
743        let gyr = collect_3xi16([gxh, gxl, gyh, gyl, gzh, gzl]).into();
744
745        let tmp = i16::from_be_bytes([tph, tpl]);
746
747        Ok(Data6Dof { acc, gyr, tmp })
748    }
749
750    /// Collects and averages `num` sampels for gyro calibration and saves them on-chip
751    pub async fn gyr_calibrate(&mut self, num: usize) -> Result<(), E> {
752        let mut offset: Vector3<i32> = Vector3::default();
753        for _ in 0..num {
754            offset += self.read_gyr_unscaled().await?.map(|x| x as i32);
755            self.delay.delay_ms(10).await;
756        }
757
758        self.set_gyr_offsets(offset.map(|x| { x / num as i32 } as i16))
759            .await
760    }
761
762    /// Set gyroscope calibration offsets by writing them to the IMU
763    pub async fn set_gyr_offsets(&mut self, offsets: Vector3<i16>) -> Result<(), E> {
764        let [[xh, xl], [yh, yl], [zh, zl]]: [[u8; 2]; 3] =
765            offsets.map(|x| (-x).to_be_bytes()).into();
766
767        self.set_user_bank(&Bank2::XgOffsUsrh, false).await?;
768
769        self.bus
770            .bus_write(&[Bank2::XgOffsUsrh.reg(), xh, xl])
771            .await?;
772        self.bus
773            .bus_write(&[Bank2::YgOffsUsrh.reg(), yh, yl])
774            .await?;
775        self.bus
776            .bus_write(&[Bank2::ZgOffsUsrh.reg(), zh, zl])
777            .await?;
778
779        Ok(())
780    }
781
782    /// Set accelerometer calibration offsets by writing them to the IMU
783    pub async fn set_acc_offsets(&mut self, offsets: Vector3<i16>) -> Result<(), E> {
784        let [[xh, xl], [yh, yl], [zh, zl]]: [[u8; 2]; 3] =
785            offsets.map(|x| (-x).to_be_bytes()).into();
786
787        self.set_user_bank(&Bank2::XgOffsUsrh, false).await?;
788
789        self.bus.bus_write(&[Bank1::XaOffsH.reg(), xh, xl]).await?;
790        self.bus.bus_write(&[Bank1::YaOffsH.reg(), yh, yl]).await?;
791        self.bus.bus_write(&[Bank1::ZaOffsH.reg(), zh, zl]).await?;
792
793        Ok(())
794    }
795
796    /// Returns the number of new readings in FIFO buffer
797    pub async fn new_data_ready(&mut self) -> u8 {
798        self.read_from::<1, _>(Bank0::DataRdyStatus)
799            .await
800            .map_or(0, |[b]| b & 0b1111)
801    }
802
803    /// Returns the scalar corresponding to the unit and range configured
804    const fn acc_scalar(&self) -> f32 {
805        self.config.acc_unit.scalar() / self.config.acc_range.divisor()
806    }
807
808    /// Returns the scalar corresponding to the unit and range configured
809    const fn gyr_scalar(&self) -> f32 {
810        self.config.gyr_unit.scalar() / self.config.gyr_range.divisor()
811    }
812}
813
814/// Collects 6 bytes into a vector of i16 values (acc/gyr only)
815const fn collect_3xi16(values: [u8; 6]) -> [i16; 3] {
816    let [xh, xl, yh, yl, zh, zl] = values;
817    [
818        i16::from_be_bytes([xh, xl]),
819        i16::from_be_bytes([yh, yl]),
820        i16::from_be_bytes([zh, zl]),
821    ]
822}
823
824/// Collects 6 bytes into a vector of i16 values (mag only)
825const fn collect_3xi16_mag(values: [u8; 6]) -> [i16; 3] {
826    let [xl, xh, yl, yh, zl, zh] = values;
827    #[cfg(feature = "align-mag")]
828    let mag = [
829        i16::from_be_bytes([xh, xl]),
830        -i16::from_be_bytes([yh, yl]),
831        -i16::from_be_bytes([zh, zl]),
832    ];
833    #[cfg(not(feature = "align-mag"))]
834    let mag = [
835        i16::from_be_bytes([xh, xl]),
836        i16::from_be_bytes([yh, yl]),
837        i16::from_be_bytes([zh, zl]),
838    ];
839
840    mag
841}
842
843#[derive(Copy, Clone, Debug)]
844#[cfg_attr(feature = "serde", derive(serde::Serialize, serde::Deserialize))]
845pub struct Icm20948Config {
846    pub acc_range: AccRange,
847    pub gyr_range: GyrRange,
848    pub acc_unit: AccUnit,
849    pub gyr_unit: GyrUnit,
850    pub acc_dlp: AccDlp,
851    pub gyr_dlp: GyrDlp,
852    pub acc_odr: u16,
853    pub gyr_odr: u8,
854}
855
856impl Default for Icm20948Config {
857    fn default() -> Self {
858        Self {
859            acc_range: AccRange::Gs2,
860            gyr_range: GyrRange::Dps1000,
861            acc_unit: AccUnit::Gs,
862            gyr_unit: GyrUnit::Dps,
863            acc_dlp: AccDlp::Disabled,
864            gyr_dlp: GyrDlp::Disabled,
865            acc_odr: 0,
866            gyr_odr: 0,
867        }
868    }
869}
870
871#[derive(Copy, Clone, Debug, Default)]
872pub enum I2cAddress {
873    /// On some modules `0x68` is the default address if pin `AD0` is low
874    X68,
875    /// On some modules `0x69` is the default address if pin `AD0` is high
876    #[default]
877    X69,
878    /// In case the ICM modules has a different address
879    Any(u8),
880}
881
882impl From<u8> for I2cAddress {
883    fn from(address: u8) -> Self {
884        I2cAddress::Any(address)
885    }
886}
887
888impl I2cAddress {
889    const fn get(&self) -> u8 {
890        match self {
891            I2cAddress::X68 => 0x68,
892            I2cAddress::X69 => 0x69,
893            I2cAddress::Any(a) => *a,
894        }
895    }
896}
897
898/// Range / sensitivity of accelerometer in Gs
899#[derive(Copy, Clone, Debug)]
900#[cfg_attr(feature = "serde", derive(serde::Serialize, serde::Deserialize))]
901pub enum AccRange {
902    Gs2 = 0b00,
903    Gs4 = 0b01,
904    Gs8 = 0b10,
905    Gs16 = 0b11,
906}
907
908impl AccRange {
909    pub const fn divisor(self) -> f32 {
910        match self {
911            Self::Gs2 => 16384.0,
912            Self::Gs4 => 8192.0,
913            Self::Gs8 => 4096.0,
914            Self::Gs16 => 2048.0,
915        }
916    }
917}
918
919/// Range / sentivity of gyroscope in degrees/second
920#[derive(Copy, Clone, Debug)]
921#[cfg_attr(feature = "serde", derive(serde::Serialize, serde::Deserialize))]
922pub enum GyrRange {
923    Dps250 = 0b00,
924    Dps500 = 0b01,
925    Dps1000 = 0b10,
926    Dps2000 = 0b11,
927}
928
929impl GyrRange {
930    pub const fn divisor(self) -> f32 {
931        match self {
932            Self::Dps250 => 131.0,
933            Self::Dps500 => 65.5,
934            Self::Dps1000 => 32.8,
935            Self::Dps2000 => 16.4,
936        }
937    }
938}
939
940/// Unit of accelerometer readings
941#[derive(Copy, Clone, Debug)]
942#[cfg_attr(feature = "serde", derive(serde::Serialize, serde::Deserialize))]
943pub enum AccUnit {
944    /// Meters per second squared (m/s^2)
945    Mpss,
946    /// Number of times of normal gravity
947    Gs,
948}
949
950impl AccUnit {
951    pub const fn scalar(self) -> f32 {
952        match self {
953            Self::Mpss => 9.82,
954            Self::Gs => 1.0,
955        }
956    }
957}
958
959/// Unit of gyroscope readings
960#[derive(Copy, Clone, Debug)]
961#[cfg_attr(feature = "serde", derive(serde::Serialize, serde::Deserialize))]
962pub enum GyrUnit {
963    /// Radians per second
964    Rps,
965    /// Degrees per second
966    Dps,
967}
968
969impl GyrUnit {
970    pub const fn scalar(self) -> f32 {
971        match self {
972            Self::Rps => 1.0f32.to_radians(),
973            Self::Dps => 1.0f32,
974        }
975    }
976}
977
978/// Digital low-pass filter for of accelerometer readings
979#[derive(Copy, Clone, Debug, PartialEq)]
980#[cfg_attr(feature = "serde", derive(serde::Serialize, serde::Deserialize))]
981pub enum AccDlp {
982    Hz473 = 7,
983    Hz246 = 1,
984    Hz111 = 2,
985    Hz50 = 3,
986    Hz24 = 4,
987    Hz12 = 5,
988    Hz6 = 6,
989    Disabled = 8,
990}
991
992/// Digital low-pass filter for of gyroscope readings
993#[derive(Copy, Clone, Debug, PartialEq)]
994#[cfg_attr(feature = "serde", derive(serde::Serialize, serde::Deserialize))]
995pub enum GyrDlp {
996    Hz361 = 7,
997    Hz196 = 0,
998    Hz152 = 1,
999    Hz120 = 2,
1000    Hz51 = 3,
1001    Hz24 = 4,
1002    Hz12 = 5,
1003    Hz6 = 6,
1004    Disabled = 8,
1005}
1006
1007#[derive(Debug)]
1008pub enum IcmError<E> {
1009    BusError(E),
1010    ImuSetupError,
1011    MagSetupError,
1012}
1013
1014impl<E> From<E> for IcmError<E> {
1015    fn from(error: E) -> Self {
1016        IcmError::BusError(error)
1017    }
1018}
icm20948_async/lib.rs

icm20948_async/
lib.rs
