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
use embedded_hal as hal;
use hal::digital::v2::OutputPin;
use super::SensorInterface;
use crate::Error;
#[cfg(feature = "rttdebug")]
use panic_rtt_core::rprintln;
pub struct SpiInterface<SPI, CSN> {
spi: SPI,
csn: CSN,
}
impl<SPI, CSN, CommE, PinE> SpiInterface<SPI, CSN>
where
SPI: hal::blocking::spi::Write<u8, Error = CommE>
+ hal::blocking::spi::Transfer<u8, Error = CommE>,
CSN: OutputPin<Error = PinE>,
{
const DIR_READ: u8 = 0x80;
pub fn new(spi: SPI, csn: CSN) -> Self {
let mut inst = Self { spi: spi, csn: csn };
let _ = inst.csn.set_high();
inst
}
pub fn release(self) -> (SPI, CSN) {
(self.spi, self.csn)
}
fn read_block(&mut self, reg: u8, buffer: &mut [u8]) -> Result<(), Error<CommE, PinE>> {
buffer[0] = reg | Self::DIR_READ;
self.csn.set_low().map_err(Error::Pin)?;
let rc = self.spi.transfer(buffer);
self.csn.set_high().map_err(Error::Pin)?;
rc.map_err(Error::Comm)?;
Ok(())
}
fn write_block(&mut self, block: &[u8]) -> Result<(), Error<CommE, PinE>> {
#[cfg(feature = "rttdebug")]
rprintln!("write {:x?} ", block);
self.csn.set_low().map_err(Error::Pin)?;
let rc = self.spi.write(block);
self.csn.set_high().map_err(Error::Pin)?;
rc.map_err(Error::Comm)?;
Ok(())
}
}
impl<SPI, CSN, CommE, PinE> SensorInterface for SpiInterface<SPI, CSN>
where
SPI: hal::blocking::spi::Write<u8, Error = CommE>
+ hal::blocking::spi::Transfer<u8, Error = CommE>,
CSN: OutputPin<Error = PinE>,
{
type InterfaceError = Error<CommE, PinE>;
fn read_vec3_i16(&mut self, reg: u8) -> Result<[i16; 3], Self::InterfaceError> {
let mut block: [u8; 7] = [0; 7];
self.read_block(reg, &mut block)?;
Ok([
(block[1] as i16) << 8 | (block[2] as i16),
(block[3] as i16) << 8 | (block[4] as i16),
(block[5] as i16) << 8 | (block[6] as i16),
])
}
fn register_read(&mut self, reg: u8) -> Result<u8, Self::InterfaceError> {
let mut block: [u8; 2] = [0; 2];
self.read_block(reg, &mut block)?;
#[cfg(feature = "rttdebug")]
rprintln!("read reg 0x{:x} {:x?} ", reg, block[1]);
Ok(block[1])
}
fn register_write(&mut self, reg: u8, val: u8) -> Result<(), Self::InterfaceError> {
let block: [u8; 2] = [reg, val];
self.write_block(&block)?;
Ok(())
}
fn using_spi(&self) -> bool {
true
}
}