enable CLK_SEL control
This commit is contained in:
juliangaal
parent
0ab57cadf4
commit
5f51ec136e
4 changed files with 178 additions and 107 deletions
|
|
@ -2,7 +2,7 @@ use mpu6050::*;
|
|||
|
||||
use linux_embedded_hal::{I2cdev, Delay};
|
||||
use i2cdev::linux::LinuxI2CError;
|
||||
use mpu6050::device::ACCEL_HPF;
|
||||
use mpu6050::device::{ACCEL_HPF, CLKSEL};
|
||||
|
||||
fn main() -> Result<(), Mpu6050Error<LinuxI2CError>> {
|
||||
let i2c = I2cdev::new("/dev/i2c-1")
|
||||
|
|
@ -28,20 +28,27 @@ fn main() -> Result<(), Mpu6050Error<LinuxI2CError>> {
|
|||
mpu.set_accel_range(range::AccelRange::G4)?;
|
||||
assert_eq!(mpu.get_accel_range()?, range::AccelRange::G4);
|
||||
|
||||
// accel_hpf
|
||||
// accel_hpf: per default RESET/no filter, see ACCEL_CONFIG
|
||||
println!("Test 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);
|
||||
mpu.set_accel_hpf(ACCEL_HPF::_2P5)?;
|
||||
assert_eq!(mpu.get_accel_hpf()?, ACCEL_HPF::_2P5);
|
||||
mpu.set_accel_hpf(ACCEL_HPF::_5)?;
|
||||
assert_eq!(mpu.get_accel_hpf()?, ACCEL_HPF::_5);
|
||||
mpu.set_accel_hpf(ACCEL_HPF::_0P63)?;
|
||||
assert_eq!(mpu.get_accel_hpf()?, ACCEL_HPF::_0P63);
|
||||
mpu.set_accel_hpf(ACCEL_HPF::_HOLD)?;
|
||||
assert_eq!(mpu.get_accel_hpf()?, ACCEL_HPF::_HOLD);
|
||||
|
||||
// test sleep
|
||||
// test sleep. Default no, in wake()
|
||||
println!("Test sleep");
|
||||
assert_eq!(mpu.get_sleep_enabled()?, false);
|
||||
mpu.set_sleep_enabled(true)?;
|
||||
assert_eq!(mpu.get_sleep_enabled()?, true);
|
||||
mpu.set_sleep_enabled(false)?;
|
||||
assert_eq!(mpu.get_sleep_enabled()?, false);
|
||||
// mpu.set_sleep_enabled(true)?;
|
||||
|
||||
// test temp enable/disable
|
||||
println!("Test temp enable/disable");
|
||||
|
|
@ -52,6 +59,24 @@ fn main() -> Result<(), Mpu6050Error<LinuxI2CError>> {
|
|||
assert_eq!(mpu.get_temp_enabled()?, true);
|
||||
assert_ne!(mpu.get_temp()?, 36.53);
|
||||
|
||||
// Test clksel: GXAXIS per default, set in wake()
|
||||
println!("Test CLKSEL");
|
||||
assert_eq!(mpu.get_clock_source()?, CLKSEL::GXAXIS);
|
||||
mpu.set_clock_source(CLKSEL::GYAXIS)?;
|
||||
assert_eq!(mpu.get_clock_source()?, CLKSEL::GYAXIS);
|
||||
mpu.set_clock_source(CLKSEL::GZAXIS)?;
|
||||
assert_eq!(mpu.get_clock_source()?, CLKSEL::GZAXIS);
|
||||
mpu.set_clock_source(CLKSEL::OSCILL)?;
|
||||
assert_eq!(mpu.get_clock_source()?, CLKSEL::OSCILL);
|
||||
mpu.set_clock_source(CLKSEL::STOP)?;
|
||||
assert_eq!(mpu.get_clock_source()?, CLKSEL::STOP);
|
||||
mpu.set_clock_source(CLKSEL::RESERV)?;
|
||||
assert_eq!(mpu.get_clock_source()?, CLKSEL::RESERV);
|
||||
mpu.set_clock_source(CLKSEL::EXT_19P2)?;
|
||||
assert_eq!(mpu.get_clock_source()?, CLKSEL::EXT_19P2);
|
||||
mpu.set_clock_source(CLKSEL::EXT_32p7)?;
|
||||
assert_eq!(mpu.get_clock_source()?, CLKSEL::EXT_32p7);
|
||||
|
||||
// reset
|
||||
println!("Test reset");
|
||||
mpu.reset_device(&mut delay)?;
|
||||
|
|
|
|||
|
|
@ -86,27 +86,27 @@
|
|||
| <ul><li> -[ ] </li></ul>|<ul><li> -[ ] </li></ul>|[0x5A] EXT_SENS_DATA_17| RO |[7:0] EXT_SENS_DATA_17|
|
||||
| <ul><li> -[ ] </li></ul>|<ul><li> -[ ] </li></ul>|[0x5B] EXT_SENS_DATA_18| RO |[7:0] EXT_SENS_DATA_18|
|
||||
| <ul><li> -[ ] </li></ul>|<ul><li> -[ ] </li></ul>|[0x5C] EXT_SENS_DATA_19| RO |[7:0] EXT_SENS_DATA_19|
|
||||
| <ul><li> -[ ] </li></ul>||[0x5D] EXT_SENS_DATA_20| RO |[7:0] EXT_SENS_DATA_20|
|
||||
| <ul><li> -[ ] </li></ul>||[0x5E] EXT_SENS_DATA_21| RO |[7:0] EXT_SENS_DATA_21|
|
||||
| <ul><li> -[ ] </li></ul>||[0x5F] EXT_SENS_DATA_22| RO |[7:0] EXT_SENS_DATA_22|
|
||||
| <ul><li> -[ ] </li></ul>||[0x60] EXT_SENS_DATA_23| RO |[7:0] EXT_SENS_DATA_23|
|
||||
| <ul><li> -[ ] </li></ul>||[0x61] MOT_DETECT_STATUS| RO |[7] MOT_XNEG [6] MOT_XPOS [5] MOT_YNEG [4] MOT_YPOS [3] MOT_ZNEG [2] MOT_ZPOS [0] MOT_ZRMOT|
|
||||
| <ul><li> -[ ] </li></ul>||[0x63] I2C_SLV0_DO| R/W | [7:0] I2C_SLV0_DO|
|
||||
| <ul><li> -[ ] </li></ul>||[0x64] I2C_SLV1_DO| R/W | [7:0] I2C_SLV1_DO|
|
||||
| <ul><li> -[ ] </li></ul>||[0x65] I2C_SLV2_DO| R/W | [7:0] I2C_SLV2_DO|
|
||||
| <ul><li> -[ ] </li></ul>||[0x66] I2C_SLV3_DO| R/W | [7:0] I2C_SLV3_DO|
|
||||
| <ul><li> -[ ] </li></ul>||[0x67] I2C_MST_DELAY_CTRL| R/W | [7] DELAY_ES_SHADOW [4] I2C_SLV4_DLY_EN [3] I2C_SLV3_DLY_EN [2] I2C_SLV2_DLY_EN [1] I2C_SLV1_DLY_EN [0] I2C_SLV0_DLY_EN|
|
||||
| <ul><li> -[ ] </li></ul>||[0x68] SIGNAL_PATH_RESET| R/W | [2] GYRO_RESET [1] ACCEL_RESET [0] TEMP_RESET|
|
||||
| <ul><li> -[ ] </li></ul>||[0x69] MOT_DETECT_CTRL| R/W | [5:4] ACCEL_ON_DELAY [3:2] FF_COUNT [1:0] MOT_COUNT|
|
||||
| <ul><li> -[ ] </li></ul>||[0x6A] USER_CTRL| R/W | [7] DMP_EN [6] FIFO_EN [5] I2C_MST_EN [4] I2C_IF_DIS [3] DMP_RESET [2] FIFO_RESET [1] I2C_MST_RESET [0] SIG_COND_RESET|
|
||||
| <ul><li> -[ ] </li></ul>||[0x6B] PWR_MGMT_1| R/W | [7] DEVICE_RESET [6] SLEEP [5] CYCLE [3] TEMP_DIS [2:0] CLK_SEL|
|
||||
| <ul><li> -[ ] </li></ul>|<ul><li> -[ ] </li></ul>|[0x5D] EXT_SENS_DATA_20| RO |[7:0] EXT_SENS_DATA_20|
|
||||
| <ul><li> -[ ] </li></ul>|<ul><li> -[ ] </li></ul>|[0x5E] EXT_SENS_DATA_21| RO |[7:0] EXT_SENS_DATA_21|
|
||||
| <ul><li> -[ ] </li></ul>|<ul><li> -[ ] </li></ul>|[0x5F] EXT_SENS_DATA_22| RO |[7:0] EXT_SENS_DATA_22|
|
||||
| <ul><li> -[ ] </li></ul>|<ul><li> -[ ] </li></ul>|[0x60] EXT_SENS_DATA_23| RO |[7:0] EXT_SENS_DATA_23|
|
||||
| <ul><li> -[ ] </li></ul>|<ul><li> -[ ] </li></ul>|[0x61] MOT_DETECT_STATUS| RO |[7] MOT_XNEG [6] MOT_XPOS [5] MOT_YNEG [4] MOT_YPOS [3] MOT_ZNEG [2] MOT_ZPOS [0] MOT_ZRMOT|
|
||||
| <ul><li> -[ ] </li></ul>|<ul><li> -[ ] </li></ul>|[0x63] I2C_SLV0_DO| R/W | [7:0] I2C_SLV0_DO|
|
||||
| <ul><li> -[ ] </li></ul>|<ul><li> -[ ] </li></ul>|[0x64] I2C_SLV1_DO| R/W | [7:0] I2C_SLV1_DO|
|
||||
| <ul><li> -[ ] </li></ul>|<ul><li> -[ ] </li></ul>|[0x65] I2C_SLV2_DO| R/W | [7:0] I2C_SLV2_DO|
|
||||
| <ul><li> -[ ] </li></ul>|<ul><li> -[ ] </li></ul>|[0x66] I2C_SLV3_DO| R/W | [7:0] I2C_SLV3_DO|
|
||||
| <ul><li> -[ ] </li></ul>|<ul><li> -[ ] </li></ul>|[0x67] I2C_MST_DELAY_CTRL| R/W | [7] DELAY_ES_SHADOW [4] I2C_SLV4_DLY_EN [3] I2C_SLV3_DLY_EN [2] I2C_SLV2_DLY_EN [1] I2C_SLV1_DLY_EN [0] I2C_SLV0_DLY_EN|
|
||||
| <ul><li> -[ ] </li></ul>|<ul><li> -[ ] </li></ul>|[0x68] SIGNAL_PATH_RESET| R/W | [2] GYRO_RESET [1] ACCEL_RESET [0] TEMP_RESET|
|
||||
| <ul><li> -[ ] </li></ul>|<ul><li> -[ ] </li></ul>|[0x69] MOT_DETECT_CTRL| R/W | [5:4] ACCEL_ON_DELAY [3:2] FF_COUNT [1:0] MOT_COUNT|
|
||||
| <ul><li> -[ ] </li></ul>|<ul><li> -[ ] </li></ul>|[0x6A] USER_CTRL| R/W | [7] DMP_EN [6] FIFO_EN [5] I2C_MST_EN [4] I2C_IF_DIS [3] DMP_RESET [2] FIFO_RESET [1] I2C_MST_RESET [0] SIG_COND_RESET|
|
||||
| <ul><li> -[x] </li></ul>|<ul><li> -[x] </li></ul>|[0x6B] PWR_MGMT_1| R/W | [7] DEVICE_RESET [6] SLEEP [5] CYCLE [3] TEMP_DIS [2:0] CLK_SEL|
|
||||
| <ul><li> -[x] </li></ul>|<ul><li> -[ ] </li></ul>|[0x6C] PWR_MGMT_2| R/W | [7] LP_WAKE_CTRL [5] STBY_ZG [4] STBY_YA [3] STBY_ZA [2] STBY_XG [1] STBY_YG [0] STBY_ZG|
|
||||
| <ul><li> -[ ] </li></ul>||[0x6D] BANK_SEL| R/W | [6] PRFTCH_EN [5] CFG_USER_BANK [4:0] MEM_SEL|
|
||||
| <ul><li> -[ ] </li></ul>||[0x6E] MEM_START_ADDR| R/W | [7:0] START_ADDR|
|
||||
| <ul><li> -[ ] </li></ul>||[0x6F] MEM_R_W| R/W | [7:0] MEM_R_W|
|
||||
| <ul><li> -[ ] </li></ul>||[0x70] DMP_CFG_1| R/W | |
|
||||
| <ul><li> -[ ] </li></ul>||[0x71] DMP_CFG_2| R/W | |
|
||||
| <ul><li> -[ ] </li></ul>||[0x72] FIFO_COUNTH| R/W | [15:0] FIFO_COUNT|
|
||||
| <ul><li> -[ ] </li></ul>||[0x73] FIFO_COUNTL| R/W ||
|
||||
| <ul><li> -[ ] </li></ul>||[0x74] FIFO_R_W| R/W | [7:0] FIFO_R_W|
|
||||
| <ul><li> -[ ] </li></ul>|<ul><li> -[ ] </li></ul>|[0x6D] BANK_SEL| R/W | [6] PRFTCH_EN [5] CFG_USER_BANK [4:0] MEM_SEL|
|
||||
| <ul><li> -[ ] </li></ul>|<ul><li> -[ ] </li></ul>|[0x6E] MEM_START_ADDR| R/W | [7:0] START_ADDR|
|
||||
| <ul><li> -[ ] </li></ul>|<ul><li> -[ ] </li></ul>|[0x6F] MEM_R_W| R/W | [7:0] MEM_R_W|
|
||||
| <ul><li> -[ ] </li></ul>|<ul><li> -[ ] </li></ul>|[0x70] DMP_CFG_1| R/W | |
|
||||
| <ul><li> -[ ] </li></ul>|<ul><li> -[ ] </li></ul>|[0x71] DMP_CFG_2| R/W | |
|
||||
| <ul><li> -[ ] </li></ul>|<ul><li> -[ ] </li></ul>|[0x72] FIFO_COUNTH| R/W | [15:0] FIFO_COUNT|
|
||||
| <ul><li> -[ ] </li></ul>|<ul><li> -[ ] </li></ul>|[0x73] FIFO_COUNTL| R/W ||
|
||||
| <ul><li> -[ ] </li></ul>|<ul><li> -[ ] </li></ul>|[0x74] FIFO_R_W| R/W | [7:0] FIFO_R_W|
|
||||
| <ul><li> -[x] </li></ul>|<ul><li> -[ ] </li></ul>|[0x75] WHO_AM_I| RO | [6:1] WHO_AM_I |
|
||||
|
|
|
|||
134
src/device.rs
134
src/device.rs
|
|
@ -64,19 +64,14 @@ pub enum Registers {
|
|||
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,
|
||||
POWER_MGMT_2 = 0x6c,
|
||||
}
|
||||
|
||||
/// Describes a bit block from bit number 'bit' to 'bit'+'length'
|
||||
pub struct BitBlock {
|
||||
start_bit: u8,
|
||||
length: u8
|
||||
pub bit: u8,
|
||||
pub length: u8
|
||||
}
|
||||
|
||||
impl Registers {
|
||||
|
|
@ -87,56 +82,64 @@ impl Registers {
|
|||
|
||||
#[allow(non_camel_case_types)]
|
||||
#[derive(Copy, Clone, Debug)]
|
||||
/// Register 107: Power Management
|
||||
pub struct PWR_MGMT_1;
|
||||
/// Register 27: Gyro Config
|
||||
pub struct GYRO_CONFIG;
|
||||
|
||||
impl PWR_MGMT_1 {
|
||||
pub const ADDR: u8 = 0x6b;
|
||||
pub const DEVICE_RESET: u8 = 7;
|
||||
pub const SLEEP: u8 = 6;
|
||||
pub const CYCLE: u8 = 5;
|
||||
pub const TEMP_DIS: u8 = 3;
|
||||
pub const CLKSEL: BitBlock = BitBlock { start_bit: 2, length: 3 };
|
||||
impl GYRO_CONFIG {
|
||||
pub const ADDR: u8 = 0x1b;
|
||||
/// Gyro x axis self test bit
|
||||
pub const XG_ST: u8 = 7;
|
||||
/// Gyro y axis self test bit
|
||||
pub const YG_ST: u8 = 6;
|
||||
/// Gyro z axis self test bit
|
||||
pub const ZG_ST: u8 = 5;
|
||||
/// Gyro Config FS_SEL
|
||||
pub const FS_SEL: BitBlock = BitBlock { bit: 4, length: 2 };
|
||||
}
|
||||
|
||||
#[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 x axis self test bit
|
||||
pub const GYRO_CONFIG_XG_ST: u8 = 7;
|
||||
/// Gyro y axis self test bit
|
||||
pub const GYRO_CONFIG_YG_ST: u8 = 6;
|
||||
/// Gyro z axis self test bit
|
||||
pub const GYRO_CONFIG_ZG_ST: u8 = 5;
|
||||
/// 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;
|
||||
/// Register 28: Accel Config
|
||||
pub struct ACCEL_CONFIG;
|
||||
|
||||
impl ACCEL_CONFIG {
|
||||
/// Base Address
|
||||
pub const ADDR: u8 = 0x1c;
|
||||
/// Accel x axis self test bit
|
||||
pub const ACCEL_CONFIG_XA_ST: u8 = 7;
|
||||
pub const XA_ST: u8 = 7;
|
||||
/// Accel y axis self test bit
|
||||
pub const ACCEL_CONFIG_YA_ST: u8 = 6;
|
||||
pub const YA_ST: u8 = 6;
|
||||
/// Accel z axis self test bit
|
||||
pub const ACCEL_CONFIG_ZA_ST: u8 = 5;
|
||||
/// 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;
|
||||
pub const ZA_ST: u8 = 5;
|
||||
/// Accel Config FS_SEL
|
||||
pub const FS_SEL: BitBlock = BitBlock { bit: 4, length: 2};
|
||||
/// Accel Config ACCEL_HPF
|
||||
pub const ACCEL_HPF: BitBlock = BitBlock { bit: 2, length: 3};
|
||||
}
|
||||
|
||||
#[allow(non_camel_case_types)]
|
||||
#[derive(Copy, Clone, Debug)]
|
||||
/// Register 107: Power Management
|
||||
pub struct PWR_MGMT_1;
|
||||
|
||||
impl PWR_MGMT_1 {
|
||||
/// Base Address
|
||||
pub const ADDR: u8 = 0x6b;
|
||||
/// Device Reset bit
|
||||
pub const DEVICE_RESET: u8 = 7;
|
||||
/// Sleep mode bit (Should be called "Low Power", doesn't actually sleep)
|
||||
pub const SLEEP: u8 = 6;
|
||||
/// Cycle bit for wake operations
|
||||
pub const CYCLE: u8 = 5;
|
||||
/// Temperature sensor enable/disable bit
|
||||
pub const TEMP_DIS: u8 = 3;
|
||||
/// Clock Control
|
||||
pub const CLKSEL: BitBlock = BitBlock { bit: 2, length: 3 };
|
||||
}
|
||||
|
||||
#[allow(non_camel_case_types)]
|
||||
#[derive(Copy, Clone, Debug, Eq, PartialEq)]
|
||||
/// Accelerometer High Pass Filter Values
|
||||
pub enum ACCEL_HPF {
|
||||
_RESET = 0,
|
||||
_5 = 1,
|
||||
|
|
@ -160,12 +163,33 @@ impl From<u8> for ACCEL_HPF {
|
|||
}
|
||||
}
|
||||
}
|
||||
//
|
||||
// #[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};
|
||||
|
||||
#[allow(non_camel_case_types)]
|
||||
#[derive(Copy, Clone, Debug, Eq, PartialEq)]
|
||||
/// Clock Source Select Values
|
||||
pub enum CLKSEL {
|
||||
OSCILL = 0,
|
||||
GXAXIS = 1,
|
||||
GYAXIS = 2,
|
||||
GZAXIS = 3,
|
||||
EXT_32p7 = 4,
|
||||
EXT_19P2 = 5,
|
||||
RESERV = 6,
|
||||
STOP = 7,
|
||||
}
|
||||
|
||||
impl From<u8> for CLKSEL {
|
||||
fn from(clk: u8) -> Self {
|
||||
match clk {
|
||||
0 => CLKSEL::OSCILL,
|
||||
1 => CLKSEL::GXAXIS,
|
||||
2 => CLKSEL::GYAXIS,
|
||||
3 => CLKSEL::GZAXIS,
|
||||
4 => CLKSEL::EXT_32p7,
|
||||
5 => CLKSEL::EXT_19P2,
|
||||
6 => CLKSEL::RESERV,
|
||||
7 => CLKSEL::STOP,
|
||||
_ => CLKSEL::GXAXIS
|
||||
}
|
||||
}
|
||||
}
|
||||
74
src/lib.rs
74
src/lib.rs
|
|
@ -46,7 +46,7 @@ pub mod bits;
|
|||
pub mod range;
|
||||
|
||||
use crate::range::*;
|
||||
use crate::device::{*, Registers::*, Bits};
|
||||
use crate::device::{*, Registers::*};
|
||||
|
||||
use libm::{powf, atan2f, sqrtf};
|
||||
use nalgebra::{Vector3, Vector2};
|
||||
|
|
@ -102,11 +102,32 @@ where
|
|||
|
||||
/// Wakes MPU6050 with all sensors enabled (default)
|
||||
fn wake<D: DelayMs<u8>>(&mut self, delay: &mut D) -> Result<(), Mpu6050Error<E>> {
|
||||
self.write_byte(POWER_MGMT_1.addr(), 0)?;
|
||||
// MPU6050 has sleep enabled by default -> set bit 0 to wake
|
||||
// Set clock source to be PLL with x-axis gyroscope reference, bits 2:0 = 001 (See Register Map )
|
||||
self.write_byte(PWR_MGMT_1::ADDR, 0x01)?;
|
||||
delay.delay_ms(100u8);
|
||||
Ok(())
|
||||
}
|
||||
|
||||
/// From Register map:
|
||||
/// "An internal 8MHz oscillator, gyroscope based clock,or external sources can be
|
||||
/// selected as the MPU-60X0 clock source.
|
||||
/// When the internal 8 MHz oscillator or an external source is chosen as the clock source,
|
||||
/// the MPU-60X0 can operate in low power modes with the gyroscopes disabled. Upon power up,
|
||||
/// the MPU-60X0clock source defaults to the internal oscillator. However, it is highly
|
||||
/// recommended that the device beconfigured to use one of the gyroscopes
|
||||
/// (or an external clocksource) as the clock reference for improved stability.
|
||||
/// The clock source can be selected according to the following table...."
|
||||
pub fn set_clock_source(&mut self, source: CLKSEL) -> Result<(), Mpu6050Error<E>> {
|
||||
Ok(self.write_bits(PWR_MGMT_1::ADDR, PWR_MGMT_1::CLKSEL.bit, PWR_MGMT_1::CLKSEL.length, source as u8)?)
|
||||
}
|
||||
|
||||
/// get current clock source
|
||||
pub fn get_clock_source(&mut self) -> Result<CLKSEL, Mpu6050Error<E>> {
|
||||
let source = self.read_bits(PWR_MGMT_1::ADDR, PWR_MGMT_1::CLKSEL.bit, PWR_MGMT_1::CLKSEL.length)?;
|
||||
Ok(CLKSEL::from(source))
|
||||
}
|
||||
|
||||
/// Init wakes MPU6050 and verifies register addr, e.g. in i2c
|
||||
pub fn init<D: DelayMs<u8>>(&mut self, delay: &mut D) -> Result<(), Mpu6050Error<E>> {
|
||||
self.wake(delay)?;
|
||||
|
|
@ -129,26 +150,27 @@ where
|
|||
/// 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)?
|
||||
self.write_bits(ACCEL_CONFIG::ADDR,
|
||||
ACCEL_CONFIG::ACCEL_HPF.bit,
|
||||
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)?;
|
||||
let mode: u8 = self.read_bits(ACCEL_CONFIG::ADDR,
|
||||
ACCEL_CONFIG::ACCEL_HPF.bit,
|
||||
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,
|
||||
self.write_bits(GYRO_CONFIG::ADDR,
|
||||
GYRO_CONFIG::FS_SEL.bit,
|
||||
GYRO_CONFIG::FS_SEL.length,
|
||||
range as u8)?;
|
||||
|
||||
self.gyro_sensitivity = range.sensitivity();
|
||||
|
|
@ -157,9 +179,9 @@ where
|
|||
|
||||
/// 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,
|
||||
self.write_bits(ACCEL_CONFIG::ADDR,
|
||||
ACCEL_CONFIG::FS_SEL.bit,
|
||||
ACCEL_CONFIG::FS_SEL.length,
|
||||
range as u8)?;
|
||||
|
||||
self.acc_sensitivity = range.sensitivity();
|
||||
|
|
@ -168,18 +190,18 @@ where
|
|||
|
||||
/// 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)?;
|
||||
let byte = self.read_bits(ACCEL_CONFIG::ADDR,
|
||||
ACCEL_CONFIG::FS_SEL.bit,
|
||||
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)?;
|
||||
let byte = self.read_bits(GYRO_CONFIG::ADDR,
|
||||
GYRO_CONFIG::FS_SEL.bit,
|
||||
GYRO_CONFIG::FS_SEL.length)?;
|
||||
|
||||
Ok(GyroRange::from(byte))
|
||||
}
|
||||
|
|
@ -218,32 +240,32 @@ where
|
|||
|
||||
/// set accel x self test
|
||||
pub fn set_accel_x_self_test(&mut self, enable: bool) -> Result<(), Mpu6050Error<E>> {
|
||||
Ok(self.write_bit(ACCEL_CONFIG.addr(), Bits::ACCEL_CONFIG_XA_ST, enable)?)
|
||||
Ok(self.write_bit(ACCEL_CONFIG::ADDR, ACCEL_CONFIG::XA_ST, enable)?)
|
||||
}
|
||||
|
||||
/// get accel x self test
|
||||
pub fn get_accel_x_self_test(&mut self) -> Result<bool, Mpu6050Error<E>> {
|
||||
Ok(self.read_bit(ACCEL_CONFIG.addr(), Bits::ACCEL_CONFIG_XA_ST)? != 0)
|
||||
Ok(self.read_bit(ACCEL_CONFIG::ADDR, ACCEL_CONFIG::XA_ST)? != 0)
|
||||
}
|
||||
|
||||
/// set accel y self test
|
||||
pub fn set_accel_y_self_test(&mut self, enable: bool) -> Result<(), Mpu6050Error<E>> {
|
||||
Ok(self.write_bit(ACCEL_CONFIG.addr(), Bits::ACCEL_CONFIG_YA_ST, enable)?)
|
||||
Ok(self.write_bit(ACCEL_CONFIG::ADDR, ACCEL_CONFIG::YA_ST, enable)?)
|
||||
}
|
||||
|
||||
/// get accel y self test
|
||||
pub fn get_accel_y_self_test(&mut self) -> Result<bool, Mpu6050Error<E>> {
|
||||
Ok(self.read_bit(ACCEL_CONFIG.addr(), Bits::ACCEL_CONFIG_YA_ST)? != 0)
|
||||
Ok(self.read_bit(ACCEL_CONFIG::ADDR, ACCEL_CONFIG::YA_ST)? != 0)
|
||||
}
|
||||
|
||||
/// set accel z self test
|
||||
pub fn set_accel_z_self_test(&mut self, enable: bool) -> Result<(), Mpu6050Error<E>> {
|
||||
Ok(self.write_bit(ACCEL_CONFIG.addr(), Bits::ACCEL_CONFIG_ZA_ST, enable)?)
|
||||
Ok(self.write_bit(ACCEL_CONFIG::ADDR, ACCEL_CONFIG::ZA_ST, enable)?)
|
||||
}
|
||||
|
||||
/// get accel z self test
|
||||
pub fn get_accel_z_self_test(&mut self) -> Result<bool, Mpu6050Error<E>> {
|
||||
Ok(self.read_bit(ACCEL_CONFIG.addr(), Bits::ACCEL_CONFIG_ZA_ST)? != 0)
|
||||
Ok(self.read_bit(ACCEL_CONFIG::ADDR, ACCEL_CONFIG::ZA_ST)? != 0)
|
||||
}
|
||||
|
||||
/// Roll and pitch estimation from raw accelerometer readings
|
||||
|
|
|
|||
Loading…
Reference in a new issue