From 930ceac8023bcd269c5e3455d49ea7078e1184b2 Mon Sep 17 00:00:00 2001 From: Julian Gaal Date: Wed, 17 Apr 2019 19:38:20 +0200 Subject: [PATCH 1/5] delete unnecessary constants file --- src/constants.rs | 42 ------------------------------------------ 1 file changed, 42 deletions(-) delete mode 100644 src/constants.rs diff --git a/src/constants.rs b/src/constants.rs deleted file mode 100644 index b89257d..0000000 --- a/src/constants.rs +++ /dev/null @@ -1,42 +0,0 @@ -//! All constants used in the driver, mostly register addresses - -/// Slave address of Mpu6050 -pub const SLAVE_ADDR: u8 = 0x68; -/// Internal register to check slave addr -pub const WHOAMI: u8 = 0x75; - -/// High Bytle Register Gyro x orientation -pub const GYRO_REGX_H: u8 = 0x43; -/// High Bytle Register Gyro y orientation -pub const GYRO_REGY_H: u8 = 0x45; -/// High Bytle Register Gyro z orientation -pub const GYRO_REGZ_H: u8 = 0x47; - -/// High Byte Register Calc roll -pub const ACC_REGX_H: u8 = 0x3b; -/// High Byte Register Calc pitch -pub const ACC_REGY_H: u8 = 0x3d; -/// High Byte Register Calc yaw -pub const ACC_REGZ_H: u8 = 0x3f; - -/// High Byte Register Temperature -pub const TEMP_OUT_H: u8 = 0x41; - -/// Register to control chip waking from sleep, enabling sensors, default: sleep -pub const POWER_MGMT_1: u8 = 0x6b; -/// Internal clock -pub const POWER_MGMT_2: u8 = 0x6c; - -/// Gyro Sensitivity -pub const FS_SEL: (f32, f32, f32, f32) = (131., 65.5, 32.8, 16.4); -/// Calcelerometer Sensitivity -pub const AFS_SEL: (f32, f32, f32, f32) = (16384., 8192., 4096., 2048.); - -/// Accelerometer config register -pub const ACCEL_CONFIG: u8 = 0x1c; - -/// gyro config register -pub const GYRO_CONFIG: u8 = 0x1b; - -/// pi -pub const PI: f32 = 3.14159; From c10d4dd303dd389adf8bd56ed34ec1624bada358 Mon Sep 17 00:00:00 2001 From: Julian Gaal Date: Wed, 17 Apr 2019 19:39:25 +0200 Subject: [PATCH 2/5] test TODO done --- README.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/README.md b/README.md index fb27f0a..7303311 100644 --- a/README.md +++ b/README.md @@ -79,5 +79,5 @@ $ cargo build --target=armv7-unknown-linux-gnueabihf - [ ] complementary filter for roll, pitch estimate - [ ] sample rate devider with register 25? or 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/)for testing? +- [x] plotting [csv data](https://plot.ly/python/plot-data-from-csv/)for testing? -> See viz branch From a8cee33f9b4523f8748b1be13becf75a20b9732b Mon Sep 17 00:00:00 2001 From: julian Date: Tue, 23 Apr 2019 10:56:54 +0200 Subject: [PATCH 3/5] move main example file --- example/src/bin/main.rs | 31 +++++++++++++++++++++++++++++++ 1 file changed, 31 insertions(+) create mode 100644 example/src/bin/main.rs diff --git a/example/src/bin/main.rs b/example/src/bin/main.rs new file mode 100644 index 0000000..170769c --- /dev/null +++ b/example/src/bin/main.rs @@ -0,0 +1,31 @@ +use mpu6050::*; +use linux_embedded_hal::{I2cdev, Delay}; +use i2cdev::linux::LinuxI2CError; + +fn main() -> Result<(), Error> { + let i2c = I2cdev::new("/dev/i2c-1") + .map_err(Error::I2c)?; + + let delay = Delay; + + let mut mpu = Mpu6050::new(i2c, delay); + mpu.init()?; + + loop { + // get roll and pitch estimate + let acc = mpu.get_acc_angles()?; + println!("r/p: {:?}", acc); + + // get temp + let temp = mpu.get_temp()?; + println!("temp: {}c", temp); + + // get gyro data, scaled with sensitivity + let gyro = mpu.get_gyro()?; + println!("gyro: {:?}", gyro); + + // get accelerometer data, scaled with sensitivity + let acc = mpu.get_acc()?; + println!("acc: {:?}", acc); + } +} From 8c027dd5816013a776810a9d8f245f76972c975d Mon Sep 17 00:00:00 2001 From: juliangaal Date: Tue, 23 Apr 2019 10:57:55 +0200 Subject: [PATCH 4/5] move main example file --- example/src/main.rs | 31 ------------------------------- 1 file changed, 31 deletions(-) delete mode 100644 example/src/main.rs diff --git a/example/src/main.rs b/example/src/main.rs deleted file mode 100644 index 170769c..0000000 --- a/example/src/main.rs +++ /dev/null @@ -1,31 +0,0 @@ -use mpu6050::*; -use linux_embedded_hal::{I2cdev, Delay}; -use i2cdev::linux::LinuxI2CError; - -fn main() -> Result<(), Error> { - let i2c = I2cdev::new("/dev/i2c-1") - .map_err(Error::I2c)?; - - let delay = Delay; - - let mut mpu = Mpu6050::new(i2c, delay); - mpu.init()?; - - loop { - // get roll and pitch estimate - let acc = mpu.get_acc_angles()?; - println!("r/p: {:?}", acc); - - // get temp - let temp = mpu.get_temp()?; - println!("temp: {}c", temp); - - // get gyro data, scaled with sensitivity - let gyro = mpu.get_gyro()?; - println!("gyro: {:?}", gyro); - - // get accelerometer data, scaled with sensitivity - let acc = mpu.get_acc()?; - println!("acc: {:?}", acc); - } -} From 968411999013c5f798be1eb6ce94732f2858248a Mon Sep 17 00:00:00 2001 From: juliangaal Date: Tue, 23 Apr 2019 10:58:15 +0200 Subject: [PATCH 5/5] basic data recorder for later debugging --- example/src/bin/recorder.rs | 62 +++++++++++++++++++++++++++++++++++++ 1 file changed, 62 insertions(+) create mode 100644 example/src/bin/recorder.rs diff --git a/example/src/bin/recorder.rs b/example/src/bin/recorder.rs new file mode 100644 index 0000000..d44e7c8 --- /dev/null +++ b/example/src/bin/recorder.rs @@ -0,0 +1,62 @@ +use mpu6050::*; +use mpu6050::Error as Mpu6050Error; +use linux_embedded_hal::{I2cdev, Delay}; +use i2cdev::linux::LinuxI2CError; +use std::io::prelude::*; +use std::fs::File; +use std::path::Path; +use std::error::Error; + +fn new_file(name: &str) -> File { + let path = Path::new(name); + let display = path.display(); + let _file = match File::create(&path) { + Err(why) => panic!("couldn't create {}: {}", + display, + why.description()), + Ok(file) => return file, + }; +} + +fn write_x_to(file: &mut File, content: String) { + match file.write_all(content.as_bytes()) { + Err(why) => { + println!("couldn't write to file: {}", why.description()); + }, + Ok(_) => {}, + } +} + +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()?; + + + let mut acc_file = new_file("acc_data.txt"); + let mut gyro_file = new_file("gyro_data.txt"); + let mut temp_file = new_file("temp_data.txt"); + let mut angles_file = new_file("angles_data.txt"); + + loop { + // get roll and pitch estimate + let acc = mpu.get_acc_angles()?; + write_x_to(&mut angles_file, format!("{},{}", acc.0, acc.1)); + + // get temp + let temp = mpu.get_temp()?; + write_x_to(&mut temp_file, format!("{}", temp)); + + // get gyro data, scaled with sensitivity + let gyro = mpu.get_gyro()?; + write_x_to(&mut gyro_file, format!("{} {} {}", gyro.0, gyro.1, gyro.2)); + + // get accelerometer data, scaled with sensitivity + let acc = mpu.get_acc()?; + write_x_to(&mut acc_file, format!("{} {} {}", acc.0, acc.1, acc.2)); + } +}