From 0ada7c9d0faf7228b960b809c9f234e45e7afc2b Mon Sep 17 00:00:00 2001 From: Julian Gaal Date: Wed, 17 Apr 2019 16:37:07 +0200 Subject: [PATCH] rename --- src/lib.rs | 38 +++++++++++++++++++------------------- src/registers.rs | 42 ++++++++++++++++++++++++++++++++++++++++++ 2 files changed, 61 insertions(+), 19 deletions(-) create mode 100644 src/registers.rs diff --git a/src/lib.rs b/src/lib.rs index 5a87843..0016cb2 100644 --- a/src/lib.rs +++ b/src/lib.rs @@ -1,12 +1,12 @@ #![no_std] -pub mod constants; +pub mod registers; ///! Mpu6050 sensor driver. ///! Register sheet: https://www.invensense.com/wp-content/uploads/2015/02/MPU-6000-Register-Map1.pdf ///! Data sheet: https://www.invensense.com/wp-content/uploads/2015/02/MPU-6500-Datasheet2.pdf -use crate::constants::*; +use crate::registers::*; use libm::{powf, atan2f, sqrtf}; use embedded_hal::{ blocking::delay::DelayMs, @@ -96,7 +96,7 @@ pub enum GyroRange { /// All possible errors in this crate #[derive(Debug)] -pub enum Error { +pub enum Mpu6050Error { /// I2C bus error I2c(E), @@ -142,7 +142,7 @@ where /// Performs software calibration with steps number of readings. /// Readings must be made with MPU6050 in resting position - pub fn soft_calib(&mut self, steps: u8) -> Result<(), Error> { + pub fn soft_calib(&mut self, steps: u8) -> Result<(), Mpu6050Error> { let mut bias = Bias::default(); for _ in 0..steps+1 { @@ -156,31 +156,31 @@ where } /// Wakes MPU6050 with all sensors enabled (default) - pub fn wake(&mut self) -> Result<(), Error> { + pub fn wake(&mut self) -> Result<(), Mpu6050Error> { self.write_u8(POWER_MGMT_1, 0)?; self.delay.delay_ms(100u8); Ok(()) } /// Init wakes MPU6050 and verifies register addr, e.g. in i2c - pub fn init(&mut self) -> Result<(), Error> { + pub fn init(&mut self) -> Result<(), Mpu6050Error> { self.wake()?; self.verify()?; Ok(()) } /// Verifies device to address 0x68 with WHOAMI Register - pub fn verify(&mut self) -> Result<(), Error> { + pub fn verify(&mut self) -> Result<(), Mpu6050Error> { let address = self.read_u8(WHOAMI)?; if address != SLAVE_ADDR { - return Err(Error::InvalidChipId(address)); + return Err(Mpu6050Error::InvalidChipId(address)); } Ok(()) } /// Roll and pitch estimation from raw accelerometer readings /// NOTE: no yaw! no magnetometer present on MPU6050 - pub fn get_acc_angles(&mut self) -> Result<(f32, f32), Error> { + pub fn get_acc_angles(&mut self) -> Result<(f32, f32), Mpu6050Error> { let (ax, ay, az) = self.get_acc()?; let roll: f32 = atan2f(ay, sqrtf(powf(ax, 2.) + powf(az, 2.))); let pitch: f32 = atan2f(-ax, sqrtf(powf(ay, 2.) + powf(az, 2.))); @@ -202,7 +202,7 @@ where } /// Reads rotation (gyro/acc) from specified register - fn read_rot(&mut self, reg: u8) -> Result<(f32, f32, f32), Error> { + fn read_rot(&mut self, reg: u8) -> Result<(f32, f32, f32), Mpu6050Error> { let mut buf: [u8; 6] = [0; 6]; self.read_bytes(reg, &mut buf)?; @@ -214,7 +214,7 @@ where } /// Accelerometer readings in m/s^2 - pub fn get_acc(&mut self) -> Result<(f32, f32, f32), Error> { + pub fn get_acc(&mut self) -> Result<(f32, f32, f32), Mpu6050Error> { let (mut ax, mut ay, mut az) = self.read_rot(ACC_REGX_H)?; ax /= self.acc_sensitivity; @@ -231,7 +231,7 @@ where } /// Gyro readings in rad/s - pub fn get_gyro(&mut self) -> Result<(f32, f32, f32), Error> { + pub fn get_gyro(&mut self) -> Result<(f32, f32, f32), Mpu6050Error> { let (mut gx, mut gy, mut gz) = self.read_rot(GYRO_REGX_H)?; gx *= PI / (180.0 * self.gyro_sensitivity); @@ -248,7 +248,7 @@ where } /// Temp in degrees celcius - pub fn get_temp(&mut self) -> Result> { + pub fn get_temp(&mut self) -> Result> { let mut buf: [u8; 2] = [0; 2]; self.read_bytes(TEMP_OUT_H, &mut buf)?; let raw_temp = self.read_word_2c(&buf[0..2]) as f32; @@ -257,25 +257,25 @@ where } /// Writes byte to register - pub fn write_u8(&mut self, reg: u8, byte: u8) -> Result<(), Error> { + pub fn write_u8(&mut self, reg: u8, byte: u8) -> Result<(), Mpu6050Error> { self.i2c.write(SLAVE_ADDR, &[reg, byte]) - .map_err(Error::I2c)?; + .map_err(Mpu6050Error::I2c)?; self.delay.delay_ms(10u8); Ok(()) } /// Reads byte from register - pub fn read_u8(&mut self, reg: u8) -> Result> { + pub fn read_u8(&mut self, reg: u8) -> Result> { let mut byte: [u8; 1] = [0; 1]; self.i2c.write_read(SLAVE_ADDR, &[reg], &mut byte) - .map_err(Error::I2c)?; + .map_err(Mpu6050Error::I2c)?; Ok(byte[0]) } /// Reads series of bytes into buf from specified reg - pub fn read_bytes(&mut self, reg: u8, buf: &mut [u8]) -> Result<(), Error> { + pub fn read_bytes(&mut self, reg: u8, buf: &mut [u8]) -> Result<(), Mpu6050Error> { self.i2c.write_read(SLAVE_ADDR, &[reg], buf) - .map_err(Error::I2c)?; + .map_err(Mpu6050Error::I2c)?; Ok(()) } } diff --git a/src/registers.rs b/src/registers.rs new file mode 100644 index 0000000..b89257d --- /dev/null +++ b/src/registers.rs @@ -0,0 +1,42 @@ +//! All constants used in the driver, mostly register addresses + +/// Slave address of Mpu6050 +pub const SLAVE_ADDR: u8 = 0x68; +/// Internal register to check slave addr +pub const WHOAMI: u8 = 0x75; + +/// High Bytle Register Gyro x orientation +pub const GYRO_REGX_H: u8 = 0x43; +/// High Bytle Register Gyro y orientation +pub const GYRO_REGY_H: u8 = 0x45; +/// High Bytle Register Gyro z orientation +pub const GYRO_REGZ_H: u8 = 0x47; + +/// High Byte Register Calc roll +pub const ACC_REGX_H: u8 = 0x3b; +/// High Byte Register Calc pitch +pub const ACC_REGY_H: u8 = 0x3d; +/// High Byte Register Calc yaw +pub const ACC_REGZ_H: u8 = 0x3f; + +/// High Byte Register Temperature +pub const TEMP_OUT_H: u8 = 0x41; + +/// Register to control chip waking from sleep, enabling sensors, default: sleep +pub const POWER_MGMT_1: u8 = 0x6b; +/// Internal clock +pub const POWER_MGMT_2: u8 = 0x6c; + +/// Gyro Sensitivity +pub const FS_SEL: (f32, f32, f32, f32) = (131., 65.5, 32.8, 16.4); +/// Calcelerometer Sensitivity +pub const AFS_SEL: (f32, f32, f32, f32) = (16384., 8192., 4096., 2048.); + +/// Accelerometer config register +pub const ACCEL_CONFIG: u8 = 0x1c; + +/// gyro config register +pub const GYRO_CONFIG: u8 = 0x1b; + +/// pi +pub const PI: f32 = 3.14159;