ank/mpu6050
1
Fork 0
Code Issues Activity

Merge pull request #25 from juliangaal/dev

Browse source 
set range (gyro, accel), set hpf (accel), bit operations
This commit is contained in:
Julian Gaal 2021-02-18 12:31:48 +01:00 committed by GitHub
commit 8f97a3ac48
No known key found for this signature in database
GPG key ID: 4AEE18F83AFDEB23
7 changed files with 448 additions and 99 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

0
examples/linux.rs → examples/simple.rs
View file

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(())
}

124
src/bits.rs Normal file
View file

@ -0,0 +1,124 @@
//! 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
// 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;
} else {
*byte &= !(1_u8 << n);
}
}
/// 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
76543210 bit numbers
xxx args: bit_start=4, length=3
00011100 mask byte
10101111 original value (sample)
10100011 original & ~mask
10101011 masked | value
*/
// 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
}
#[cfg(test)]
mod tests {
use super::*;
extern crate std;
use std::*;
use crate::device::*;
#[test]
fn get_bit_test() {
assert_eq!(get_bit(4, 2), 1);
assert_eq!(get_bit(4, 1), 0);
assert_eq!(get_bit(4, 0), 0);
}
#[test]
fn set_bit_test() {
let mut byte = 4_u8.to_be_bytes();
// enable bit 1
set_bit(&mut byte[0], 1, true);
assert_eq!(byte[0], 6);
// disable bit 1
set_bit(&mut byte[0], 1, false);
assert_eq!(byte[0], 4);
// enable bit 3
set_bit(&mut byte[0], 3, true);
assert_eq!(byte[0], 12);
}
#[test]
fn set_get_bits_test() {
// 010 value to write
// 76543210 bit numbers
// xxx args: bit_start=4, length=3
// 00011100 mask byte
// 10101111 original value (sample)
// 10100011 original & ~mask
// 10101011 masked | value
let mut original_value: u8 = 175;
let value: u8 = 2;
let bitstart: u8 = 4;
let length: u8 = 3;
set_bits(&mut original_value, bitstart, length, value);
assert_eq!(original_value, 0b10101011);
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);
}
}

114
src/device.rs Normal file
View file

@ -0,0 +1,114 @@
//! All constants used in the driver, mostly register addresses
//! 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,
/// Internal register to check slave addr
WHOAMI = 0x75,
/// High Byte Register Gyro x orientation
GYRO_REGX_H = 0x43,
/// High Byte Register Gyro y orientation
GYRO_REGY_H = 0x45,
/// High Byte 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,
}
impl Registers {
pub fn addr(&self) -> u8 {
*self as u8
}
}
#[allow(non_camel_case_types)]
#[derive(Copy, Clone, Debug)]
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};

170
src/lib.rs
View file

@ -41,9 +41,13 @@
#![no_std]
pub mod registers;
pub mod device;
pub mod bits;
pub mod range;
use crate::range::*;
use crate::device::{*, Registers::*, Bits};
use crate::registers::Registers::*;
use libm::{powf, atan2f, sqrtf};
use nalgebra::{Vector3, Vector2};
use embedded_hal::{
@ -57,55 +61,6 @@ pub const PI: f32 = core::f32::consts::PI;
/// PI / 180, for conversion to radians
pub const PI_180: f32 = PI / 180.0;
/// Gyro Sensitivity
pub const FS_SEL: (f32, f32, f32, f32) = (131., 65.5, 32.8, 16.4);
/// Accelerometer Sensitivity
pub const AFS_SEL: (f32, f32, f32, f32) = (16384., 8192., 4096., 2048.);
// Helper struct to convert Sensor measurement range to appropriate values defined in datasheet
struct Sensitivity(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(AFS_SEL.0),
AccelRange::G4 => return Sensitivity(AFS_SEL.1),
AccelRange::G8 => return Sensitivity(AFS_SEL.2),
AccelRange::G16 => return Sensitivity(AFS_SEL.3),
}
}
}
// Converts gyro range to correction/scaling factor, see table p. 31 or register sheet
impl From<GyroRange> for Sensitivity {
fn from(range: GyroRange) -> Sensitivity {
match range {
GyroRange::DEG250 => return Sensitivity(FS_SEL.0),
GyroRange::DEG500 => return Sensitivity(FS_SEL.1),
GyroRange::DEG1000 => return Sensitivity(FS_SEL.2),
GyroRange::DEG2000 => return Sensitivity(FS_SEL.3),
}
}
}
/// Defines accelerometer range/sensivity
pub enum AccelRange {
G2,
G4,
G8,
G16,
}
/// Defines gyro range/sensitivity
pub enum GyroRange {
DEG250,
DEG500,
DEG1000,
DEG2000,
}
/// All possible errors in this crate
#[derive(Debug)]
pub enum Mpu6050Error<E> {
@ -131,8 +86,8 @@ where
pub fn new(i2c: I) -> Self {
Mpu6050 {
i2c,
acc_sensitivity: AFS_SEL.0,
gyro_sensitivity: FS_SEL.0,
acc_sensitivity: ACCEL_SENS.0,
gyro_sensitivity: GYRO_SENS.0,
}
}
@ -140,14 +95,14 @@ 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(),
}
}
/// Wakes MPU6050 with all sensors enabled (default)
fn wake<D: DelayMs<u8>>(&mut self, delay: &mut D) -> Result<(), Mpu6050Error<E>> {
self.write_u8(POWER_MGMT_1.addr(), 0)?;
self.write_byte(POWER_MGMT_1.addr(), 0)?;
delay.delay_ms(100u8);
Ok(())
}
@ -156,18 +111,79 @@ 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(())
}
/// Verifies device to address 0x68 with WHOAMI.addr() Register
fn verify(&mut self) -> Result<(), Mpu6050Error<E>> {
let address = self.read_u8(WHOAMI.addr())?;
let address = self.read_byte(WHOAMI.addr())?;
if address != SLAVE_ADDR.addr() {
return Err(Mpu6050Error::InvalidChipId(address));
}
Ok(())
}
/// set accel high pass filter mode
pub fn set_accel_hpf(&mut self, mode: ACCEL_HPF) -> Result<(), Mpu6050Error<E>> {
Ok(
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)?;
Ok(GyroRange::from(byte))
}
/// Roll and pitch estimation from raw accelerometer readings
/// NOTE: no yaw! no magnetometer present on MPU6050
pub fn get_acc_angles(&mut self) -> Result<Vector2<f32>, Mpu6050Error<E>> {
@ -228,11 +244,12 @@ where
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)
// According to revision 4.2
Ok((raw_temp / TEMP_SENSITIVITY) + TEMP_OFFSET)
}
/// Writes byte to register
pub fn write_u8(&mut self, reg: u8, byte: u8) -> Result<(), Mpu6050Error<E>> {
pub fn write_byte(&mut self, reg: u8, byte: u8) -> Result<(), Mpu6050Error<E>> {
self.i2c.write(SLAVE_ADDR.addr(), &[reg, byte])
.map_err(Mpu6050Error::I2c)?;
// delay disabled for dev build
@ -240,9 +257,39 @@ where
// self.delay.delay_ms(10u8);
Ok(())
}
/// Enables bit n at register address reg
pub fn write_bit(&mut self, reg: u8, bit_n: u8, enable: bool) -> Result<(), Mpu6050Error<E>> {
let mut byte: [u8; 1] = [0; 1];
self.read_bytes(reg, &mut byte)?;
bits::set_bit(&mut byte[0], bit_n, enable);
Ok(self.write_byte(reg, byte[0])?)
}
/// Write bits data at reg from start_bit to start_bit+length
pub fn write_bits(&mut self, reg: u8, start_bit: u8, length: u8, data: u8) -> Result<(), Mpu6050Error<E>> {
let mut byte: [u8; 1] = [0; 1];
self.read_bytes(reg, &mut byte)?;
bits::set_bits(&mut byte[0], start_bit, length, data);
Ok(self.write_byte(reg, byte[0])?)
}
/// Read bit n from register
fn read_bit(&mut self, reg: u8, bit_n: u8) -> Result<u8, Mpu6050Error<E>> {
let mut byte: [u8; 1] = [0; 1];
self.read_bytes(reg, &mut byte)?;
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)?;
Ok(bits::get_bits(byte[0], start_bit, length))
}
/// Reads byte from register
pub fn read_u8(&mut self, reg: u8) -> Result<u8, Mpu6050Error<E>> {
pub fn read_byte(&mut self, reg: u8) -> Result<u8, Mpu6050Error<E>> {
let mut byte: [u8; 1] = [0; 1];
self.i2c.write_read(SLAVE_ADDR.addr(), &[reg], &mut byte)
.map_err(Mpu6050Error::I2c)?;
@ -256,3 +303,4 @@ where
Ok(())
}
}

68
src/range.rs Normal file
View file

@ -0,0 +1,68 @@
use crate::device::*;
/// Defines accelerometer range/sensivity
#[derive(Debug, Eq, PartialEq, Copy, Clone)]
pub enum AccelRange {
G2 = 0,
G4,
G8,
G16,
}
/// Defines gyro range/sensitivity
#[derive(Debug, Eq, PartialEq, Copy, Clone)]
pub enum GyroRange {
D250 = 0,
D500,
D1000,
D2000,
}
impl From<u8> for GyroRange {
fn from(range: u8) -> Self
{
match range {
0 => GyroRange::D250,
1 => GyroRange::D500,
2 => GyroRange::D1000,
3 => GyroRange::D2000,
_ => GyroRange::D250
}
}
}
impl From<u8> for AccelRange {
fn from(range: u8) -> Self
{
match range {
0 => AccelRange::G2,
1 => AccelRange::G4,
2 => AccelRange::G8,
3 => AccelRange::G16,
_ => 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,
}
}
}

38
src/registers.rs
View file

@ -1,38 +0,0 @@
//! 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
SLAVE_ADDR = 0x68,
/// Internal register to check slave addr
WHOAMI = 0x75,
/// High Byte Register Gyro x orientation
GYRO_REGX_H = 0x43,
/// High Byte Register Gyro y orientation
GYRO_REGY_H = 0x45,
/// High Byte 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,
}
impl Registers {
pub fn addr(&self) -> u8 {
*self as u8
}
}