Skip to main content

mpu6050_driver/
lib.rs

1//! # mpu6050-driver
2//!
3//! `no_std` [embedded-hal](https://docs.rs/embedded-hal) 1.0 driver for the
4//! InvenSense MPU-6050 6-axis IMU.
5//!
6//! ## Quick start
7//!
8//! ```no_run
9//! use mpu6050_driver::{
10//!     AccelRange, Address, Dlpf, GyroRange, Mpu6050, raw_to_imu_sample,
11//! };
12//!
13//! # fn run<I2C>(i2c: I2C) -> Result<(), I2C::Error>
14//! # where
15//! #     I2C: embedded_hal::i2c::I2c,
16//! # {
17//! let mut mpu = Mpu6050::new(i2c, Address::Ad0Low);
18//!
19//! mpu.wake()?;
20//! mpu.set_accel_range(AccelRange::G2)?;
21//! mpu.set_gyro_range(GyroRange::Dps250)?;
22//! mpu.set_dlpf(Dlpf::Cfg2)?;
23//! mpu.set_sample_rate_divider(4)?; // ~200 Hz with Cfg2
24//!
25//! let _identity = mpu.identity()?;
26//! let raw = mpu.read_raw_accel_gyro_temp()?;
27//! let _sample = raw_to_imu_sample(raw);
28//! # Ok(())
29//! # }
30//! ```
31//!
32//! Provide any type that implements [`embedded_hal::i2c::I2c`]. Use
33//! [`Address::Ad0Low`] (`0x68`) or [`Address::Ad0High`] (`0x69`) according to the
34//! AD0 pin wiring.
35//!
36//! ## What this crate covers
37//!
38//! - Power: [`Mpu6050::wake`], [`Mpu6050::reset`]
39//! - Identity: [`Mpu6050::who_am_i`], [`Mpu6050::identity`]
40//! - Ranges / filter / rate: [`Mpu6050::set_accel_range`], [`Mpu6050::set_gyro_range`],
41//!   [`Mpu6050::set_dlpf`], [`Mpu6050::set_sample_rate_divider`]
42//! - Motion reads: [`Mpu6050::read_raw_accel_gyro_temp`], [`Mpu6050::read_raw_checked`],
43//!   [`Mpu6050::read_raw_with_retry`]
44//! - FIFO and interrupts: see the `fifo` and interrupt helpers on [`Mpu6050`]
45//! - Conversion: [`raw_to_imu_sample`], [`RawAccelGyroTemp::temp_degrees_c`]
46//! - Magnitudes: [`ImuSample::accel_magnitude_g`], [`ImuSample::gyro_magnitude_dps`]
47//!
48//! ## Is this module usable?
49//!
50//! This crate does **not** authenticate chips or detect clones. [`Mpu6050::identity`]
51//! only decodes `WHO_AM_I`.
52//!
53//! After bring-up, hold the board still and check
54//! [`ImuSample::accel_magnitude_g`] (near **1 g**) and
55//! [`ImuSample::gyro_magnitude_dps`] (near zero aside from bias/noise). See the
56//! `stationary_usability` example. Large sustained errors usually mean wiring,
57//! power, address, or configuration problems — not a branding verdict.
58//!
59//! Multi-sample threshold reports live in the
60//! [mpu6050-validation-workspace](https://github.com/naoNao89/mpu6050-validation-workspace)
61//! (`imu-tool`), not in this published crate.
62
63#![no_std]
64
65use embedded_hal::i2c::I2c;
66mod config;
67mod fifo;
68mod interrupt;
69mod power;
70mod raw;
71mod registers;
72mod self_test;
73
74pub use config::{AccelRange, Address, Dlpf, DlpfReadError, GyroRange, Identity};
75pub use fifo::{FIFO_ACCEL_GYRO_FRAME_BYTES, FifoReadDiagnostics};
76pub use interrupt::{IntStatus, InterruptEnable};
77pub use raw::{
78    ACCEL_LSB_PER_G_2G, GYRO_LSB_PER_DPS_250DPS, ImuSample, RawAccelGyroTemp, RawReadOutcome,
79    RawRetryPolicy, RawSampleSuspicion, TEMP_LSB_PER_DEG_C, TEMP_OFFSET_DEG_C, raw_to_imu_sample,
80};
81
82/// Blocking MPU-6050 driver over an [`embedded_hal::i2c::I2c`] bus.
83///
84/// Construct with [`Mpu6050::new`], call [`Mpu6050::wake`] after power-up, then
85/// configure ranges/filter and read samples.
86pub struct Mpu6050<I2C> {
87    i2c: I2C,
88    address: Address,
89}
90
91impl<I2C> Mpu6050<I2C> {
92    /// Creates a driver handle. Does not touch the bus.
93    pub const fn new(i2c: I2C, address: Address) -> Self {
94        Self { i2c, address }
95    }
96
97    /// Consumes the driver and returns the underlying I2C peripheral.
98    pub fn release(self) -> I2C {
99        self.i2c
100    }
101}
102
103impl<I2C> Mpu6050<I2C>
104where
105    I2C: I2c,
106{
107    pub(crate) fn read_register(&mut self, register: u8) -> Result<u8, I2C::Error> {
108        let mut value = [0_u8];
109        self.i2c
110            .write_read(self.address.as_u8(), &[register], &mut value)?;
111        Ok(value[0])
112    }
113
114    pub(crate) fn write_register(&mut self, register: u8, value: u8) -> Result<(), I2C::Error> {
115        self.i2c.write(self.address.as_u8(), &[register, value])
116    }
117
118    pub(crate) fn write_masked(
119        &mut self,
120        register: u8,
121        mask: u8,
122        value: u8,
123    ) -> Result<(), I2C::Error> {
124        let current = self.read_register(register)?;
125        self.write_register(register, (current & !mask) | (value & mask))
126    }
127}
128
129#[cfg(test)]
130extern crate std;
131
132#[cfg(test)]
133mod tests {
134    use super::*;
135    use crate::registers::INT_STATUS_FIFO_OFLOW;
136    use embedded_hal::i2c::{ErrorType, Operation, SevenBitAddress};
137    use std::collections::VecDeque;
138    use std::vec::Vec;
139
140    const CLEAN_RAW: RawAccelGyroTemp = RawAccelGyroTemp::new([1, 2, 3], 4, [5, 6, 7]);
141    const SUSPICIOUS_RAW: RawAccelGyroTemp = RawAccelGyroTemp::new([i16::MAX, 2, 3], 4, [5, 6, 7]);
142    const SUSPICIOUS_RETRY_RAW: RawAccelGyroTemp =
143        RawAccelGyroTemp::new([1, 2, 3], 4, [-1, -1, -1]);
144    const OBSERVED_POWER_OF_TWO_MINUS_ONE_RAW: RawAccelGyroTemp =
145        RawAccelGyroTemp::new([1, 2, 3], 4, [16_383, -1, -1]);
146    const OBSERVED_PARTIAL_MINUS_ONE_RAW: RawAccelGyroTemp =
147        RawAccelGyroTemp::new([1, 2, 3], 4, [704, 8_191, -1]);
148
149    #[derive(Clone, Copy, Debug, Eq, PartialEq)]
150    enum FakeError {
151        Bus,
152    }
153
154    impl embedded_hal::i2c::Error for FakeError {
155        fn kind(&self) -> embedded_hal::i2c::ErrorKind {
156            embedded_hal::i2c::ErrorKind::Other
157        }
158    }
159
160    enum FakeResponse {
161        Raw(RawAccelGyroTemp),
162        Error(FakeError),
163    }
164
165    struct FakeI2c {
166        responses: VecDeque<FakeResponse>,
167        write_read_count: usize,
168    }
169
170    impl FakeI2c {
171        fn new(responses: Vec<FakeResponse>) -> Self {
172            Self {
173                responses: responses.into(),
174                write_read_count: 0,
175            }
176        }
177    }
178
179    impl ErrorType for FakeI2c {
180        type Error = FakeError;
181    }
182
183    impl I2c for FakeI2c {
184        fn read(&mut self, _address: SevenBitAddress, _read: &mut [u8]) -> Result<(), Self::Error> {
185            Ok(())
186        }
187
188        fn write(&mut self, _address: SevenBitAddress, _write: &[u8]) -> Result<(), Self::Error> {
189            Ok(())
190        }
191
192        fn write_read(
193            &mut self,
194            _address: SevenBitAddress,
195            write: &[u8],
196            read: &mut [u8],
197        ) -> Result<(), Self::Error> {
198            assert_eq!(write, &[registers::ACCEL_XOUT_H]);
199            assert_eq!(read.len(), 14);
200            self.write_read_count += 1;
201            match self.responses.pop_front().expect("missing fake response") {
202                FakeResponse::Raw(raw) => {
203                    let values = [
204                        raw.accel[0],
205                        raw.accel[1],
206                        raw.accel[2],
207                        raw.temp,
208                        raw.gyro[0],
209                        raw.gyro[1],
210                        raw.gyro[2],
211                    ];
212                    for (chunk, value) in read.chunks_exact_mut(2).zip(values) {
213                        chunk.copy_from_slice(&value.to_be_bytes());
214                    }
215                    Ok(())
216                }
217                FakeResponse::Error(error) => Err(error),
218            }
219        }
220
221        fn transaction(
222            &mut self,
223            _address: SevenBitAddress,
224            _operations: &mut [Operation<'_>],
225        ) -> Result<(), Self::Error> {
226            Ok(())
227        }
228    }
229
230    struct FifoFakeI2c {
231        fifo_bytes: VecDeque<u8>,
232        fifo_rw_calls: usize,
233    }
234
235    impl FifoFakeI2c {
236        fn new(fifo_bytes: Vec<u8>) -> Self {
237            Self {
238                fifo_bytes: fifo_bytes.into(),
239                fifo_rw_calls: 0,
240            }
241        }
242    }
243
244    impl ErrorType for FifoFakeI2c {
245        type Error = FakeError;
246    }
247
248    impl I2c for FifoFakeI2c {
249        fn read(&mut self, _address: SevenBitAddress, _read: &mut [u8]) -> Result<(), Self::Error> {
250            Ok(())
251        }
252
253        fn write(&mut self, _address: SevenBitAddress, _write: &[u8]) -> Result<(), Self::Error> {
254            Ok(())
255        }
256
257        fn write_read(
258            &mut self,
259            _address: SevenBitAddress,
260            write: &[u8],
261            read: &mut [u8],
262        ) -> Result<(), Self::Error> {
263            assert_eq!(write, &[registers::FIFO_R_W]);
264            self.fifo_rw_calls += 1;
265            for byte in read {
266                *byte = self.fifo_bytes.pop_front().expect("missing FIFO byte");
267            }
268            Ok(())
269        }
270
271        fn transaction(
272            &mut self,
273            _address: SevenBitAddress,
274            _operations: &mut [Operation<'_>],
275        ) -> Result<(), Self::Error> {
276            Ok(())
277        }
278    }
279
280    struct FifoDiagnosticFakeI2c {
281        queue: VecDeque<(u8, Result<Vec<u8>, FakeError>)>,
282        fifo_rw_calls: usize,
283    }
284
285    impl FifoDiagnosticFakeI2c {
286        fn new(queue: Vec<(u8, Vec<u8>)>) -> Self {
287            Self::with_results(
288                queue
289                    .into_iter()
290                    .map(|(reg, data)| (reg, Ok(data)))
291                    .collect(),
292            )
293        }
294
295        fn with_results(queue: Vec<(u8, Result<Vec<u8>, FakeError>)>) -> Self {
296            Self {
297                queue: queue.into(),
298                fifo_rw_calls: 0,
299            }
300        }
301    }
302
303    impl ErrorType for FifoDiagnosticFakeI2c {
304        type Error = FakeError;
305    }
306
307    impl I2c for FifoDiagnosticFakeI2c {
308        fn read(&mut self, _address: SevenBitAddress, _read: &mut [u8]) -> Result<(), Self::Error> {
309            Ok(())
310        }
311
312        fn write(&mut self, _address: SevenBitAddress, _write: &[u8]) -> Result<(), Self::Error> {
313            Ok(())
314        }
315
316        fn write_read(
317            &mut self,
318            _address: SevenBitAddress,
319            write: &[u8],
320            read: &mut [u8],
321        ) -> Result<(), Self::Error> {
322            let (register, data) = self.queue.pop_front().expect("missing diagnostic response");
323            assert_eq!(write, &[register]);
324            let data = data?;
325            if register == registers::FIFO_R_W {
326                self.fifo_rw_calls += 1;
327            }
328            read.copy_from_slice(&data[..read.len()]);
329            Ok(())
330        }
331
332        fn transaction(
333            &mut self,
334            _address: SevenBitAddress,
335            _operations: &mut [Operation<'_>],
336        ) -> Result<(), Self::Error> {
337            Ok(())
338        }
339    }
340
341    #[test]
342    fn address_values_match_ad0_pin_state() {
343        assert_eq!(Address::Ad0Low.as_u8(), 0x68);
344        assert_eq!(Address::Ad0High.as_u8(), 0x69);
345    }
346
347    #[test]
348    fn fifo_burst_read_uses_single_transaction() {
349        const FIFO_TEST_FILL_BYTE: u8 = 0xA5;
350
351        let fake = FifoFakeI2c::new(std::vec![
352            FIFO_TEST_FILL_BYTE;
353            FIFO_ACCEL_GYRO_FRAME_BYTES
354        ]);
355        let mut mpu = Mpu6050::new(fake, Address::Ad0Low);
356        let mut buf = [0_u8; FIFO_ACCEL_GYRO_FRAME_BYTES];
357
358        mpu.read_fifo_bytes(&mut buf).unwrap();
359
360        assert_eq!(buf, [FIFO_TEST_FILL_BYTE; FIFO_ACCEL_GYRO_FRAME_BYTES]);
361        assert_eq!(mpu.release().fifo_rw_calls, 1);
362    }
363
364    #[test]
365    fn fifo_zero_length_read_uses_no_transaction() {
366        let fake = FifoFakeI2c::new(std::vec![]);
367        let mut mpu = Mpu6050::new(fake, Address::Ad0Low);
368        let mut buf = [];
369
370        mpu.read_fifo_bytes(&mut buf).unwrap();
371
372        assert_eq!(mpu.release().fifo_rw_calls, 0);
373    }
374
375    #[test]
376    fn fifo_diagnostics_reports_fields_and_helpers() {
377        const FIFO_TWO_FRAMES: u16 = (FIFO_ACCEL_GYRO_FRAME_BYTES as u16) * 2;
378        const FIFO_ONE_FRAME: u16 = FIFO_ACCEL_GYRO_FRAME_BYTES as u16;
379
380        let fake = FifoDiagnosticFakeI2c::new(std::vec![
381            (
382                registers::FIFO_COUNTH,
383                FIFO_TWO_FRAMES.to_be_bytes().to_vec()
384            ),
385            (registers::INT_STATUS, std::vec![0]),
386            (
387                registers::FIFO_R_W,
388                std::vec![0x5A; FIFO_ACCEL_GYRO_FRAME_BYTES]
389            ),
390            (
391                registers::FIFO_COUNTH,
392                FIFO_ONE_FRAME.to_be_bytes().to_vec()
393            ),
394        ]);
395        let mut mpu = Mpu6050::new(fake, Address::Ad0Low);
396        let mut buf = [0_u8; FIFO_ACCEL_GYRO_FRAME_BYTES];
397
398        let diagnostics = mpu.read_fifo_bytes_with_diagnostics(&mut buf).unwrap();
399
400        assert_eq!(diagnostics.fifo_count_before_bytes, FIFO_TWO_FRAMES);
401        assert_eq!(diagnostics.fifo_bytes_requested, FIFO_ONE_FRAME);
402        assert_eq!(diagnostics.fifo_count_after_bytes, FIFO_ONE_FRAME);
403        assert!(!diagnostics.fifo_overflow_seen);
404        assert!(diagnostics.int_status_read_ok);
405        assert!(diagnostics.read_len_frame_aligned);
406        assert!(diagnostics.fifo_count_before_frame_aligned);
407        assert!(diagnostics.fifo_count_after_frame_aligned);
408        assert!(diagnostics.had_requested_bytes_before_read);
409        assert!(diagnostics.fifo_count_delta_ok);
410        assert!(diagnostics.frame_usable());
411        assert!(!diagnostics.should_reset_fifo());
412        assert_eq!(mpu.release().fifo_rw_calls, 1);
413    }
414
415    #[test]
416    fn fifo_diagnostics_helpers_flag_overflow_and_misalignment() {
417        let overflow = FifoReadDiagnostics {
418            fifo_count_before_bytes: 24,
419            fifo_bytes_requested: 12,
420            fifo_count_after_bytes: 12,
421            fifo_overflow_seen: true,
422            int_status_read_ok: true,
423            read_len_frame_aligned: true,
424            fifo_count_before_frame_aligned: true,
425            fifo_count_after_frame_aligned: true,
426            had_requested_bytes_before_read: true,
427            fifo_count_delta_ok: true,
428        };
429        assert!(!overflow.frame_usable());
430        assert!(overflow.should_reset_fifo());
431
432        let misaligned_refill = FifoReadDiagnostics {
433            fifo_count_before_bytes: 13,
434            fifo_bytes_requested: 12,
435            fifo_count_after_bytes: 12,
436            fifo_overflow_seen: false,
437            int_status_read_ok: false,
438            read_len_frame_aligned: true,
439            fifo_count_before_frame_aligned: false,
440            fifo_count_after_frame_aligned: true,
441            had_requested_bytes_before_read: true,
442            fifo_count_delta_ok: false,
443        };
444        assert!(!misaligned_refill.frame_usable());
445        assert!(misaligned_refill.should_reset_fifo());
446    }
447
448    fn run_fifo_diagnostics(
449        before: u16,
450        int_status: Result<u8, FakeError>,
451        read_len: usize,
452        after: u16,
453    ) -> FifoReadDiagnostics {
454        let mut queue = std::vec![
455            (registers::FIFO_COUNTH, Ok(before.to_be_bytes().to_vec())),
456            (
457                registers::INT_STATUS,
458                int_status.map(|value| std::vec![value])
459            ),
460        ];
461        if read_len > 0 {
462            queue.push((registers::FIFO_R_W, Ok(std::vec![0x5A; read_len])));
463        }
464        queue.push((registers::FIFO_COUNTH, Ok(after.to_be_bytes().to_vec())));
465        let fake = FifoDiagnosticFakeI2c::with_results(queue);
466        let mut mpu = Mpu6050::new(fake, Address::Ad0Low);
467        let mut buf = std::vec![0_u8; read_len];
468        mpu.read_fifo_bytes_with_diagnostics(&mut buf).unwrap()
469    }
470
471    #[test]
472    fn fifo_diagnostics_zero_before_requested_frame_is_not_usable() {
473        let diagnostics = run_fifo_diagnostics(0, Ok(0), FIFO_ACCEL_GYRO_FRAME_BYTES, 0);
474        assert!(!diagnostics.frame_usable());
475        assert!(!diagnostics.had_requested_bytes_before_read);
476        assert!(!diagnostics.fifo_count_delta_ok);
477    }
478
479    #[test]
480    fn fifo_diagnostics_underflow_requested_two_frames_is_not_usable() {
481        let diagnostics = run_fifo_diagnostics(12, Ok(0), FIFO_ACCEL_GYRO_FRAME_BYTES * 2, 0);
482        assert!(!diagnostics.frame_usable());
483        assert!(!diagnostics.had_requested_bytes_before_read);
484        assert!(!diagnostics.fifo_count_delta_ok);
485    }
486
487    #[test]
488    fn fifo_diagnostics_unaligned_read_length_is_not_usable() {
489        let diagnostics = run_fifo_diagnostics(24, Ok(0), FIFO_ACCEL_GYRO_FRAME_BYTES / 2, 18);
490        assert!(!diagnostics.read_len_frame_aligned);
491        assert!(!diagnostics.frame_usable());
492    }
493
494    #[test]
495    fn fifo_diagnostics_zero_length_is_not_usable() {
496        let diagnostics = run_fifo_diagnostics(0, Ok(0), 0, 0);
497        assert!(diagnostics.read_len_frame_aligned);
498        assert_eq!(diagnostics.fifo_bytes_requested, 0);
499        assert!(!diagnostics.frame_usable());
500    }
501
502    #[test]
503    fn fifo_diagnostics_int_status_error_is_best_effort() {
504        let diagnostics =
505            run_fifo_diagnostics(12, Err(FakeError::Bus), FIFO_ACCEL_GYRO_FRAME_BYTES, 0);
506        assert!(!diagnostics.int_status_read_ok);
507        assert!(!diagnostics.fifo_overflow_seen);
508    }
509
510    #[test]
511    fn fifo_diagnostics_overflow_bit_is_not_usable() {
512        let diagnostics = run_fifo_diagnostics(
513            12,
514            Ok(INT_STATUS_FIFO_OFLOW),
515            FIFO_ACCEL_GYRO_FRAME_BYTES,
516            0,
517        );
518        assert!(diagnostics.fifo_overflow_seen);
519        assert!(!diagnostics.frame_usable());
520    }
521
522    #[test]
523    fn imu_sample_magnitudes_match_vector_norm() {
524        let sample = ImuSample::from_g_dps([3.0, 4.0, 0.0], [0.0, 5.0, 12.0]);
525        assert!((sample.accel_magnitude_g() - 5.0).abs() < 1e-12);
526        assert!((sample.gyro_magnitude_dps() - 13.0).abs() < 1e-12);
527    }
528
529    #[test]
530    fn raw_values_convert_to_default_accel_and_gyro_units() {
531        let raw = RawAccelGyroTemp::new([16_384, -16_384, 8_192], 0, [131, -131, 65]);
532        let sample = raw.to_imu_sample();
533        assert_eq!(sample.accel_g, [1.0, -1.0, 0.5]);
534        assert_eq!(sample.gyro_dps[0], 1.0);
535        assert_eq!(sample.gyro_dps[1], -1.0);
536        assert!((sample.gyro_dps[2] - (65.0 / 131.0)).abs() < f64::EPSILON);
537        assert_eq!(sample.timestamp_s, None);
538        assert_eq!(sample.sequence, None);
539    }
540
541    #[test]
542    fn raw_temperature_converts_to_degrees_celsius() {
543        let raw = RawAccelGyroTemp::new([0; 3], 340, [0; 3]);
544        assert!((raw.temp_degrees_c() - 37.53).abs() < f64::EPSILON);
545    }
546
547    #[test]
548    fn regression_raw_sample_flags_gyro_all_minus_one_as_suspicious() {
549        let raw = RawAccelGyroTemp::new([1, 2, 3], 25, [-1, -1, -1]);
550        assert!(raw.is_suspicious());
551    }
552
553    #[test]
554    fn regression_raw_sample_flags_observed_gyro_power_of_two_minus_one_as_suspicious() {
555        let raw = RawAccelGyroTemp::new([-6428, -10508, -9212], 4096, [16_383, -1, -1]);
556        assert!(raw.is_suspicious());
557        assert_eq!(
558            RawSampleSuspicion::classify(raw),
559            Some(RawSampleSuspicion::GyroPowerOfTwoMinusOne)
560        );
561    }
562
563    #[test]
564    fn regression_raw_sample_flags_observed_partial_minus_one_gyro_as_suspicious() {
565        let raw = RawAccelGyroTemp::new([-6368, -10576, -9228], 4144, [704, 8191, -1]);
566        assert!(raw.is_suspicious());
567        assert_eq!(
568            RawSampleSuspicion::classify(raw),
569            Some(RawSampleSuspicion::GyroPowerOfTwoMinusOne)
570        );
571    }
572
573    #[test]
574    fn regression_raw_sample_flags_partial_minus_one_gyro_without_power_sentinel() {
575        let raw = RawAccelGyroTemp::new([1, 2, 3], 4, [700, -1, -320]);
576        assert!(raw.is_suspicious());
577        assert_eq!(
578            RawSampleSuspicion::classify(raw),
579            Some(RawSampleSuspicion::GyroPartialMinusOne)
580        );
581    }
582
583    #[test]
584    fn regression_raw_sample_flags_i16_sentinels_as_suspicious() {
585        let raw = RawAccelGyroTemp::new([i16::MAX, 2, 3], 25, [4, i16::MIN, 6]);
586        assert!(raw.is_suspicious());
587    }
588
589    #[test]
590    fn regression_raw_sample_accepts_nominal_values() {
591        let raw = RawAccelGyroTemp::new([-6500, -9900, -9600], 3700, [720, 190, -320]);
592        assert!(!raw.is_suspicious());
593    }
594
595    #[test]
596    fn primitive_read_performs_one_transaction() {
597        let fake = FakeI2c::new(std::vec![FakeResponse::Raw(CLEAN_RAW)]);
598        let mut mpu = Mpu6050::new(fake, Address::Ad0Low);
599        assert_eq!(mpu.read_raw_accel_gyro_temp(), Ok(CLEAN_RAW));
600        assert_eq!(mpu.release().write_read_count, 1);
601    }
602
603    #[test]
604    fn clean_checked_read_performs_one_transaction_and_returns_clean() {
605        let fake = FakeI2c::new(std::vec![FakeResponse::Raw(CLEAN_RAW)]);
606        let mut mpu = Mpu6050::new(fake, Address::Ad0Low);
607        assert_eq!(
608            mpu.read_raw_checked(),
609            Ok(RawReadOutcome::Clean { raw: CLEAN_RAW })
610        );
611        assert_eq!(mpu.release().write_read_count, 1);
612    }
613
614    #[test]
615    fn suspicious_checked_read_with_zero_retries_rejects_after_one_transaction() {
616        let fake = FakeI2c::new(std::vec![FakeResponse::Raw(SUSPICIOUS_RAW)]);
617        let mut mpu = Mpu6050::new(fake, Address::Ad0Low);
618        assert_eq!(
619            mpu.read_raw_checked(),
620            Ok(RawReadOutcome::RejectedSuspicious {
621                raw: SUSPICIOUS_RAW,
622                suspicion: RawSampleSuspicion::AccelSentinel,
623                retries: 0,
624            })
625        );
626        assert_eq!(mpu.release().write_read_count, 1);
627    }
628
629    #[test]
630    fn suspicious_then_clean_returns_recovered_after_two_transactions() {
631        let fake = FakeI2c::new(std::vec![
632            FakeResponse::Raw(SUSPICIOUS_RAW),
633            FakeResponse::Raw(CLEAN_RAW),
634        ]);
635        let mut mpu = Mpu6050::new(fake, Address::Ad0Low);
636        assert_eq!(
637            mpu.read_raw_with_retry(RawRetryPolicy::reject_after_retries(1)),
638            Ok(RawReadOutcome::Recovered {
639                raw: CLEAN_RAW,
640                first_suspicion: RawSampleSuspicion::AccelSentinel,
641                retries: 1,
642            })
643        );
644        assert_eq!(mpu.release().write_read_count, 2);
645    }
646
647    #[test]
648    fn observed_power_sentinel_then_clean_returns_recovered_after_two_transactions() {
649        let fake = FakeI2c::new(std::vec![
650            FakeResponse::Raw(OBSERVED_POWER_OF_TWO_MINUS_ONE_RAW),
651            FakeResponse::Raw(CLEAN_RAW),
652        ]);
653        let mut mpu = Mpu6050::new(fake, Address::Ad0Low);
654        assert_eq!(
655            mpu.read_raw_with_retry(RawRetryPolicy::reject_after_retries(1)),
656            Ok(RawReadOutcome::Recovered {
657                raw: CLEAN_RAW,
658                first_suspicion: RawSampleSuspicion::GyroPowerOfTwoMinusOne,
659                retries: 1,
660            })
661        );
662        assert_eq!(mpu.release().write_read_count, 2);
663    }
664
665    #[test]
666    fn suspicious_then_suspicious_returns_rejected_suspicious() {
667        let fake = FakeI2c::new(std::vec![
668            FakeResponse::Raw(SUSPICIOUS_RAW),
669            FakeResponse::Raw(SUSPICIOUS_RETRY_RAW),
670        ]);
671        let mut mpu = Mpu6050::new(fake, Address::Ad0Low);
672        assert_eq!(
673            mpu.read_raw_with_retry(RawRetryPolicy::reject_after_retries(1)),
674            Ok(RawReadOutcome::RejectedSuspicious {
675                raw: SUSPICIOUS_RETRY_RAW,
676                suspicion: RawSampleSuspicion::GyroAllMinusOne,
677                retries: 1,
678            })
679        );
680    }
681
682    #[test]
683    fn observed_outlier_then_outlier_returns_rejected_suspicious() {
684        let fake = FakeI2c::new(std::vec![
685            FakeResponse::Raw(OBSERVED_POWER_OF_TWO_MINUS_ONE_RAW),
686            FakeResponse::Raw(OBSERVED_PARTIAL_MINUS_ONE_RAW),
687        ]);
688        let mut mpu = Mpu6050::new(fake, Address::Ad0Low);
689        assert_eq!(
690            mpu.read_raw_with_retry(RawRetryPolicy::reject_after_retries(1)),
691            Ok(RawReadOutcome::RejectedSuspicious {
692                raw: OBSERVED_PARTIAL_MINUS_ONE_RAW,
693                suspicion: RawSampleSuspicion::GyroPowerOfTwoMinusOne,
694                retries: 1,
695            })
696        );
697    }
698
699    #[test]
700    fn suspicious_then_bus_error_returns_retry_error_preserving_first_raw_and_suspicion() {
701        let fake = FakeI2c::new(std::vec![
702            FakeResponse::Raw(SUSPICIOUS_RAW),
703            FakeResponse::Error(FakeError::Bus),
704        ]);
705        let mut mpu = Mpu6050::new(fake, Address::Ad0Low);
706        assert_eq!(
707            mpu.read_raw_with_retry(RawRetryPolicy::reject_after_retries(1)),
708            Ok(RawReadOutcome::RetryError {
709                first_raw: SUSPICIOUS_RAW,
710                first_suspicion: RawSampleSuspicion::AccelSentinel,
711                retries: 1,
712                error: FakeError::Bus,
713            })
714        );
715    }
716
717    #[test]
718    fn accept_policy_returns_accepted_suspicious_after_retries_exhausted() {
719        let fake = FakeI2c::new(std::vec![
720            FakeResponse::Raw(SUSPICIOUS_RAW),
721            FakeResponse::Raw(SUSPICIOUS_RETRY_RAW),
722        ]);
723        let mut mpu = Mpu6050::new(fake, Address::Ad0Low);
724        assert_eq!(
725            mpu.read_raw_with_retry(RawRetryPolicy::accept_after_retries(1)),
726            Ok(RawReadOutcome::AcceptedSuspicious {
727                raw: SUSPICIOUS_RETRY_RAW,
728                suspicion: RawSampleSuspicion::GyroAllMinusOne,
729                retries: 1,
730            })
731        );
732    }
733}