1#![cfg_attr(not(test), no_std)]
4
5use embedded_hal_async::delay::DelayNs;
6
7pub mod blocking;
8
9pub const PRIMARY_ADDRESS: u8 = 0x68;
10pub const SECONDARY_ADDRESS: u8 = 0x69;
11
12pub const PWR_MGMT_1_CLKSEL_PLL_X_GYRO: u8 = 0x01;
25
26pub const WAKE_UP_DELAY_MS: u32 = 30;
39
40pub mod regs {
41 pub const XG_OFFS_TC: u8 = 0x00; pub const YG_OFFS_TC: u8 = 0x01; pub const ZG_OFFS_TC: u8 = 0x02; pub const X_FINE_GAIN: u8 = 0x03; pub const Y_FINE_GAIN: u8 = 0x04; pub const Z_FINE_GAIN: u8 = 0x05; pub const XA_OFFS_H: u8 = 0x06; pub const XA_OFFS_L_TC: u8 = 0x07;
49 pub const YA_OFFS_H: u8 = 0x08; pub const YA_OFFS_L_TC: u8 = 0x09;
51 pub const ZA_OFFS_H: u8 = 0x0A; pub const ZA_OFFS_L_TC: u8 = 0x0B;
53 pub const XG_OFFS_USRH: u8 = 0x13; pub const XG_OFFS_USRL: u8 = 0x14;
55 pub const YG_OFFS_USRH: u8 = 0x15; pub const YG_OFFS_USRL: u8 = 0x16;
57 pub const ZG_OFFS_USRH: u8 = 0x17; pub const ZG_OFFS_USRL: u8 = 0x18;
59 pub const SMPLRT_DIV: u8 = 0x19;
60 pub const CONFIG: u8 = 0x1A;
61 pub const GYRO_CONFIG: u8 = 0x1B;
62 pub const ACCEL_CONFIG: u8 = 0x1C;
63 pub const FF_THR: u8 = 0x1D;
64 pub const FF_DUR: u8 = 0x1E;
65 pub const MOT_THR: u8 = 0x1F;
66 pub const MOT_DUR: u8 = 0x20;
67 pub const ZRMOT_THR: u8 = 0x21;
68 pub const ZRMOT_DUR: u8 = 0x22;
69 pub const FIFO_EN: u8 = 0x23;
70 pub const I2C_MST_CTRL: u8 = 0x24;
71 pub const I2C_SLV0_ADDR: u8 = 0x25;
72 pub const I2C_SLV0_REG: u8 = 0x26;
73 pub const I2C_SLV0_CTRL: u8 = 0x27;
74 pub const I2C_SLV1_ADDR: u8 = 0x28;
75 pub const I2C_SLV1_REG: u8 = 0x29;
76 pub const I2C_SLV1_CTRL: u8 = 0x2A;
77 pub const I2C_SLV2_ADDR: u8 = 0x2B;
78 pub const I2C_SLV2_REG: u8 = 0x2C;
79 pub const I2C_SLV2_CTRL: u8 = 0x2D;
80 pub const I2C_SLV3_ADDR: u8 = 0x2E;
81 pub const I2C_SLV3_REG: u8 = 0x2F;
82 pub const I2C_SLV3_CTRL: u8 = 0x30;
83 pub const I2C_SLV4_ADDR: u8 = 0x31;
84 pub const I2C_SLV4_REG: u8 = 0x32;
85 pub const I2C_SLV4_DO: u8 = 0x33;
86 pub const I2C_SLV4_CTRL: u8 = 0x34;
87 pub const I2C_SLV4_DI: u8 = 0x35;
88 pub const I2C_MST_STATUS: u8 = 0x36;
89 pub const INT_PIN_CFG: u8 = 0x37;
90 pub const INT_ENABLE: u8 = 0x38;
91 pub const DMP_INT_STATUS: u8 = 0x39;
92 pub const INT_STATUS: u8 = 0x3A;
93 pub const ACCEL_XOUT_H: u8 = 0x3B;
94 pub const ACCEL_XOUT_L: u8 = 0x3C;
95 pub const ACCEL_YOUT_H: u8 = 0x3D;
96 pub const ACCEL_YOUT_L: u8 = 0x3E;
97 pub const ACCEL_ZOUT_H: u8 = 0x3F;
98 pub const ACCEL_ZOUT_L: u8 = 0x40;
99 pub const TEMP_OUT_H: u8 = 0x41;
100 pub const TEMP_OUT_L: u8 = 0x42;
101 pub const GYRO_XOUT_H: u8 = 0x43;
102 pub const GYRO_XOUT_L: u8 = 0x44;
103 pub const GYRO_YOUT_H: u8 = 0x45;
104 pub const GYRO_YOUT_L: u8 = 0x46;
105 pub const GYRO_ZOUT_H: u8 = 0x47;
106 pub const GYRO_ZOUT_L: u8 = 0x48;
107 pub const EXT_SENS_DATA_00: u8 = 0x49;
108 pub const EXT_SENS_DATA_01: u8 = 0x4A;
109 pub const EXT_SENS_DATA_02: u8 = 0x4B;
110 pub const EXT_SENS_DATA_03: u8 = 0x4C;
111 pub const EXT_SENS_DATA_04: u8 = 0x4D;
112 pub const EXT_SENS_DATA_05: u8 = 0x4E;
113 pub const EXT_SENS_DATA_06: u8 = 0x4F;
114 pub const EXT_SENS_DATA_07: u8 = 0x50;
115 pub const EXT_SENS_DATA_08: u8 = 0x51;
116 pub const EXT_SENS_DATA_09: u8 = 0x52;
117 pub const EXT_SENS_DATA_10: u8 = 0x53;
118 pub const EXT_SENS_DATA_11: u8 = 0x54;
119 pub const EXT_SENS_DATA_12: u8 = 0x55;
120 pub const EXT_SENS_DATA_13: u8 = 0x56;
121 pub const EXT_SENS_DATA_14: u8 = 0x57;
122 pub const EXT_SENS_DATA_15: u8 = 0x58;
123 pub const EXT_SENS_DATA_16: u8 = 0x59;
124 pub const EXT_SENS_DATA_17: u8 = 0x5A;
125 pub const EXT_SENS_DATA_18: u8 = 0x5B;
126 pub const EXT_SENS_DATA_19: u8 = 0x5C;
127 pub const EXT_SENS_DATA_20: u8 = 0x5D;
128 pub const EXT_SENS_DATA_21: u8 = 0x5E;
129 pub const EXT_SENS_DATA_22: u8 = 0x5F;
130 pub const EXT_SENS_DATA_23: u8 = 0x60;
131 pub const MOT_DETECT_STATUS: u8 = 0x61;
132 pub const I2C_SLV0_DO: u8 = 0x63;
133 pub const I2C_SLV1_DO: u8 = 0x64;
134 pub const I2C_SLV2_DO: u8 = 0x65;
135 pub const I2C_SLV3_DO: u8 = 0x66;
136 pub const I2C_MST_DELAY_CTRL: u8 = 0x67;
137 pub const SIGNAL_PATH_RESET: u8 = 0x68;
138 pub const MOT_DETECT_CTRL: u8 = 0x69;
139 pub const USER_CTRL: u8 = 0x6A;
140 pub const PWR_MGMT_1: u8 = 0x6B;
141 pub const PWR_MGMT_2: u8 = 0x6C;
142 pub const BANK_SEL: u8 = 0x6D;
143 pub const MEM_START_ADDR: u8 = 0x6E;
144 pub const MEM_R_W: u8 = 0x6F;
145 pub const DMP_CFG_1: u8 = 0x70;
146 pub const DMP_CFG_2: u8 = 0x71;
147 pub const FIFO_COUNTH: u8 = 0x72;
148 pub const FIFO_COUNTL: u8 = 0x73;
149 pub const FIFO_R_W: u8 = 0x74;
150 pub const WHO_AM_I: u8 = 0x75;
151}
152
153pub mod consts {
154 pub const DEV_ID_MPU6050: u8 = 0x68;
155 pub const DEV_ID_MPU6500: u8 = 0x70;
156 pub const DEV_ID_MPU9250: u8 = 0x71;
157 pub const DEV_ID_MPU9255: u8 = 0x73;
158}
159
160#[derive(Debug)]
161pub enum Error<E> {
162 Bus(E),
163 InvalidDevice,
164}
165
166impl<E> From<E> for Error<E> {
167 fn from(e: E) -> Self {
168 Error::Bus(e)
169 }
170}
171
172#[derive(Debug, Clone, Copy, Default)]
175pub enum DlpfCfg {
176 Hz260 = 0,
178 #[default]
180 Hz184 = 1,
181 Hz94 = 2,
183 Hz44 = 3,
185 Hz21 = 4,
187 Hz10 = 5,
189 Hz5 = 6,
191}
192
193#[derive(Debug, Clone, Copy, Default)]
195pub enum GyroRange {
196 Deg250 = 0,
198 Deg500 = 1,
200 #[default]
202 Deg1000 = 2,
203 Deg2000 = 3,
205}
206
207impl GyroRange {
208 #[inline]
209 fn lsb_sensitivity(&self) -> f32 {
210 match *self {
211 GyroRange::Deg250 => 131.0,
212 GyroRange::Deg500 => 65.5,
213 GyroRange::Deg1000 => 32.8,
214 GyroRange::Deg2000 => 16.4,
215 }
216 }
217
218 #[inline]
225 pub const fn config_bits(self) -> u8 {
226 (self as u8) << 3
227 }
228}
229
230#[derive(Debug, Clone, Copy, Default)]
233pub enum AccelRange {
234 #[default]
236 G2 = 0,
237 G4 = 1,
239 G8 = 2,
241 G16 = 3,
243}
244
245impl AccelRange {
246 #[inline]
247 fn lsb_sensitivity(&self) -> f32 {
248 match *self {
249 AccelRange::G2 => 16384.0,
250 AccelRange::G4 => 8192.0,
251 AccelRange::G8 => 4096.0,
252 AccelRange::G16 => 2048.0,
253 }
254 }
255
256 #[inline]
258 pub const fn config_bits(self) -> u8 {
259 (self as u8) << 3
260 }
261}
262
263#[derive(Debug, Default)]
264pub struct Config {
265 pub lpf: DlpfCfg,
266 pub gyro_range: GyroRange,
267 pub accel_range: AccelRange,
268}
269
270pub struct MPU6050<I2C: embedded_hal_async::i2c::I2c> {
271 addr: u8,
272 i2c: I2C,
273 gyro_range: GyroRange,
274 accel_range: AccelRange,
275}
276
277impl<I2C: embedded_hal_async::i2c::I2c> MPU6050<I2C> {
278 pub fn new(i2c: I2C, addr: u8) -> Self {
279 Self {
280 addr,
281 i2c,
282 gyro_range: GyroRange::Deg1000,
283 accel_range: AccelRange::G2,
284 }
285 }
286
287 pub fn new_primary(i2c: I2C) -> Self {
288 Self::new(i2c, PRIMARY_ADDRESS)
289 }
290
291 pub async fn init(&mut self, config: Config, mut delay: impl DelayNs) -> Result<(), Error<I2C::Error>> {
299 let who_am_i = self.read_reg(regs::WHO_AM_I).await?;
300 if who_am_i != consts::DEV_ID_MPU6050
301 && who_am_i != consts::DEV_ID_MPU6500
302 && who_am_i != consts::DEV_ID_MPU9250
303 && who_am_i != consts::DEV_ID_MPU9255
304 {
305 return Err(Error::InvalidDevice);
306 }
307
308 self.write_reg(regs::PWR_MGMT_1, PWR_MGMT_1_CLKSEL_PLL_X_GYRO).await?;
310
311 self.write_reg(regs::CONFIG, config.lpf as u8).await?;
313
314 self.write_reg(regs::GYRO_CONFIG, config.gyro_range.config_bits())
316 .await?;
317
318 self.write_reg(regs::ACCEL_CONFIG, config.accel_range.config_bits())
320 .await?;
321
322 self.gyro_range = config.gyro_range;
323 self.accel_range = config.accel_range;
324
325 delay.delay_ms(WAKE_UP_DELAY_MS).await;
327
328 Ok(())
329 }
330
331 pub async fn read_raw_accel(&mut self) -> Result<(i16, i16, i16), Error<I2C::Error>> {
332 let mut buf = [0u8; 6];
333 self.read_regs(regs::ACCEL_XOUT_H, &mut buf).await?;
334
335 let x = i16::from_be_bytes([buf[0], buf[1]]);
336 let y = i16::from_be_bytes([buf[2], buf[3]]);
337 let z = i16::from_be_bytes([buf[4], buf[5]]);
338
339 Ok((x, y, z))
340 }
341
342 pub async fn read_raw_gyro(&mut self) -> Result<(i16, i16, i16), Error<I2C::Error>> {
343 let mut buf = [0u8; 6];
344 self.read_regs(regs::GYRO_XOUT_H, &mut buf).await?;
345
346 let x = i16::from_be_bytes([buf[0], buf[1]]);
347 let y = i16::from_be_bytes([buf[2], buf[3]]);
348 let z = i16::from_be_bytes([buf[4], buf[5]]);
349
350 Ok((x, y, z))
351 }
352
353 pub async fn read_accel(&mut self) -> Result<(f32, f32, f32), Error<I2C::Error>> {
355 let (x, y, z) = self.read_raw_accel().await?;
356 let lsb_sensitivity = self.accel_range.lsb_sensitivity();
357 Ok((
358 x as f32 / lsb_sensitivity,
359 y as f32 / lsb_sensitivity,
360 z as f32 / lsb_sensitivity,
361 ))
362 }
363
364 pub async fn read_gyro(&mut self) -> Result<(f32, f32, f32), Error<I2C::Error>> {
366 let (x, y, z) = self.read_raw_gyro().await?;
367 let lsb_sensitivity = self.gyro_range.lsb_sensitivity();
368 Ok((
369 x as f32 / lsb_sensitivity,
370 y as f32 / lsb_sensitivity,
371 z as f32 / lsb_sensitivity,
372 ))
373 }
374
375 pub async fn read_temperature(&mut self) -> Result<f32, Error<I2C::Error>> {
377 let mut buf = [0u8; 2];
378 self.read_regs(regs::TEMP_OUT_H, &mut buf).await?;
379
380 let temp = i16::from_be_bytes([buf[0], buf[1]]);
381 Ok((temp as f32) / 340.0 + 36.53)
382 }
383
384 pub async fn read_reg(&mut self, reg: u8) -> Result<u8, Error<I2C::Error>> {
385 let mut buf = [0u8; 1];
386 self.i2c.write_read(self.addr, &[reg], &mut buf).await?;
387 Ok(buf[0])
388 }
389
390 pub async fn read_regs(&mut self, reg: u8, buf: &mut [u8]) -> Result<(), Error<I2C::Error>> {
391 self.i2c.write_read(self.addr, &[reg], buf).await?;
392 Ok(())
393 }
394
395 pub async fn write_reg(&mut self, reg: u8, value: u8) -> Result<(), Error<I2C::Error>> {
396 self.i2c.write(self.addr, &[reg, value]).await?;
397 Ok(())
398 }
399}
400
401#[cfg(test)]
402mod tests {
403 use super::*;
404
405 #[test]
406 fn gyro_range_lands_in_fs_sel() {
407 assert_eq!(GyroRange::Deg250.config_bits(), 0b0000_0000);
410 assert_eq!(GyroRange::Deg500.config_bits(), 0b0000_1000);
411 assert_eq!(GyroRange::Deg1000.config_bits(), 0b0001_0000);
412 assert_eq!(GyroRange::Deg2000.config_bits(), 0b0001_1000);
413
414 for range in [
415 GyroRange::Deg250,
416 GyroRange::Deg500,
417 GyroRange::Deg1000,
418 GyroRange::Deg2000,
419 ] {
420 let fs_sel = (range.config_bits() >> 3) & 0b11;
421 assert_eq!(fs_sel, range as u8, "FS_SEL must encode the requested range");
422 }
423 }
424
425 #[test]
426 fn accel_range_lands_in_afs_sel() {
427 assert_eq!(AccelRange::G2.config_bits(), 0b0000_0000);
428 assert_eq!(AccelRange::G4.config_bits(), 0b0000_1000);
429 assert_eq!(AccelRange::G8.config_bits(), 0b0001_0000);
430 assert_eq!(AccelRange::G16.config_bits(), 0b0001_1000);
431
432 for range in [AccelRange::G2, AccelRange::G4, AccelRange::G8, AccelRange::G16] {
433 let afs_sel = (range.config_bits() >> 3) & 0b11;
434 assert_eq!(afs_sel, range as u8, "AFS_SEL must encode the requested range");
435 }
436 }
437}