merge with dev

Cargo.toml

@@ -1,6 +1,6 @@
 [package]
 name = "mpu6050"
-version = "0.1.2"
+version = "0.1.3"
 authors = ["Julian Gaal <gjulian@uos.de>"]
 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"

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

README.md

@@ -1,14 +1,12 @@
-# MPU 6050 Rust Driver 
+# `mpu6050` ![crates.io](https://img.shields.io/crates/v/mpu6050.svg)
-
+> no_std driver for the MPU6050 6-axis IMU
-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).
 
 [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<LinuxI2CError>> {
     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<LinuxI2CError>> {
 
         // 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

example/Cargo.toml

@@ -1,10 +0,0 @@
-[package]
-name = "example"
-version = "0.1.0"
-authors = ["Julian Gaal <juliangaal@protonmail.com>"]
-edition = "2018"
-
-[dependencies]
-mpu6050 = { path = "../" } 
-i2cdev = "0.4.1"
-linux-embedded-hal = "0.2.2"

examples/linux.rs

@@ -19,11 +19,11 @@ fn main() -> Result<(), Mpu6050Error<LinuxI2CError>> {
     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<LinuxI2CError>> {
 
         // 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);
     }
 }

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.
+use crate::registers::Registers::*;
-///! 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 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<T> {
+    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<T> {
     /// get radians from degree
@@ -54,45 +111,89 @@ impl UnitConv<f32> for f32 {
     }   
 }
 
+impl Access<f32> for Vector3<f32> {
+    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<f32> for Vector2<f32> {
+    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
+    /// accelerometer axis bias
-    ax: f32,
+    acc: Vector3<f32>,
-    /// accelerometer y axis bias
-    ay: f32,
-    /// accelerometer z axis bias
-    az: f32,
     /// gyro x axis bias
-    gx: f32,
+    gyro: Vector3<f32>,
-    /// gyro y axis bias
-    gy: f32,
-    /// gyro z axis bias
-    gz: f32,
     /// temperature AVERAGE: can't get bias!
-    t: f32,
+    temp: f32,
+}
+
+impl Default for Bias {
+    fn default() -> Bias {
+        Bias {
+            acc: Vector3::<f32>::zeros(),
+            gyro: Vector3::<f32>::zeros(),
+            temp: 0.0,
+        }
+    }
 }
 
 impl Bias {
-    fn add(&mut self, acc: RotReading, gyro: RotReading, temp: f32) {
+    fn add(&mut self, acc: Vector3<f32>, gyro: Vector3<f32>, temp: f32) {
-        self.ax += acc.x;
+        self.acc += acc;
-        self.ay += acc.y;
+        self.gyro += gyro;
-        self.az += acc.z;
+        self.temp += temp; 
-        self.gx += gyro.x;
-        self.gy += gyro.y;
-        self.gz += gyro.z;
-        self.t += temp; 
     }
 
     fn scale(&mut self, n: u8) {
         let n = n as f32;
-        self.ax /= n;
+        self.acc /= n;
-        self.ay /= n;
+        self.gyro /= n;
-        self.az /= n;
+        self.temp /= n;
-        self.gx /= n;
-        self.gy /= n;
-        self.gz /= n;
-        self.t /= 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) {
+    fn add_diff(&mut self, acc_diff: Vector3<f32>, gyro_diff: Vector3<f32>, temp_diff: f32) {
-        self.ax += acc_diff.0;
+        self.acc += acc_diff;
-        self.ay += acc_diff.1;
+        self.gyro += gyro_diff;
-        self.az += acc_diff.2;
+        self.temp += temp_diff; 
-        self.gx += gyro_diff.0;
-        self.gy += gyro_diff.1;
-        self.gz += gyro_diff.2;
-        self.t += temp_diff; 
     }
 }
 
-/// Struct for rotation reading: gyro or accelerometer.
+/// Vector2 for Roll/Pitch Reading
-/// see indivisual type definitions
+impl UnitConv<Vector2<f32>> for Vector2<f32> {
-#[derive(Debug)]
+    fn to_rad(&self) -> Vector2<f32> {
-pub struct RotReading {
+        Vector2::<f32>::new(
-    pub x: f32,
+            self.x().to_rad(),
-    pub y: f32,
+            self.y().to_rad(),
-    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<RPReading> for RPReading {
-    fn to_rad(&self) -> RPReading {
-        RPReading {
-            roll: self.roll.to_rad(),
-            pitch: self.pitch.to_rad(),
-        }
     }
 
     fn to_rad_mut(&mut self) {
-        self.roll.to_rad_mut();
+        self[0].to_rad_mut();
-        self.pitch.to_rad_mut();
+        self[1].to_rad_mut();
     }
 
-    fn to_deg(&self) -> RPReading {
+    fn to_deg(&self) -> Vector2<f32> {
-        RPReading {
+        Vector2::<f32>::new(
-            roll: self.roll.to_deg(),
+            self.x().to_deg(),
-            pitch: self.pitch.to_deg(),
+            self.y().to_deg(),
-        }
+        )
     }
 
     fn to_deg_mut(&mut self) {
-        self.roll.to_deg_mut();
+        self[0].to_deg_mut();
-        self.pitch.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<E>> {
-        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<E>> {
-        let address = self.read_u8(WHOAMI)?;
+        let address = self.read_u8(WHOAMI.addr())?;
-        if address != SLAVE_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 acc_diff = Vector3::<f32>::zeros(); 
-        let mut gyro_diff: (f32, f32, f32);
+        let mut gyro_diff = Vector3::<f32>::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)); 
+           acc_diff.set_x(powf(acc.x() - bias.acc.x(), 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));
+           acc_diff.set_y(powf(acc.y() - bias.acc.y(), 2.0));
-           temp_diff = powf(temp - bias.t, 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<RPReading, Mpu6050Error<E>> {
+    pub fn get_acc_angles(&mut self) -> Result<Vector2<f32>, Mpu6050Error<E>> {
         let acc = self.get_acc()?;
-        let roll: f32 = atan2f(acc.y, sqrtf(powf(acc.x, 2.) + powf(acc.z, 2.)));
+        Ok(Vector2::<f32>::new(
-        let pitch: f32 = atan2f(-acc.x, sqrtf(powf(acc.y, 2.) + powf(acc.z, 2.)));
+            atan2f(acc.y(), sqrtf(powf(acc.x(), 2.) + powf(acc.z(), 2.))),
-        Ok(RPReading::new(roll, pitch))
+            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<RPReading, Mpu6050Error<E>> {
+    pub fn get_acc_angles_avg(&mut self, steps: Steps) -> Result<Vector2<f32>, Mpu6050Error<E>> {
         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<RotReading, Mpu6050Error<E>> {
+    fn read_rot(&mut self, reg: u8) -> Result<Vector3<f32>, Mpu6050Error<E>> {
         let mut buf: [u8; 6] = [0; 6];
         self.read_bytes(reg, &mut buf)?;
 
-        let xr = self.read_word_2c(&buf[0..2]);
+        Ok(Vector3::<f32>::new(
-        let yr = self.read_word_2c(&buf[2..4]);
+            self.read_word_2c(&buf[0..2]) as f32,
-        let zr = self.read_word_2c(&buf[4..6]);
+            self.read_word_2c(&buf[2..4]) as f32,
-        
+            self.read_word_2c(&buf[4..6]) as f32
-        Ok(RotReading::new(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<AccReading, Mpu6050Error<E>> {
+    pub fn get_acc(&mut self) -> Result<Vector3<f32>, Mpu6050Error<E>> {
-        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 /= self.acc_sensitivity;
-        acc.y /= self.acc_sensitivity;
-        acc.z /= self.acc_sensitivity;
 
         if let Some(ref bias) = self.bias { 
-            acc.x -= bias.ax;
+            acc -= bias.acc;
-            acc.y -= bias.ay;
-            acc.z -= bias.az;
         }
 
         Ok(acc)
     }
 
     /// Accelerometer readings in m/s^2 - averaged
-    pub fn get_acc_avg(&mut self, steps: Steps) -> Result<AccReading, Mpu6050Error<E>> {
+    pub fn get_acc_avg(&mut self, steps: Steps) -> Result<Vector3<f32>, Mpu6050Error<E>> {
         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<GyroReading, Mpu6050Error<E>> {
+    pub fn get_gyro(&mut self) -> Result<Vector3<f32>, Mpu6050Error<E>> {
-        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 *= PI / (180.0 * self.gyro_sensitivity);
-        gyro.y *= PI / (180.0 * self.gyro_sensitivity);
-        gyro.z *= PI / (180.0 * self.gyro_sensitivity);
 
         if let Some(ref bias) = self.bias {
-            gyro.x -= bias.gx;
+            gyro -= bias.gyro;
-            gyro.y -= bias.gy;
-            gyro.z -= bias.gz;
         }
 
         Ok(gyro)
     }
     
     /// Gyro readings in rad/s
-    pub fn get_gyro_avg(&mut self, steps: Steps) -> Result<GyroReading, Mpu6050Error<E>> {
+    pub fn get_gyro_avg(&mut self, steps: Steps) -> Result<Vector3<f32>, Mpu6050Error<E>> {
         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<f32, Mpu6050Error<E>> {
         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<E>> {
-        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<u8, Mpu6050Error<E>> {
         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<E>> {
-        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::<f32>::new(1., 1., 1.);
+        let o = v.clone();
+        v *= 3.;
+        assert_eq!(Vector3::<f32>::new(3., 3., 3.), v);
+        v /= 3.;
+        assert_eq!(o, v);
+        v -= o;
+        assert_eq!(Vector3::<f32>::new(0., 0., 0.), v);
+    }
 }

src/registers.rs

@@ -1,42 +1,38 @@
 //! All constants used in the driver, mostly register addresses
 
+#[allow(non_camel_case_types)]
+#[derive(Copy, Clone, Debug)]
+pub enum Registers {
     /// Slave address of Mpu6050
-pub const SLAVE_ADDR: u8 = 0x68;
+    SLAVE_ADDR = 0x68,
     /// Internal register to check slave addr
-pub const WHOAMI: u8 = 0x75;
+    WHOAMI = 0x75,
-
     /// High Bytle Register Gyro x orientation
-pub const GYRO_REGX_H: u8 = 0x43;
+    GYRO_REGX_H = 0x43,
     /// High Bytle Register Gyro y orientation
-pub const GYRO_REGY_H: u8 = 0x45;
+    GYRO_REGY_H = 0x45,
     /// High Bytle Register Gyro z orientation
-pub const GYRO_REGZ_H: u8 = 0x47;
+    GYRO_REGZ_H = 0x47,
- 
     /// High Byte Register Calc roll
-pub const ACC_REGX_H: u8 = 0x3b;
+    ACC_REGX_H = 0x3b,
     /// High Byte Register Calc pitch
-pub const ACC_REGY_H: u8 = 0x3d;
+    ACC_REGY_H = 0x3d,
     /// High Byte Register Calc yaw
-pub const ACC_REGZ_H: u8 = 0x3f;
+    ACC_REGZ_H = 0x3f,
-
     /// High Byte Register Temperature
-pub const TEMP_OUT_H: u8 = 0x41;
+    TEMP_OUT_H = 0x41,
-
     /// Register to control chip waking from sleep, enabling sensors, default: sleep
-pub const POWER_MGMT_1: u8 = 0x6b; 
+    POWER_MGMT_1 = 0x6b, 
     /// Internal clock
-pub const POWER_MGMT_2: u8 = 0x6c; 
+    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
-pub const ACCEL_CONFIG: u8  = 0x1c;
+    ACCEL_CONFIG  = 0x1c,
-
     /// gyro config register
-pub const GYRO_CONFIG: u8 = 0x1b;
+    GYRO_CONFIG = 0x1b,
+}
 
-/// pi
+impl Registers {
-pub const PI: f32 = 3.14159265358979323846;
+    pub fn addr(&self) -> u8 {
+        *self as u8
+    } 
+}