capsules_extra/
icm42670p.rs

1// Licensed under the Apache License, Version 2.0 or the MIT License.
2// SPDX-License-Identifier: Apache-2.0 OR MIT
3// Copyright Tock Contributors 2026.
4
5//! Driver for the TDK InvenSense ICM-42670-P 6-Axis IMU (accelerometer +
6//! gyroscope).
7//!
8//! <https://invensense.tdk.com/products/motion-tracking/6-axis/icm-42670-p/>
9
10use core::cell::Cell;
11use kernel::ErrorCode;
12use kernel::hil::i2c;
13use kernel::hil::sensors::{NineDof, NineDofClient};
14use kernel::hil::time::{self, Alarm, ConvertTicks};
15use kernel::utilities::cells::{OptionalCell, TakeCell};
16use kernel::utilities::registers::LocalRegisterCopy;
17use kernel::utilities::registers::register_bitfields;
18
19/// The ICM-42670-P's I2C address depends on the level its `AP_AD0` pin is
20/// wired to: `0x68` when low (the common/default wiring), `0x69` when
21/// high.
22pub const BASE_ADDR: u8 = 0x68;
23/// The alternate I2C address, when `AP_AD0` is wired high.
24pub const ALT_ADDR: u8 = 0x69;
25
26/// Register addresses used by this driver.
27mod register {
28    /// First (high) byte of the 6-byte accelerometer X/Y/Z burst.
29    pub const ACCEL_DATA_X1: u8 = 0x0B;
30    /// First (high) byte of the 6-byte gyroscope X/Y/Z burst.
31    pub const GYRO_DATA_X1: u8 = 0x11;
32    /// First of three consecutive registers (`PWR_MGMT0`, `GYRO_CONFIG0`,
33    /// `ACCEL_CONFIG0`) written together to power on and configure both
34    /// sensors.
35    pub const PWR_MGMT0: u8 = 0x1F;
36}
37
38register_bitfields![u8,
39    PWR_MGMT0 [
40        GYRO_MODE OFFSET(2) NUMBITS(2) [
41            LowNoise = 0b11,
42        ],
43        ACCEL_MODE OFFSET(0) NUMBITS(2) [
44            LowNoise = 0b11,
45        ],
46    ],
47    GYRO_CONFIG0 [
48        GYRO_UI_FS_SEL OFFSET(5) NUMBITS(2) [],
49        GYRO_ODR OFFSET(0) NUMBITS(4) [],
50    ],
51    ACCEL_CONFIG0 [
52        ACCEL_UI_FS_SEL OFFSET(5) NUMBITS(2) [],
53        ACCEL_ODR OFFSET(0) NUMBITS(4) [],
54    ],
55];
56
57/// `{GYRO,ACCEL}_ODR` value for 100 Hz, the same encoding for both.
58const ODR_100HZ: u8 = 0b1001;
59
60/// `ACCEL_UI_FS_SEL` value for ±2g, the accelerometer's most sensitive
61/// (best resolution) range.
62const ACCEL_FS_SEL_2G: u8 = 0b11;
63/// Accelerometer sensitivity at `ACCEL_FS_SEL_2G`, in LSB per g.
64const ACCEL_SENSITIVITY_LSB_PER_G: i32 = 16384;
65
66/// `GYRO_UI_FS_SEL` value for ±250 º/s, the gyroscope's most sensitive
67/// (best resolution) range.
68const GYRO_FS_SEL_250DPS: u8 = 0b11;
69/// Gyroscope sensitivity at `GYRO_FS_SEL_250DPS`, in LSB per (º/s),
70/// multiplied by 10.
71///
72/// 131 is exactly an integer number of LSB per º/s here, but this is kept
73/// as a x10 fixed-point value since nearby ranges aren't whole numbers,
74/// e.g. 65.5 LSB/(º/s) for ±500 º/s.
75const GYRO_SENSITIVITY_LSB_PER_DPS_X10: i32 = 1310;
76
77/// Time to wait after configuring the sensor before its output is valid.
78///
79/// The datasheet's "Accelerometer Startup Time" and "Gyroscope Start-Up
80/// Time" are 10 ms and 30 ms respectively (from power-on to valid data),
81/// with margin.
82const STARTUP_DELAY_MS: u32 = 40;
83
84#[derive(Clone, Copy, PartialEq)]
85enum State {
86    Idle,
87    /// Burst-writing `PWR_MGMT0`/`GYRO_CONFIG0`/`ACCEL_CONFIG0`; waiting
88    /// for the I2C write to finish. Only happens once, the first time
89    /// either sensor is read.
90    Configuring,
91    /// Waiting out `STARTUP_DELAY_MS` after `Configuring`, before the
92    /// requested reading can proceed.
93    StartupDelay,
94    /// Reading back the 6-byte accelerometer or gyroscope result
95    /// (whichever `pending` says was requested).
96    Reading,
97}
98
99#[derive(Clone, Copy, PartialEq)]
100enum Reading {
101    Accelerometer,
102    Gyroscope,
103}
104
105/// Convert two big-endian, two's-complement bytes (as the ICM-42670-P
106/// returns each axis: high byte first) into a signed 16-bit value.
107fn be16(hi: u8, lo: u8) -> i16 {
108    (((hi as u16) << 8) | (lo as u16)) as i16
109}
110
111pub struct Icm42670p<'a, A: Alarm<'a>, I: i2c::I2CDevice<'a>> {
112    i2c: &'a I,
113    alarm: &'a A,
114    nine_dof_client: OptionalCell<&'a dyn NineDofClient>,
115    state: Cell<State>,
116    buffer: TakeCell<'static, [u8]>,
117    /// Set once `PWR_MGMT0`/`GYRO_CONFIG0`/`ACCEL_CONFIG0` have been
118    /// written successfully: later reads can skip straight to `Reading`.
119    configured: Cell<bool>,
120    /// Which reading is in progress (valid whenever `state != Idle`).
121    pending: Cell<Reading>,
122}
123
124impl<'a, A: Alarm<'a>, I: i2c::I2CDevice<'a>> Icm42670p<'a, A, I> {
125    pub fn new(i2c: &'a I, buffer: &'static mut [u8], alarm: &'a A) -> Icm42670p<'a, A, I> {
126        Icm42670p {
127            i2c,
128            alarm,
129            nine_dof_client: OptionalCell::empty(),
130            state: Cell::new(State::Idle),
131            buffer: TakeCell::new(buffer),
132            configured: Cell::new(false),
133            pending: Cell::new(Reading::Accelerometer),
134        }
135    }
136
137    fn start_reading(&self, reading: Reading) -> Result<(), ErrorCode> {
138        if self.state.get() != State::Idle {
139            return Err(ErrorCode::BUSY);
140        }
141
142        self.pending.set(reading);
143
144        if self.configured.get() {
145            self.start_read()
146        } else {
147            self.start_configure()
148        }
149    }
150
151    /// Burst-write `PWR_MGMT0`, `GYRO_CONFIG0`, and `ACCEL_CONFIG0`
152    /// (consecutive registers) to power on both sensors in low-noise
153    /// mode at their most sensitive range and a 100 Hz output data rate.
154    fn start_configure(&self) -> Result<(), ErrorCode> {
155        self.buffer.take().map_or(Err(ErrorCode::NOMEM), |buffer| {
156            self.state.set(State::Configuring);
157
158            let mut pwr_mgmt0: LocalRegisterCopy<u8, PWR_MGMT0::Register> =
159                LocalRegisterCopy::new(0);
160            pwr_mgmt0.write(PWR_MGMT0::GYRO_MODE::LowNoise + PWR_MGMT0::ACCEL_MODE::LowNoise);
161
162            let mut gyro_config0: LocalRegisterCopy<u8, GYRO_CONFIG0::Register> =
163                LocalRegisterCopy::new(0);
164            gyro_config0.write(
165                GYRO_CONFIG0::GYRO_UI_FS_SEL.val(GYRO_FS_SEL_250DPS)
166                    + GYRO_CONFIG0::GYRO_ODR.val(ODR_100HZ),
167            );
168
169            let mut accel_config0: LocalRegisterCopy<u8, ACCEL_CONFIG0::Register> =
170                LocalRegisterCopy::new(0);
171            accel_config0.write(
172                ACCEL_CONFIG0::ACCEL_UI_FS_SEL.val(ACCEL_FS_SEL_2G)
173                    + ACCEL_CONFIG0::ACCEL_ODR.val(ODR_100HZ),
174            );
175
176            buffer[0] = register::PWR_MGMT0;
177            buffer[1] = pwr_mgmt0.get();
178            buffer[2] = gyro_config0.get();
179            buffer[3] = accel_config0.get();
180
181            self.i2c.enable();
182            match self.i2c.write(buffer, 4) {
183                Ok(()) => Ok(()),
184                Err((error, buffer)) => {
185                    self.buffer.replace(buffer);
186                    self.i2c.disable();
187                    self.state.set(State::Idle);
188                    Err(error.into())
189                }
190            }
191        })
192    }
193
194    /// Read back the 6-byte accelerometer or gyroscope result, per
195    /// `self.pending`.
196    fn start_read(&self) -> Result<(), ErrorCode> {
197        self.buffer.take().map_or(Err(ErrorCode::NOMEM), |buffer| {
198            self.state.set(State::Reading);
199
200            buffer[0] = match self.pending.get() {
201                Reading::Accelerometer => register::ACCEL_DATA_X1,
202                Reading::Gyroscope => register::GYRO_DATA_X1,
203            };
204
205            self.i2c.enable();
206            match self.i2c.write_read(buffer, 1, 6) {
207                Ok(()) => Ok(()),
208                Err((error, buffer)) => {
209                    self.buffer.replace(buffer);
210                    self.i2c.disable();
211                    self.state.set(State::Idle);
212                    Err(error.into())
213                }
214            }
215        })
216    }
217
218    /// Abort on an I2C error: disable the bus, return to `Idle`, and
219    /// report the failure the same way a bad reading is reported (there's
220    /// no error type in `NineDofClient::callback`).
221    fn abort(&self, buffer: &'static mut [u8]) {
222        self.buffer.replace(buffer);
223        self.i2c.disable();
224        self.state.set(State::Idle);
225        self.nine_dof_client.map(|client| client.callback(0, 0, 0));
226    }
227}
228
229impl<'a, A: Alarm<'a>, I: i2c::I2CDevice<'a>> time::AlarmClient for Icm42670p<'a, A, I> {
230    fn alarm(&self) {
231        // After the startup delay, take the reading.
232        if let Err(_error) = self.start_read() {
233            self.i2c.disable();
234            self.state.set(State::Idle);
235            self.nine_dof_client.map(|client| client.callback(0, 0, 0));
236        }
237    }
238}
239
240impl<'a, A: Alarm<'a>, I: i2c::I2CDevice<'a>> i2c::I2CClient for Icm42670p<'a, A, I> {
241    fn command_complete(&self, buffer: &'static mut [u8], status: Result<(), i2c::Error>) {
242        if status.is_err() {
243            self.abort(buffer);
244            return;
245        }
246
247        match self.state.get() {
248            State::Configuring => {
249                self.buffer.replace(buffer);
250                self.configured.set(true);
251                self.state.set(State::StartupDelay);
252                self.alarm
253                    .set_alarm(self.alarm.now(), self.alarm.ticks_from_ms(STARTUP_DELAY_MS));
254            }
255            State::Reading => {
256                let (sensitivity, x, y, z) = match self.pending.get() {
257                    Reading::Accelerometer => (
258                        ACCEL_SENSITIVITY_LSB_PER_G,
259                        be16(buffer[0], buffer[1]),
260                        be16(buffer[2], buffer[3]),
261                        be16(buffer[4], buffer[5]),
262                    ),
263                    Reading::Gyroscope => (
264                        GYRO_SENSITIVITY_LSB_PER_DPS_X10,
265                        be16(buffer[0], buffer[1]),
266                        be16(buffer[2], buffer[3]),
267                        be16(buffer[4], buffer[5]),
268                    ),
269                };
270                // Accelerometer values are reported in milli-g, gyroscope
271                // values in milli-degrees-per-second (the same convention
272                // `lsm6dsoxtr.rs` and `fxos8700cq.rs` use); like those
273                // drivers, the signed result is bit-reinterpreted (not
274                // numerically converted) into the `usize` this HIL's
275                // callback carries it in.
276                let scale = match self.pending.get() {
277                    Reading::Accelerometer => 1000,
278                    Reading::Gyroscope => 10000,
279                };
280                let x = ((x as i32 * scale) / sensitivity) as usize;
281                let y = ((y as i32 * scale) / sensitivity) as usize;
282                let z = ((z as i32 * scale) / sensitivity) as usize;
283
284                self.buffer.replace(buffer);
285                self.i2c.disable();
286                self.state.set(State::Idle);
287                self.nine_dof_client.map(|client| client.callback(x, y, z));
288            }
289            State::Idle | State::StartupDelay => {}
290        }
291    }
292}
293
294impl<'a, A: Alarm<'a>, I: i2c::I2CDevice<'a>> NineDof<'a> for Icm42670p<'a, A, I> {
295    fn set_client(&self, client: &'a dyn NineDofClient) {
296        self.nine_dof_client.set(client);
297    }
298
299    fn read_accelerometer(&self) -> Result<(), ErrorCode> {
300        self.start_reading(Reading::Accelerometer)
301    }
302
303    fn read_gyroscope(&self) -> Result<(), ErrorCode> {
304        self.start_reading(Reading::Gyroscope)
305    }
306}
capsules_extra/icm42670p.rs

capsules_extra/
icm42670p.rs
