1#![no_std]
2
3pub mod bus;
4pub mod measurement;
5#[macro_use]
6pub mod registers;
7
8use embedded_hal::blocking::delay::DelayMs;
9use embedded_hal::spi::{Mode, MODE_3};
10
11use bus::RegAccess;
12pub use measurement::{Acceleration, Gyro, Temperature};
13use registers::*;
14
15pub enum IntPinConfig {
16 IntReadClear = 4,
17}
18
19pub enum Interrupt {
20 DataReady = 0,
21}
22
23pub enum ClockSource {
24 Internal = 0,
25 PLLGyroX = 1,
26 PLLGyroY = 2,
27 PLLGyroZ = 3,
28 PLLExternal32_768KHz = 4,
29 PLLExternal19_2MHz = 5,
30 Stop = 7,
31}
32
33pub const SPI_MODE: Mode = MODE_3;
34
35#[derive(Default)]
36pub struct FifoEnable {
37 pub temperature: bool,
38 pub x_g_force: bool,
39 pub y_g_force: bool,
40 pub z_g_force: bool,
41 pub acceleration: bool,
42 pub slave2: bool,
43 pub slave1: bool,
44 pub slave0: bool,
45}
46
47impl Into<u8> for FifoEnable {
48 fn into(self) -> u8 {
49 (self.temperature as u8) << 7
50 | (self.x_g_force as u8) << 6
51 | (self.y_g_force as u8) << 5
52 | (self.z_g_force as u8) << 4
53 | (self.acceleration as u8) << 3
54 | (self.slave2 as u8) << 2
55 | (self.slave1 as u8) << 1
56 | (self.slave0 as u8) << 0
57 }
58}
59
60pub struct MPU6000<BUS> {
61 bus: BUS,
62 dlpf_enabled: bool,
63 whoami: u8,
64}
65
66impl<E, BUS: RegAccess<Error = E>> MPU6000<BUS> {
67 pub fn new(bus: BUS) -> Self {
68 MPU6000 { bus, dlpf_enabled: false, whoami: 0x68 }
69 }
70
71 pub fn set_register(&mut self, reg: Register, offset: u8, len: u8, bits: u8) -> Result<(), E> {
72 let mut value = self.bus.read(reg)?;
73 let mask = (1u8 << len) - 1;
74 value &= !(mask << offset);
75 value |= (bits & mask) << offset;
76 self.bus.write(reg, value)
77 }
78
79 pub fn set_slave_address(&mut self, address: u8) {
80 self.whoami = address
81 }
82
83 pub fn whoami(&mut self) -> Result<u8, E> {
84 self.bus.read(Register::WhoAmI)
85 }
86
87 pub fn product_id(&mut self) -> Result<u8, E> {
88 self.bus.read(Register::ProductId)
89 }
90
91 pub fn verify(&mut self) -> Result<bool, E> {
92 Ok(self.whoami()? == self.whoami && self.product_id()? != ProductId::Unknown as u8)
93 }
94
95 pub fn reset<D: DelayMs<u8>>(&mut self, delay: &mut D) -> Result<(), E> {
97 let reset_bit = PowerManagement1::DeviceReset as u8;
98 self.bus.write(Register::PowerManagement1, reset_bit)?;
99 delay.delay_ms(150u8.into());
100
101 let value = SignalPathReset::TemperatureReset as u8
102 | SignalPathReset::AccelerometerReset as u8
103 | SignalPathReset::GyroReset as u8;
104 self.bus.write(Register::SignalPathReset, value)?;
105 delay.delay_ms(150u8.into());
106 Ok(())
107 }
108
109 pub fn set_sleep(&mut self, enable: bool) -> Result<(), E> {
110 self.set_register(Register::PowerManagement1, 6, 1, enable as u8)?;
111 Ok(())
112 }
113
114 pub fn set_clock_source(&mut self, source: ClockSource) -> Result<(), E> {
115 self.set_register(Register::PowerManagement1, 0, 3, source as u8)
116 }
117
118 pub fn set_dlpf(&mut self, value: u8) -> Result<(), E> {
119 self.dlpf_enabled = 0 < value && value < 7;
120 self.set_register(Register::Configuration, 0, 3, value as u8)
121 }
122
123 pub fn set_i2c_disable(&mut self, disable: bool) -> Result<(), E> {
124 self.set_register(Register::UserControl, 2, 1, disable as u8)
125 }
126
127 pub fn set_sample_rate(&mut self, rate: u16) -> Result<(), E> {
129 let divider = if !self.dlpf_enabled { 8_000 } else { 1_000 } / rate - 1;
130 self.bus.write(Register::SampleRateDivider, divider as u8)
131 }
132
133 pub fn set_int_pin_config(&mut self, pin_config: IntPinConfig, enable: bool) -> Result<(), E> {
134 self.set_register(Register::IntPinConfig, pin_config as u8, 1, enable as u8)
135 }
136
137 pub fn set_interrupt_enable(&mut self, interrupt: Interrupt, enable: bool) -> Result<(), E> {
138 self.set_register(Register::InterruptEnable, interrupt as u8, 1, enable as u8)
139 }
140
141 pub fn enable_fifo(&mut self, fifo_enable: FifoEnable) -> Result<(), E> {
142 let value: u8 = fifo_enable.into();
143 self.bus.write(Register::FifoEnable, value)
144 }
145
146 pub fn enable_fifo_buffer(&mut self) -> Result<(), E> {
147 let value = self.bus.read(Register::UserControl)?;
148 self.bus.write(Register::UserControl, value | 1 << 6)
149 }
150
151 pub fn get_fifo_counter(&mut self) -> Result<u16, E> {
152 let high = self.bus.read(Register::FifoCountHigh)?;
153 let low = self.bus.read(Register::FifoCountLow)?;
154 return Ok((high as u16) << 8 | low as u16);
155 }
156
157 pub fn set_gyro_sensitive(&mut self, sensitive: GyroSensitive) -> Result<(), E> {
158 self.bus.write(Register::GyroConfig, (sensitive as u8) << 3)
159 }
160
161 pub fn read_acceleration(&mut self) -> Result<Acceleration, E> {
162 let mut buffer = [0u8; 6];
163 self.bus.reads(Register::AccelerometerXHigh, &mut buffer)?;
164 Ok((&buffer).into())
165 }
166
167 pub fn read_gyro(&mut self) -> Result<Gyro, E> {
168 let mut buffer = [0u8; 6];
169 self.bus.reads(Register::GyroXHigh, &mut buffer)?;
170 Ok((&buffer).into())
171 }
172
173 pub fn read_temperature(&mut self) -> Result<Temperature, E> {
174 let mut buffer = [0u8; 2];
175 self.bus.reads(Register::AccelerometerXHigh, &mut buffer)?;
176 Ok((&buffer).into())
177 }
178
179 pub fn read_all(&mut self) -> Result<(Acceleration, Temperature, Gyro), E> {
180 let mut buffer = [0u8; 14];
181 self.bus.reads(Register::AccelerometerXHigh, &mut buffer)?;
182 let mut array = [0i16; 7];
183 for i in 0..7 {
184 array[i] = i16::from_be_bytes([buffer[i * 2], buffer[i * 2 + 1]])
185 }
186 let accel = [array[0], array[1], array[2]];
187 let gyro = [array[4], array[5], array[6]];
188 Ok((Acceleration(accel), Temperature(array[3]), Gyro(gyro)))
189 }
190
191 pub fn set_accelerometer_sensitive(
192 &mut self,
193 sensitive: AccelerometerSensitive,
194 ) -> Result<(), E> {
195 self.bus.write(Register::AccelerometerConfig, (sensitive as u8) << 3)
196 }
197}
198
199impl<BUS> MPU6000<BUS> {
200 pub fn free(self) -> BUS {
201 self.bus
202 }
203}
204
205mod test {
206 use embedded_hal::blocking::delay::{DelayMs, DelayUs};
207 use embedded_hal::blocking::spi::{Transfer, Write};
208 use embedded_hal::digital::v2::OutputPin;
209
210 struct StubSPI {}
211
212 impl Write<u8> for StubSPI {
213 type Error = &'static str;
214 fn write(&mut self, _bytes: &[u8]) -> Result<(), &'static str> {
215 Ok(())
216 }
217 }
218
219 impl Transfer<u8> for StubSPI {
220 type Error = &'static str;
221 fn transfer<'w>(&mut self, bytes: &'w mut [u8]) -> Result<&'w [u8], &'static str> {
222 for b in bytes.iter_mut() {
223 *b = 100;
224 }
225 Ok(bytes)
226 }
227 }
228
229 struct StubOutputPin {}
230 impl OutputPin for StubOutputPin {
231 type Error = &'static str;
232 fn set_high(&mut self) -> Result<(), &'static str> {
233 Ok(())
234 }
235
236 fn set_low(&mut self) -> Result<(), &'static str> {
237 Ok(())
238 }
239 }
240
241 struct Nodelay {}
242
243 impl DelayMs<u8> for Nodelay {
244 fn delay_ms(&mut self, _ms: u8) {}
245 }
246
247 impl DelayUs<u8> for Nodelay {
248 fn delay_us(&mut self, _us: u8) {}
249 }
250
251 #[test]
252 fn test_functional() {
253 extern crate std;
254
255 use crate::bus::SpiBus;
256 use crate::registers::{AccelerometerSensitive, GyroSensitive};
257 use crate::MPU6000;
258
259 let spi_bus = SpiBus::new(StubSPI {}, StubOutputPin {}, Nodelay {});
260 let mut mpu6000 = MPU6000::new(spi_bus);
261 let mut delay = Nodelay {};
262 mpu6000.reset(&mut delay).ok();
263 mpu6000.set_sleep(false).ok();
264 let sensitive = accelerometer_sensitive!(+/-16g, 2048LSB/g);
265 mpu6000.set_accelerometer_sensitive(sensitive).ok();
266 let sensitive = gyro_sensitive!(+/-2000dps, 16.4LSB/dps);
267 mpu6000.set_gyro_sensitive(sensitive).ok();
268 mpu6000.read_all().ok();
269 }
270
271 #[test]
272 fn test_macro() {
273 extern crate std;
274
275 use crate::registers::{AccelerometerSensitive, GyroSensitive};
276 assert_eq!(
277 accelerometer_sensitive!(+/-2g, 16384LSB/g),
278 AccelerometerSensitive::Sensitive16384
279 );
280 assert_eq!(
281 accelerometer_sensitive!(+/-4g, 8192LSB/g),
282 AccelerometerSensitive::Sensitive8192
283 );
284 assert_eq!(gyro_sensitive!(+/-1000dps, 32.8LSB/dps), GyroSensitive::Sensitive32_8);
285 assert_eq!(gyro_sensitive!(+/-2000dps, 16.4LSB/dps), GyroSensitive::Sensitive16_4);
286 }
287}