ank/mpu6050
1
Fork 0
Code Issues Activity

set range, hpf

Browse source
This commit is contained in:
juliangaal 2021-02-18 12:27:26 +01:00
parent f36aa2c73b
commit ea54b0accf
7 changed files with 217 additions and 93 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

31
examples/bits.rs
View file

@ -1,31 +0,0 @@
use mpu6050::*;
use linux_embedded_hal::{I2cdev, Delay};
use i2cdev::linux::LinuxI2CError;
fn main() -> Result<(), Mpu6050Error<LinuxI2CError>> {
let i2c = I2cdev::new("/dev/i2c-1")
.map_err(Mpu6050Error::I2c)?;
let mut delay = Delay;
let mut mpu = Mpu6050::new(i2c);
mpu.init(&mut delay)?;
println!("{:#?}", mpu.get_gyro_range());
mpu.set_gyro_range(range::GyroRange::D500)?;
use std::{thread, time};
let ten_millis = time::Duration::from_millis(1000);
thread::sleep(ten_millis);
println!("{:#?}", mpu.get_gyro_range());
loop {
// mpu.
}
}
// MPU6050_RA_MOT_DETECT_CTRL, 0x69, = 3
// MPU6050_RA_INT_ENABLE, 0x38, = 1
// MPU6050_RA_ACCEL_CONFIG, 0x1C, = 1
// MPU6050_RA_MOT_THR, 0x1F, = 2
// MPU6050_RA_ZRMOT_DUR, 0x22, = 1

33
examples/test.rs Normal file
View file

@ -0,0 +1,33 @@
use mpu6050::*;
use linux_embedded_hal::{I2cdev, Delay};
use i2cdev::linux::LinuxI2CError;
use mpu6050::device::ACCEL_HPF;
fn main() -> Result<(), Mpu6050Error<LinuxI2CError>> {
let i2c = I2cdev::new("/dev/i2c-1")
.map_err(Mpu6050Error::I2c)?;
let mut delay = Delay;
let mut mpu = Mpu6050::new(i2c);
mpu.init(&mut delay)?;
// Test gyro config
assert_eq!(mpu.get_gyro_range()?, range::GyroRange::D250);
mpu.set_gyro_range(range::GyroRange::D500)?;
assert_eq!(mpu.get_gyro_range()?, range::GyroRange::D500);
// Test accel config
assert_eq!(mpu.get_accel_range()?, range::AccelRange::G2);
mpu.set_accel_range(range::AccelRange::G4)?;
assert_eq!(mpu.get_accel_range()?, range::AccelRange::G4);
// accel_hpf
assert_eq!(mpu.get_accel_hpf()?, ACCEL_HPF::_RESET);
mpu.set_accel_hpf(ACCEL_HPF::_1P25);
assert_eq!(mpu.get_accel_hpf()?, ACCEL_HPF::_1P25);
println!("Test successful");
Ok(())
}

46
src/bits.rs
View file

@ -1,22 +1,33 @@
// Mostly taken from https://github.com/jrowberg/i2cdevlib/blob/master/Arduino/I2Cdev/I2Cdev.cpp
// and tested
//! Bit operations on registers
//! Mostly taken from https://github.com/jrowberg/i2cdevlib/blob/master/Arduino/I2Cdev/I2Cdev.cpp
//! updated and tested
/// get bit n of byte
pub fn get_bit(byte: u8, n: u8) -> u8 {
(byte >> n) & 1
}
/// get bits start - start+length from byte
pub fn get_bits(mut byte: u8, bit_start: u8, length: u8) -> u8 {
// 01101001 read byte
// 76543210 bit numbers
// xxx args: bit_start=4, length=3
// 010 masked
// -> 010 shifted
let mask: u8 = ((1 << length) - 1) << (bit_start - length + 1);
byte &= mask;
byte >>= bit_start - length + 1;
// without mask_shift, strange behavior occurs, wenn bit_start < length.
// e.g. bit_start=2, length = 2
// in SOME cases, you get an 'attempt to subtract with overflow' exception, when
// bitstart - length + 1 = 0
// therefore just "cut off" at 0 shift
let mask_shift: u8 = if bit_start < length { 0 } else { bit_start - length + 1 };
let mask: u8 = ((1 << length) - 1) << mask_shift;
byte &= mask as u8;
byte >>= mask_shift;
byte
}
/// set bit n in byte
pub fn set_bit(byte: &mut u8, n: u8, enable: bool) {
if enable {
*byte |= 1_u8 << n;
@ -25,6 +36,7 @@ pub fn set_bit(byte: &mut u8, n: u8, enable: bool) {
}
}
/// Fill bits bitstart-bitstart+length in byte with data
pub fn set_bits(byte: &mut u8, bit_start: u8, length: u8, mut data: u8) {
/*
010 value to write
@ -35,8 +47,15 @@ pub fn set_bits(byte: &mut u8, bit_start: u8, length: u8, mut data: u8) {
10100011 original & ~mask
10101011 masked | value
*/
let mask: u8 = ((1 << length) - 1) << (bit_start - length + 1);
data <<= bit_start - length + 1; // shift data into correct position
// without mask_shift, strange behavior occurs, wenn bit_start < length.
// e.g. bit_start=2, length = 2
// in SOME cases, you get an 'attempt to subtract with overflow' exception, when
// bitstart - length + 1 = 0
// therefore just "cut off" at 0 shift
let mask_shift: u8 = if bit_start < length { 0 } else { bit_start - length + 1 };
let mask: u8 = ((1 << length) - 1) << mask_shift;
data <<= mask_shift; // shift data into correct position
data &= mask; // zero all non-important bits in data
*byte &= !(mask); // zero all important bits in existing byte
*byte |= data; // combine data with existing byte
@ -45,6 +64,10 @@ pub fn set_bits(byte: &mut u8, bit_start: u8, length: u8, mut data: u8) {
#[cfg(test)]
mod tests {
use super::*;
extern crate std;
use std::*;
use crate::device::*;
#[test]
fn get_bit_test() {
@ -88,5 +111,14 @@ mod tests {
let bits = get_bits(original_value, bitstart, length);
assert_eq!(value, bits);
// simulate accel_hpf
let bitstart = Bits::ACCEL_CONFIG_ACCEL_HPF_BIT;
let length = Bits::ACCEL_CONFIG_ACCEL_HPF_LENGTH;
assert_eq!(get_bits(original_value, bitstart, length), 0b00000011);
let mode: u8 = 7;
set_bits(&mut original_value, bitstart, length, mode);
assert_eq!(get_bits(original_value, bitstart, length), 0b00000111);
}
}

64
src/device.rs
View file

@ -2,8 +2,22 @@
//! Register map: https://arduino.ua/docs/RM-MPU-6000A.pdf
//! Datasheet with WAY more info about interrupts (Revision 3.2) https://www.cdiweb.com/datasheets/invensense/ps-mpu-6000a.pdf
//!
/// Gyro Sensitivity
pub const GYRO_SENS: (f32, f32, f32, f32) = (131., 65.5, 32.8, 16.4);
/// Accelerometer Sensitivity
pub const ACCEL_SENS: (f32, f32, f32, f32) = (16384., 8192., 4096., 2048.);
/// Temperature Offset
pub const TEMP_OFFSET: f32 = 36.53;
/// Temperature Sensitivity
pub const TEMP_SENSITIVITY: f32 = 340.;
#[allow(non_camel_case_types)]
#[derive(Copy, Clone, Debug)]
/// Register addresses
pub enum Registers {
/// Slave address of Mpu6050
SLAVE_ADDR = 0x68,
@ -47,6 +61,54 @@ pub struct Bits;
impl Bits {
/// Accelerometer high pass filter bit: See 4.5 Register 28
pub const ACCEL_HPF_BIT: u8 = 3;
/// Gyro Config FS_SEL start bit
pub const GYRO_CONFIG_FS_SEL_BIT: u8 = 4;
/// Gyro Config FS_SEL length
pub const GYRO_CONFIG_FS_SEL_LENGTH: u8 = 3;
}
/// Accel Config FS_SEL start bit
pub const ACCEL_CONFIG_FS_SEL_BIT: u8 = 4;
/// Accel Config FS_SEL length
pub const ACCEL_CONFIG_FS_SEL_LENGTH: u8 = 2;
/// Accel Config FS_SEL start bit
pub const ACCEL_CONFIG_ACCEL_HPF_BIT: u8 = 2;
/// Accel Config FS_SEL length
pub const ACCEL_CONFIG_ACCEL_HPF_LENGTH: u8 = 3;
}
#[allow(non_camel_case_types)]
#[derive(Copy, Clone, Debug, Eq, PartialEq)]
pub enum ACCEL_HPF {
_RESET = 0,
_5 = 1,
_2P5 = 2,
_1P25 = 3,
_0P63 = 4,
_HOLD = 7
}
impl From<u8> for ACCEL_HPF {
fn from(range: u8) -> Self
{
match range {
0 => ACCEL_HPF::_RESET,
1 => ACCEL_HPF::_5,
2 => ACCEL_HPF::_2P5,
3 => ACCEL_HPF::_1P25,
4 => ACCEL_HPF::_0P63,
7 => ACCEL_HPF::_HOLD,
_ => ACCEL_HPF::_RESET,
}
}
}
//
// #[derive(Copy, Clone, Debug)]
// pub struct BitBlock {
// reg: u8,
// start_bit: u8,
// length: u8
// }
//
// pub const ACONFIG_ACCEL_HBF: BitBlock = BitBlock { reg: Registers::ACCEL_CONFIG.addr(), start_bit: Bits::ACCEL_CONFIG_ACCEL_HBF_BIT, length: Bits::ACCEL_CONFIG_ACCEL_HBF_LENGTH};

67
src/lib.rs
View file

@ -43,12 +43,10 @@
pub mod device;
pub mod bits;
pub mod sensitivity;
pub mod range;
use crate::sensitivity::*;
use crate::range::*;
use crate::device::{Registers::*, Bits};
use crate::device::{*, Registers::*, Bits};
use libm::{powf, atan2f, sqrtf};
use nalgebra::{Vector3, Vector2};
@ -97,8 +95,8 @@ where
pub fn new_with_sens(i2c: I, arange: AccelRange, grange: GyroRange) -> Self {
Mpu6050 {
i2c,
acc_sensitivity: Sensitivity::from(arange).0,
gyro_sensitivity: Sensitivity::from(grange).0,
acc_sensitivity: arange.sensitivity(),
gyro_sensitivity: grange.sensitivity(),
}
}
@ -113,6 +111,9 @@ where
pub fn init<D: DelayMs<u8>>(&mut self, delay: &mut D) -> Result<(), Mpu6050Error<E>> {
self.wake(delay)?;
self.verify()?;
self.set_accel_range(AccelRange::G2)?;
self.set_gyro_range(GyroRange::D250)?;
self.set_accel_hpf(ACCEL_HPF::_RESET)?;
Ok(())
}
@ -125,19 +126,60 @@ where
Ok(())
}
pub fn set_gyro_range(&mut self, scale: GyroRange) -> Result<(), Mpu6050Error<E>> {
/// set accel high pass filter mode
pub fn set_accel_hpf(&mut self, mode: ACCEL_HPF) -> Result<(), Mpu6050Error<E>> {
Ok(
self.write_bits(GYRO_CONFIG.addr(),
Bits::GYRO_CONFIG_FS_SEL_BIT,
Bits::GYRO_CONFIG_FS_SEL_LENGTH,
scale as u8)?
self.write_bits(ACCEL_CONFIG.addr(),
Bits::ACCEL_CONFIG_ACCEL_HPF_BIT,
Bits::ACCEL_CONFIG_ACCEL_HPF_LENGTH, mode as u8)?
)
}
/// get accel high pass filter mode
pub fn get_accel_hpf(&mut self) -> Result<ACCEL_HPF, Mpu6050Error<E>> {
let mode: u8 = self.read_bits(ACCEL_CONFIG.addr(),
Bits::ACCEL_CONFIG_ACCEL_HPF_BIT,
Bits::ACCEL_CONFIG_ACCEL_HPF_LENGTH)?;
Ok(ACCEL_HPF::from(mode))
}
/// Set gyro range, and update sensitivity accordingly
pub fn set_gyro_range(&mut self, range: GyroRange) -> Result<(), Mpu6050Error<E>> {
self.write_bits(GYRO_CONFIG.addr(),
Bits::GYRO_CONFIG_FS_SEL_BIT,
Bits::GYRO_CONFIG_FS_SEL_LENGTH,
range as u8)?;
self.gyro_sensitivity = range.sensitivity();
Ok(())
}
/// set accel range, and update sensitivy accordingly
pub fn set_accel_range(&mut self, range: AccelRange) -> Result<(), Mpu6050Error<E>> {
self.write_bits(ACCEL_CONFIG.addr(),
Bits::ACCEL_CONFIG_FS_SEL_BIT,
Bits::ACCEL_CONFIG_FS_SEL_LENGTH,
range as u8)?;
self.acc_sensitivity = range.sensitivity();
Ok(())
}
/// get current accel_range
pub fn get_accel_range(&mut self) -> Result<AccelRange, Mpu6050Error<E>> {
let byte = self.read_bits(ACCEL_CONFIG.addr(),
Bits::ACCEL_CONFIG_FS_SEL_BIT,
Bits::ACCEL_CONFIG_FS_SEL_LENGTH)?;
Ok(AccelRange::from(byte))
}
/// get current gyro range
pub fn get_gyro_range(&mut self) -> Result<GyroRange, Mpu6050Error<E>> {
let byte = self.read_bits(GYRO_CONFIG.addr(),
Bits::GYRO_CONFIG_FS_SEL_BIT,
Bits::GYRO_CONFIG_FS_SEL_LENGTH)?;
Bits::GYRO_CONFIG_FS_SEL_BIT,
Bits::GYRO_CONFIG_FS_SEL_LENGTH)?;
Ok(GyroRange::from(byte))
}
@ -239,6 +281,7 @@ where
Ok(bits::get_bit(byte[0], bit_n))
}
/// Read bits at register reg, starting with bit start_bit, until start_bit+length
pub fn read_bits(&mut self, reg: u8, start_bit: u8, length: u8) -> Result<u8, Mpu6050Error<E>> {
let mut byte: [u8; 1] = [0; 1];
self.read_bytes(reg, &mut byte)?;

29
src/range.rs
View file

@ -1,5 +1,7 @@
use crate::device::*;
/// Defines accelerometer range/sensivity
#[derive(Debug)]
#[derive(Debug, Eq, PartialEq, Copy, Clone)]
pub enum AccelRange {
G2 = 0,
G4,
@ -8,7 +10,7 @@ pub enum AccelRange {
}
/// Defines gyro range/sensitivity
#[derive(Debug)]
#[derive(Debug, Eq, PartialEq, Copy, Clone)]
pub enum GyroRange {
D250 = 0,
D500,
@ -40,4 +42,27 @@ impl From<u8> for AccelRange {
_ => AccelRange::G2
}
}
}
impl AccelRange {
pub(crate) fn sensitivity(&self) -> f32 {
match &self {
AccelRange::G2 => ACCEL_SENS.0,
AccelRange::G4 => ACCEL_SENS.1,
AccelRange::G8 => ACCEL_SENS.2,
AccelRange::G16 => ACCEL_SENS.3,
}
}
}
// Converts gyro range to correction/scaling factor, see table p. 31 or register sheet
impl GyroRange {
pub(crate) fn sensitivity(&self) -> f32 {
match &self {
GyroRange::D250 => GYRO_SENS.0,
GyroRange::D500 => GYRO_SENS.1,
GyroRange::D1000 => GYRO_SENS.2,
GyroRange::D2000 => GYRO_SENS.3,
}
}
}

40
src/sensitivity.rs
View file

@ -1,40 +0,0 @@
use crate::range::*;
/// Gyro Sensitivity
pub const GYRO_SENS: (f32, f32, f32, f32) = (131., 65.5, 32.8, 16.4);
/// Accelerometer Sensitivity
pub const ACCEL_SENS: (f32, f32, f32, f32) = (16384., 8192., 4096., 2048.);
/// Temperature Offset
pub const TEMP_OFFSET: f32 = 36.53;
/// Temperature Sensitivity
pub const TEMP_SENSITIVITY: f32 = 340.;
// Helper struct to convert Sensor measurement range to appropriate values defined in datasheet
pub(crate) struct Sensitivity(pub(crate) f32);
// Converts accelerometer range to correction/scaling factor, see table p. 29 or register sheet
impl From<AccelRange> for Sensitivity {
fn from(range: AccelRange) -> Sensitivity {
match range {
AccelRange::G2 => return Sensitivity(ACCEL_SENS.0),
AccelRange::G4 => return Sensitivity(ACCEL_SENS.1),
AccelRange::G8 => return Sensitivity(ACCEL_SENS.2),
AccelRange::G16 => return Sensitivity(ACCEL_SENS.3),
}
}
}
// Converts gyro range to correction/scaling factor, see table p. 31 or register sheet
impl From<GyroRange> for Sensitivity {
fn from(range: GyroRange) -> Self {
match range {
GyroRange::D250 => return Sensitivity(GYRO_SENS.0),
GyroRange::D500 => return Sensitivity(GYRO_SENS.1),
GyroRange::D1000 => return Sensitivity(GYRO_SENS.2),
GyroRange::D2000 => return Sensitivity(GYRO_SENS.3),
}
}
}