diff --git a/.gitignore b/.gitignore index f8d7c8b..ff90831 100644 --- a/.gitignore +++ b/.gitignore @@ -1,4 +1,24 @@ -**/*.rs.bk -.#* +# Generated by Cargo /target/ +**/target/ + +# Remove Cargo.lock from gitignore if creating an executable, leave it for libraries +# More information here https://doc.rust-lang.org/cargo/guide/cargo-toml-vs-cargo-lock.html Cargo.lock + +# Backup files +**/*.rs.bk +*~ +.#* + +# IDE files +.idea/ +.vscode/ +*.swp +*.swo + +# macOS specific files +.DS_Store + +# Windows specific files +Thumbs.db diff --git a/Cargo.toml b/Cargo.toml index e521e3b..abd834b 100644 --- a/Cargo.toml +++ b/Cargo.toml @@ -1,20 +1,26 @@ [package] -authors = ["Roma Sokolov", "Alexander Zhuravlev", "Jorge Aparicio "] +authors = [ + "Roma Sokolov", + "Alexander Zhuravlev", + "Jorge Aparicio ", +] name = "mpu9250" -version = "0.25.0" +version = "0.30.0" description = "no_std driver for the MPU9250 & onboard AK8963 (accelerometer + gyroscope + magnetometer IMU)" keywords = ["arm", "cortex-m", "stm32", "hal"] license = "MIT OR Apache-2.0" readme = "README.md" repository = "https://github.com/copterust/mpu9250" +edition = "2024" [dependencies] -embedded-hal = "0.2.3" -bitflags = "1.0" +# embedded-hal = "0.2.3" +embedded-hal = "1.0.0" +bitflags = "2.8.0" libm = { version = "0.2.1", optional = true } [dev-dependencies] -linux-embedded-hal = "0.3" +linux-embedded-hal = "0.4.0" [features] # Enable the I2C MPU interface. This disables the SPI interface. All function diff --git a/examples/legacy/README.md b/examples/legacy/README.md new file mode 100644 index 0000000..50b36c9 --- /dev/null +++ b/examples/legacy/README.md @@ -0,0 +1,5 @@ +# Examples - Old Style + +This folder contains the original examples from before the transition to embedded-hal 1.0. They have been preserved here as a reference but have not been updated to work with the current version of the library. + +These examples are provided for historical reference only. For up-to-date examples that work with the current version of the library, please refer to the main examples directory. diff --git a/examples/bbblue.rs b/examples/legacy/bbblue.rs similarity index 100% rename from examples/bbblue.rs rename to examples/legacy/bbblue.rs diff --git a/examples/bbblue_dmp.rs b/examples/legacy/bbblue_dmp.rs similarity index 65% rename from examples/bbblue_dmp.rs rename to examples/legacy/bbblue_dmp.rs index b06b2c1..f3eb622 100644 --- a/examples/bbblue_dmp.rs +++ b/examples/legacy/bbblue_dmp.rs @@ -11,9 +11,9 @@ extern crate mpu9250; use std::io::{self, Write}; -use hal::sysfs_gpio; use hal::Delay; use hal::I2cdev; +use hal::sysfs_gpio; use mpu9250::DMP_FIRMWARE; use mpu9250::{Error, Mpu9250}; @@ -30,27 +30,25 @@ fn main() { let mut stdout = stdout.lock(); writeln!(&mut stdout, " Normalized quaternion").unwrap(); - let mut mpu9250 = - Mpu9250::dmp_default(i2c, &mut Delay, &DMP_FIRMWARE).expect("unable to load firmware"); + let mut mpu9250 = Mpu9250::dmp_default(i2c, &mut Delay, &DMP_FIRMWARE) + .expect("unable to load firmware"); loop { match event.poll(1000).unwrap() { - Some(_) => - match mpu9250.dmp_all::<[f32; 3], [f64; 4]>() { - Ok(measure) => { - write!(&mut stdout, - "\r{:?}", - measure).unwrap(); - stdout.flush().unwrap(); - }, - Err(Error::DmpDataNotReady) => (), - Err(_) => (), + Some(_) => match mpu9250.dmp_all::<[f32; 3], [f64; 4]>() { + Ok(measure) => { + write!(&mut stdout, "\r{:?}", measure).unwrap(); + stdout.flush().unwrap(); }, + Err(Error::DmpDataNotReady) => (), + Err(_) => (), + }, None => { write!(&mut stdout, "\nTimeout\n").unwrap(); mpu9250.reset_fifo(&mut Delay).unwrap(); - } + }, } } - }).unwrap(); + }) + .unwrap(); } diff --git a/examples/rpi.rs b/examples/legacy/rpi.rs similarity index 100% rename from examples/rpi.rs rename to examples/legacy/rpi.rs diff --git a/examples/rp235x/.cargo/config.toml b/examples/rp235x/.cargo/config.toml new file mode 100644 index 0000000..75d5a95 --- /dev/null +++ b/examples/rp235x/.cargo/config.toml @@ -0,0 +1,11 @@ +[target.'cfg(all(target_arch = "arm", target_os = "none"))'] +# pick one of the following options by uncommenting it +# runner = "elf2uf2-rs -d" #and then flash manually +# runner = "picotool load -u -v -x -t elf" +runner = "probe-rs run --chip RP235x --protocol swd --speed 16000 --probe 2e8a:000c:E6633861A3543E38" + +[build] +target = "thumbv8m.main-none-eabihf" + +[env] +DEFMT_LOG = "debug" diff --git a/examples/rp235x/Cargo.toml b/examples/rp235x/Cargo.toml new file mode 100644 index 0000000..a210c42 --- /dev/null +++ b/examples/rp235x/Cargo.toml @@ -0,0 +1,40 @@ +[package] +name = "examples" +version = "0.1.0" +edition = "2024" + +[[example]] +name = "basic" +path = "src/basic.rs" + +[dependencies] +embassy-rp = { version = "0.3.1", features = [ + "defmt", + "unstable-pac", + "time-driver", + "critical-section-impl", + "rp235xa", +] } +embassy-embedded-hal = { version = "0.3.0", features = ["defmt"] } +embassy-sync = { version = "0.6.2", features = ["defmt"] } +embassy-executor = { version = "0.7.0", features = [ + "task-arena-size-98304", + "arch-cortex-m", + "executor-thread", + "executor-interrupt", + "defmt", +] } +embassy-time = { version = "0.4.0", features = [ + "defmt", + "defmt-timestamp-uptime", +] } +embassy-futures = { version = "0.1.1" } +defmt = "0.3" +defmt-rtt = "0.4" +cortex-m-rt = "0.7.5" +critical-section = "1.2.0" +panic-probe = { version = "0.3", features = ["print-defmt"] } +num-traits = { version = "0.2", default-features = false } +libm = "0.2.1" + +mpu9250 = { path = "../..", features = ["i2c", "dmp"] } diff --git a/examples/rp235x/README.md b/examples/rp235x/README.md new file mode 100644 index 0000000..9853a31 --- /dev/null +++ b/examples/rp235x/README.md @@ -0,0 +1,28 @@ +# MPU6050-DMP Examples + +This directory contains examples demonstrating use of the MPU9250 sensor. + +## Examples + +### [Basic](src/basic.rs) + +Basic example showing how to: + +- Initialize the sensor with I2C +- Perform sensor calibration +- Read accelerometer, gyroscope and temperature data + +## Building and Running + +1. Connect your MPU6050 to a Raspberry Pi Pico 2: + - SDA -> GP16 + - SCL -> GP17 + - VCC -> 3.3V + - GND -> GND + +2. Build and flash an example: + + ```bash + # For the basic example + cargo run --example basic + ``` diff --git a/examples/rp235x/build.rs b/examples/rp235x/build.rs new file mode 100644 index 0000000..4c7ebd7 --- /dev/null +++ b/examples/rp235x/build.rs @@ -0,0 +1,35 @@ +//! This build script copies the `memory.x` file from the crate root into +//! a directory where the linker can always find it at build time. +//! For many projects this is optional, as the linker always searches the +//! project root directory -- wherever `Cargo.toml` is. However, if you +//! are using a workspace or have a more complicated build setup, this +//! build script becomes required. Additionally, by requesting that +//! Cargo re-run the build script whenever `memory.x` is changed, +//! updating `memory.x` ensures a rebuild of the application with the +//! new memory settings. + +use std::env; +use std::fs::File; +use std::io::Write; +use std::path::PathBuf; + +fn main() { + // Put `memory.x` in our output directory and ensure it's + // on the linker search path. + let out = &PathBuf::from(env::var_os("OUT_DIR").unwrap()); + File::create(out.join("memory.x")) + .unwrap() + .write_all(include_bytes!("memory.x")) + .unwrap(); + println!("cargo:rustc-link-search={}", out.display()); + + // By default, Cargo will re-run a build script whenever + // any file in the project changes. By specifying `memory.x` + // here, we ensure the build script is only re-run when + // `memory.x` is changed. + println!("cargo:rerun-if-changed=memory.x"); + + println!("cargo:rustc-link-arg=--nmagic"); + println!("cargo:rustc-link-arg=-Tlink.x"); + println!("cargo:rustc-link-arg=-Tdefmt.x"); +} diff --git a/examples/rp235x/memory.x b/examples/rp235x/memory.x new file mode 100644 index 0000000..7774920 --- /dev/null +++ b/examples/rp235x/memory.x @@ -0,0 +1,74 @@ +MEMORY { + /* + * The RP2350 has either external or internal flash. + * + * 2 MiB is a safe default here, although a Pico 2 has 4 MiB. + */ + FLASH : ORIGIN = 0x10000000, LENGTH = 2048K + /* + * RAM consists of 8 banks, SRAM0-SRAM7, with a striped mapping. + * This is usually good for performance, as it distributes load on + * those banks evenly. + */ + RAM : ORIGIN = 0x20000000, LENGTH = 512K + /* + * RAM banks 8 and 9 use a direct mapping. They can be used to have + * memory areas dedicated for some specific job, improving predictability + * of access times. + * Example: Separate stacks for core0 and core1. + */ + SRAM4 : ORIGIN = 0x20080000, LENGTH = 4K + SRAM5 : ORIGIN = 0x20081000, LENGTH = 4K +} + +SECTIONS { + /* ### Boot ROM info + * + * Goes after .vector_table, to keep it in the first 4K of flash + * where the Boot ROM (and picotool) can find it + */ + .start_block : ALIGN(4) + { + __start_block_addr = .; + KEEP(*(.start_block)); + } > FLASH + +} INSERT AFTER .vector_table; + +/* move .text to start /after/ the boot info */ +_stext = ADDR(.start_block) + SIZEOF(.start_block); + +SECTIONS { + /* ### Picotool 'Binary Info' Entries + * + * Picotool looks through this block (as we have pointers to it in our + * header) to find interesting information. + */ + .bi_entries : ALIGN(4) + { + /* We put this in the header */ + __bi_entries_start = .; + /* Here are the entries */ + KEEP(*(.bi_entries)); + /* Keep this block a nice round size */ + . = ALIGN(4); + /* We put this in the header */ + __bi_entries_end = .; + } > FLASH +} INSERT AFTER .text; + +SECTIONS { + /* ### Boot ROM extra info + * + * Goes after everything in our program, so it can contain a signature. + */ + .end_block : ALIGN(4) + { + __end_block_addr = .; + KEEP(*(.end_block)); + } > FLASH + +} INSERT AFTER .uninit; + +PROVIDE(start_to_end = __end_block_addr - __start_block_addr); +PROVIDE(end_to_start = __start_block_addr - __end_block_addr); diff --git a/examples/rp235x/src/basic.rs b/examples/rp235x/src/basic.rs new file mode 100644 index 0000000..dc09483 --- /dev/null +++ b/examples/rp235x/src/basic.rs @@ -0,0 +1,127 @@ +//! Embassy Async MPU6050-DMP Example +//! +//! This example demonstrates using the MPU6050 sensor with Embassy's async runtime on a Raspberry Pi Pico. +//! It shows how to: +//! - Initialize the sensor with async I2C +//! - Load and initialize the DMP firmware +//! - Perform sensor calibration +//! - Continuously read accelerometer, gyroscope and temperature data +//! +//! Hardware Setup: +//! - Connect MPU6050 to Raspberry Pi Pico: +//! - SDA -> GP14 +//! - SCL -> GP15 +//! - VCC -> 3.3V +//! - GND -> GND + +#![no_std] +#![no_main] + +use defmt::info; +use embassy_executor::Spawner; +use embassy_rp::{block::ImageDef, config::Config, i2c::InterruptHandler}; +use embassy_time::{Delay, Duration, Timer}; +use {defmt_rtt as _, panic_probe as _}; + +// mpu9250 +use mpu9250::Mpu9250; + +embassy_rp::bind_interrupts!(struct Irqs { + I2C0_IRQ => InterruptHandler; +}); + +/// Firmware image type for bootloader +#[unsafe(link_section = ".start_block")] +#[used] +pub static IMAGE_DEF: ImageDef = ImageDef::secure_exe(); + +/// Firmware entry point +#[embassy_executor::main] +async fn main(_spawner: Spawner) { + let p = embassy_rp::init(Config::default()); + + // Initialize the MPU9250 sensor using I2C0 + let sda = p.PIN_16; + let scl = p.PIN_17; + let config = embassy_rp::i2c::Config::default(); + let bus = embassy_rp::i2c::I2c::new_async(p.I2C0, scl, sda, Irqs, config); + + let mut delay = Delay; + + let mut sensor = Mpu9250::imu_default(bus, &mut delay).unwrap(); + info!("Sensor Initialized"); + + // Connection test + let who_am_i = sensor.who_am_i().unwrap(); + info!("WHO_AM_I: 0x{:x}", who_am_i); + + // let mag_who_am_i = sensor.ak8963_who_am_i().unwrap(); + // info!("AK8963 WHO_AM_I: 0x{:x}", mag_who_am_i); + + info!("Test measurement before calibration..."); + let all = sensor.all::<[f32; 3]>().unwrap(); + info!( + "Accel XYZ(m/s^2): {} {} {} | Gyro XYZ (rad/s): {} {} {} | Temp (C): {}", + all.accel[0], + all.accel[1], + all.accel[2], + all.gyro[0], + all.gyro[1], + all.gyro[2], + all.temp + ); + + info!("Calibrating sensor..."); + let accel_biases = + sensor.calibrate_at_rest::<_, [f32; 3]>(&mut delay).unwrap(); + info!( + "Calibration complete. Accel biases: {} {} {}", + accel_biases[0], accel_biases[1], accel_biases[2] + ); + + info!("Test measurement after calibration..."); + let all = sensor.all::<[f32; 3]>().unwrap(); + info!( + "Accel XYZ(m/s^2): {} {} {} | Gyro XYZ (rad/s): {} {} {} | Temp (C): {}", + all.accel[0] - accel_biases[0], + all.accel[1] - accel_biases[1], + all.accel[2] - accel_biases[2], + all.gyro[0], + all.gyro[1], + all.gyro[2], + all.temp + ); + + Timer::after(Duration::from_millis(1000)).await; + info!("Starting data read loop..."); + + loop { + let all = sensor.all::<[f32; 3]>().unwrap(); + // info!( + // "Accel XYZ(m/s^2): {} {} {} | Gyro XYZ (rad/s): {} {} {} | Mag Field XYZ(uT): {} {} {} | Temp (C): {}", + // all.accel[0], + // all.accel[1], + // all.accel[2], + // all.gyro[0], + // all.gyro[1], + // all.gyro[2], + // all.mag[0], + // all.mag[1], + // all.mag[2], + // all.temp + // ); + + info!( + "Accel XYZ(m/s^2): {} {} {} | Gyro XYZ (rad/s): {} {} {} | Temp (C): {}", + all.accel[0] - accel_biases[0], + all.accel[1] - accel_biases[1], + all.accel[2] - accel_biases[2], + all.gyro[0], + all.gyro[1], + all.gyro[2], + all.temp + ); + + Timer::after(Duration::from_millis(1000)).await; + } +} diff --git a/src/ak8963.rs b/src/ak8963.rs index cd75a26..7075689 100644 --- a/src/ak8963.rs +++ b/src/ak8963.rs @@ -1,6 +1,6 @@ //! AK8963, I2C magnetometer -use hal::blocking::delay::DelayMs; +use hal::delay::DelayNs; // I2C slave address pub const I2C_ADDRESS: u8 = 0x0c; @@ -37,7 +37,7 @@ impl Register { } } -/// Decribes a type that can communicate with the +/// Describes a type that can communicate with the /// MPU's on-board magnetometer, the AK8963 pub trait AK8963 { /// Associated error type @@ -48,15 +48,11 @@ pub trait AK8963 { /// It may not make sense to call this more than once. However, it is /// absolutely necessary to call it at least once if you need the /// magnetometer - fn init>(&mut self, - delay: &mut D) - -> Result<(), Self::Error>; + fn init(&mut self, delay: &mut D) -> Result<(), Self::Error>; /// Perform final initialization. Invoked after acquiring the magnetomter's /// calibration values and setting the sampling rate and resolution. - fn finalize>(&mut self, - _: &mut D) - -> Result<(), Self::Error> { + fn finalize(&mut self, _: &mut D) -> Result<(), Self::Error> { Ok(()) } diff --git a/src/conf.rs b/src/conf.rs index 179c17d..46e99b6 100644 --- a/src/conf.rs +++ b/src/conf.rs @@ -572,12 +572,15 @@ impl MpuConfig { -> &mut Self { match self.dmp_configuration.as_mut() { Some(mut x) => x.features.gyro_auto_calibrate = feature, - None => self.dmp_configuration = - Some(DmpConfiguration { features: - DmpFeatures { gyro_auto_calibrate: - feature, - ..Default::default() }, - ..Default::default() }), + None => { + self.dmp_configuration = Some(DmpConfiguration { + features: DmpFeatures { + gyro_auto_calibrate: feature, + ..Default::default() + }, + ..Default::default() + }) + }, } self } diff --git a/src/device.rs b/src/device.rs index 5138ee6..4795d10 100644 --- a/src/device.rs +++ b/src/device.rs @@ -1,11 +1,11 @@ -use ak8963::{self, AK8963}; +use crate::ak8963::{self, AK8963}; -use hal::blocking::delay::DelayMs; -use hal::blocking::i2c; -use hal::blocking::spi; -use hal::digital::v2::OutputPin; +use hal::delay::DelayNs; +use hal::digital::OutputPin; +use hal::i2c::{self, I2c, Operation as I2cOperation, SevenBitAddress}; +use hal::spi::{self, Operation as SpiOperation, SpiBus}; -use Register; +use crate::Register; /// MPU's I2C address (AD0 low) const MPU_I2C_ADDR: u8 = 0x68; @@ -71,8 +71,8 @@ pub struct SpiDevice { ncs: GPIO, } -impl SpiDevice - where SPI: spi::Write + spi::Transfer, +impl SpiDevice + where SPI: SpiBus, NCS: OutputPin { /// Create a new SpiDevice @@ -82,8 +82,8 @@ impl SpiDevice } } -impl Releasable for SpiDevice - where SPI: spi::Write + spi::Transfer, +impl Releasable for SpiDevice + where SPI: SpiBus, NCS: OutputPin { type Released = (SPI, NCS); @@ -110,11 +110,11 @@ impl core::convert::From for SpiError { } } -impl Device for SpiDevice - where SPI: spi::Write + spi::Transfer, +impl Device for SpiDevice + where SPI: SpiBus, NCS: OutputPin { - type Error = SpiError; + type Error = SpiError; // Note: implementation is consistent with previous Mpu9250 private // methods. Using read and modify as default trait impls @@ -125,7 +125,7 @@ impl Device for SpiDevice -> Result<(), Self::Error> { buffer[0] = reg.read_address(); self.ncs.set_low().map_err(SpiError::NCSError)?; - self.spi.transfer(buffer)?; + self.spi.transfer_in_place(buffer)?; self.ncs.set_high().map_err(SpiError::NCSError)?; Ok(()) @@ -159,30 +159,28 @@ impl Device for SpiDevice } } -impl AK8963 for SpiDevice - where SPI: spi::Write + spi::Transfer, +impl AK8963 for SpiDevice + where SPI: SpiBus, NCS: OutputPin { - type Error = SpiError; + type Error = SpiError; - fn init>(&mut self, - delay: &mut D) - -> Result<(), Self::Error> { + fn init(&mut self, delay: &mut D) -> Result<(), Self::Error> { // Isolate the auxiliary master I2C bus (AUX_CL, AUX_DA) // disable the slave I2C bus, make serial interface SPI only // reset the master I2C bus Device::write(self, Register::USER_CTRL, 0x32)?; - delay.delay_ms(10); + delay.delay_ns(10 * 1_000_000); Ok(()) } - fn finalize>(&mut self, - delay: &mut D) - -> Result<(), Self::Error> { + fn finalize(&mut self, + delay: &mut D) + -> Result<(), Self::Error> { // set aux I2C frequency to 400 KHz (should be configurable?) Device::write(self, Register::I2C_MST_CTRL, 0x0d)?; - delay.delay_ms(10); + delay.delay_ns(10 * 1_000_000); // configure sampling of magnetometer Device::write(self, @@ -193,7 +191,7 @@ impl AK8963 for SpiDevice ak8963::Register::XOUT_L.addr())?; Device::write(self, Register::I2C_SLV0_CTRL, 0x87)?; - delay.delay_ms(10); + delay.delay_ns(10 * 1_000_000); Ok(()) } @@ -258,10 +256,7 @@ pub struct I2cDevice { i2c: I2C, } -impl I2cDevice - where I2C: i2c::Read - + i2c::Write - + i2c::WriteRead +impl I2cDevice where I2C: I2c { /// Create a new I2C device pub fn new(i2c: I2C) -> Self { @@ -269,10 +264,7 @@ impl I2cDevice } } -impl Releasable for I2cDevice - where I2C: i2c::Read - + i2c::Write - + i2c::WriteRead +impl Releasable for I2cDevice where I2C: I2c { type Released = I2C; @@ -281,18 +273,18 @@ impl Releasable for I2cDevice } } -impl Device for I2cDevice - where I2C: i2c::Read - + i2c::Write - + i2c::WriteRead +impl Device for I2cDevice where I2C: I2c { - type Error = I2CError; + type Error = I2CError; fn read_many(&mut self, reg: Register, buffer: &mut [u8]) -> Result<(), Self::Error> { - self.i2c.write_read(MPU_I2C_ADDR, &[reg as u8], &mut buffer[1..])?; + let reg_addr = [reg as u8]; + let mut operations = [I2cOperation::Write(®_addr), + I2cOperation::Read(&mut buffer[1..])]; + self.i2c.transaction(MPU_I2C_ADDR, &mut operations)?; Ok(()) } @@ -320,18 +312,13 @@ impl Device for I2cDevice } } -impl AK8963 for I2cDevice - where I2C: i2c::Read - + i2c::Write - + i2c::WriteRead +impl AK8963 for I2cDevice where I2C: I2c { - type Error = I2CError; + type Error = I2CError; - fn init>(&mut self, - delay: &mut D) - -> Result<(), Self::Error> { + fn init(&mut self, delay: &mut D) -> Result<(), Self::Error> { Device::write(self, Register::USER_CTRL, 0)?; - delay.delay_ms(10); + delay.delay_ns(10 * 1_000_000); const LATCH_INT_EN: u8 = 1 << 5; const INT_ANYRD_CLEAR: u8 = 1 << 4; @@ -343,14 +330,17 @@ impl AK8963 for I2cDevice | INT_ANYRD_CLEAR | ACTL_ACTIVE_LOW | BYPASS_EN)?; - delay.delay_ms(10); + delay.delay_ns(10 * 1_000_000); Ok(()) } fn read(&mut self, reg: ak8963::Register) -> Result { let mut buffer = [0; 1]; - self.i2c.write_read(ak8963::I2C_ADDRESS, &[reg.addr()], &mut buffer)?; + let reg_addr = [reg.addr()]; + let mut operations = + [I2cOperation::Write(®_addr), I2cOperation::Read(&mut buffer)]; + self.i2c.transaction(ak8963::I2C_ADDRESS, &mut operations)?; Ok(buffer[0]) } @@ -369,9 +359,10 @@ impl AK8963 for I2cDevice // to read register ST2. We're required to read ST2 after each // reading, otherwise the magnetometer blocks sampling. We can // achieve this in one I2C transaction - self.i2c.write_read(ak8963::I2C_ADDRESS, - &[ak8963::Register::XOUT_L.addr()], - buffer)?; + let reg_addr = [ak8963::Register::XOUT_L.addr()]; + let mut operations = + [I2cOperation::Write(®_addr), I2cOperation::Read(buffer)]; + self.i2c.transaction(ak8963::I2C_ADDRESS, &mut operations)?; buffer[..].rotate_right(1); buffer[0] = 0; // Zero out ST2 afer rotation @@ -396,8 +387,8 @@ pub trait NineDOFDevice: -> Result<(), ::Error>; } -impl NineDOFDevice for SpiDevice - where SPI: spi::Write + spi::Transfer, +impl NineDOFDevice for SpiDevice + where SPI: SpiBus, NCS: OutputPin { fn read_9dof(&mut self, @@ -409,10 +400,7 @@ impl NineDOFDevice for SpiDevice } } -impl NineDOFDevice for I2cDevice - where I2C: i2c::Read - + i2c::Write - + i2c::WriteRead +impl NineDOFDevice for I2cDevice where I2C: I2c { fn read_9dof(&mut self, reg: Register, diff --git a/src/lib.rs b/src/lib.rs index c04ca45..832072f 100644 --- a/src/lib.rs +++ b/src/lib.rs @@ -71,8 +71,10 @@ use ak8963::AK8963; use core::marker::PhantomData; -use hal::blocking::delay::DelayMs; -use hal::spi::{Mode, Phase, Polarity}; +use hal::delay::DelayNs; +use hal::digital::OutputPin; +use hal::i2c::{I2c, SevenBitAddress}; +use hal::spi::{Mode, Phase, Polarity, SpiBus}; pub use conf::*; pub use types::*; @@ -80,7 +82,7 @@ pub use types::*; #[doc(hidden)] pub use device::Releasable; pub use device::{ - Device, I2CError, I2cDevice, NineDOFDevice, SpiDevice, SpiError + Device, I2CError, I2cDevice, NineDOFDevice, SpiDevice, SpiError, }; /// Suported MPUx devices @@ -96,8 +98,8 @@ pub enum MpuXDevice { impl MpuXDevice { fn imu_supported(b: u8) -> bool { b == (MpuXDevice::MPU9250 as u8) - || b == (MpuXDevice::MPU9255 as u8) - || b == (MpuXDevice::MPU6500 as u8) + || b == (MpuXDevice::MPU9255 as u8) + || b == (MpuXDevice::MPU6500 as u8) } fn marg_supported(b: u8) -> bool { @@ -171,23 +173,24 @@ const TEMP_ROOM_OFFSET: f32 = 0.0; #[cfg(not(feature = "i2c"))] mod spi_defs { use super::*; - use hal::blocking::spi; - use hal::digital::v2::OutputPin; + use hal::digital::OutputPin; + use hal::spi::SpiBus; // SPI device, 6DOF - impl Mpu9250, Imu> - where SPI: spi::Write + spi::Transfer, - NCS: OutputPin + impl Mpu9250, Imu> + where + SPI: SpiBus, + NCS: OutputPin, { /// Creates a new [`Imu`] driver from a SPI peripheral and a NCS pin /// with default configuration. pub fn imu_default( spi: SPI, ncs: NCS, - delay: &mut D) - -> Result as device::Device>::Error>> - where D: DelayMs + delay: &mut D, + ) -> Result as device::Device>::Error>> + where + D: DelayNs, { Self::imu(spi, ncs, delay, &mut MpuConfig::imu()) } @@ -200,10 +203,10 @@ mod spi_defs { spi: SPI, ncs: NCS, delay: &mut D, - config: &mut MpuConfig) - -> Result as device::Device>::Error>> - where D: DelayMs + config: &mut MpuConfig, + ) -> Result as device::Device>::Error>> + where + D: DelayNs, { let dev = SpiDevice::new(spi, ncs); Self::new_imu(dev, delay, config) @@ -211,7 +214,7 @@ mod spi_defs { /// Creates a new Imu driver from a SPI peripheral and a NCS pin with /// provided configuration [`Config`]. Reinit function can be used to - /// re-initialize SPI bus. Usecase: change SPI speed for faster data + /// re-initialize SPI bus. Use case: change SPI speed for faster data /// transfer: /// "Communication with all registers of the device is /// performed using either I2C at 400kHz or SPI at 1M Hz. @@ -224,22 +227,27 @@ mod spi_defs { ncs: NCS, delay: &mut D, config: &mut MpuConfig, - reinit_fn: F) - -> Result as device::Device>::Error>> - where D: DelayMs, - F: FnOnce(SPI, NCS) -> Option<(SPI, NCS)> + reinit_fn: F, + ) -> Result as device::Device>::Error>> + where + D: DelayNs, + F: FnOnce(SPI, NCS) -> Option<(SPI, NCS)>, { let dev = SpiDevice::new(spi, ncs); let mpu = Self::new_imu(dev, delay, config)?; - mpu.reinit_spi_device(reinit_fn) + mpu.reset_device(|spidev| { + let (cspi, cncs) = spidev.release(); + reinit_fn(cspi, cncs) + .map(|(nspi, nncs)| SpiDevice::new(nspi, nncs)) + }) } } // SPI device, 9 DOF - impl Mpu9250, Marg> - where SPI: spi::Write + spi::Transfer, - NCS: OutputPin + impl Mpu9250, Marg> + where + SPI: SpiBus, + NCS: OutputPin, { /// Creates a new [`Marg`] driver from a SPI peripheral and a NCS pin /// with default [`Config`]. @@ -248,10 +256,10 @@ mod spi_defs { pub fn marg_default( spi: SPI, ncs: NCS, - delay: &mut D) - -> Result as device::Device>::Error>> - where D: DelayMs + delay: &mut D, + ) -> Result as device::Device>::Error>> + where + D: DelayNs, { Mpu9250::marg(spi, ncs, delay, &mut MpuConfig::marg()) } @@ -264,10 +272,10 @@ mod spi_defs { spi: SPI, ncs: NCS, delay: &mut D, - config: &mut MpuConfig) - -> Result as device::Device>::Error>> - where D: DelayMs + config: &mut MpuConfig, + ) -> Result as device::Device>::Error>> + where + D: DelayNs, { let dev = SpiDevice::new(spi, ncs); Self::new_marg(dev, delay, config) @@ -275,7 +283,7 @@ mod spi_defs { /// Creates a new MARG driver from a SPI peripheral and a NCS pin /// with provided configuration [`Config`]. Reinit function can be used - /// to re-initialize SPI bus. Usecase: change SPI speed for + /// to re-initialize SPI bus. Use case: change SPI speed for /// faster data transfer: /// "Communication with all registers of the device is /// performed using either I2C at 400kHz or SPI at 1M Hz. @@ -288,32 +296,37 @@ mod spi_defs { ncs: NCS, delay: &mut D, config: &mut MpuConfig, - reinit_fn: F) - -> Result as device::Device>::Error>> - where D: DelayMs, - F: FnOnce(SPI, NCS) -> Option<(SPI, NCS)> + reinit_fn: F, + ) -> Result as device::Device>::Error>> + where + D: DelayNs, + F: FnOnce(SPI, NCS) -> Option<(SPI, NCS)>, { let dev = SpiDevice::new(spi, ncs); let mpu = Self::new_marg(dev, delay, config)?; - mpu.reinit_spi_device(reinit_fn) + mpu.reset_device(|spidev| { + let (cspi, cncs) = spidev.release(); + reinit_fn(cspi, cncs) + .map(|(nspi, nncs)| SpiDevice::new(nspi, nncs)) + }) } } #[cfg(feature = "dmp")] - impl Mpu9250, Dmp> - where SPI: spi::Write + spi::Transfer, - NCS: OutputPin + impl Mpu9250, Dmp> + where + SPI: SpiBus, + NCS: OutputPin, { /// Create a new dmp device with default configuration pub fn dmp_default( spi: SPI, ncs: NCS, delay: &mut D, - firmware: &[u8]) - -> Result as device::Device>::Error>> - where D: DelayMs + firmware: &[u8], + ) -> Result as device::Device>::Error>> + where + D: DelayNs, { let dev = SpiDevice::new(spi, ncs); Self::new_dmp(dev, delay, &mut MpuConfig::dmp(), firmware) @@ -325,10 +338,10 @@ mod spi_defs { ncs: NCS, delay: &mut D, config: &mut MpuConfig, - firmware: &[u8]) - -> Result as device::Device>::Error>> - where D: DelayMs + firmware: &[u8], + ) -> Result as device::Device>::Error>> + where + D: DelayNs, { let dev = SpiDevice::new(spi, ncs); Self::new_dmp(dev, delay, config, firmware) @@ -336,29 +349,15 @@ mod spi_defs { } // SPI device, any mode - impl Mpu9250, MODE> - where SPI: spi::Write + spi::Transfer, - NCS: OutputPin + impl Mpu9250, MODE> + where + SPI: SpiBus, + NCS: OutputPin, { /// Destroys the driver recovering the SPI peripheral and the NCS pin pub fn release(self) -> (SPI, NCS) { self.dev.release() } - - fn reinit_spi_device( - self, - reinit_fn: F) - -> Result as device::Device>::Error>> - where F: FnOnce(SPI, NCS) -> Option<(SPI, NCS)> - { - self.reset_device(|spidev| { - let (cspi, cncs) = spidev.release(); - reinit_fn(cspi, cncs).map(|(nspi, nncs)| { - SpiDevice::new(nspi, nncs) - }) - }) - } } } @@ -368,20 +367,20 @@ pub use spi_defs::*; #[cfg(feature = "i2c")] mod i2c_defs { use super::*; - use hal::blocking::i2c; + use hal::i2c::{I2c, SevenBitAddress}; - impl Mpu9250, Imu> - where I2C: i2c::Read - + i2c::Write - + i2c::WriteRead + impl Mpu9250, Imu> + where + I2C: I2c, { /// Creates a new [`Imu`] driver from an I2C peripheral /// with default configuration. pub fn imu_default( i2c: I2C, - delay: &mut D) - -> Result as device::Device>::Error>> - where D: DelayMs + delay: &mut D, + ) -> Result as device::Device>::Error>> + where + D: DelayNs, { Mpu9250::imu(i2c, delay, &mut MpuConfig::imu()) } @@ -393,9 +392,10 @@ mod i2c_defs { pub fn imu( i2c: I2C, delay: &mut D, - config: &mut MpuConfig) - -> Result as device::Device>::Error>> - where D: DelayMs + config: &mut MpuConfig, + ) -> Result as device::Device>::Error>> + where + D: DelayNs, { let dev = I2cDevice::new(i2c); Mpu9250::new_imu(dev, delay, config) @@ -403,7 +403,7 @@ mod i2c_defs { /// Creates a new IMU driver from an I2C peripheral /// with provided configuration [`Config`]. Reinit function can be used - /// to re-initialize I2C bus. Usecase: change I2C speed for + /// to re-initialize I2C bus. Use case: change I2C speed for /// faster data transfer. /// /// [`Config`]: ./conf/struct.MpuConfig.html @@ -411,21 +411,24 @@ mod i2c_defs { i2c: I2C, delay: &mut D, config: &mut MpuConfig, - reinit_fn: F) - -> Result as device::Device>::Error>> - where D: DelayMs, - F: FnOnce(I2C) -> Option + reinit_fn: F, + ) -> Result as device::Device>::Error>> + where + D: DelayNs, + F: FnOnce(I2C) -> Option, { let dev = I2cDevice::new(i2c); let mpu = Self::new_imu(dev, delay, config)?; - mpu.reinit_i2c_device(reinit_fn) + mpu.reset_device(|i2cdev| { + let i2c = i2cdev.release(); + reinit_fn(i2c).map(|i2c| I2cDevice::new(i2c)) + }) } } - impl Mpu9250, Marg> - where I2C: i2c::Read - + i2c::Write - + i2c::WriteRead + impl Mpu9250, Marg> + where + I2C: I2c, { /// Creates a new [`Marg`] driver from an I2C peripheral with /// default [`Config`]. @@ -433,9 +436,10 @@ mod i2c_defs { /// [`Config`]: ./conf/struct.MpuConfig.html pub fn marg_default( i2c: I2C, - delay: &mut D) - -> Result as device::Device>::Error>> - where D: DelayMs + delay: &mut D, + ) -> Result as device::Device>::Error>> + where + D: DelayNs, { Mpu9250::marg(i2c, delay, &mut MpuConfig::marg()) } @@ -447,9 +451,10 @@ mod i2c_defs { pub fn marg( i2c: I2C, delay: &mut D, - config: &mut MpuConfig) - -> Result as device::Device>::Error>> - where D: DelayMs + config: &mut MpuConfig, + ) -> Result as device::Device>::Error>> + where + D: DelayNs, { let dev = I2cDevice::new(i2c); Self::new_marg(dev, delay, config) @@ -457,7 +462,7 @@ mod i2c_defs { /// Creates a new MARG driver from an I2C peripheral /// with provided configuration [`Config`]. Reinit function can be used - /// to re-initialize I2C bus. Usecase: change I2C speed for + /// to re-initialize I2C bus. Use case: change I2C speed for /// faster data transfer. /// /// [`Config`]: ./conf/struct.MpuConfig.html @@ -465,31 +470,35 @@ mod i2c_defs { i2c: I2C, delay: &mut D, config: &mut MpuConfig, - reinit_fn: F) - -> Result as device::Device>::Error>> - where D: DelayMs, - F: FnOnce(I2C) -> Option + reinit_fn: F, + ) -> Result as device::Device>::Error>> + where + D: DelayNs, + F: FnOnce(I2C) -> Option, { let dev = I2cDevice::new(i2c); let mpu = Self::new_marg(dev, delay, config)?; - mpu.reinit_i2c_device(reinit_fn) + mpu.reset_device(|i2cdev| { + let i2c = i2cdev.release(); + reinit_fn(i2c).map(|i2c| I2cDevice::new(i2c)) + }) } } #[cfg(feature = "dmp")] - impl Mpu9250, Dmp> - where I2C: i2c::Read - + i2c::Write - + i2c::WriteRead + impl Mpu9250, Dmp> + where + I2C: I2c, { /// Creates a new DMP driver from an I2C peripheral with default /// configuration pub fn dmp_default( i2c: I2C, delay: &mut D, - firmware: &[u8]) - -> Result as device::Device>::Error>> - where D: DelayMs + firmware: &[u8], + ) -> Result as device::Device>::Error>> + where + D: DelayNs, { let dev = I2cDevice::new(i2c); Self::new_dmp(dev, delay, &mut MpuConfig::dmp(), firmware) @@ -500,9 +509,10 @@ mod i2c_defs { i2c: I2C, delay: &mut D, config: &mut MpuConfig, - firmware: &[u8]) - -> Result as device::Device>::Error>> - where D: DelayMs + firmware: &[u8], + ) -> Result as device::Device>::Error>> + where + D: DelayNs, { let dev = I2cDevice::new(i2c); Self::new_dmp(dev, delay, config, firmware) @@ -510,27 +520,14 @@ mod i2c_defs { } // I2C device, any mode - impl Mpu9250, MODE> - where I2C: i2c::Read - + i2c::Write - + i2c::WriteRead + impl Mpu9250, MODE> + where + I2C: I2c, { /// Destroys the driver, recovering the I2C peripheral pub fn release(self) -> I2C { self.dev.release() } - - fn reinit_i2c_device( - self, - reinit_fn: F) - -> Result as device::Device>::Error>> - where F: FnOnce(I2C) -> Option - { - self.reset_device(|i2cdev| { - let i2c = i2cdev.release(); - reinit_fn(i2c).map(|i2c| I2cDevice::new(i2c)) - }) - } } } @@ -538,31 +535,34 @@ mod i2c_defs { pub use i2c_defs::*; // Any device, 6DOF -impl Mpu9250 where DEV: Device +impl Mpu9250 +where + DEV: Device, { /// Private constructor that creates an IMU-based MPU with the /// specified device. - fn new_imu(dev: DEV, - delay: &mut D, - config: &mut MpuConfig) - -> Result> - where D: DelayMs + fn new_imu( + dev: DEV, + delay: &mut D, + config: &mut MpuConfig, + ) -> Result> + where + D: DelayNs, { - let mut mpu9250 = - Mpu9250 { dev, - raw_mag_sensitivity_adjustments: [0; 3], - mag_sensitivity_adjustments: [0.0; 3], - gyro_scale: config.gyro_scale.unwrap_or_default(), - accel_scale: config.accel_scale.unwrap_or_default(), - mag_scale: MagScale::default(), - accel_data_rate: config.accel_data_rate - .unwrap_or_default(), - gyro_temp_data_rate: config.gyro_temp_data_rate - .unwrap_or_default(), - sample_rate_divisor: config.sample_rate_divisor, - dmp_configuration: config.dmp_configuration, - packet_size: 0, - _mode: PhantomData }; + let mut mpu9250 = Mpu9250 { + dev, + raw_mag_sensitivity_adjustments: [0; 3], + mag_sensitivity_adjustments: [0.0; 3], + gyro_scale: config.gyro_scale.unwrap_or_default(), + accel_scale: config.accel_scale.unwrap_or_default(), + mag_scale: MagScale::default(), + accel_data_rate: config.accel_data_rate.unwrap_or_default(), + gyro_temp_data_rate: config.gyro_temp_data_rate.unwrap_or_default(), + sample_rate_divisor: config.sample_rate_divisor, + dmp_configuration: config.dmp_configuration, + packet_size: 0, + _mode: PhantomData, + }; mpu9250.init_mpu(delay)?; let wai = mpu9250.who_am_i()?; if MpuXDevice::imu_supported(wai) { @@ -577,10 +577,12 @@ impl Mpu9250 where DEV: Device transpose(config.gyro_scale.map(|v| self.gyro_scale(v)))?; transpose(config.accel_scale.map(|v| self.accel_scale(v)))?; transpose(config.accel_data_rate.map(|v| self.accel_data_rate(v)))?; - transpose(config.gyro_temp_data_rate - .map(|v| self.gyro_temp_data_rate(v)))?; - transpose(config.sample_rate_divisor - .map(|v| self.sample_rate_divisor(v)))?; + transpose( + config.gyro_temp_data_rate.map(|v| self.gyro_temp_data_rate(v)), + )?; + transpose( + config.sample_rate_divisor.map(|v| self.sample_rate_divisor(v)), + )?; Ok(()) } @@ -588,7 +590,8 @@ impl Mpu9250 where DEV: Device /// Reads and returns raw unscaled Accelerometer + Gyroscope + Thermometer /// measurements (LSB). pub fn unscaled_all(&mut self) -> Result, E> - where T: From<[i16; 3]> + where + T: From<[i16; 3]>, { let buffer = &mut [0; 15]; self.dev.read_many(Register::ACCEL_XOUT_H, &mut buffer[..])?; @@ -596,15 +599,18 @@ impl Mpu9250 where DEV: Device let temp = ((u16(buffer[7]) << 8) | u16(buffer[8])) as i16; let gyro = self.to_vector(buffer, 8).into(); - Ok(UnscaledImuMeasurements { accel, - gyro, - temp }) + Ok(UnscaledImuMeasurements { + accel, + gyro, + temp, + }) } /// Reads and returns Accelerometer + Gyroscope + Thermometer /// measurements scaled and converted to respective units. pub fn all(&mut self) -> Result, E> - where T: From<[f32; 3]> + where + T: From<[f32; 3]>, { let buffer = &mut [0; 15]; self.dev.read_many(Register::ACCEL_XOUT_H, &mut buffer[..])?; @@ -613,14 +619,16 @@ impl Mpu9250 where DEV: Device let temp = self.scale_temp(buffer, 6); let gyro = self.scale_gyro(buffer, 8).into(); - Ok(ImuMeasurements { accel, - gyro, - temp }) + Ok(ImuMeasurements { + accel, + gyro, + temp, + }) } /// Calculates the average of the at-rest readings of accelerometer and /// gyroscope and then loads the resulting biases into gyro - /// offset registers. Retunrs either Ok with accelerometer biases, or + /// offset registers. Returns either Ok with accelerometer biases, or /// Err(Error), where Error::CalibrationError means soft error, and user /// can proceed on their own risk. /// @@ -628,11 +636,13 @@ impl Mpu9250 where DEV: Device /// /// NOTE: MPU is able to store accelerometer biases, to apply them /// automatically, but at this moment it does not work. - pub fn calibrate_at_rest(&mut self, - delay: &mut D) - -> Result> - where D: DelayMs, - T: From<[f32; 3]> + pub fn calibrate_at_rest( + &mut self, + delay: &mut D, + ) -> Result> + where + D: DelayNs, + T: From<[f32; 3]>, { Ok(self._calibrate_at_rest(delay)?.into()) } @@ -640,31 +650,33 @@ impl Mpu9250 where DEV: Device // Any device, 9DOF impl Mpu9250 - where DEV: Device + AK8963 + NineDOFDevice +where + DEV: Device + AK8963 + NineDOFDevice, { // Private constructor that creates a MARG-based MPU with - // the specificed device. - fn new_marg(dev: DEV, - delay: &mut D, - config: &mut MpuConfig) - -> Result> - where D: DelayMs + // the specified device. + fn new_marg( + dev: DEV, + delay: &mut D, + config: &mut MpuConfig, + ) -> Result> + where + D: DelayNs, { - let mut mpu9250 = - Mpu9250 { dev, - raw_mag_sensitivity_adjustments: [0; 3], - mag_sensitivity_adjustments: [0.0; 3], - gyro_scale: config.gyro_scale.unwrap_or_default(), - accel_scale: config.accel_scale.unwrap_or_default(), - mag_scale: config.mag_scale.unwrap_or_default(), - accel_data_rate: config.accel_data_rate - .unwrap_or_default(), - gyro_temp_data_rate: config.gyro_temp_data_rate - .unwrap_or_default(), - sample_rate_divisor: config.sample_rate_divisor, - dmp_configuration: config.dmp_configuration, - packet_size: 0, - _mode: PhantomData }; + let mut mpu9250 = Mpu9250 { + dev, + raw_mag_sensitivity_adjustments: [0; 3], + mag_sensitivity_adjustments: [0.0; 3], + gyro_scale: config.gyro_scale.unwrap_or_default(), + accel_scale: config.accel_scale.unwrap_or_default(), + mag_scale: config.mag_scale.unwrap_or_default(), + accel_data_rate: config.accel_data_rate.unwrap_or_default(), + gyro_temp_data_rate: config.gyro_temp_data_rate.unwrap_or_default(), + sample_rate_divisor: config.sample_rate_divisor, + dmp_configuration: config.dmp_configuration, + packet_size: 0, + _mode: PhantomData, + }; mpu9250.init_mpu(delay)?; let wai = mpu9250.who_am_i()?; if MpuXDevice::marg_supported(wai) { @@ -680,7 +692,7 @@ impl Mpu9250 /// Calculates the average of the at-rest readings of accelerometer and /// gyroscope and then loads the resulting biases into gyro - /// offset registers. Retunrs either Ok with accelerometer biases, or + /// offset registers. Returns either Ok with accelerometer biases, or /// Err(Error), where Error::CalibrationError means soft error, and user /// can proceed on their own risk. /// @@ -688,11 +700,13 @@ impl Mpu9250 /// /// NOTE: MPU is able to store accelerometer biases, to apply them /// automatically, but at this moment it does not work. - pub fn calibrate_at_rest(&mut self, - delay: &mut D) - -> Result> - where D: DelayMs, - T: From<[f32; 3]> + pub fn calibrate_at_rest( + &mut self, + delay: &mut D, + ) -> Result> + where + D: DelayNs, + T: From<[f32; 3]>, { let accel_biases = self._calibrate_at_rest(delay)?; self.init_ak8963(delay)?; @@ -700,34 +714,36 @@ impl Mpu9250 } fn init_ak8963(&mut self, delay: &mut D) -> Result<(), E> - where D: DelayMs + where + D: DelayNs, { AK8963::init(&mut self.dev, delay)?; - delay.delay_ms(10); + delay.delay_ns(10 * 1_000_000); // First extract the factory calibration for each magnetometer axis // Power down AK8963::write(&mut self.dev, ak8963::Register::CNTL1, 0x00)?; - delay.delay_ms(10); + delay.delay_ns(10 * 1_000_000); // Fuse ROM access mode AK8963::write(&mut self.dev, ak8963::Register::CNTL1, 0x0F)?; - delay.delay_ms(20); + delay.delay_ns(20 * 1_000_000); let mag_x_bias = AK8963::read(&mut self.dev, ak8963::Register::ASAX)?; let mag_y_bias = AK8963::read(&mut self.dev, ak8963::Register::ASAY)?; let mag_z_bias = AK8963::read(&mut self.dev, ak8963::Register::ASAZ)?; // Return x-axis sensitivity adjustment values, etc. self.raw_mag_sensitivity_adjustments = [mag_x_bias, mag_y_bias, mag_z_bias]; - self.mag_sensitivity_adjustments = - [((mag_x_bias - 128) as f32) / 256. + 1., - ((mag_y_bias - 128) as f32) / 256. + 1., - ((mag_z_bias - 128) as f32) / 256. + 1.]; + self.mag_sensitivity_adjustments = [ + ((mag_x_bias - 128) as f32) / 256. + 1., + ((mag_y_bias - 128) as f32) / 256. + 1., + ((mag_z_bias - 128) as f32) / 256. + 1., + ]; // Power down magnetometer AK8963::write(&mut self.dev, ak8963::Register::CNTL1, 0x00)?; - delay.delay_ms(10); + delay.delay_ns(10 * 1_000_000); // Set magnetometer data resolution and sample ODR self._mag_scale()?; - delay.delay_ms(10); + delay.delay_ns(10 * 1_000_000); AK8963::finalize(&mut self.dev, delay)?; @@ -740,10 +756,12 @@ impl Mpu9250 transpose(config.accel_scale.map(|v| self.accel_scale(v)))?; transpose(config.mag_scale.map(|v| self.mag_scale(v)))?; transpose(config.accel_data_rate.map(|v| self.accel_data_rate(v)))?; - transpose(config.gyro_temp_data_rate - .map(|v| self.gyro_temp_data_rate(v)))?; - transpose(config.sample_rate_divisor - .map(|v| self.sample_rate_divisor(v)))?; + transpose( + config.gyro_temp_data_rate.map(|v| self.gyro_temp_data_rate(v)), + )?; + transpose( + config.sample_rate_divisor.map(|v| self.sample_rate_divisor(v)), + )?; Ok(()) } @@ -751,42 +769,52 @@ impl Mpu9250 /// Reads and returns raw unscaled Accelerometer + Gyroscope + Thermometer /// + Magnetometer measurements (LSB). pub fn unscaled_all(&mut self) -> Result, E> - where T: From<[i16; 3]> + where + T: From<[i16; 3]>, { let buffer = &mut [0; 21]; - NineDOFDevice::read_9dof(&mut self.dev, - Register::ACCEL_XOUT_H, - buffer)?; + NineDOFDevice::read_9dof( + &mut self.dev, + Register::ACCEL_XOUT_H, + buffer, + )?; let accel = self.to_vector(buffer, 0).into(); let temp = ((u16(buffer[7]) << 8) | u16(buffer[8])) as i16; let gyro = self.to_vector(buffer, 8).into(); let mag = self.to_vector_inverted(buffer, 14).into(); - Ok(UnscaledMargMeasurements { accel, - gyro, - temp, - mag }) + Ok(UnscaledMargMeasurements { + accel, + gyro, + temp, + mag, + }) } /// Reads and returns Accelerometer + Gyroscope + Thermometer + Magnetometer /// measurements scaled and converted to respective units. pub fn all(&mut self) -> Result, E> - where T: From<[f32; 3]> + where + T: From<[f32; 3]>, { let buffer = &mut [0; 21]; - NineDOFDevice::read_9dof(&mut self.dev, - Register::ACCEL_XOUT_H, - buffer)?; + NineDOFDevice::read_9dof( + &mut self.dev, + Register::ACCEL_XOUT_H, + buffer, + )?; let accel = self.scale_accel(buffer, 0).into(); let temp = self.scale_temp(buffer, 6); let gyro = self.scale_gyro(buffer, 8).into(); let mag = self.scale_and_correct_mag(buffer, 14).into(); - Ok(MargMeasurements { accel, - gyro, - temp, - mag }) + Ok(MargMeasurements { + accel, + gyro, + temp, + mag, + }) } /// Perform magnetometer self-test @@ -803,13 +831,15 @@ impl Mpu9250 // register should be kept “0”) AK8963::write(&mut self.dev, ak8963::Register::ASTC, 0b01000000)?; // (3) Set Self-test Mode. (MODE[3:0]=“1000”) - AK8963::write(&mut self.dev, - ak8963::Register::CNTL1, - control | 0b00001000)?; + AK8963::write( + &mut self.dev, + ak8963::Register::CNTL1, + control | 0b00001000, + )?; // (4) Check Data Ready or not by any of the following method. // - Polling DRDY bit of ST1 register while AK8963::read(&mut self.dev, ak8963::Register::ST1)? & 0b00000001 - != 0 + != 0 {} // When Data Ready, proceed to the next step. // (5) Read measurement data (HXL to HZH) @@ -842,14 +872,17 @@ impl Mpu9250 let resolution = self.mag_scale.resolution(); let raw = self.to_vector_inverted(buffer, offset); - [raw[0] as f32 * resolution * self.mag_sensitivity_adjustments[0], - raw[1] as f32 * resolution * self.mag_sensitivity_adjustments[1], - raw[2] as f32 * resolution * self.mag_sensitivity_adjustments[2]] + [ + raw[0] as f32 * resolution * self.mag_sensitivity_adjustments[0], + raw[1] as f32 * resolution * self.mag_sensitivity_adjustments[1], + raw[2] as f32 * resolution * self.mag_sensitivity_adjustments[2], + ] } /// Reads and returns raw unscaled Magnetometer measurements (LSB). pub fn unscaled_mag(&mut self) -> Result - where T: From<[i16; 3]> + where + T: From<[i16; 3]>, { let buffer = &mut [0; 7]; self.dev.read_xyz(buffer)?; @@ -859,7 +892,8 @@ impl Mpu9250 /// Read and returns Magnetometer measurements scaled, adjusted for factory /// sensitivities, and converted to microTeslas. pub fn mag(&mut self) -> Result - where T: From<[f32; 3]> + where + T: From<[f32; 3]>, { let buffer = &mut [0; 7]; self.dev.read_xyz(buffer)?; @@ -868,19 +902,21 @@ impl Mpu9250 /// Returns raw mag sensitivity adjustments pub fn raw_mag_sensitivity_adjustments(&self) -> T - where T: From<[u8; 3]> + where + T: From<[u8; 3]>, { self.raw_mag_sensitivity_adjustments.into() } /// Returns mag sensitivity adjustments pub fn mag_sensitivity_adjustments(&self) -> T - where T: From<[f32; 3]> + where + T: From<[f32; 3]>, { self.mag_sensitivity_adjustments.into() } - /// Configures magnetrometer full reading scale ([`MagScale`]) + /// Configures magnetometer full reading scale ([`MagScale`]) /// /// [`Mag scale`]: ./conf/enum.MagScale.html pub fn mag_scale(&mut self, scale: MagScale) -> Result<(), E> { @@ -891,9 +927,11 @@ impl Mpu9250 fn _mag_scale(&mut self) -> Result<(), E> { // Set magnetometer data resolution and sample ODR let scale = self.mag_scale as u8; - AK8963::write(&mut self.dev, - ak8963::Register::CNTL1, - scale << 4 | MMODE)?; + AK8963::write( + &mut self.dev, + ak8963::Register::CNTL1, + scale << 4 | MMODE, + )?; Ok(()) } @@ -914,52 +952,58 @@ impl Mpu9250 // Any device, DMP #[cfg(feature = "dmp")] -impl Mpu9250 where DEV: Device +impl Mpu9250 +where + DEV: Device, { /// Private constructor that creates a DMP-based MPU with the /// specified device. - fn new_dmp(dev: DEV, - delay: &mut D, - config: &mut MpuConfig, - firmware: &[u8]) - -> Result> - where D: DelayMs + fn new_dmp( + dev: DEV, + delay: &mut D, + config: &mut MpuConfig, + firmware: &[u8], + ) -> Result> + where + D: DelayNs, { - let mut mpu9250 = - Mpu9250 { dev, - raw_mag_sensitivity_adjustments: [0; 3], - mag_sensitivity_adjustments: [0.0; 3], - gyro_scale: config.gyro_scale - .unwrap_or(GyroScale::_2000DPS), - accel_scale: config.accel_scale - .unwrap_or(AccelScale::_8G), - mag_scale: config.mag_scale.unwrap_or_default(), - accel_data_rate: - config.accel_data_rate - .unwrap_or(AccelDataRate::DlpfConf(Dlpf::_1)), - gyro_temp_data_rate: - config.gyro_temp_data_rate - .unwrap_or(GyroTempDataRate::DlpfConf(Dlpf::_1)), - sample_rate_divisor: config.sample_rate_divisor - .or(Some(4)), - dmp_configuration: Some(config.dmp_configuration - .unwrap_or_default()), - packet_size: config.dmp_configuration - .unwrap_or_default() - .features - .packet_size(), - _mode: PhantomData }; + let mut mpu9250 = Mpu9250 { + dev, + raw_mag_sensitivity_adjustments: [0; 3], + mag_sensitivity_adjustments: [0.0; 3], + gyro_scale: config.gyro_scale.unwrap_or(GyroScale::_2000DPS), + accel_scale: config.accel_scale.unwrap_or(AccelScale::_8G), + mag_scale: config.mag_scale.unwrap_or_default(), + accel_data_rate: config + .accel_data_rate + .unwrap_or(AccelDataRate::DlpfConf(Dlpf::_1)), + gyro_temp_data_rate: config + .gyro_temp_data_rate + .unwrap_or(GyroTempDataRate::DlpfConf(Dlpf::_1)), + sample_rate_divisor: config.sample_rate_divisor.or(Some(4)), + dmp_configuration: Some( + config.dmp_configuration.unwrap_or_default(), + ), + packet_size: config + .dmp_configuration + .unwrap_or_default() + .features + .packet_size(), + _mode: PhantomData, + }; mpu9250.init_mpu(delay)?; mpu9250.init_dmp(delay, firmware)?; Ok(mpu9250) } /// Logic to init the dmp - fn init_dmp(&mut self, - delay: &mut D, - firmware: &[u8]) - -> Result<(), Error> - where D: DelayMs + fn init_dmp( + &mut self, + delay: &mut D, + firmware: &[u8], + ) -> Result<(), Error> + where + D: DelayNs, { let conf = self.dmp_configuration.unwrap_or_default(); // disable i2c master mode and enable fifo @@ -968,10 +1012,12 @@ impl Mpu9250 where DEV: Device delay.delay_ms(3); // enable i2c bypass - self.interrupt_config(InterruptConfig::LATCH_INT_EN - | InterruptConfig::INT_ANYRD_CLEAR - | InterruptConfig::ACL - | InterruptConfig::BYPASS_EN)?; + self.interrupt_config( + InterruptConfig::LATCH_INT_EN + | InterruptConfig::INT_ANYRD_CLEAR + | InterruptConfig::ACL + | InterruptConfig::BYPASS_EN, + )?; // load firmware self.load_firmware(firmware)?; @@ -988,9 +1034,13 @@ impl Mpu9250 where DEV: Device let div = [0, conf.rate as u8]; self.write_mem(DmpMemory::D_0_22, &div)?; - self.write_mem(DmpMemory::CFG_6, - &[0xfe, 0xf2, 0xab, 0xc4, 0xaa, 0xf1, 0xdf, 0xdf, - 0xbb, 0xaf, 0xdf, 0xdf])?; + self.write_mem( + DmpMemory::CFG_6, + &[ + 0xfe, 0xf2, 0xab, 0xc4, 0xaa, 0xf1, 0xdf, 0xdf, 0xbb, 0xaf, + 0xdf, 0xdf, + ], + )?; // turn on the dmp self.dev.write(Register::INT_ENABLE, 0)?; @@ -998,11 +1048,13 @@ impl Mpu9250 where DEV: Device // enable i2c bypass self.dev.write(Register::USER_CTRL, FIFO_EN)?; - delay.delay_ms(10); - self.interrupt_config(InterruptConfig::LATCH_INT_EN - | InterruptConfig::INT_ANYRD_CLEAR - | InterruptConfig::ACL - | InterruptConfig::BYPASS_EN)?; + delay.delay_ns(10 * 1_000_000); + self.interrupt_config( + InterruptConfig::LATCH_INT_EN + | InterruptConfig::INT_ANYRD_CLEAR + | InterruptConfig::ACL + | InterruptConfig::BYPASS_EN, + )?; self.dev.write(Register::FIFO_EN, 0)?; self.dev.write(Register::INT_ENABLE, 0x02)?; @@ -1010,9 +1062,10 @@ impl Mpu9250 where DEV: Device self.reset_fifo(delay)?; // set interrupt mode - self.write_mem(DmpMemory::CFG_FIFO_ON_EVENT, - &[0xd8, 0xb1, 0xb9, 0xf3, 0x8b, 0xa3, 0x91, 0xb6, - 0x09, 0xb4, 0xd9])?; + self.write_mem( + DmpMemory::CFG_FIFO_ON_EVENT, + &[0xd8, 0xb1, 0xb9, 0xf3, 0x8b, 0xa3, 0x91, 0xb6, 0x09, 0xb4, 0xd9], + )?; Ok(()) } @@ -1040,7 +1093,8 @@ impl Mpu9250 where DEV: Device /// Write the provided slice at the specified address in dmp memory fn write_mem(&mut self, addr: T, data: &[u8]) -> Result<(), Error> - where T: Into + Copy + where + T: Into + Copy, { self.dev.write(Register::BANK_SEL, (addr.into() >> 8) as u8)?; self.dev.write(Register::MEM_ADDR, (addr.into() & 0xff) as u8)?; @@ -1050,7 +1104,8 @@ impl Mpu9250 where DEV: Device /// Read dmp memory at the specified address into data fn read_mem(&mut self, addr: T, data: &mut [u8]) -> Result<(), Error> - where T: Into + Copy + where + T: Into + Copy, { self.dev.write(Register::BANK_SEL, (addr.into() >> 8) as u8)?; self.dev.write(Register::MEM_ADDR, (addr.into() & 0xff) as u8)?; @@ -1060,13 +1115,16 @@ impl Mpu9250 where DEV: Device /// Select which dmp features should be enabled fn set_dmp_feature(&mut self, delay: &mut D) -> Result<(), Error> - where D: DelayMs + where + D: DelayNs, { let features = self.dmp_configuration.unwrap_or_default().features; - const GYRO_SF: [u8; 4] = [(46_850_825 >> 24) as u8, - (46_850_825 >> 16) as u8, - (46_850_825 >> 8) as u8, - (46_850_825 & 0xff) as u8]; + const GYRO_SF: [u8; 4] = [ + (46_850_825 >> 24) as u8, + (46_850_825 >> 16) as u8, + (46_850_825 >> 8) as u8, + (46_850_825 & 0xff) as u8, + ]; self.write_mem(DmpMemory::D_0_104, &GYRO_SF)?; let mut conf = [0xa3 as u8; 10]; @@ -1097,36 +1155,48 @@ impl Mpu9250 where DEV: Device Ok(()) }; - set_config(DmpMemory::CFG_27, - features.tap | features.android_orient, - &[0x20], - &[0xd8])?; - set_config(DmpMemory::CFG_MOTION_BIAS, - features.gyro_auto_calibrate, - &[0xb8, 0xaa, 0xb3, 0x8d, 0xb4, 0x98, 0x0d, 0x35, 0x5d], - &[0xb8, 0xaa, 0xaa, 0xaa, 0xb0, 0x88, 0xc3, 0xc5, 0xc7])?; + set_config( + DmpMemory::CFG_27, + features.tap | features.android_orient, + &[0x20], + &[0xd8], + )?; + set_config( + DmpMemory::CFG_MOTION_BIAS, + features.gyro_auto_calibrate, + &[0xb8, 0xaa, 0xb3, 0x8d, 0xb4, 0x98, 0x0d, 0x35, 0x5d], + &[0xb8, 0xaa, 0xaa, 0xaa, 0xb0, 0x88, 0xc3, 0xc5, 0xc7], + )?; if features.raw_gyro { - set_config(DmpMemory::CFG_GYRO_RAW_DATA, - features.gyro_auto_calibrate, - &[0xb2, 0x8b, 0xb6, 0x9b], - &[0xb0, 0x80, 0xb4, 0x90])?; + set_config( + DmpMemory::CFG_GYRO_RAW_DATA, + features.gyro_auto_calibrate, + &[0xb2, 0x8b, 0xb6, 0x9b], + &[0xb0, 0x80, 0xb4, 0x90], + )?; } // TODO handle tap set_config(DmpMemory::CFG_20, features.tap, &[0xf8], &[0xd8])?; - set_config(DmpMemory::CFG_ANDROID_ORIENT_INT, - features.android_orient, - &[0xd9], - &[0xd8])?; - set_config(DmpMemory::CFG_LP_QUAT, - features.quat, - &[0xc0, 0xc2, 0xc4, 0xc6], - &[0x8b, 0x8b, 0x8b, 0x8b])?; - set_config(DmpMemory::CFG_8, - features.quat6, - &[0x20, 0x28, 0x30, 0x38], - &[0xa3, 0xa3, 0xa3, 0xa3])?; + set_config( + DmpMemory::CFG_ANDROID_ORIENT_INT, + features.android_orient, + &[0xd9], + &[0xd8], + )?; + set_config( + DmpMemory::CFG_LP_QUAT, + features.quat, + &[0xc0, 0xc2, 0xc4, 0xc6], + &[0x8b, 0x8b, 0x8b, 0x8b], + )?; + set_config( + DmpMemory::CFG_8, + features.quat6, + &[0x20, 0x28, 0x30, 0x38], + &[0xa3, 0xa3, 0xa3, 0xa3], + )?; self.reset_fifo(delay)?; @@ -1136,10 +1206,11 @@ impl Mpu9250 where DEV: Device /// Reads and returns raw unscaled DMP measurement depending on /// activated features(LSB). pub fn dmp_unscaled_all( - &mut self) - -> Result, Error> - where T1: From<[i16; 3]>, - T2: From<[i32; 4]> + &mut self, + ) -> Result, Error> + where + T1: From<[i16; 3]>, + T2: From<[i32; 4]>, { let features = self.dmp_configuration.unwrap_or_default().features; @@ -1151,9 +1222,11 @@ impl Mpu9250 where DEV: Device let mut offset = 0; let mut measures: UnscaledDmpMeasurement = - UnscaledDmpMeasurement { quaternion: None, - accel: None, - gyro: None }; + UnscaledDmpMeasurement { + quaternion: None, + accel: None, + gyro: None, + }; if features.quat6 || features.quat { measures.quaternion = Some(self.to_quat(&buffer).into()); offset += 16; @@ -1172,10 +1245,12 @@ impl Mpu9250 where DEV: Device /// Read all measurement from DMP /// Reads and returns DMP measurement scaled depending on /// activated features(LSB). - pub fn dmp_all(&mut self) - -> Result, Error> - where T1: From<[f32; 3]>, - T2: From<[f64; 4]> + pub fn dmp_all( + &mut self, + ) -> Result, Error> + where + T1: From<[f32; 3]>, + T2: From<[f64; 4]>, { let features = self.dmp_configuration.unwrap_or_default().features; @@ -1186,10 +1261,11 @@ impl Mpu9250 where DEV: Device } let mut offset = 0; - let mut measures: DmpMeasurement = - DmpMeasurement { quaternion: None, - accel: None, - gyro: None }; + let mut measures: DmpMeasurement = DmpMeasurement { + quaternion: None, + accel: None, + gyro: None, + }; if features.quat6 || features.quat { measures.quaternion = Some(self.to_norm_quat(&buffer).into()); offset += 16; @@ -1207,32 +1283,36 @@ impl Mpu9250 where DEV: Device /// Parse quaternion from fifo buffer fn to_quat(&self, buffer: &[u8]) -> [i32; 4] { - [(buffer[1] as i32) << 24 - | (buffer[2] as i32) << 16 - | (buffer[3] as i32) << 8 - | buffer[4] as i32, - (buffer[5] as i32) << 24 - | (buffer[6] as i32) << 16 - | (buffer[7] as i32) << 8 - | buffer[8] as i32, - (buffer[9] as i32) << 24 - | (buffer[10] as i32) << 16 - | (buffer[11] as i32) << 8 - | buffer[12] as i32, - (buffer[13] as i32) << 24 - | (buffer[14] as i32) << 16 - | (buffer[15] as i32) << 8 - | buffer[16] as i32] + [ + (buffer[1] as i32) << 24 + | (buffer[2] as i32) << 16 + | (buffer[3] as i32) << 8 + | buffer[4] as i32, + (buffer[5] as i32) << 24 + | (buffer[6] as i32) << 16 + | (buffer[7] as i32) << 8 + | buffer[8] as i32, + (buffer[9] as i32) << 24 + | (buffer[10] as i32) << 16 + | (buffer[11] as i32) << 8 + | buffer[12] as i32, + (buffer[13] as i32) << 24 + | (buffer[14] as i32) << 16 + | (buffer[15] as i32) << 8 + | buffer[16] as i32, + ] } /// Normalized the quaternion fn to_norm_quat(&self, buffer: &[u8]) -> [f64; 4] { let quat = self.to_quat(buffer); //TODO handle this better, here is an ugly map on fixed size array - let quat = [f64::from(quat[0]), - f64::from(quat[1]), - f64::from(quat[2]), - f64::from(quat[3])]; + let quat = [ + f64::from(quat[0]), + f64::from(quat[1]), + f64::from(quat[2]), + f64::from(quat[3]), + ]; let sum = libm::sqrt(quat.iter().map(|x| libm::pow(*x, 2.0)).sum::()); [quat[0] / sum, quat[1] / sum, quat[2] / sum, quat[3] / sum] @@ -1240,10 +1320,13 @@ impl Mpu9250 where DEV: Device } // Any device, any mode -impl Mpu9250 where DEV: Device +impl Mpu9250 +where + DEV: Device, { fn init_mpu(&mut self, delay: &mut D) -> Result<(), E> - where D: DelayMs + where + D: DelayNs, { // Stop all communication with peripherals (such as AK8963). // If the chip is already powered up and if the communication is already @@ -1285,7 +1368,8 @@ impl Mpu9250 where DEV: Device } fn reset_device(self, f: F) -> Result> - where F: FnOnce(DEV) -> Option + where + F: FnOnce(DEV) -> Option, { let raw_mag_sensitivity_adjustments = self.raw_mag_sensitivity_adjustments; @@ -1300,18 +1384,20 @@ impl Mpu9250 where DEV: Device let packet_size = self.packet_size; let _mode = self._mode; if let Some(new_dev) = f(self.dev) { - Ok(Mpu9250 { dev: new_dev, - raw_mag_sensitivity_adjustments, - mag_sensitivity_adjustments, - gyro_scale, - accel_scale, - mag_scale, - accel_data_rate, - gyro_temp_data_rate, - sample_rate_divisor, - dmp_configuration, - packet_size, - _mode }) + Ok(Mpu9250 { + dev: new_dev, + raw_mag_sensitivity_adjustments, + mag_sensitivity_adjustments, + gyro_scale, + accel_scale, + mag_scale, + accel_data_rate, + gyro_temp_data_rate, + sample_rate_divisor, + dmp_configuration, + packet_size, + _mode, + }) } else { Err(Error::ReInitError) } @@ -1359,7 +1445,7 @@ impl Mpu9250 where DEV: Device self.dev.modify(Register::INT_ENABLE, |r| r & !ie.bits()) } - /// Get interrupt configurtion + /// Get interrupt configuration pub fn get_interrupt_config(&mut self) -> Result { let bits = self.dev.read(Register::INT_PIN_CFG)?; Ok(InterruptConfig::from_bits_truncate(bits)) @@ -1372,7 +1458,8 @@ impl Mpu9250 where DEV: Device /// Reset the internal FIFO pub fn reset_fifo(&mut self, delay: &mut D) -> Result<(), Error> - where D: DelayMs + where + D: DelayNs, { self.dev.write(Register::INT_ENABLE, 0)?; self.dev.write(Register::FIFO_EN, 0)?; @@ -1390,7 +1477,7 @@ impl Mpu9250 where DEV: Device /// Return the number of byte left in the FIFO. /// - If the number is positive, bytes are left in the FIFO /// - If the number is negative, only `data.len() - 1 - size` bytes were - /// avilable and were not read + /// available and were not read /// - If the number is 0, the FIFO is empty and data has been filled fully pub fn read_fifo(&mut self, data: &mut [u8]) -> Result> { let mut buffer: [u8; 3] = [0; 3]; @@ -1404,7 +1491,8 @@ impl Mpu9250 where DEV: Device /// Reads and returns unscaled accelerometer measurements (LSB). pub fn unscaled_accel(&mut self) -> Result - where T: From<[i16; 3]> + where + T: From<[i16; 3]>, { let buffer = &mut [0; 7]; self.dev.read_many(Register::ACCEL_XOUT_H, buffer)?; @@ -1413,16 +1501,18 @@ impl Mpu9250 where DEV: Device /// Reads and returns accelerometer measurements scaled and converted to g. pub fn accel(&mut self) -> Result - where T: From<[f32; 3]> + where + T: From<[f32; 3]>, { let buffer = &mut [0; 7]; self.dev.read_many(Register::ACCEL_XOUT_H, buffer)?; Ok(self.scale_accel(buffer, 0).into()) } - /// Reads and returns unsacled Gyroscope measurements (LSB). + /// Reads and returns unscaled Gyroscope measurements (LSB). pub fn unscaled_gyro(&mut self) -> Result - where T: From<[i16; 3]> + where + T: From<[i16; 3]>, { let buffer = &mut [0; 7]; self.dev.read_many(Register::GYRO_XOUT_H, buffer)?; @@ -1431,7 +1521,8 @@ impl Mpu9250 where DEV: Device /// Reads and returns gyroscope measurements scaled and converted to rad/s. pub fn gyro(&mut self) -> Result - where T: From<[f32; 3]> + where + T: From<[f32; 3]>, { let buffer = &mut [0; 7]; self.dev.read_many(Register::GYRO_XOUT_H, buffer)?; @@ -1441,9 +1532,10 @@ impl Mpu9250 where DEV: Device /// Configures accelerometer data rate config ([`AccelDataRate`]). /// /// [`AccelDataRate`]: ./conf/enum.AccelDataRate.html - pub fn accel_data_rate(&mut self, - accel_data_rate: AccelDataRate) - -> Result<(), E> { + pub fn accel_data_rate( + &mut self, + accel_data_rate: AccelDataRate, + ) -> Result<(), E> { self.accel_data_rate = accel_data_rate; self._accel_data_rate() } @@ -1477,9 +1569,10 @@ impl Mpu9250 where DEV: Device /// ([`GyroTempDataRate`]). /// /// [`GyroTempDataRate`]: ./conf/enum.GyroTempDataRate.html - pub fn gyro_temp_data_rate(&mut self, - data_rate: GyroTempDataRate) - -> Result<(), E> { + pub fn gyro_temp_data_rate( + &mut self, + data_rate: GyroTempDataRate, + ) -> Result<(), E> { self.gyro_temp_data_rate = data_rate; self._gyro_temp_data_rate() } @@ -1551,10 +1644,12 @@ impl Mpu9250 where DEV: Device Ok(()) } - fn _calibrate_at_rest(&mut self, - delay: &mut D) - -> Result<[f32; 3], Error> - where D: DelayMs + fn _calibrate_at_rest( + &mut self, + delay: &mut D, + ) -> Result<[f32; 3], Error> + where + D: DelayNs, { // First save current values, as we reset them below let orig_gyro_scale = self.gyro_scale; @@ -1644,10 +1739,14 @@ impl Mpu9250 where DEV: Device // Biases are additive, so change sign on // calculated average gyro biases - self.set_unscaled_gyro_bias(false, - [(gyro_biases[0] / -4) as i16, - (gyro_biases[1] / -4) as i16, - (gyro_biases[2] / -4) as i16])?; + self.set_unscaled_gyro_bias( + false, + [ + (gyro_biases[0] / -4) as i16, + (gyro_biases[1] / -4) as i16, + (gyro_biases[2] / -4) as i16, + ], + )?; // Compute accelerometer biases to be returned let resolution = self.accel_scale.resolution(); @@ -1661,41 +1760,50 @@ impl Mpu9250 where DEV: Device self.sample_rate_divisor = orig_sample_rate_divisor; self.init_mpu(delay)?; - Ok([accel_biases[0] as f32 * scale, + Ok([ + accel_biases[0] as f32 * scale, accel_biases[1] as f32 * scale, - accel_biases[2] as f32 * scale]) + accel_biases[2] as f32 * scale, + ]) } /// Get unscaled gyroscope biases. /// Output format is +-1000dps fn get_unscaled_gyro_bias(&mut self) -> Result<[i16; 3], Error> { - Ok([(self.dev.read(Register::XG_OFFSET_H)? as i16) << 8 - | self.dev.read(Register::XG_OFFSET_L)? as i16, + Ok([ + (self.dev.read(Register::XG_OFFSET_H)? as i16) << 8 + | self.dev.read(Register::XG_OFFSET_L)? as i16, (self.dev.read(Register::YG_OFFSET_H)? as i16) << 8 - | self.dev.read(Register::YG_OFFSET_L)? as i16, + | self.dev.read(Register::YG_OFFSET_L)? as i16, (self.dev.read(Register::ZG_OFFSET_H)? as i16) << 8 - | self.dev.read(Register::ZG_OFFSET_L)? as i16]) + | self.dev.read(Register::ZG_OFFSET_L)? as i16, + ]) } /// Get scaled gyroscope biases. pub fn get_gyro_bias(&mut self) -> Result> - where T: From<[f32; 3]> + where + T: From<[f32; 3]>, { let biases = self.get_unscaled_gyro_bias()?; let scale = GyroScale::_1000DPS.resolution(); - Ok([biases[0] as f32 * scale, + Ok([ + biases[0] as f32 * scale, biases[1] as f32 * scale, - biases[2] as f32 * scale].into()) + biases[2] as f32 * scale, + ] + .into()) } /// Set unscaled gyroscope biases. /// In relative mode it will add the new biases to the existing ones instead /// of replacing them. Input format is +-1000dps - fn set_unscaled_gyro_bias(&mut self, - relative: bool, - biases: [i16; 3]) - -> Result<(), Error> { + fn set_unscaled_gyro_bias( + &mut self, + relative: bool, + biases: [i16; 3], + ) -> Result<(), Error> { let mut new_biases = biases; if relative { @@ -1706,14 +1814,20 @@ impl Mpu9250 where DEV: Device } } - self.dev.write(Register::XG_OFFSET_H, - ((new_biases[0] >> 8) & 0xFF) as u8)?; + self.dev.write( + Register::XG_OFFSET_H, + ((new_biases[0] >> 8) & 0xFF) as u8, + )?; self.dev.write(Register::XG_OFFSET_L, (new_biases[0] & 0xFF) as u8)?; - self.dev.write(Register::YG_OFFSET_H, - ((new_biases[1] >> 8) & 0xFF) as u8)?; + self.dev.write( + Register::YG_OFFSET_H, + ((new_biases[1] >> 8) & 0xFF) as u8, + )?; self.dev.write(Register::YG_OFFSET_L, (new_biases[1] & 0xFF) as u8)?; - self.dev.write(Register::ZG_OFFSET_H, - ((new_biases[2] >> 8) & 0xFF) as u8)?; + self.dev.write( + Register::ZG_OFFSET_H, + ((new_biases[2] >> 8) & 0xFF) as u8, + )?; self.dev.write(Register::ZG_OFFSET_L, (new_biases[2] & 0xFF) as u8)?; Ok(()) @@ -1722,52 +1836,65 @@ impl Mpu9250 where DEV: Device /// Set scaled gyro biases. /// In relative mode it will add the new biases to the existing ones instead /// of replacing them. - pub fn set_gyro_bias(&mut self, - relative: bool, - biases: T) - -> Result<(), Error> - where T: Into<[f32; 3]> + pub fn set_gyro_bias( + &mut self, + relative: bool, + biases: T, + ) -> Result<(), Error> + where + T: Into<[f32; 3]>, { let biases = biases.into(); let scale = GyroScale::_1000DPS.resolution(); - self.set_unscaled_gyro_bias(relative, - [(biases[0] / scale) as i16, - (biases[1] / scale) as i16, - (biases[2] / scale) as i16]) + self.set_unscaled_gyro_bias( + relative, + [ + (biases[0] / scale) as i16, + (biases[1] / scale) as i16, + (biases[2] / scale) as i16, + ], + ) } /// Get unscaled accelerometer biases. /// Output format is +-16G fn get_unscaled_accel_bias(&mut self) -> Result<[i16; 3], Error> { - Ok([(self.dev.read(Register::XA_OFFSET_H)? as i16) << 8 - | self.dev.read(Register::XA_OFFSET_L)? as i16, + Ok([ + (self.dev.read(Register::XA_OFFSET_H)? as i16) << 8 + | self.dev.read(Register::XA_OFFSET_L)? as i16, (self.dev.read(Register::YA_OFFSET_H)? as i16) << 8 - | self.dev.read(Register::YA_OFFSET_L)? as i16, + | self.dev.read(Register::YA_OFFSET_L)? as i16, (self.dev.read(Register::ZA_OFFSET_H)? as i16) << 8 - | self.dev.read(Register::ZA_OFFSET_L)? as i16]) + | self.dev.read(Register::ZA_OFFSET_L)? as i16, + ]) } /// Get scaled accelerometer biases. pub fn get_accel_bias(&mut self) -> Result> - where T: From<[f32; 3]> + where + T: From<[f32; 3]>, { let biases = self.get_unscaled_accel_bias()?; let scale = G * AccelScale::_16G.resolution(); - Ok([biases[0] as f32 * scale, + Ok([ + biases[0] as f32 * scale, biases[1] as f32 * scale, - biases[2] as f32 * scale].into()) + biases[2] as f32 * scale, + ] + .into()) } /// Set unscaled accelerometer biases. /// Keep in mind that the registers contain factory-supplied values after /// reset. In relative mode it will add the new biases to the existing /// ones instead of replacing them. Input format is +-16G. - fn set_unscaled_accel_bias(&mut self, - relative: bool, - biases: [i16; 3]) - -> Result<(), Error> { + fn set_unscaled_accel_bias( + &mut self, + relative: bool, + biases: [i16; 3], + ) -> Result<(), Error> { let mut new_biases = self.get_unscaled_accel_bias()?; // Do not touch the last bit @@ -1778,14 +1905,20 @@ impl Mpu9250 where DEV: Device new_biases[i] += biases[i] & !1; } - self.dev.write(Register::XA_OFFSET_H, - ((new_biases[0] >> 8) & 0xFF) as u8)?; + self.dev.write( + Register::XA_OFFSET_H, + ((new_biases[0] >> 8) & 0xFF) as u8, + )?; self.dev.write(Register::XA_OFFSET_L, (new_biases[0] & 0xFF) as u8)?; - self.dev.write(Register::YA_OFFSET_H, - ((new_biases[1] >> 8) & 0xFF) as u8)?; + self.dev.write( + Register::YA_OFFSET_H, + ((new_biases[1] >> 8) & 0xFF) as u8, + )?; self.dev.write(Register::YA_OFFSET_L, (new_biases[1] & 0xFF) as u8)?; - self.dev.write(Register::ZA_OFFSET_H, - ((new_biases[2] >> 8) & 0xFF) as u8)?; + self.dev.write( + Register::ZA_OFFSET_H, + ((new_biases[2] >> 8) & 0xFF) as u8, + )?; self.dev.write(Register::ZA_OFFSET_L, (new_biases[2] & 0xFF) as u8)?; Ok(()) @@ -1795,31 +1928,41 @@ impl Mpu9250 where DEV: Device /// Keep in mind that the registers contain factory-supplied values after /// reset. In relative mode it will add the new biases to the existing /// ones instead of replacing them. - pub fn set_accel_bias(&mut self, - relative: bool, - biases: T) - -> Result<(), Error> - where T: Into<[f32; 3]> + pub fn set_accel_bias( + &mut self, + relative: bool, + biases: T, + ) -> Result<(), Error> + where + T: Into<[f32; 3]>, { let biases = biases.into(); let scale = G * AccelScale::_16G.resolution(); - self.set_unscaled_accel_bias(relative, - [(biases[0] / scale) as i16, - (biases[1] / scale) as i16, - (biases[2] / scale) as i16]) + self.set_unscaled_accel_bias( + relative, + [ + (biases[0] / scale) as i16, + (biases[1] / scale) as i16, + (biases[2] / scale) as i16, + ], + ) } fn to_vector(&self, buffer: &[u8], offset: usize) -> [i16; 3] { - [((u16(buffer[offset + 1]) << 8) | u16(buffer[offset + 2])) as i16, - ((u16(buffer[offset + 3]) << 8) | u16(buffer[offset + 4])) as i16, - ((u16(buffer[offset + 5]) << 8) | u16(buffer[offset + 6])) as i16] + [ + ((u16(buffer[offset + 1]) << 8) | u16(buffer[offset + 2])) as i16, + ((u16(buffer[offset + 3]) << 8) | u16(buffer[offset + 4])) as i16, + ((u16(buffer[offset + 5]) << 8) | u16(buffer[offset + 6])) as i16, + ] } fn to_vector_inverted(&self, buffer: &[u8], offset: usize) -> [i16; 3] { - [((u16(buffer[offset + 2]) << 8) + u16(buffer[offset + 1])) as i16, - ((u16(buffer[offset + 4]) << 8) + u16(buffer[offset + 3])) as i16, - ((u16(buffer[offset + 6]) << 8) + u16(buffer[offset + 5])) as i16] + [ + ((u16(buffer[offset + 2]) << 8) + u16(buffer[offset + 1])) as i16, + ((u16(buffer[offset + 4]) << 8) + u16(buffer[offset + 3])) as i16, + ((u16(buffer[offset + 6]) << 8) + u16(buffer[offset + 5])) as i16, + ] } /// Reads the WHO_AM_I register; should return `0x71` @@ -1847,8 +1990,10 @@ impl Mpu9250 { } /// SPI mode -pub const MODE: Mode = Mode { polarity: Polarity::IdleHigh, - phase: Phase::CaptureOnSecondTransition }; +pub const MODE: Mode = Mode { + polarity: Polarity::IdleHigh, + phase: Phase::CaptureOnSecondTransition, +}; #[allow(dead_code)] #[allow(non_camel_case_types)] @@ -2193,5 +2338,5 @@ fn transpose(o: Option>) -> Result, E> { } fn u16(u: u8) -> u16 { - return u as u16; + u as u16 }