From 717e9e6149e37df41ee2b8ec8db2a98daa0796a9 Mon Sep 17 00:00:00 2001 From: Julian Gaal Date: Mon, 15 Apr 2019 21:14:37 +0200 Subject: [PATCH] correct gyro readings. Acc and gyrp raw working --- README.md | 4 +- src/bin/.linux.rs.swp | Bin 12288 -> 0 bytes src/bin/linux.rs | 9 ++- src/lib.rs | 171 +++++++++++++++++++++++++++++++----------- 4 files changed, 139 insertions(+), 45 deletions(-) delete mode 100644 src/bin/.linux.rs.swp diff --git a/README.md b/README.md index 1164fc3..48f7833 100644 --- a/README.md +++ b/README.md @@ -11,8 +11,8 @@ cross-compile with `cargo build --bin main --target=arm-unknown-linux-gnueabihf` - [ ] init with custom settings - [x] device verification - [ ] basic feature support as described [here](https://github.com/Tijndagamer/mpu6050/blob/master/mpu6050/mpu6050.py) -- [ ] read gyro data -- [ ] read acc data +- [x] read gyro data +- [x] read acc data - [ ] read temp data - [ ] timer/clock control with PWR_MGMT_2 - [ ] internal clock, register 108 `POWER_MGMT_2`, [will cycle between sleep mode and waking up to take a single sample of data from active sensors at a rate determined by LP_WAKE_CTRL](https://www.invensense.com/wp-content/uploads/2015/02/MPU-6000-Register-Map1.pdf) (page 41-43) diff --git a/src/bin/.linux.rs.swp b/src/bin/.linux.rs.swp deleted file mode 100644 index b0fb9dcca118d88597196710f45f78def0bf3326..0000000000000000000000000000000000000000 GIT binary patch literal 0 HcmV?d00001 literal 12288 zcmeI&zi-n(6bJB^?v$#~f$f?uHj3l?pjC7U=>Q!fwL(H_C#1-UFXF(yT8^DkRVn`r zO#Bs0l@L4s0waF{FK2^jh76gA^gZdxvG3i>nI;M(od-(>rM zQ(rD7%IAVdJr0tgN>k?~O)pa^2d(kH-@(Or5kKTc_o95&_Pp!%*lSi@nivrW+BmN8 z`r@^k=26yj+?ue}dZ&vv`a$c3z3cbI4N*Q-f{WU6wozWnds$tZVP}Ra9;q-?;qYw` WNqJN3qE`Hbd68K5-b?wkOTPhwRE}x@ diff --git a/src/bin/linux.rs b/src/bin/linux.rs index cf941dd..49a1678 100644 --- a/src/bin/linux.rs +++ b/src/bin/linux.rs @@ -10,5 +10,12 @@ fn main() -> Result<(), Error> { let mut mpu = Mpu6050::new(i2c, delay); mpu.init()?; - Ok(()) + loop { + match mpu.get_gyro() { + Ok(r) => { + println!("{:?}", r); + }, + Err(_) => {} , + } + } } diff --git a/src/lib.rs b/src/lib.rs index ef7c44e..e42a692 100644 --- a/src/lib.rs +++ b/src/lib.rs @@ -1,4 +1,4 @@ -//#![no_std] +#![no_std] ///! Mpu6050 sensor driver. ///! Datasheet: @@ -7,22 +7,22 @@ use embedded_hal::{ blocking::i2c::{Read, Write, WriteRead}, }; -const Mpu6050_SLAVE_ADDR: u8 = 0x68; -const Mpu6050_WHOAMI: u8 = 0x75; +const MPU6050_SLAVE_ADDR: u8 = 0x68; +const MPU6050_WHOAMI: u8 = 0x75; /// High Bytle Register Gyro x orientation -const Mpu6050_GYRO_REGX_H: u8 = 0x43; +const MPU6050_GYRO_REGX_H: u8 = 0x43; /// High Bytle Register Gyro y orientation -const Mpu6050_GYRO_REGY_H: u8 = 0x45; +const MPU6050_GYRO_REGY_H: u8 = 0x45; /// High Bytle Register Gyro z orientation -const Mpu6050_GYRO_REGZ_H: u8 = 0x47; +const MPU6050_GYRO_REGZ_H: u8 = 0x47; -/// High Byte Register Acc roll -const Mpu6050_ACC_REGX_H: u8 = 0x3b; -/// High Byte Register Acc pitch -const Mpu6050_ACC_REGY_H: u8 = 0x3d; -/// High Byte Register Acc yaw -const Mpu6050_ACC_REGZ_H: u8 = 0x3f; +/// High Byte Register Calc roll +const MPU6050_ACC_REGX_H: u8 = 0x3b; +/// High Byte Register Calc pitch +const MPU6050_ACC_REGY_H: u8 = 0x3d; +/// High Byte Register Calc yaw +const MPU6050_ACC_REGZ_H: u8 = 0x3f; /// Register to control chip waking from sleep, enabling sensors, default: sleep const POWER_MGMT_1: u8 = 0x6b; @@ -30,9 +30,40 @@ const POWER_MGMT_1: u8 = 0x6b; const POWER_MGMT_2: u8 = 0x6c; /// Gyro Sensitivity -const FS_SEL: (f64, f64, f64, f64) = (131., 65.5, 32.8, 16.4); -/// Accelerometer Sensitivity -const AFS_SEL: (f64, f64, f64, f64) = (16384., 8192., 4096., 2048.); +const FS_SEL: (f32, f32, f32, f32) = (131., 65.5, 32.8, 16.4); +/// Calcelerometer Sensitivity +const AFS_SEL: (f32, f32, f32, f32) = (16384., 8192., 4096., 2048.); + +const PI: f32 = 3.14159; + +const GRAVITIY_MS2: f32 = 9.80665; + +const ACCEL_RANGE_2G: u8 = 0x00; +const ACCEL_RANGE_4G: u8 = 0x08; +const ACCEL_RANGE_8G: u8 = 0x10; +const ACCEL_RANGE_16G: u8 = 0x18; + +const GYRO_RANGE_250DEG: u8 = 0x00; +const GYRO_RANGE_500DEG: u8 = 0x08; +const GYRO_RANGE_1000DEG: u8 = 0x10; +const GYRO_RANGE_2000DEG: u8 = 0x18; + +const ACCEL_CONFIG: u8 = 0x1c; +const GYRO_CONFIG: u8 = 0x1b; + +enum AccelRange { + G2, + G4, + G8, + G16, +} + +enum GyroRange { + DEG250, + DEG500, + DEG1000, + DEG2000, +} /// All possible errors in this crate #[derive(Debug)] @@ -44,12 +75,6 @@ pub enum Error { InvalidChipId(u8), } -pub struct Rotation { - x: T, - y: T, - z: T, -} - pub struct Mpu6050 { i2c: I, delay: D @@ -67,43 +92,105 @@ where } } - pub fn init(&mut self) -> Result<(), Error> { - self.write(POWER_MGMT_1, 0)?; - - self.verify()?; + pub fn soft_calibration(steps: u8, ) -> Result<(), Error> { + Ok(()) + } + pub fn wake(&mut self) -> Result<(), Error> { + self.write_u8(POWER_MGMT_1, 0)?; self.delay.delay_ms(100u8); Ok(()) } - pub fn verify(&mut self) -> Result<(), Error> { - let address = self.read(Mpu6050_WHOAMI)?; - if address != Mpu6050_SLAVE_ADDR { - return Err(Error::InvalidChipId(address)); - } - - println!("address {}", address); + pub fn init(&mut self) -> Result<(), Error> { + self.wake()?; + self.verify()?; Ok(()) } - pub fn gyro_data(&mut self) -> Result, Error> { - Ok(Rotation { x: 1., y: 2., z: 3. }) + pub fn verify(&mut self) -> Result<(), Error> { + let address = self.read_u8(MPU6050_WHOAMI)?; + if address != MPU6050_SLAVE_ADDR { + return Err(Error::InvalidChipId(address)); + } + Ok(()) } - pub fn write(&mut self, address: u8, byte: u8) -> Result<(), Error> { - self.i2c.write(Mpu6050_SLAVE_ADDR, &[address, byte]) - .map_err(Error::I2c)?; + /// Implements complementary filter for roll/pitch + /// NOTE: yaw will always point up, sensor has no magnetometer to allow fusion + pub fn rpy(&mut self) -> Result<(f32, f32, f32), Error> { + Ok((0.0, 0.0, 0.0)) + } + + // TODO work on removing unnecessary type conversion + fn read_word_2c(&self, byte: &[u8]) -> i32 { + let high: i32 = byte[0] as i32; + let low: i32 = byte[1] as i32; + let mut word: i32 = (high << 8) + low; + + if word >= 0x8000 { + word = -((65535 - word) + 1); + } + + word + } + + fn read_rot(&mut self, reg: u8, scaling: f32) -> Result<(f32, f32, f32), Error> { + let mut buf: [u8; 6] = [0; 6]; + let bytes = self.read_bytes(reg, &mut buf)?; + + let xr = self.read_word_2c(&buf[0..2]); + let yr = self.read_word_2c(&buf[2..4]); + let zr = self.read_word_2c(&buf[4..6]); + Ok((xr as f32, yr as f32, zr as f32)) // returning as f32 makes future calculations easier + } + + /// Accelerometer readings in m/s^2 + pub fn get_acc(&mut self) -> Result<(f32, f32, f32), Error> { + let mut buf: [u8; 6] = [0; 6]; + let (mut ax, mut ay, mut az) = self.read_rot(MPU6050_ACC_REGX_H, AFS_SEL.0)?; + + ax /= AFS_SEL.0; + ay /= AFS_SEL.0; + az /= AFS_SEL.0; + + Ok((ax, ay, az)) + } + + /// Gyro readings in rad/s + pub fn get_gyro(&mut self) -> Result<(f32, f32, f32), Error> { + let mut buf: [u8; 6] = [0; 6]; + let (mut gx, mut gy, mut gz) = self.read_rot(MPU6050_GYRO_REGX_H, FS_SEL.0)?; + + if gy >= 0x8000 as f32 { + panic!("Shit"); + } + + gx *= PI / (180.0 * FS_SEL.0); + gy *= PI / (180.0 * FS_SEL.0); + gz *= PI / (180.0 * FS_SEL.0); + + Ok((gx, gy, gz)) + } + + pub fn write_u8(&mut self, reg: u8, byte: u8) -> Result<(), Error> { + self.i2c.write(MPU6050_SLAVE_ADDR, &[reg, byte]) + .map_err(Error::I2c)?; self.delay.delay_ms(10u8); Ok(()) } - pub fn read(&mut self, address: u8) -> Result> { - let mut buffer = [0]; - self.i2c.write_read(Mpu6050_SLAVE_ADDR, &[address], &mut buffer) + pub fn read_u8(&mut self, reg: u8) -> Result> { + let mut byte: [u8; 1] = [0; 1]; + self.i2c.write_read(MPU6050_SLAVE_ADDR, &[reg], &mut byte) .map_err(Error::I2c)?; + Ok(byte[0]) + } - println!("GOT {:?}", &buffer); - Ok(buffer[0]) + pub fn read_bytes(&mut self, reg: u8, buf: &mut [u8]) -> Result<(), Error> { + self.i2c.write_read(MPU6050_SLAVE_ADDR, &[reg], buf) + .map_err(Error::I2c)?; + Ok(()) } }