commit 991383bdce3d499f8ec13a078b54e1f0b6c20b64
Author: Daniel P. <daniel@steew.eu>
Date: Sun Nov 02 22:03:42 2025 +0000
use mpu6050 async
diff --git a/fw/RustFM/Cargo.toml b/fw/RustFM/Cargo.toml
index 1930c59..31f9549 100644
--- a/fw/RustFM/Cargo.toml
+++ b/fw/RustFM/Cargo.toml
@@ -57 +57 @@ edition = "2021"
[dependencies]
# mpu6050 = "0.1.6"
-mpu6050-dmp = "0.6.1"
+mpu6050-dmp = {version = "0.6.1", features = ["async"]}
embassy-executor = { version = "0.9.0", features = ["defmt", "arch-cortex-m", "executor-thread"] }
embassy-time = { version = "0.5.0", features = ["defmt"] }
embassy-sync = { version = "0.7.2", features = [ "defmt" ] }
diff --git a/fw/RustFM/src/main.rs b/fw/RustFM/src/main.rs
index 954f462..8b03cad 100644
--- a/fw/RustFM/src/main.rs
+++ b/fw/RustFM/src/main.rs
@@ -115 +129 @@
#![no_std]
#![no_main]
-use defmt::{trace};
+use defmt::info;
use embassy_executor::Spawner;
use embassy_stm32::{
- exti::ExtiInput, gpio::{AnyPin, Level, Output, Speed}, i2c::{self, I2c, Master}, mode::Blocking, rcc::{self}, Config, Peri
+ bind_interrupts,
+ exti::ExtiInput,
+ gpio::{AnyPin, Level, Output, Speed},
+ i2c::{self, I2c, Master},
+ mode::Async,
+ rcc::{self},
+ Config, Peri,
};
-use embassy_time::{Timer, Delay};
+use embassy_time::{Delay, Timer};
use {defmt_rtt as _, panic_probe as _};
-use mpu6050_dmp::{sensor::Mpu6050};
+use mpu6050_dmp::{
+ calibration::CalibrationParameters, quaternion::Quaternion, sensor_async::Mpu6050,
+ yaw_pitch_roll::YawPitchRoll,
+};
+
+bind_interrupts!(struct Irqs {
+ I2C1_EV => i2c::EventInterruptHandler<embassy_stm32::peripherals::I2C1>;
+ I2C1_ER => i2c::ErrorInterruptHandler<embassy_stm32::peripherals::I2C1>;
+});
#[embassy_executor::main]
async fn main(spawner: Spawner) {
@@ -2014 +3423 @@ async fn main(spawner: Spawner) {
mode: rcc::HseMode::Oscillator,
});
let p = embassy_stm32::init(config);
-
- let iic = I2c::new_blocking(p.I2C1, p.PA9, p.PA10, i2c::Config::default());
+
+ // let iic = I2c::new_blocking(p.I2C1, p.PA9, p.PA10, i2c::Config::default());
+ let iic = I2c::new(
+ p.I2C1,
+ p.PA9,
+ p.PA10,
+ Irqs,
+ p.DMA1_CH6,
+ p.DMA1_CH7,
+ i2c::Config::default(),
+ );
let _lsb_pin = Output::new(p.PA11, Level::Low, Speed::Low);
let ok_pin = p.PC14;
let fail_pin = p.PC15;
let txpin = p.PA6;
-
+
let _rxen = Output::new(p.PB0, Level::High, Speed::Low);
let _txen = Output::new(p.PB1, Level::Low, Speed::Low);
@@ -3614 +5913 @@ async fn main(spawner: Spawner) {
core::mem::forget(_lsb_pin);
let imu_int = ExtiInput::new(p.PA12, p.EXTI12, embassy_stm32::gpio::Pull::Down);
-
+
spawner.spawn(transmit(txpin.into())).unwrap();
spawner.spawn(read_mpu(iic, imu_int.into())).unwrap();
spawner.spawn(status_leds(ok_pin.into())).unwrap();
Timer::after_millis(500).await;
spawner.spawn(status_leds(fail_pin.into())).unwrap();
-
}
#[embassy_executor::task(pool_size = 2)]
@@ -6931 +9155 @@ async fn transmit(p: Peri<'static, AnyPin>) {
loop {
txpin.toggle();
- Timer::after_millis(4).await;
+ Timer::after_millis(1).await;
}
}
-
#[embassy_executor::task]
-async fn read_mpu(iic: I2c<'static, Blocking, Master>, mut ext: ExtiInput<'static>) {
- let mpu = Mpu6050::new(iic, mpu6050_dmp::address::Address::default());
- let mut mpu = mpu.unwrap();
- mpu.initialize_dmp(&mut Delay).unwrap();
- mpu.enable_dmp().unwrap();
- mpu.set_clock_source(mpu6050_dmp::clock_source::ClockSource::Xgyro).unwrap();
- let mut fifo : [u8; 6] = [0; 6];
- mpu.interrupt_fifo_oflow_en().unwrap();
- mpu.enable_fifo().unwrap();
- mpu.interrupt_read_clear().unwrap();
+async fn read_mpu(iic: I2c<'static, Async, Master>, mut ext: ExtiInput<'static>) {
+ let mut mpu = Mpu6050::new(iic, mpu6050_dmp::address::Address::default())
+ .await
+ .unwrap();
+ mpu.initialize_dmp(&mut Delay).await.unwrap();
+ // Configure calibration parameters
+ let calibration_params = CalibrationParameters::new(
+ mpu6050_dmp::accel::AccelFullScale::G2,
+ mpu6050_dmp::gyro::GyroFullScale::Deg2000,
+ mpu6050_dmp::calibration::ReferenceGravity::ZN,
+ );
+
+ // info!("Calibrating Sensor");
+ // mpu
+ // .calibrate(&mut Delay, &calibration_params)
+ // .await
+ // .unwrap();
+ // info!("Sensor Calibrated");
+ mpu.enable_dmp().await.unwrap();
+ mpu.load_firmware().await.unwrap();
+ mpu.boot_firmware().await.unwrap();
+ // mpu.set_clock_source(mpu6050_dmp::clock_source::ClockSource::Xgyro).unwrap();
+ let mut fifo: [u8; 28] = [0; 28];
+ mpu.interrupt_fifo_oflow_en().await.unwrap();
+ mpu.enable_fifo().await.unwrap();
loop {
ext.wait_for_rising_edge().await;
- trace!("Received new data");
- mpu.read_fifo(&mut fifo).unwrap();
+ // info!("Received new data");
+ mpu.read_fifo(&mut fifo).await.unwrap();
// let accel = mpu.gyro().unwrap();
- // trace!("X: {}, Y: {}, Z: {}" , accel.x(), accel.y(), accel.z());
- trace!("{}", fifo);
- mpu.interrupt_read_clear().unwrap();
+ // trace!("X: {}, Y: {}, Z: {}" , accel.x(), accel.y(),accel.z());
+ // trace!("{}", fifo);
+ let quat = Quaternion::from_bytes(&fifo[..16]).unwrap().normalize();
+ let ypr = YawPitchRoll::from(quat);
+ let yaw_deg = ypr.yaw * 180.0 / core::f32::consts::PI;
+ let pitch_deg = ypr.pitch * 180.0 / core::f32::consts::PI;
+ let roll_deg = ypr.roll * 180.0 / core::f32::consts::PI;
+
+ info!(
+ " Angles [deg]: yaw={}, pitch={}, roll={}",
+ yaw_deg, pitch_deg, roll_deg
+ );
+ mpu.interrupt_read_clear().await.unwrap();
+ mpu.reset_fifo().await.unwrap();
}
}
-
