From 06e1768318e4e65b7f6a0823a3cbc09db30e1743 Mon Sep 17 00:00:00 2001 From: Julian Gaal Date: Mon, 15 Apr 2019 23:27:36 +0200 Subject: [PATCH] implement temperature support --- README.md | 14 +++++++--- src/bin/linux.rs | 29 ++++++++++++++++++-- src/constants.rs | 19 +++++++------ src/lib.rs | 70 ++++++++++++++++++++++++++++++++---------------- 4 files changed, 95 insertions(+), 37 deletions(-) diff --git a/README.md b/README.md index b6c31d6..2e8773d 100644 --- a/README.md +++ b/README.md @@ -2,10 +2,16 @@ Platform agnostic driver for MPU6050 sensor. -### Linux example +### Basic usage - `linux_embedded_hal` example +``` + +``` + Requirements: `apt-get install -qq gcc-arm-linux-gnueabihf libc6-armhf-cross libc6-dev-armhf-cross` cross-compile with `cargo build --bin main --target=arm-unknown-linux-gnueabihf` + + ## TODO - [x] init with default settings - [ ] init with custom settings @@ -15,11 +21,11 @@ cross-compile with `cargo build --bin main --target=arm-unknown-linux-gnueabihf` - [x] read acc data - [x] software calibration - [x] roll, pitch estimation accelerometer only +- [x] read temp data +- [ ] rename constants to better match datasheet - [ ] complementary filter for roll, pitch estimate -- [ ] read temp data +- [ ] sample rate devider with register 25? time step? - [ ] 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) - [ ] plotting [csv data](https://plot.ly/python/plot-data-from-csv/)? -- [ ] complementary filter -- [ ] time step diff --git a/src/bin/linux.rs b/src/bin/linux.rs index 0a91276..fa4cbdb 100644 --- a/src/bin/linux.rs +++ b/src/bin/linux.rs @@ -10,12 +10,37 @@ fn main() -> Result<(), Error> { let mut mpu = Mpu6050::new(i2c, delay); mpu.init()?; - //mpu.soft_calibrate(200)?; + //mpu.soft_calib(200)?; loop { + // get roll and pitch estimate match mpu.get_acc_angles() { Ok(r) => { - println!("{:?}", r); + println!("r/p: {:?}", r); + }, + Err(_) => {} , + } + + // get temp + match mpu.get_temp() { + Ok(r) => { + println!("temp: {}c", r); + }, + Err(_) => {} , + } + + // get gyro data, scaled with sensitivity + match mpu.get_gyro() { + Ok(r) => { + println!("gyro: {:?}", r); + }, + Err(_) => {} , + } + + // get accelerometer data, scaled with sensitivity + match mpu.get_acc() { + Ok(r) => { + println!("acc: {:?}", r); }, Err(_) => {} , } diff --git a/src/constants.rs b/src/constants.rs index 2e3656d..63a0464 100644 --- a/src/constants.rs +++ b/src/constants.rs @@ -1,19 +1,22 @@ -pub const MPU6050_SLAVE_ADDR: u8 = 0x68; -pub const MPU6050_WHOAMI: u8 = 0x75; +pub const SLAVE_ADDR: u8 = 0x68; +pub const WHOAMI: u8 = 0x75; /// High Bytle Register Gyro x orientation -pub const MPU6050_GYRO_REGX_H: u8 = 0x43; +pub const GYRO_REGX_H: u8 = 0x43; /// High Bytle Register Gyro y orientation -pub const MPU6050_GYRO_REGY_H: u8 = 0x45; +pub const GYRO_REGY_H: u8 = 0x45; /// High Bytle Register Gyro z orientation -pub const MPU6050_GYRO_REGZ_H: u8 = 0x47; +pub const GYRO_REGZ_H: u8 = 0x47; /// High Byte Register Calc roll -pub const MPU6050_ACC_REGX_H: u8 = 0x3b; +pub const ACC_REGX_H: u8 = 0x3b; /// High Byte Register Calc pitch -pub const MPU6050_ACC_REGY_H: u8 = 0x3d; +pub const ACC_REGY_H: u8 = 0x3d; /// High Byte Register Calc yaw -pub const MPU6050_ACC_REGZ_H: u8 = 0x3f; +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; diff --git a/src/lib.rs b/src/lib.rs index bb41d7e..d832b38 100644 --- a/src/lib.rs +++ b/src/lib.rs @@ -1,12 +1,12 @@ #![no_std] ///! Mpu6050 sensor driver. -///! Datasheet: +///! Datasheet: https://www.invensense.com/wp-content/uploads/2015/02/MPU-6000-Register-Map1.pdf pub mod constants; use crate::constants::*; -use libm as m; +use libm::{powf, atan2f, sqrtf}; use embedded_hal::{ blocking::delay::DelayMs, blocking::i2c::{Read, Write, WriteRead}, @@ -44,14 +44,14 @@ impl Bias { } } -enum AccelRange { +pub enum AccelRange { G2, G4, G8, G16, } -enum GyroRange { +pub enum GyroRange { DEG250, DEG500, DEG1000, @@ -72,6 +72,8 @@ pub struct Mpu6050 { i2c: I, delay: D, bias: Option, + acc_sensitivity: f32, + gyro_sensitivity: f32, } impl Mpu6050 @@ -79,14 +81,18 @@ where I: Write + WriteRead, D: DelayMs, { + /// Side effect free constructor with default sensitivies, no calibration pub fn new(i2c: I, delay: D) -> Self { Mpu6050 { i2c, delay, bias: None, + acc_sensitivity: AFS_SEL.0, + gyro_sensitivity: FS_SEL.0, } } + /// Performs software calibration. Readings must be made with MPU6050 in resting position pub fn soft_calib(&mut self, steps: u8) -> Result<(), Error> { let mut bias = Bias::default(); @@ -100,35 +106,40 @@ where Ok(()) } + /// Wakes MPU6050 with all sensors enabled (default) pub fn wake(&mut self) -> Result<(), Error> { 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> { self.wake()?; self.verify()?; Ok(()) } + /// Verifies device to address 0x68 with WHOAMI Register pub fn verify(&mut self) -> Result<(), Error> { - let address = self.read_u8(MPU6050_WHOAMI)?; - if address != MPU6050_SLAVE_ADDR { + let address = self.read_u8(WHOAMI)?; + if address != SLAVE_ADDR { return Err(Error::InvalidChipId(address)); } Ok(()) } - /// NOTE: yaw will always point up, sensor has no magnetometer to allow fusion + /// 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> { let (ax, ay, az) = self.get_acc()?; - let roll: f32 = m::atan2f(ay, m::sqrtf(m::powf(ax, 2.) + m::powf(az, 2.))); - let pitch: f32 = m::atan2f(-ax, m::sqrtf(m::powf(ay, 2.) + m::powf(az, 2.))); + let roll: f32 = atan2f(ay, sqrtf(powf(ax, 2.) + powf(az, 2.))); + let pitch: f32 = atan2f(-ax, sqrtf(powf(ay, 2.) + powf(az, 2.))); Ok((roll, pitch)) } - // TODO work on removing unnecessary type conversion + /// Converts 2 bytes number in 2 compliment + /// i16?! whats 0x8000?! fn read_word_2c(&self, byte: &[u8]) -> i32 { let high: i32 = byte[0] as i32; let low: i32 = byte[1] as i32; @@ -141,9 +152,10 @@ where word } + /// Reads rotation (gyro/acc) from specified register 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)?; + self.read_bytes(reg, &mut buf)?; let xr = self.read_word_2c(&buf[0..2]); let yr = self.read_word_2c(&buf[2..4]); @@ -154,11 +166,11 @@ where /// Accelerometer readings in m/s^2 pub fn get_acc(&mut self) -> Result<(f32, f32, f32), Error> { - let (mut ax, mut ay, mut az) = self.read_rot(MPU6050_ACC_REGX_H, AFS_SEL.0)?; + let (mut ax, mut ay, mut az) = self.read_rot(ACC_REGX_H, AFS_SEL.0)?; - ax /= AFS_SEL.0; - ay /= AFS_SEL.0; - az /= AFS_SEL.0; + ax /= self.acc_sensitivity; + ay /= self.acc_sensitivity; + az /= self.acc_sensitivity; if let Some(ref bias) = self.bias { ax -= bias.ax; @@ -171,11 +183,11 @@ where /// Gyro readings in rad/s pub fn get_gyro(&mut self) -> Result<(f32, f32, f32), Error> { - let (mut gx, mut gy, mut gz) = self.read_rot(MPU6050_GYRO_REGX_H, FS_SEL.0)?; + let (mut gx, mut gy, mut gz) = self.read_rot(GYRO_REGX_H, FS_SEL.0)?; - gx *= PI / (180.0 * FS_SEL.0); - gy *= PI / (180.0 * FS_SEL.0); - gz *= PI / (180.0 * FS_SEL.0); + gx *= PI / (180.0 * self.gyro_sensitivity); + gy *= PI / (180.0 * self.gyro_sensitivity); + gz *= PI / (180.0 * self.gyro_sensitivity); if let Some(ref bias) = self.bias { gx -= bias.gx; @@ -186,22 +198,34 @@ where Ok((gx, gy, gz)) } + /// Temp in degrees celcius + 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; + + Ok((raw_temp / 340.) + 36.53) + } + + /// Writes byte to register pub fn write_u8(&mut self, reg: u8, byte: u8) -> Result<(), Error> { - self.i2c.write(MPU6050_SLAVE_ADDR, &[reg, byte]) + self.i2c.write(SLAVE_ADDR, &[reg, byte]) .map_err(Error::I2c)?; self.delay.delay_ms(10u8); Ok(()) } - + + /// Reads byte from register 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) + self.i2c.write_read(SLAVE_ADDR, &[reg], &mut byte) .map_err(Error::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> { - self.i2c.write_read(MPU6050_SLAVE_ADDR, &[reg], buf) + self.i2c.write_read(SLAVE_ADDR, &[reg], buf) .map_err(Error::I2c)?; Ok(()) }