ank/mpu6050
1
Fork 0
Code Issues Activity

implement readbit(s), writebit(s)

Browse source
This commit is contained in:
juliangaal 2021-02-18 01:30:53 +01:00
parent e7c0e5ac20
commit f36aa2c73b
7 changed files with 225 additions and 115 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

31
examples/bits.rs Normal file
View file

@ -0,0 +1,31 @@
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

31
examples/motion_detection.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)?;
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);
}
}

76
src/bits.rs
View file

@ -1,44 +1,92 @@
// Mostly taken from https://github.com/jrowberg/i2cdevlib/blob/master/Arduino/I2Cdev/I2Cdev.cpp
// and tested
pub fn get_bit_n(byte: &[u8; 1], n: u8) -> u8 {
(byte[0] >> n) & 1
pub fn get_bit(byte: u8, n: u8) -> u8 {
(byte >> n) & 1
}
pub fn set_bit_n(byte: &mut [u8; 1], n: u8, enable: bool) {
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;
byte
}
pub fn set_bit(byte: &mut u8, n: u8, enable: bool) {
if enable {
byte[0] |= 1_u8 << n;
*byte |= 1_u8 << n;
} else {
byte[0] &= !(1_u8 << n);
*byte &= !(1_u8 << n);
}
}
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
*/
let mask: u8 = ((1 << length) - 1) << (bit_start - length + 1);
data <<= bit_start - length + 1; // 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::*;
#[test]
fn get_bit_n_test() {
let byte = 4_u8.to_be_bytes();
assert_eq!(get_bit_n(&byte, 2), 1);
assert_eq!(get_bit_n(&byte, 1), 0);
assert_eq!(get_bit_n(&byte, 0), 0);
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_n_test() {
fn set_bit_test() {
let mut byte = 4_u8.to_be_bytes();
// enable bit 1
set_bit_n(&mut byte, 1, true);
set_bit(&mut byte[0], 1, true);
assert_eq!(byte[0], 6);
// disable bit 1
set_bit_n(&mut byte, 1, false);
set_bit(&mut byte[0], 1, false);
assert_eq!(byte[0], 4);
// enable bit 3
set_bit_n(&mut byte, 3, true);
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);
}
}

13
src/registers.rs → src/device.rs
View file

@ -41,13 +41,12 @@ impl Registers {
#[allow(non_camel_case_types)]
#[derive(Copy, Clone, Debug)]
pub enum Bits {
/// Accelerometer high pass filter bit: See 4.5 Register 28
ACCEL_HPF_BIT = 2,
}
pub struct Bits;
impl Bits {
pub fn byte(&self) -> u8 {
*self as u8
}
/// Accelerometer high pass filter bit: See 4.5 Register 28
pub const ACCEL_HPF_BIT: u8 = 3;
pub const GYRO_CONFIG_FS_SEL_BIT: u8 = 4;
pub const GYRO_CONFIG_FS_SEL_LENGTH: u8 = 3;
}

106
src/lib.rs
View file

@ -41,17 +41,21 @@
#![no_std]
pub mod registers;
mod bits;
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::registers::Registers::*;
use libm::{powf, atan2f, sqrtf};
use nalgebra::{Vector3, Vector2};
use embedded_hal::{
blocking::delay::DelayMs,
blocking::i2c::{Write, WriteRead},
};
use crate::registers::Registers;
/// PI, f32
pub const PI: f32 = core::f32::consts::PI;
@ -59,61 +63,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.);
/// 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
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> {
@ -139,8 +88,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,
}
}
@ -176,6 +125,23 @@ where
Ok(())
}
pub fn set_gyro_range(&mut self, scale: GyroRange) -> 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)?
)
}
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>> {
@ -254,7 +220,15 @@ where
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_n(byte[0], bit_n, enable);
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])?)
}
@ -262,7 +236,13 @@ where
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_n(&byte, bit_n))
Ok(bits::get_bit(byte[0], bit_n))
}
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

43
src/range.rs Normal file
View file

@ -0,0 +1,43 @@
/// Defines accelerometer range/sensivity
#[derive(Debug)]
pub enum AccelRange {
G2 = 0,
G4,
G8,
G16,
}
/// Defines gyro range/sensitivity
#[derive(Debug)]
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
}
}
}

40
src/sensitivity.rs Normal file
View file

@ -0,0 +1,40 @@
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),
}
}
}