diff --git a/examples/calibrate.rs b/examples/calibrate.rs new file mode 100644 index 0000000..20f37e6 --- /dev/null +++ b/examples/calibrate.rs @@ -0,0 +1,99 @@ +// Finds offset values to calibrate the sensor. +// Before running this example, you should let your mpu6050 warm up for around 10 minutes until it reaches a stable temperature +// (because temperature affects sensor readings). + +use mpu6050::*; +use device::{AccelRange, GyroRange}; +use linux_embedded_hal::{I2cdev, i2cdev::linux::LinuxI2CError, Delay}; +use embedded_hal::blocking::delay::DelayMs; + +//Change this line to the error type of your I2c backend. +type I2cErr = LinuxI2CError; + +//Change this line to the name of the main struct of your I2c backend. +type I2c = I2cdev; + +const ITERATIONS: i32 = 1000; +const ACC_ZERO: [f32; 3] = [0.0, 0.0, 1.0]; +const GYRO_ZERO: [f32; 3] = [0.0, 0.0, 0.0]; +const SPEED: f32 = 0.1; //This value determines how precise the offsets are. By making this value smaller, you can increase the precision of the offsets, but you should also increase the number of iterations accordingly. +const DELAY: u16 = 10; //The number of milliseconds between each reading. + +const TEST_ITERATIONS: i32 = 1000; + +fn main() -> Result<(), Mpu6050Error> { + let i2c = I2c:: + + new("/dev/i2c-1") //Change this line to whatever is required to create your I2c backend. + + .map_err(Mpu6050Error::I2c)?; + let mut delay = Delay; + let mut mpu = Mpu6050::new_with_sens(i2c, + + AccelRange::G2, GyroRange::D250 //Change these sensitivities to the ones you use. + + ); + + mpu.init(&mut delay)?; + println!("---CALCULATING OFFSETS---"); + //Calculate offsets + for i in 0..ITERATIONS { + let acc = mpu.get_acc()?; + let gyro = mpu.get_gyro()?; + + let delta = ACC_ZERO[0] - acc.x; + mpu.acc_offsets.x += delta * SPEED; + let delta = ACC_ZERO[1] - acc.y; + mpu.acc_offsets.y += delta * SPEED; + let delta = ACC_ZERO[2] - acc.z; + mpu.acc_offsets.z += delta * SPEED; + + let delta = GYRO_ZERO[0] - gyro.x; + mpu.gyro_offsets.x += delta * SPEED; + let delta = GYRO_ZERO[1] - gyro.y; + mpu.gyro_offsets.y += delta * SPEED; + let delta = GYRO_ZERO[2] - gyro.z; + mpu.gyro_offsets.z += delta * SPEED; + + if i % (ITERATIONS / 10) == 0 {println!("{} Iterations \ncurrent Acc_offsets: {}, current Gyro_offsets: {}", i, mpu.acc_offsets, mpu.gyro_offsets);} + + delay.delay_ms(DELAY); + + + } + println!("---FINISHED---"); + println!("Offsets: acc: {}, gyro: {}", mpu.acc_offsets, mpu.gyro_offsets); + //Testing offsets + println!("---TESTING---"); + let inverse_i = 1.0 / TEST_ITERATIONS as f32; + let mut error = 0.0; + for i in 0..TEST_ITERATIONS { + let acc = mpu.get_acc()?; + let gyro = mpu.get_gyro()?; + + error += {let delta = ACC_ZERO[0] - acc.x; delta * delta} * inverse_i; + error += {let delta = ACC_ZERO[1] - acc.y; delta * delta} * inverse_i; + error += {let delta = ACC_ZERO[2] - acc.z; delta * delta} * inverse_i; + + error += {let delta = GYRO_ZERO[0] - gyro.x; delta * delta} * inverse_i; + error += {let delta = GYRO_ZERO[1] - gyro.y; delta * delta} * inverse_i; + error += {let delta = GYRO_ZERO[2] - gyro.z; delta * delta} * inverse_i; + + if i % (TEST_ITERATIONS / 3) == 0 {println!("current Acc_readings: {}, current Gyro_readings: {}", acc, gyro);} + + delay.delay_ms(DELAY); + } + + println!("---FINISHED---"); + println!("average error: {}", error); + println!("\nYou can copy and paste the following into your code:\n"); + println!("//Accelerometer offsets of Mpu6050"); + println!("mpu.acc_offsets.x = {};", mpu.acc_offsets.x); + println!("mpu.acc_offsets.y = {};", mpu.acc_offsets.y); + println!("mpu.acc_offsets.z = {};", mpu.acc_offsets.z); + println!("//Gyroscope offsets of Mpu6050"); + println!("mpu.gyro_offsets.x = {};", mpu.gyro_offsets.x); + println!("mpu.gyro_offsets.y = {};", mpu.gyro_offsets.y); + println!("mpu.gyro_offsets.z = {};", mpu.gyro_offsets.z); + Ok(()) +} diff --git a/examples/motion_detection.rs b/examples/motion_detection.rs index 25e9299..0e5e01d 100644 --- a/examples/motion_detection.rs +++ b/examples/motion_detection.rs @@ -1,6 +1,5 @@ use mpu6050::{*, device::MOT_DETECT_STATUS}; -use linux_embedded_hal::{I2cdev, Delay}; -use i2cdev::linux::LinuxI2CError; +use linux_embedded_hal::{I2cdev, i2cdev::linux::LinuxI2CError, Delay}; use embedded_hal::blocking::delay::DelayMs; fn main() -> Result<(), Mpu6050Error> { diff --git a/examples/simple.rs b/examples/simple.rs index 9a78468..12a42d4 100644 --- a/examples/simple.rs +++ b/examples/simple.rs @@ -1,6 +1,5 @@ use mpu6050::*; -use linux_embedded_hal::{I2cdev, Delay}; -use i2cdev::linux::LinuxI2CError; +use linux_embedded_hal::{I2cdev, i2cdev::linux::LinuxI2CError, Delay}; fn main() -> Result<(), Mpu6050Error> { let i2c = I2cdev::new("/dev/i2c-1") diff --git a/examples/test.rs b/examples/test.rs index 4be8015..b864466 100644 --- a/examples/test.rs +++ b/examples/test.rs @@ -1,7 +1,6 @@ use mpu6050::{*, device::*}; -use linux_embedded_hal::{I2cdev, Delay}; -use i2cdev::linux::LinuxI2CError; +use linux_embedded_hal::{I2cdev, i2cdev::linux::LinuxI2CError, Delay}; use mpu6050::device::{ACCEL_HPF, CLKSEL}; fn main() -> Result<(), Mpu6050Error> { diff --git a/src/lib.rs b/src/lib.rs index de1524f..693213f 100644 --- a/src/lib.rs +++ b/src/lib.rs @@ -79,6 +79,12 @@ pub struct Mpu6050 { slave_addr: u8, acc_sensitivity: f32, gyro_sensitivity: f32, + /// The offsets for the accelerometer, used for calibration. This offset is applied automatically in `fn get_acc()`, directly to the raw values. + /// This offset is different for each sensitivity mode, so you should generate it for each mode, and change it when switching modes. + pub acc_offsets: Vector3, + /// The offsets for the gyroscope, used for calibration. This offset is applied automatically in `fn get_gyro()`, directly to the raw values. + /// This offset is different for each sensitivity mode, so you should generate it for each mode, and change it when switching modes. + pub gyro_offsets: Vector3, } impl Mpu6050 @@ -92,6 +98,8 @@ where slave_addr: DEFAULT_SLAVE_ADDR, acc_sensitivity: ACCEL_SENS.0, gyro_sensitivity: GYRO_SENS.0, + acc_offsets: Default::default(), + gyro_offsets: Default::default(), } } @@ -102,6 +110,8 @@ where slave_addr: DEFAULT_SLAVE_ADDR, acc_sensitivity: arange.sensitivity(), gyro_sensitivity: grange.sensitivity(), + acc_offsets: Default::default(), + gyro_offsets: Default::default(), } } @@ -112,6 +122,8 @@ where slave_addr, acc_sensitivity: ACCEL_SENS.0, gyro_sensitivity: GYRO_SENS.0, + acc_offsets: Default::default(), + gyro_offsets: Default::default(), } } @@ -122,6 +134,8 @@ where slave_addr, acc_sensitivity: arange.sensitivity(), gyro_sensitivity: grange.sensitivity(), + acc_offsets: Default::default(), + gyro_offsets: Default::default(), } } @@ -356,6 +370,7 @@ where pub fn get_acc(&mut self) -> Result, Mpu6050Error> { let mut acc = self.read_rot(ACC_REGX_H)?; acc /= self.acc_sensitivity; + acc += self.acc_offsets; Ok(acc) } @@ -365,6 +380,7 @@ where let mut gyro = self.read_rot(GYRO_REGX_H)?; gyro *= PI_180 / self.gyro_sensitivity; + gyro += self.gyro_offsets; Ok(gyro) }