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
136
137
138
139
140
141
142
143
144
145
146
#![feature(alloc_system)]
extern crate alloc_system;
extern crate tessel;
use std::io;
#[repr(u8)]
pub enum ScaleRange {
Scale2G = 0x00,
Scale4G = 0x01,
Scale8G = 0x10,
}
#[repr(u8)]
#[allow(dead_code)]
enum Command {
OutXMsb = 0x01,
XyzDataCfg = 0x0E,
WhoAmI = 0x0D,
CtrlReg1 = 0x2A,
CtrlReg4 = 0x2D,
}
#[repr(u8)]
#[allow(non_camel_case_types)]
pub enum SampleRate {
Rate800 = 0,
Rate400 = 1,
Rate200 = 2,
Rate100 = 3,
Rate50 = 4,
Rate12_5 = 5,
Rate6_25 = 6,
Rate1_56 = 7,
}
const I2C_ID: u8 = 0x1d;
#[allow(dead_code)]
pub struct Accelerometer<'a> {
i2c: tessel::I2cPort<'a>,
i1: tessel::Pin<'a>,
i2: tessel::Pin<'a>,
}
impl<'a> Accelerometer<'a> {
pub fn new<'b>(port: &'b mut tessel::Port) -> Accelerometer<'b> {
let (i2c, gpio) = port.i2c();
let (i1, i2) = gpio.pin_select((5, 6));
Accelerometer {
i2c: i2c,
i1: i1,
i2: i2,
}
}
fn read_register(&mut self, cmd: Command) -> io::Result<u8> {
let mut xr: [u8; 1] = [0; 1];
try!(self.read_registers(cmd, &mut xr));
Ok(xr[0])
}
fn read_registers(&mut self, cmd: Command, buf: &mut [u8]) -> io::Result<()> {
try!(self.i2c.transfer(I2C_ID, &[cmd as u8], buf));
Ok(())
}
fn write_register(&mut self, cmd: Command, value: u8) -> io::Result<()> {
self.i2c.send(I2C_ID, &[cmd as u8, value]);
Ok(())
}
pub fn connect(&mut self) -> io::Result<()> {
if try!(self.read_register(Command::WhoAmI)) != 0x2A {
return Err(io::Error::new(io::ErrorKind::InvalidData, "Invalid connection code."))
}
try!(self.set_scale_range(ScaleRange::Scale2G));
try!(self.set_sample_rate(SampleRate::Rate1_56));
Ok(())
}
fn standby_enable(&mut self) -> io::Result<()> {
let value = try!(self.read_register(Command::CtrlReg1));
self.write_register(Command::CtrlReg1, value & !(0x01u8))
}
fn standby_disable(&mut self) -> io::Result<()> {
let value = try!(self.read_register(Command::CtrlReg1));
self.write_register(Command::CtrlReg1, value | (0x01u8))
}
pub fn set_scale_range(&mut self, range: ScaleRange) -> io::Result<()> {
try!(self.standby_enable());
try!(self.write_register(Command::XyzDataCfg, range as u8));
try!(self.standby_disable());
Ok(())
}
pub fn set_sample_rate(&mut self, rate: SampleRate) -> io::Result<()> {
try!(self.standby_enable());
let mut value = try!(self.read_register(Command::CtrlReg1));
value &= 0b11000111;
try!(self.write_register(Command::CtrlReg1, value | ((rate as u8) << 3)));
try!(self.standby_disable());
Ok(())
}
pub fn read_acceleration(&mut self) -> io::Result<(f64, f64, f64)> {
let mut buf = [0; 6];
try!(self.read_registers(Command::OutXMsb, &mut buf));
let mut out = vec![0.0, 0.0, 0.0];
for (i, win) in buf.chunks(2).enumerate() {
let g = (((win[0] as u16) << 8) | (win[1] as u16)) >> 4;
let dim = if win[0] > 0x7F {
-(1 + 0xFFF - (g as i16))
} else {
g as i16
};
let scale_range = 2.0;
out[i] = (dim as f64) / ((1 << 11) as f64) * scale_range;
}
Ok((out[0], out[1], out[2]))
}
}