From 1ca32fedf35055f87770886bd49846e27e109fb0 Mon Sep 17 00:00:00 2001 From: Julian Gaal Date: Sat, 27 Apr 2019 17:37:52 +0200 Subject: [PATCH 01/22] Update README.md --- README.md | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/README.md b/README.md index 3bf2d9b..20c5aff 100644 --- a/README.md +++ b/README.md @@ -9,7 +9,8 @@ Platform agnostic driver for [MPU 6050 6-axis IMU](https://www.invensense.com/pr Visualization options for testing and development in [viz branch](https://github.com/juliangaal/mpu6050/tree/viz/viz) -### Basic usage - [`linux_embedded_hal`](https://github.com/rust-embedded/linux-embedded-hal) example +### Basic usage +[`linux_embedded_hal`](https://github.com/rust-embedded/linux-embedded-hal) example ```rust use mpu6050::*; use linux_embedded_hal::{I2cdev, Delay}; @@ -64,7 +65,6 @@ fn main() -> Result<(), Error> { } ``` -*Note*: this example uses API of version on local master branch. Some functions may not be available on published crate yet. #### Compile linux example (Raspberry Pi 3B) files [here](https://github.com/juliangaal/mpu6050/blob/master/example/) From c85ae24e119996b71a274e87b4d29a2748eae30f Mon Sep 17 00:00:00 2001 From: juliangaal Date: Mon, 29 Apr 2019 09:14:55 +0200 Subject: [PATCH 02/22] Makefile for easier testing --- Makefile | 19 +++++++++++++++++++ 1 file changed, 19 insertions(+) create mode 100644 Makefile diff --git a/Makefile b/Makefile new file mode 100644 index 0000000..8531891 --- /dev/null +++ b/Makefile @@ -0,0 +1,19 @@ +target = armv7-unknown-linux-gnueabihf +mode = release +home = $(shell pwd) + +.DEFAULT_GOAL = build + +build: + cargo build + +ext: + cd $(home)/example && cargo build --$(mode) --target $(target) + +viz: + cd $(home)/viz/viz && cargo build --$(mode) --target $(target) + +deploy: ext + scp $(home)/example/target/$(target)/$(mode)/example pi@192.168.1.136: + +.PHONY: deploy build ext viz From 29efdc29d7c1727e2d2ab8dff60523c25ce307cb Mon Sep 17 00:00:00 2001 From: Julian Gaal Date: Wed, 1 May 2019 09:30:56 +0200 Subject: [PATCH 03/22] remove image --- README.md | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/README.md b/README.md index 20c5aff..af2f02b 100644 --- a/README.md +++ b/README.md @@ -1,5 +1,4 @@ -# MPU 6050 Rust Driver - +# MPU 6050 Rust Driver Platform agnostic driver for [MPU 6050 6-axis IMU](https://www.invensense.com/products/motion-tracking/6-axis/mpu-6500/) using [`embedded_hal`](https://github.com/rust-embedded/embedded-hal). From 44a67e8fdd1ecafa03338dd9e715fa1e9b49e083 Mon Sep 17 00:00:00 2001 From: Julian Gaal Date: Wed, 8 May 2019 18:50:47 +0200 Subject: [PATCH 04/22] make registers dedicated enum --- src/lib.rs | 31 +++++++++++++--------- src/registers.rs | 68 +++++++++++++++++++++++++----------------------- 2 files changed, 53 insertions(+), 46 deletions(-) diff --git a/src/lib.rs b/src/lib.rs index deb7f1d..26d353f 100644 --- a/src/lib.rs +++ b/src/lib.rs @@ -1,12 +1,12 @@ #![no_std] -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::registers::*; +pub mod registers; + +use crate::registers::Registers::*; use libm::{powf, atan2f, sqrtf}; use embedded_hal::{ blocking::delay::DelayMs, @@ -15,6 +15,11 @@ use embedded_hal::{ /// pi, taken straight from C pub const PI: f32 = 3.14159265358979323846; +/// 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.); + /// Operations trait for sensor readings pub trait MutOps { @@ -302,7 +307,7 @@ where /// Wakes MPU6050 with all sensors enabled (default) pub fn wake(&mut self) -> Result<(), Mpu6050Error> { - self.write_u8(POWER_MGMT_1, 0)?; + self.write_u8(POWER_MGMT_1.addr(), 0)?; self.delay.delay_ms(100u8); Ok(()) } @@ -314,10 +319,10 @@ where Ok(()) } - /// Verifies device to address 0x68 with WHOAMI Register + /// Verifies device to address 0x68 with WHOAMI.addr() Register pub fn verify(&mut self) -> Result<(), Mpu6050Error> { - let address = self.read_u8(WHOAMI)?; - if address != SLAVE_ADDR { + let address = self.read_u8(WHOAMI.addr())?; + if address != SLAVE_ADDR.addr() { return Err(Mpu6050Error::InvalidChipId(address)); } Ok(()) @@ -431,7 +436,7 @@ where /// Accelerometer readings in m/s^2 pub fn get_acc(&mut self) -> Result> { - let mut acc = self.read_rot(ACC_REGX_H)?; + let mut acc = self.read_rot(ACC_REGX_H.addr())?; acc.x /= self.acc_sensitivity; acc.y /= self.acc_sensitivity; @@ -458,7 +463,7 @@ where /// Gyro readings in rad/s pub fn get_gyro(&mut self) -> Result> { - let mut gyro = self.read_rot(GYRO_REGX_H)?; + let mut gyro = self.read_rot(GYRO_REGX_H.addr())?; gyro.x *= PI / (180.0 * self.gyro_sensitivity); gyro.y *= PI / (180.0 * self.gyro_sensitivity); @@ -486,7 +491,7 @@ where /// 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)?; + self.read_bytes(TEMP_OUT_H.addr(), &mut buf)?; let raw_temp = self.read_word_2c(&buf[0..2]) as f32; Ok((raw_temp / 340.) + 36.53) @@ -503,7 +508,7 @@ where /// Writes byte to register pub fn write_u8(&mut self, reg: u8, byte: u8) -> Result<(), Mpu6050Error> { - self.i2c.write(SLAVE_ADDR, &[reg, byte]) + self.i2c.write(SLAVE_ADDR.addr(), &[reg, byte]) .map_err(Mpu6050Error::I2c)?; self.delay.delay_ms(10u8); Ok(()) @@ -512,14 +517,14 @@ where /// Reads byte from register 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) + self.i2c.write_read(SLAVE_ADDR.addr(), &[reg], &mut byte) .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<(), Mpu6050Error> { - self.i2c.write_read(SLAVE_ADDR, &[reg], buf) + self.i2c.write_read(SLAVE_ADDR.addr(), &[reg], buf) .map_err(Mpu6050Error::I2c)?; Ok(()) } diff --git a/src/registers.rs b/src/registers.rs index 468a8b4..b090bc8 100644 --- a/src/registers.rs +++ b/src/registers.rs @@ -1,42 +1,44 @@ //! 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; +#[allow(non_camel_case_types)] +#[derive(Copy, Clone, Debug)] +pub enum Registers { + /// Slave address of Mpu6050 + SLAVE_ADDR = 0x68, + /// Internal register to check slave addr + WHOAMI = 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 Bytle Register Gyro x orientation + GYRO_REGX_H = 0x43, + /// High Bytle Register Gyro y orientation + GYRO_REGY_H = 0x45, + /// High Bytle Register Gyro z orientation + GYRO_REGZ_H = 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 Calc roll + ACC_REGX_H = 0x3b, + /// High Byte Register Calc pitch + ACC_REGY_H = 0x3d, + /// High Byte Register Calc yaw + ACC_REGZ_H = 0x3f, -/// High Byte Register Temperature -pub const TEMP_OUT_H: u8 = 0x41; + /// High Byte Register Temperature + TEMP_OUT_H = 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; + /// Register to control chip waking from sleep, enabling sensors, default: sleep + POWER_MGMT_1 = 0x6b, + /// Internal clock + POWER_MGMT_2 = 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 + ACCEL_CONFIG = 0x1c, -/// Accelerometer config register -pub const ACCEL_CONFIG: u8 = 0x1c; + /// gyro config register + GYRO_CONFIG = 0x1b, +} -/// gyro config register -pub const GYRO_CONFIG: u8 = 0x1b; - -/// pi -pub const PI: f32 = 3.14159265358979323846; +impl Registers { + pub fn addr(&self) -> u8 { + *self as u8 + } +} From b9dbcf889612f8f566cfe8376fc86f3eac31d0ef Mon Sep 17 00:00:00 2001 From: Julian Gaal Date: Wed, 8 May 2019 19:02:28 +0200 Subject: [PATCH 05/22] remove unnecessary spaces --- src/registers.rs | 6 ------ 1 file changed, 6 deletions(-) diff --git a/src/registers.rs b/src/registers.rs index b090bc8..f9b2998 100644 --- a/src/registers.rs +++ b/src/registers.rs @@ -7,32 +7,26 @@ pub enum Registers { SLAVE_ADDR = 0x68, /// Internal register to check slave addr WHOAMI = 0x75, - /// High Bytle Register Gyro x orientation GYRO_REGX_H = 0x43, /// High Bytle Register Gyro y orientation GYRO_REGY_H = 0x45, /// High Bytle Register Gyro z orientation GYRO_REGZ_H = 0x47, - /// High Byte Register Calc roll ACC_REGX_H = 0x3b, /// High Byte Register Calc pitch ACC_REGY_H = 0x3d, /// High Byte Register Calc yaw ACC_REGZ_H = 0x3f, - /// High Byte Register Temperature TEMP_OUT_H = 0x41, - /// Register to control chip waking from sleep, enabling sensors, default: sleep POWER_MGMT_1 = 0x6b, /// Internal clock POWER_MGMT_2 = 0x6c, - /// Accelerometer config register ACCEL_CONFIG = 0x1c, - /// gyro config register GYRO_CONFIG = 0x1b, } From 61b8b62b506cfd41118972ec7ba7632e46227ed3 Mon Sep 17 00:00:00 2001 From: Julian Gaal Date: Wed, 8 May 2019 19:02:51 +0200 Subject: [PATCH 06/22] documentation improvements --- src/lib.rs | 49 +++++++++++++++++++++++++++++++++++++++++++++---- 1 file changed, 45 insertions(+), 4 deletions(-) diff --git a/src/lib.rs b/src/lib.rs index 26d353f..214ee6a 100644 --- a/src/lib.rs +++ b/src/lib.rs @@ -1,8 +1,49 @@ -#![no_std] +//! Mpu6050 sensor driver. +//! +//! Register sheet [here](https://www.invensense.com/wp-content/uploads/2015/02/MPU-6000-Register-Map1.pdf), +//! Data sheet [here](https://www.invensense.com/wp-content/uploads/2015/02/MPU-6500-Datasheet2.pdf) +//! +//! To use this driver you must provide a concrete `embedded_hal` implementation.//! This example uses `linux_embedded_hal` +//! ``` +//! let i2c = I2cdev::new("/dev/i2c-1") +//! .map_err(Mpu6050Error::I2c)?; +//! +//! let delay = Delay; +//! +//! let mut mpu = Mpu6050::new(i2c, delay); +//! mpu.init()?; +//! mpu.soft_calib(Steps(100))?; +//! mpu.calc_variance(Steps(50))?; +//! +//! println!("Calibrated with bias: {:?}", mpu.get_bias().unwrap()); +//! println!("Calculated variance: {:?}", mpu.get_variance().unwrap()); +//! +//! // get roll and pitch estimate +//! let acc = mpu.get_acc_angles()?; +//! +//! // get roll and pitch estimate - averaged accross n readings (steps) +//! let acc = mpu.get_acc_angles_avg(Steps(5))?; +//! +//! // get temp +//! let temp = mpu.get_temp()?; +//! +//! // get temp - averages across n readings (steps) +//! let temp = mpu.get_temp_avg(Steps(5))?; +//! +//! // get gyro data, scaled with sensitivity +//! let gyro = mpu.get_gyro()?; +//! +//! // get gyro data, scaled with sensitivity - averaged across n readings (steps) +//! let gyro = mpu.get_gyro_avg(Steps(5))?; +//! +//! // get accelerometer data, scaled with sensitivity +//! let acc = mpu.get_acc()?; +//! +//! // get accelerometer data, scaled with sensitivity - averages across n readings (steps) +//! let acc = mpu.get_acc_avg(Steps(5))?; +//! -///! 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 +#![no_std] pub mod registers; From 7ab5fc90f4970bc31901bd432a9d13c8e8decd45 Mon Sep 17 00:00:00 2001 From: Julian Gaal Date: Thu, 16 May 2019 13:09:30 +0200 Subject: [PATCH 07/22] enable nalgebra with no_std support --- Cargo.toml | 4 ++++ src/lib.rs | 1 + 2 files changed, 5 insertions(+) diff --git a/Cargo.toml b/Cargo.toml index 01c4367..ed8c0d1 100644 --- a/Cargo.toml +++ b/Cargo.toml @@ -13,3 +13,7 @@ license = "MIT" [dependencies] embedded-hal = "0.2.2" libm = "0.1.2" + +[dependencies.nalgebra] +default-features = false +version = "0.18" diff --git a/src/lib.rs b/src/lib.rs index 214ee6a..977d871 100644 --- a/src/lib.rs +++ b/src/lib.rs @@ -49,6 +49,7 @@ pub mod registers; use crate::registers::Registers::*; use libm::{powf, atan2f, sqrtf}; +use nalgebra as na; use embedded_hal::{ blocking::delay::DelayMs, blocking::i2c::{Write, WriteRead}, From c1aa3c1f8bd2b6b08b6ffb2ec1f322c7ee251544 Mon Sep 17 00:00:00 2001 From: juliangaal Date: Mon, 20 May 2019 16:00:24 +0200 Subject: [PATCH 08/22] migrate to nalgebra --- Cargo.toml | 2 +- src/lib.rs | 316 ++++++++++++++++++++++++++--------------------------- 2 files changed, 156 insertions(+), 162 deletions(-) diff --git a/Cargo.toml b/Cargo.toml index ed8c0d1..47892e5 100644 --- a/Cargo.toml +++ b/Cargo.toml @@ -16,4 +16,4 @@ libm = "0.1.2" [dependencies.nalgebra] default-features = false -version = "0.18" +version = "0.18.0" diff --git a/src/lib.rs b/src/lib.rs index 977d871..f0bd123 100644 --- a/src/lib.rs +++ b/src/lib.rs @@ -41,7 +41,7 @@ //! //! // get accelerometer data, scaled with sensitivity - averages across n readings (steps) //! let acc = mpu.get_acc_avg(Steps(5))?; -//! +//! ``` #![no_std] @@ -49,7 +49,7 @@ pub mod registers; use crate::registers::Registers::*; use libm::{powf, atan2f, sqrtf}; -use nalgebra as na; +use nalgebra::{Vector3, Vector2}; use embedded_hal::{ blocking::delay::DelayMs, blocking::i2c::{Write, WriteRead}, @@ -71,6 +71,16 @@ pub trait MutOps { fn scale(&mut self, n: u8); } +/// Helpers for sensor readings +pub trait Access { + fn x(&self) -> T; + fn set_x(&mut self, val: T); + fn y(&self) -> T; + fn set_y(&mut self, val: T); + fn z(&self) -> T; + fn set_z(&mut self, val: T); +} + /// Trait for conversion from/to radians/degree pub trait UnitConv { /// get radians from degree @@ -101,45 +111,89 @@ impl UnitConv for f32 { } } +impl Access for Vector3 { + fn x(&self) -> f32 { + self[0] + } + + fn set_x(&mut self, val: f32) { + self[0] = val; + } + + fn y(&self) -> f32 { + self[1] + } + + fn set_y(&mut self, val: f32) { + self[1] = val; + } + + fn z(&self) -> f32 { + self[2] + } + + fn set_z(&mut self, val: f32) { + self[2] = val + } +} + +impl Access for Vector2 { + fn x(&self) -> f32 { + self[0] + } + + fn set_x(&mut self, val: f32) { + self[0] = val; + } + + fn y(&self) -> f32 { + self[1] + } + + fn set_y(&mut self, val: f32) { + self[1] = val; + } + + fn z(&self) -> f32 { + -1.0 + } + + fn set_z(&mut self, _val: f32) {} +} + /// Used for bias calculation of chip in mpu::soft_calib -#[derive(Default, Debug, Clone)] +#[derive(Debug, Clone)] pub struct Bias { - /// accelerometer x axis bias - ax: f32, - /// accelerometer y axis bias - ay: f32, - /// accelerometer z axis bias - az: f32, + /// accelerometer axis bias + acc: Vector3, /// gyro x axis bias - gx: f32, - /// gyro y axis bias - gy: f32, - /// gyro z axis bias - gz: f32, + gyro: Vector3, /// temperature AVERAGE: can't get bias! - t: f32, + temp: f32, +} + +impl Default for Bias { + fn default() -> Bias { + Bias { + acc: Vector3::::zeros(), + gyro: Vector3::::zeros(), + temp: 0.0, + } + } } impl Bias { - fn add(&mut self, acc: RotReading, gyro: RotReading, temp: f32) { - self.ax += acc.x; - self.ay += acc.y; - self.az += acc.z; - self.gx += gyro.x; - self.gy += gyro.y; - self.gz += gyro.z; - self.t += temp; + fn add(&mut self, acc: Vector3, gyro: Vector3, temp: f32) { + self.acc += acc; + self.gyro += gyro; + self.temp += temp; } fn scale(&mut self, n: u8) { let n = n as f32; - self.ax /= n; - self.ay /= n; - self.az /= n; - self.gx /= n; - self.gy /= n; - self.gz /= n; - self.t /= n; + self.acc /= n; + self.gyro /= n; + self.temp /= n; } } @@ -147,108 +201,37 @@ impl Bias { pub type Variance = Bias; impl Variance { - fn add_diff(&mut self, acc_diff: (f32, f32, f32), gyro_diff: (f32, f32, f32), temp_diff: f32) { - self.ax += acc_diff.0; - self.ay += acc_diff.1; - self.az += acc_diff.2; - self.gx += gyro_diff.0; - self.gy += gyro_diff.1; - self.gz += gyro_diff.2; - self.t += temp_diff; + fn add_diff(&mut self, acc_diff: Vector3, gyro_diff: Vector3, temp_diff: f32) { + self.acc += acc_diff; + self.gyro += gyro_diff; + self.temp += temp_diff; } } -/// Struct for rotation reading: gyro or accelerometer. -/// see indivisual type definitions -#[derive(Debug)] -pub struct RotReading { - pub x: f32, - pub y: f32, - pub z: f32, -} - -/// Accelerometer reading -pub type AccReading = RotReading; -/// Gyro Reading -pub type GyroReading = RotReading; - -impl RotReading { - fn new(x: f32, y: f32, z: f32) -> Self { - RotReading { - x, - y, - z, - } - } -} - -impl MutOps for RotReading { - fn add(&mut self, operand: &Self) { - self.x += operand.x; - self.y += operand.y; - self.z += operand.z; - } - - fn scale(&mut self, n: u8) { - let n = n as f32; - self.x /= n; - self.y /= n; - self.z /= n; - } -} - -/// struct for Roll/Pitch Reading -#[derive(Debug)] -pub struct RPReading { - pub roll: f32, - pub pitch: f32, -} - -impl RPReading { - fn new(roll: f32, pitch: f32) -> Self { - RPReading { - roll, - pitch, - } - } -} - -impl MutOps for RPReading { - fn add(&mut self, operand: &Self) { - self.roll += operand.roll; - self.pitch += operand.pitch; - } - - fn scale(&mut self, n: u8) { - let n = n as f32; - self.roll /= n; - self.pitch /= n; - } -} - -impl UnitConv for RPReading { - fn to_rad(&self) -> RPReading { - RPReading { - roll: self.roll.to_rad(), - pitch: self.pitch.to_rad(), - } +/// Vector2 for Roll/Pitch Reading +impl UnitConv> for Vector2 { + fn to_rad(&self) -> Vector2 { + Vector2::::new( + self.x().to_rad(), + self.y().to_rad(), + ) } fn to_rad_mut(&mut self) { - self.roll.to_rad_mut(); - self.pitch.to_rad_mut(); + self[0].to_rad_mut(); + self[1].to_rad_mut(); } - fn to_deg(&self) -> RPReading { - RPReading { - roll: self.roll.to_deg(), - pitch: self.pitch.to_deg(), - } + fn to_deg(&self) -> Vector2 { + Vector2::::new( + self.x().to_deg(), + self.y().to_deg(), + ) } fn to_deg_mut(&mut self) { - self.roll.to_deg_mut(); - self.pitch.to_deg_mut(); + self[0].to_deg_mut(); + self[1].to_deg_mut(); } } @@ -381,7 +364,7 @@ where } bias.scale(steps.0); - bias.az -= 1.0; // gravity compensation + bias.acc[2] -= 1.0; // gravity compensation self.bias = Some(bias); Ok(()) @@ -404,15 +387,19 @@ where let mut acc = self.get_acc()?; let mut gyro = self.get_gyro()?; let mut temp = self.get_temp()?; - let mut acc_diff: (f32, f32, f32); - let mut gyro_diff: (f32, f32, f32); + let mut acc_diff = Vector3::::zeros(); + let mut gyro_diff = Vector3::::zeros(); let mut temp_diff: f32; let bias = self.bias.clone().unwrap(); for _ in 0..iterations { - acc_diff = (powf(acc.x - bias.ax, 2.0), powf(acc.y - bias.ay, 2.0), powf(acc.z - bias.az, 2.0)); - gyro_diff = (powf(gyro.x - bias.gx, 2.0), powf(gyro.y - bias.gy, 2.0), powf(gyro.z - bias.gz, 2.0)); - temp_diff = powf(temp - bias.t, 2.0); + acc_diff.set_x(powf(acc.x() - bias.acc.x(), 2.0)); + acc_diff.set_y(powf(acc.y() - bias.acc.y(), 2.0)); + acc_diff.set_z(powf(acc.z() - bias.acc.z(), 2.0)); + gyro_diff.set_x(powf(gyro.x() - bias.gyro.x(), 2.0)); + gyro_diff.set_y(powf(gyro.y() - bias.gyro.y(), 2.0)); + gyro_diff.set_z(powf(gyro.z() - bias.gyro.z(), 2.0)); + temp_diff = powf(temp - bias.temp, 2.0); variance.add_diff(acc_diff, gyro_diff, temp_diff); acc = self.get_acc()?; gyro = self.get_gyro()?; @@ -420,7 +407,7 @@ where } variance.scale(iterations-1); - variance.az -= 1.0; // gravity compensation + variance.acc[2] -= 1.0; // gravity compensation self.variance = Some(variance); Ok(()) @@ -433,20 +420,21 @@ where /// Roll and pitch estimation from raw accelerometer readings /// NOTE: no yaw! no magnetometer present on MPU6050 - pub fn get_acc_angles(&mut self) -> Result> { + pub fn get_acc_angles(&mut self) -> Result, Mpu6050Error> { let acc = self.get_acc()?; - let roll: f32 = atan2f(acc.y, sqrtf(powf(acc.x, 2.) + powf(acc.z, 2.))); - let pitch: f32 = atan2f(-acc.x, sqrtf(powf(acc.y, 2.) + powf(acc.z, 2.))); - Ok(RPReading::new(roll, pitch)) + Ok(Vector2::::new( + atan2f(acc.y(), sqrtf(powf(acc.x(), 2.) + powf(acc.z(), 2.))), + atan2f(-acc.x(), sqrtf(powf(acc.y(), 2.) + powf(acc.z(), 2.))) + )) } /// Roll and pitch estimation from raw accelerometer - averaged across window readings - pub fn get_acc_angles_avg(&mut self, steps: Steps) -> Result> { + pub fn get_acc_angles_avg(&mut self, steps: Steps) -> Result, Mpu6050Error> { let mut acc = self.get_acc_angles()?; for _ in 0..steps.0-1 { - acc.add(&self.get_acc_angles()?); + acc += self.get_acc_angles()?; } - acc.scale(steps.0); + acc /= steps.0 as f32; Ok(acc) } @@ -465,68 +453,61 @@ where } /// Reads rotation (gyro/acc) from specified register - fn read_rot(&mut self, reg: u8) -> Result> { + fn read_rot(&mut self, reg: u8) -> Result, Mpu6050Error> { let mut buf: [u8; 6] = [0; 6]; 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(RotReading::new(xr as f32, yr as f32, zr as f32)) // returning as f32 makes future calculations easier + Ok(Vector3::::new( + self.read_word_2c(&buf[0..2]) as f32, + self.read_word_2c(&buf[2..4]) as f32, + self.read_word_2c(&buf[4..6]) as f32 + )) } /// Accelerometer readings in m/s^2 - pub fn get_acc(&mut self) -> Result> { + pub fn get_acc(&mut self) -> Result, Mpu6050Error> { let mut acc = self.read_rot(ACC_REGX_H.addr())?; - acc.x /= self.acc_sensitivity; - acc.y /= self.acc_sensitivity; - acc.z /= self.acc_sensitivity; + acc /= self.acc_sensitivity; if let Some(ref bias) = self.bias { - acc.x -= bias.ax; - acc.y -= bias.ay; - acc.z -= bias.az; + acc -= bias.acc; } Ok(acc) } /// Accelerometer readings in m/s^2 - averaged - pub fn get_acc_avg(&mut self, steps: Steps) -> Result> { + pub fn get_acc_avg(&mut self, steps: Steps) -> Result, Mpu6050Error> { let mut acc = self.get_acc()?; for _ in 0..steps.0-1 { - acc.add(&self.get_acc()?); + acc += self.get_acc()?; } - acc.scale(steps.0); + acc /= steps.0 as f32; Ok(acc) } /// Gyro readings in rad/s - pub fn get_gyro(&mut self) -> Result> { + pub fn get_gyro(&mut self) -> Result, Mpu6050Error> { let mut gyro = self.read_rot(GYRO_REGX_H.addr())?; - gyro.x *= PI / (180.0 * self.gyro_sensitivity); - gyro.y *= PI / (180.0 * self.gyro_sensitivity); - gyro.z *= PI / (180.0 * self.gyro_sensitivity); + gyro *= PI / (180.0 * self.gyro_sensitivity); if let Some(ref bias) = self.bias { - gyro.x -= bias.gx; - gyro.y -= bias.gy; - gyro.z -= bias.gz; + gyro -= bias.gyro; } Ok(gyro) } /// Gyro readings in rad/s - pub fn get_gyro_avg(&mut self, steps: Steps) -> Result> { + pub fn get_gyro_avg(&mut self, steps: Steps) -> Result, Mpu6050Error> { let mut gyro = self.get_gyro()?; for _ in 0..steps.0-1 { - gyro.add(&self.get_gyro()?); + gyro += self.get_gyro()?; } - gyro.scale(steps.0); + + gyro /= steps.0 as f32; Ok(gyro) } @@ -575,6 +556,7 @@ where #[cfg(test)] mod tests { use super::*; + use nalgebra as na; #[test] fn test_unit_conv() { @@ -590,4 +572,16 @@ mod tests { rad.to_deg_mut(); assert_eq!(rad, 57.295776); } + + #[test] + fn test_nalgebra() { + let mut v = Vector3::::new(1., 1., 1.); + let o = v.clone(); + v *= 3.; + assert_eq!(Vector3::::new(3., 3., 3.), v); + v /= 3.; + assert_eq!(o, v); + v -= o; + assert_eq!(Vector3::::new(0., 0., 0.), v); + } } From 3708b3c35e6480482b411eba209fe9fe51cd87f7 Mon Sep 17 00:00:00 2001 From: juliangaal Date: Mon, 20 May 2019 16:20:34 +0200 Subject: [PATCH 09/22] build example in base cargo.toml with dev-dependencies --- {example/.cargo => .cargo}/config | 0 Cargo.toml | 6 ++++ examples/.cargo/config | 2 ++ {example => examples}/Cargo.toml | 0 examples/main.rs | 52 +++++++++++++++++++++++++++++++ {example => examples}/src/main.rs | 0 6 files changed, 60 insertions(+) rename {example/.cargo => .cargo}/config (100%) create mode 100644 examples/.cargo/config rename {example => examples}/Cargo.toml (100%) create mode 100644 examples/main.rs rename {example => examples}/src/main.rs (100%) diff --git a/example/.cargo/config b/.cargo/config similarity index 100% rename from example/.cargo/config rename to .cargo/config diff --git a/Cargo.toml b/Cargo.toml index 47892e5..6d2d91e 100644 --- a/Cargo.toml +++ b/Cargo.toml @@ -17,3 +17,9 @@ libm = "0.1.2" [dependencies.nalgebra] default-features = false version = "0.18.0" + +[dev-dependencies] +i2cdev = "0.4.1" +linux-embedded-hal = "0.2.2" + + diff --git a/examples/.cargo/config b/examples/.cargo/config new file mode 100644 index 0000000..0c1c209 --- /dev/null +++ b/examples/.cargo/config @@ -0,0 +1,2 @@ +[target.armv7-unknown-linux-gnueabihf] +linker = "arm-linux-gnueabihf-gcc" diff --git a/example/Cargo.toml b/examples/Cargo.toml similarity index 100% rename from example/Cargo.toml rename to examples/Cargo.toml diff --git a/examples/main.rs b/examples/main.rs new file mode 100644 index 0000000..13a2989 --- /dev/null +++ b/examples/main.rs @@ -0,0 +1,52 @@ +use mpu6050::*; +use linux_embedded_hal::{I2cdev, Delay}; +use i2cdev::linux::LinuxI2CError; + +fn main() -> Result<(), Mpu6050Error> { + let i2c = I2cdev::new("/dev/i2c-1") + .map_err(Mpu6050Error::I2c)?; + + let delay = Delay; + + let mut mpu = Mpu6050::new(i2c, delay); + mpu.init()?; + mpu.soft_calib(Steps(100))?; + mpu.calc_variance(Steps(50))?; + + println!("Calibrated with bias: {:?}", mpu.get_bias().unwrap()); + println!("Calculated variance: {:?}", mpu.get_variance().unwrap()); + + loop { + // get roll and pitch estimate + let acc = mpu.get_acc_angles()?; + println!("r/p: {}", acc); + + // get roll and pitch estimate - averaged accross n readings (steps) + let acc = mpu.get_acc_angles_avg(Steps(5))?; + println!("r/p avg: {}", acc); + + // get temp + let temp = mpu.get_temp()?; + println!("temp: {}c", temp); + + // get temp - averages across n readings (steps) + let temp = mpu.get_temp_avg(Steps(5))?; + println!("temp avg: {}c", temp); + + // get gyro data, scaled with sensitivity + let gyro = mpu.get_gyro()?; + println!("gyro: {}", gyro); + + // get gyro data, scaled with sensitivity - averaged across n readings (steps) + let gyro = mpu.get_gyro_avg(Steps(5))?; + println!("gyro avg: {}", gyro); + + // get accelerometer data, scaled with sensitivity + let acc = mpu.get_acc()?; + println!("acc: {}", acc); + + // get accelerometer data, scaled with sensitivity - averages across n readings (steps) + let acc = mpu.get_acc_avg(Steps(5))?; + println!("acc avg: {}", acc); + } +} diff --git a/example/src/main.rs b/examples/src/main.rs similarity index 100% rename from example/src/main.rs rename to examples/src/main.rs From e8db6c2423a24bd7600b370d52212acec5998cc4 Mon Sep 17 00:00:00 2001 From: Julian Gaal Date: Mon, 20 May 2019 17:52:29 +0200 Subject: [PATCH 10/22] Update README.md --- README.md | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/README.md b/README.md index af2f02b..9a906e6 100644 --- a/README.md +++ b/README.md @@ -88,8 +88,10 @@ $ cargo build --target=armv7-unknown-linux-gnueabihf ## TODO - [x] init with default settings - [ ] init with custom settings + - [x] custom sensitivity + - [ ] custom device initialization - [x] device verification -- [ ] basic feature support as described [here](https://github.com/Tijndagamer/mpu6050/blob/master/mpu6050/mpu6050.py) +- [x] basic feature support as described [here](https://github.com/Tijndagamer/mpu6050/blob/master/mpu6050/mpu6050.py) - [x] read gyro data - [x] read acc data - [x] software calibration From ef3779db94fc70d1484e6516b2f4e7adbff701f7 Mon Sep 17 00:00:00 2001 From: Julian Gaal Date: Tue, 21 May 2019 08:57:45 +0200 Subject: [PATCH 11/22] add crates.io badge --- README.md | 7 ++----- 1 file changed, 2 insertions(+), 5 deletions(-) diff --git a/README.md b/README.md index 9a906e6..98e760b 100644 --- a/README.md +++ b/README.md @@ -1,11 +1,8 @@ -# MPU 6050 Rust Driver - -Platform agnostic driver for [MPU 6050 6-axis IMU](https://www.invensense.com/products/motion-tracking/6-axis/mpu-6500/) using [`embedded_hal`](https://github.com/rust-embedded/embedded-hal). +# `mpu6050` ![crates.io](https://img.shields.io/crates/v/mpu6050.svg) +> no_std driver for the MPU6050 6-axis IMU [Datasheet](https://www.invensense.com/wp-content/uploads/2015/02/MPU-6500-Datasheet2.pdf) | [Register Map Sheet](https://www.invensense.com/wp-content/uploads/2015/02/MPU-6000-Register-Map1.pdf) -[Docs](https://docs.rs/mpu6050/0.1.2/mpu6050/) | [Crate](https://crates.io/crates/mpu6050) - Visualization options for testing and development in [viz branch](https://github.com/juliangaal/mpu6050/tree/viz/viz) ### Basic usage From aadf2e6c532d1e541b3f5ab11c0d6202a5aa54fa Mon Sep 17 00:00:00 2001 From: juliangaal Date: Tue, 21 May 2019 09:18:27 +0200 Subject: [PATCH 12/22] bump libm version --- Cargo.toml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Cargo.toml b/Cargo.toml index 47892e5..b06215e 100644 --- a/Cargo.toml +++ b/Cargo.toml @@ -12,7 +12,7 @@ license = "MIT" [dependencies] embedded-hal = "0.2.2" -libm = "0.1.2" +libm = "0.1.3" [dependencies.nalgebra] default-features = false From 55b07342a02ba6c85f4eda583a013fb9ad6f624b Mon Sep 17 00:00:00 2001 From: juliangaal Date: Tue, 21 May 2019 09:19:06 +0200 Subject: [PATCH 13/22] formatting --- Makefile | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/Makefile b/Makefile index 8531891..880b951 100644 --- a/Makefile +++ b/Makefile @@ -8,10 +8,10 @@ build: cargo build ext: - cd $(home)/example && cargo build --$(mode) --target $(target) + cd $(home)/example && cargo build --$(mode) --target=$(target) viz: - cd $(home)/viz/viz && cargo build --$(mode) --target $(target) + cd $(home)/viz/viz && cargo build --$(mode) --target=$(target) deploy: ext scp $(home)/example/target/$(target)/$(mode)/example pi@192.168.1.136: From 59f7834c61f5f1822d37b6a689d7e1978e6d68d8 Mon Sep 17 00:00:00 2001 From: juliangaal Date: Tue, 21 May 2019 09:30:18 +0200 Subject: [PATCH 14/22] use cargo examples structure --- examples/Cargo.toml | 10 ------- examples/{main.rs => linux.rs} | 0 examples/src/main.rs | 52 ---------------------------------- 3 files changed, 62 deletions(-) delete mode 100644 examples/Cargo.toml rename examples/{main.rs => linux.rs} (100%) delete mode 100644 examples/src/main.rs diff --git a/examples/Cargo.toml b/examples/Cargo.toml deleted file mode 100644 index 76a2d21..0000000 --- a/examples/Cargo.toml +++ /dev/null @@ -1,10 +0,0 @@ -[package] -name = "example" -version = "0.1.0" -authors = ["Julian Gaal "] -edition = "2018" - -[dependencies] -mpu6050 = { path = "../" } -i2cdev = "0.4.1" -linux-embedded-hal = "0.2.2" diff --git a/examples/main.rs b/examples/linux.rs similarity index 100% rename from examples/main.rs rename to examples/linux.rs diff --git a/examples/src/main.rs b/examples/src/main.rs deleted file mode 100644 index ae4386a..0000000 --- a/examples/src/main.rs +++ /dev/null @@ -1,52 +0,0 @@ -use mpu6050::*; -use linux_embedded_hal::{I2cdev, Delay}; -use i2cdev::linux::LinuxI2CError; - -fn main() -> Result<(), Mpu6050Error> { - let i2c = I2cdev::new("/dev/i2c-1") - .map_err(Mpu6050Error::I2c)?; - - let delay = Delay; - - let mut mpu = Mpu6050::new(i2c, delay); - mpu.init()?; - mpu.soft_calib(Steps(100))?; - mpu.calc_variance(Steps(50))?; - - println!("Calibrated with bias: {:?}", mpu.get_bias().unwrap()); - println!("Calculated variance: {:?}", mpu.get_variance().unwrap()); - - loop { - // get roll and pitch estimate - let acc = mpu.get_acc_angles()?; - println!("r/p: {:?}", acc); - - // get roll and pitch estimate - averaged accross n readings (steps) - let acc = mpu.get_acc_angles_avg(Steps(5))?; - println!("r/p avg: {:?}", acc); - - // get temp - let temp = mpu.get_temp()?; - println!("temp: {}c", temp); - - // get temp - averages across n readings (steps) - let temp = mpu.get_temp_avg(Steps(5))?; - println!("temp avg: {}c", temp); - - // get gyro data, scaled with sensitivity - let gyro = mpu.get_gyro()?; - println!("gyro: {:?}", gyro); - - // get gyro data, scaled with sensitivity - averaged across n readings (steps) - let gyro = mpu.get_gyro_avg(Steps(5))?; - println!("gyro avg: {:?}", gyro); - - // get accelerometer data, scaled with sensitivity - let acc = mpu.get_acc()?; - println!("acc: {:?}", acc); - - // get accelerometer data, scaled with sensitivity - averages across n readings (steps) - let acc = mpu.get_acc_avg(Steps(5))?; - println!("acc avg: {:?}", acc); - } -} From 182e7b5eca0600915ac67ffdb0f256eb8549be9c Mon Sep 17 00:00:00 2001 From: Julian Gaal Date: Thu, 23 May 2019 08:40:59 +0200 Subject: [PATCH 15/22] apply nix patch --- Cargo.toml | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/Cargo.toml b/Cargo.toml index b3357c6..875dcb0 100644 --- a/Cargo.toml +++ b/Cargo.toml @@ -22,4 +22,6 @@ version = "0.18.0" i2cdev = "0.4.1" linux-embedded-hal = "0.2.2" - +# nix version bump, to fix https://github.com/nix-rust/nix/issues/1057#event-2356640839 +[patch.crates-io] +i2cdev = { git = "https://github.com/juliangaal/rust-i2cdev" } From cefa081630439a481cd5aff70a761ac52d46acb8 Mon Sep 17 00:00:00 2001 From: Julian Gaal Date: Thu, 23 May 2019 08:42:42 +0200 Subject: [PATCH 16/22] adapt Makefile to example structure --- Makefile | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Makefile b/Makefile index 880b951..acb475f 100644 --- a/Makefile +++ b/Makefile @@ -8,7 +8,7 @@ build: cargo build ext: - cd $(home)/example && cargo build --$(mode) --target=$(target) + cargo build --examples --$(mode) --target=$(target) viz: cd $(home)/viz/viz && cargo build --$(mode) --target=$(target) From f42bd47882ea5e9894e72f0663e7339959293a4a Mon Sep 17 00:00:00 2001 From: Julian Gaal Date: Thu, 23 May 2019 08:52:56 +0200 Subject: [PATCH 17/22] rm ununsed imports --- src/lib.rs | 1 - 1 file changed, 1 deletion(-) diff --git a/src/lib.rs b/src/lib.rs index f0bd123..46bdfcc 100644 --- a/src/lib.rs +++ b/src/lib.rs @@ -556,7 +556,6 @@ where #[cfg(test)] mod tests { use super::*; - use nalgebra as na; #[test] fn test_unit_conv() { From f8b64f8fcaafe916477901bdfeb2656d2a9ce51e Mon Sep 17 00:00:00 2001 From: Julian Gaal Date: Thu, 23 May 2019 08:53:52 +0200 Subject: [PATCH 18/22] delete .cargo --- examples/.cargo/config | 2 -- 1 file changed, 2 deletions(-) delete mode 100644 examples/.cargo/config diff --git a/examples/.cargo/config b/examples/.cargo/config deleted file mode 100644 index 0c1c209..0000000 --- a/examples/.cargo/config +++ /dev/null @@ -1,2 +0,0 @@ -[target.armv7-unknown-linux-gnueabihf] -linker = "arm-linux-gnueabihf-gcc" From c4083ea87de0d88dede8cbea01a21563a3e925e8 Mon Sep 17 00:00:00 2001 From: Julian Gaal Date: Wed, 29 May 2019 16:14:20 +0200 Subject: [PATCH 19/22] remove i2cdev patch - fixed in i2cdev 4.1.2 with nix 0.14 --- Cargo.toml | 6 +----- 1 file changed, 1 insertion(+), 5 deletions(-) diff --git a/Cargo.toml b/Cargo.toml index 875dcb0..d8c9086 100644 --- a/Cargo.toml +++ b/Cargo.toml @@ -19,9 +19,5 @@ default-features = false version = "0.18.0" [dev-dependencies] -i2cdev = "0.4.1" +i2cdev = "0.4.2" linux-embedded-hal = "0.2.2" - -# nix version bump, to fix https://github.com/nix-rust/nix/issues/1057#event-2356640839 -[patch.crates-io] -i2cdev = { git = "https://github.com/juliangaal/rust-i2cdev" } From b86b9d77c1975d37b6b42e2b6682cad739e9233f Mon Sep 17 00:00:00 2001 From: juliangaal Date: Thu, 6 Jun 2019 19:29:17 +0200 Subject: [PATCH 20/22] adjust to example restructuring --- Makefile | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/Makefile b/Makefile index acb475f..391f1a4 100644 --- a/Makefile +++ b/Makefile @@ -7,13 +7,13 @@ home = $(shell pwd) build: cargo build -ext: +linux: cargo build --examples --$(mode) --target=$(target) viz: cd $(home)/viz/viz && cargo build --$(mode) --target=$(target) -deploy: ext - scp $(home)/example/target/$(target)/$(mode)/example pi@192.168.1.136: +upload: linux + scp $(home)/target/armv7-unknown-linux-gnueabihf/release/examples/linux pi@192.168.1.136: .PHONY: deploy build ext viz From e25c04d782ff129a1144b596106c6cfd2818d946 Mon Sep 17 00:00:00 2001 From: juliangaal Date: Thu, 6 Jun 2019 19:29:35 +0200 Subject: [PATCH 21/22] update linux example compilation instructions --- README.md | 8 +++----- 1 file changed, 3 insertions(+), 5 deletions(-) diff --git a/README.md b/README.md index 98e760b..aa42ddc 100644 --- a/README.md +++ b/README.md @@ -63,23 +63,21 @@ fn main() -> Result<(), Error> { ``` #### Compile linux example (Raspberry Pi 3B) -files [here](https://github.com/juliangaal/mpu6050/blob/master/example/) Requirements: ```bash -$ apt-get install -qq gcc-armv7-linux-gnueabihf libc6-armhf-cross libc6-dev-armhf-cross +$ apt-get install -qq gcc-arm-linux-gnueabihf libc6-armhf-cross libc6-dev-armhf-cross ``` -and all dependencies in `example/Cargo.toml` Rustup: ```bash $ rustup target add armv7-unknown-linux-gnueabihf ``` -To configure the linker use `example/.cargo/config` +To configure the linker use `.cargo/config` file cross-compile with ```bash -$ cargo build --target=armv7-unknown-linux-gnueabihf +$ cargo build --examples --target=armv7-unknown-linux-gnueabihf ``` ## TODO From dd1d0a176c56142a4c64617a79b9cfadd2dab0b7 Mon Sep 17 00:00:00 2001 From: juliangaal Date: Thu, 6 Jun 2019 19:40:51 +0200 Subject: [PATCH 22/22] bump version: nalgebra migration --- Cargo.toml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Cargo.toml b/Cargo.toml index d8c9086..8302425 100644 --- a/Cargo.toml +++ b/Cargo.toml @@ -1,6 +1,6 @@ [package] name = "mpu6050" -version = "0.1.2" +version = "0.1.3" authors = ["Julian Gaal "] edition = "2018"