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 01c4367..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" @@ -12,4 +12,12 @@ license = "MIT" [dependencies] embedded-hal = "0.2.2" -libm = "0.1.2" +libm = "0.1.3" + +[dependencies.nalgebra] +default-features = false +version = "0.18.0" + +[dev-dependencies] +i2cdev = "0.4.2" +linux-embedded-hal = "0.2.2" diff --git a/Makefile b/Makefile new file mode 100644 index 0000000..391f1a4 --- /dev/null +++ b/Makefile @@ -0,0 +1,19 @@ +target = armv7-unknown-linux-gnueabihf +mode = release +home = $(shell pwd) + +.DEFAULT_GOAL = build + +build: + cargo build + +linux: + cargo build --examples --$(mode) --target=$(target) + +viz: + cd $(home)/viz/viz && cargo build --$(mode) --target=$(target) + +upload: linux + scp $(home)/target/armv7-unknown-linux-gnueabihf/release/examples/linux pi@192.168.1.136: + +.PHONY: deploy build ext viz diff --git a/README.md b/README.md index 0bcf69d..79d13d4 100644 --- a/README.md +++ b/README.md @@ -1,14 +1,12 @@ -# 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 - [`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}; @@ -31,11 +29,11 @@ fn main() -> Result<(), Error> { loop { // get roll and pitch estimate let acc = mpu.get_acc_angles()?; - println!("r/p: {:?}", acc); - + 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); + println!("r/p avg: {}", acc); // get temp let temp = mpu.get_temp()?; @@ -47,49 +45,48 @@ fn main() -> Result<(), Error> { // get gyro data, scaled with sensitivity let gyro = mpu.get_gyro()?; - println!("gyro: {:?}", 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); + println!("gyro avg: {}", gyro); // get accelerometer data, scaled with sensitivity let acc = mpu.get_acc()?; - println!("acc: {:?}", 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); - + println!("acc avg: {}", acc); + } } ``` -*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/) 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 - [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 diff --git a/example/Cargo.toml b/example/Cargo.toml deleted file mode 100644 index 76a2d21..0000000 --- a/example/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/example/src/main.rs b/examples/linux.rs similarity index 86% rename from example/src/main.rs rename to examples/linux.rs index ae4386a..13a2989 100644 --- a/example/src/main.rs +++ b/examples/linux.rs @@ -19,11 +19,11 @@ fn main() -> Result<(), Mpu6050Error> { loop { // get roll and pitch estimate let acc = mpu.get_acc_angles()?; - println!("r/p: {:?}", acc); - + 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); + println!("r/p avg: {}", acc); // get temp let temp = mpu.get_temp()?; @@ -35,18 +35,18 @@ fn main() -> Result<(), Mpu6050Error> { // get gyro data, scaled with sensitivity let gyro = mpu.get_gyro()?; - println!("gyro: {:?}", 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); + println!("gyro avg: {}", gyro); // get accelerometer data, scaled with sensitivity let acc = mpu.get_acc()?; - println!("acc: {:?}", 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); + println!("acc avg: {}", acc); } } diff --git a/src/lib.rs b/src/lib.rs index deb7f1d..46bdfcc 100644 --- a/src/lib.rs +++ b/src/lib.rs @@ -1,13 +1,55 @@ +//! 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))?; +//! ``` + #![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::*; +use crate::registers::Registers::*; use libm::{powf, atan2f, sqrtf}; +use nalgebra::{Vector3, Vector2}; use embedded_hal::{ blocking::delay::DelayMs, blocking::i2c::{Write, WriteRead}, @@ -15,6 +57,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 { @@ -24,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 @@ -54,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; } } @@ -100,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(); } } @@ -302,7 +332,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 +344,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(()) @@ -334,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(()) @@ -357,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()?; @@ -373,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(()) @@ -386,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) } @@ -418,75 +453,68 @@ 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> { - let mut acc = self.read_rot(ACC_REGX_H)?; + 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> { - let mut gyro = self.read_rot(GYRO_REGX_H)?; + 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) } /// 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 +531,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 +540,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(()) } @@ -543,4 +571,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); + } } diff --git a/src/registers.rs b/src/registers.rs index 468a8b4..f9b2998 100644 --- a/src/registers.rs +++ b/src/registers.rs @@ -1,42 +1,38 @@ //! 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 + 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, +} -/// 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.14159265358979323846; +impl Registers { + pub fn addr(&self) -> u8 { + *self as u8 + } +}