1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99

use core::convert::{TryFrom, TryInto};
use embedded_hal::blocking::i2c::WriteRead;

use crate::{ctrl1xl, ctrl2g, AccelValue, GyroValue, Register};

#[repr(u8)]
pub enum SensorTag {
    Empty,
    GyroscopeNC,
    AccelerometerNC,
    Other(u8),
}

impl TryFrom<u8> for SensorTag {
    type Error = ();

    fn try_from(v: u8) -> Result<Self, Self::Error> {
        match v {
            0x00 => Ok(SensorTag::Empty),
            0x01 => Ok(SensorTag::GyroscopeNC),
            0x02 => Ok(SensorTag::AccelerometerNC),
            x if x <= 0x19 => Ok(SensorTag::Other(x)),
            _ => Err(()),
        }
    }
}

#[derive(Copy, Clone, Debug, defmt::Format)]
pub enum Value {
    Empty,
    Gyro(GyroValue),
    Accel(AccelValue),
    Other(u8, [u8; 6]),
}

const ADDR: u8 = 0x78;

pub struct FifoOut {
    pub address: u8,
}

impl Register for FifoOut {}

impl FifoOut {
    pub fn new(address: u8) -> Self {
        FifoOut { address }
    }

    /// Pop a value from the FIFO.
    pub fn pop<I2C>(
        &mut self,
        i2c: &mut I2C,
        gyro_scale: ctrl2g::Fs,
        accel_scale: ctrl1xl::Fs_Xl,
    ) -> Result<Value, I2C::Error>
    where
        I2C: WriteRead,
    {
        let mut out = [0u8; 7];
        i2c.write_read(self.address, &[ADDR], &mut out)?;

        let (tag, out) = out.split_at(1);
        let tag = tag[0] >> 3;
        let out: &[u8; 6] = out.try_into().expect("must be 6!");

        match tag.try_into() {
            Ok(SensorTag::Empty) => Ok(Value::Empty),
            Ok(SensorTag::GyroscopeNC) => Ok(Value::Gyro(GyroValue::from_msr(gyro_scale, out))),
            Ok(SensorTag::AccelerometerNC) => {
                Ok(Value::Accel(AccelValue::from_msr(accel_scale, out)))
            }
            Ok(SensorTag::Other(u)) => Ok(Value::Other(u, *out)),
            _ => unreachable!(),
        }
    }
}

#[cfg(test)]
mod tests {
    use super::*;
    use embedded_hal_mock::i2c::{Mock, Transaction};

    #[test]
    fn test_pop_gyro() {
        let mut i2c = Mock::new(&[Transaction::write_read(
            0x6b,
            vec![0x78],
            vec![0x01 << 3, 0, 1, 0, 2, 0, 4],
        )]);

        let mut f = FifoOut::new(crate::DEFAULT_I2C_ADDRESS);
        let v = f
            .pop(&mut i2c, ctrl2g::Fs::Dps250, ctrl1xl::Fs_Xl::G2)
            .unwrap();

        assert!(matches!(v, Value::Gyro(_)));
        println!("{:?}", v);
    }
}