bno080 0.1.3

BNO080 AHRS driver
Documentation 
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
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135

extern crate std;

use super::PACKET_HEADER_LENGTH;

use core::ops::Shr;
use embedded_hal::blocking::{
    delay::DelayMs,
    i2c::{Read, Write, WriteRead},
};
use std::collections::VecDeque;

struct FakeDelay {}

impl DelayMs<u8> for FakeDelay {
    fn delay_ms(&mut self, _ms: u8) {
        // no-op
    }
}

const MAX_FAKE_PACKET_SIZE: usize = 512;

//divides up packets into segments
pub struct FakePacket {
    pub addr: u8,
    pub len: usize,
    pub buf: [u8; MAX_FAKE_PACKET_SIZE],
}

impl FakePacket {
    pub fn new_from_slice(slice: &[u8]) -> Self {
        let src_len = slice.len();
        let mut inst = Self {
            addr: 0,
            len: src_len,
            buf: [0; MAX_FAKE_PACKET_SIZE],
        };
        inst.buf[..src_len].copy_from_slice(&slice);
        inst
    }
}

pub struct FakeI2cPort {
    pub available_packets: VecDeque<FakePacket>,
    pub sent_packets: VecDeque<FakePacket>,
}

impl FakeI2cPort {
    pub fn new() -> Self {
        FakeI2cPort {
            available_packets: VecDeque::with_capacity(3),
            sent_packets: VecDeque::with_capacity(3),
        }
    }

    /// Enqueue a packet to be received later
    pub fn add_available_packet(&mut self, bytes: &[u8]) {
        let pack = FakePacket::new_from_slice(bytes);
        self.available_packets.push_back(pack);
    }
}

impl Read for FakeI2cPort {
    type Error = ();

    fn read(&mut self, addr: u8, buffer: &mut [u8]) -> Result<(), Self::Error> {
        let next_pack =
            self.available_packets.pop_front().unwrap_or(FakePacket {
                addr: addr,
                len: 0,
                buf: [0; MAX_FAKE_PACKET_SIZE],
            });

        let src_len = next_pack.len;
        if src_len == 0 {
            return Ok(());
        }

        let dest_len = buffer.len();

        if src_len > dest_len {
            //only read as much as the reader has room for,
            //then push the remainder back onto the queue as a remainder packet
            let read_len = dest_len;
            buffer[..read_len].copy_from_slice(&next_pack.buf[..read_len]);
            let remainder_len = src_len - read_len;
            let mut remainder_packet = FakePacket {
                addr: addr,
                len: remainder_len + 4,
                buf: [0; MAX_FAKE_PACKET_SIZE],
            };
            remainder_packet.buf
                [PACKET_HEADER_LENGTH..PACKET_HEADER_LENGTH + remainder_len]
                .copy_from_slice(
                    &next_pack.buf[read_len..read_len + remainder_len],
                );
            remainder_packet.buf[0] = ((remainder_len + 4) & 0xFF) as u8;
            remainder_packet.buf[1] =
                ((((remainder_len + 4) & 0xFF00) as u16).shr(8) as u8) | 0x80; //set continuation flag
            self.available_packets.push_front(remainder_packet);
        } else if src_len == dest_len {
            let read_len = src_len;
            buffer[..read_len].copy_from_slice(&next_pack.buf[..read_len]);
        } else {
            // src_len < dest_len
            panic!("src_len {} dest_len {}", src_len, dest_len);
        }

        Ok(())
    }
}

impl Write for FakeI2cPort {
    type Error = ();

    fn write(&mut self, _addr: u8, _bytes: &[u8]) -> Result<(), Self::Error> {
        let sent_pack = FakePacket::new_from_slice(_bytes);
        self.sent_packets.push_back(sent_pack);
        Ok(())
    }
}

impl WriteRead for FakeI2cPort {
    type Error = ();

    fn write_read(
        &mut self,
        address: u8,
        send_buf: &[u8],
        recv_buf: &mut [u8],
    ) -> Result<(), Self::Error> {
        self.write(address, send_buf)?;
        self.read(address, recv_buf)?;
        Ok(())
    }
}