1#![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
82pub struct Mpu6050<I2C> {
87 i2c: I2C,
88 address: Address,
89}
90
91impl<I2C> Mpu6050<I2C> {
92 pub const fn new(i2c: I2C, address: Address) -> Self {
94 Self { i2c, address }
95 }
96
97 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}