commit d7e69aa47470080c4f28d57318af70ab10c75ba1
Author: Daniel Pérez <steew@psi.my.domain>
Date: Sun Jan 11 23:35:31 2026 +0000
Split the MPU data UART tx into another thread
diff --git a/fw/RustFM/src/main.rs b/fw/RustFM/src/main.rs
index 1499c5a..91aaeec 100644
--- a/fw/RustFM/src/main.rs
+++ b/fw/RustFM/src/main.rs
@@ -47 +411 @@
mod mpu;
mod rf;
+use embassy_sync::{blocking_mutex::raw::{CriticalSectionRawMutex, ThreadModeRawMutex}, rwlock::RwLock};
+use heapless::pool::arc::Arc;
use mpu::read_mpu;
+use mpu::BUFFERED_QUATERNIONS;
+use mpu6050_dmp::quaternion::Quaternion;
use rf::transmit;
use embassy_executor::Spawner;
@@ -177 +2112 @@ use embassy_stm32::{
usart::{self, Uart},
Config, Peri,
};
+use embassy_sync::channel::Channel;
+use embassy_sync::blocking_mutex::raw::NoopRawMutex;
+
use embassy_time::Timer;
+use crate::mpu::{QuaternionDataBuffer, telemetry_sender};
+
use {defmt_rtt as _, panic_probe as _};
bind_interrupts!(struct Irqs {
@@ -616 +709 @@ async fn main(spawner: Spawner) {
let imu_int = ExtiInput::new(p.PA12, p.EXTI12, embassy_stm32::gpio::Pull::Down);
+ // create shared channel between the MPU polling thread and the UART sender for the quaternion data buffer.
+ static QUATERNION_CHANNEL: Channel::<ThreadModeRawMutex, Quaternion, BUFFERED_QUATERNIONS> = Channel::<ThreadModeRawMutex, Quaternion, BUFFERED_QUATERNIONS>::new();
+
let tmtry_uart = Uart::new(
p.USART3,
p.PC5,
@@ -7311 +8516 @@ async fn main(spawner: Spawner) {
.unwrap();
tmtry_uart.set_baudrate(9600).unwrap();
+ // dedicated task for the RF transmitter
spawner.spawn(transmit(txpin.into())).unwrap();
+ // dedicated task for MPU data readings
spawner
- .spawn(read_mpu(iic, imu_int.into(), tmtry_uart.into()))
+ .spawn(read_mpu(iic, imu_int.into(), QUATERNION_CHANNEL.sender()))
.unwrap();
-
+ // dedicated task for UART telemetry
+ spawner.spawn(telemetry_sender(tmtry_uart, QUATERNION_CHANNEL.receiver()));
+ // dedicated task for physical status leds
+ // TODO: replace with timer interrupt
spawner.spawn(status_leds(ok_pin.into())).unwrap();
Timer::after_millis(500).await;
spawner.spawn(status_leds(fail_pin.into())).unwrap();
diff --git a/fw/RustFM/src/mpu.rs b/fw/RustFM/src/mpu.rs
index dd6536a..05416f8 100644
--- a/fw/RustFM/src/mpu.rs
+++ b/fw/RustFM/src/mpu.rs
@@ -110 +113 @@
+use cortex_m::prelude::_embedded_hal_blocking_serial_Write;
use embassy_stm32::{
exti::ExtiInput,
i2c::{I2c, Master},
mode::Async,
usart::Uart,
};
-use embassy_time::Delay;
+use embassy_sync::{blocking_mutex::raw::{CriticalSectionRawMutex, NoopRawMutex, ThreadModeRawMutex}, channel::{Channel, Receiver, Sender}, rwlock::RwLock};
+use embassy_time::{Delay, WithTimeout};
+use heapless::pool::arc::Arc;
use mpu6050_dmp::{
calibration::CalibrationParameters, quaternion::Quaternion, sensor_async::Mpu6050,
yaw_pitch_roll::YawPitchRoll,
@@ -2631 +2951 @@ struct MotionState {
}
+pub const BUFFERED_QUATERNIONS: usize = 5;
+
+#[embassy_executor::task]
+pub async fn telemetry_sender(
+ mut telemetry_port: Uart<'static, Async>,
+ channel: Receiver<'static, ThreadModeRawMutex, Quaternion, BUFFERED_QUATERNIONS>,
+) {
+ loop {
+ // await until we receive a new quaternion packet from the sync channel
+ let next_quaternion_value = channel.receive().await;
+ // write the received values in the telemetry UART
+ // TODO: data synchronization for the receiver
+ telemetry_port.write(&next_quaternion_value.w.to_le_bytes()).await;
+ telemetry_port.write(&next_quaternion_value.x.to_le_bytes()).await;
+ telemetry_port.write(&next_quaternion_value.y.to_le_bytes()).await;
+ telemetry_port.write(&next_quaternion_value.z.to_le_bytes()).await;
+ }
+}
+
#[embassy_executor::task]
pub async fn read_mpu(
iic: I2c<'static, Async, Master>,
mut ext: ExtiInput<'static>,
- tmtry: Uart<'static, Async>,
+ channel: Sender<'static, ThreadModeRawMutex, Quaternion, BUFFERED_QUATERNIONS>,
) {
let mut mpu = Mpu6050::new(iic, mpu6050_dmp::address::Address::default())
.await
.unwrap();
+ // initialize the DMP processor for the MPU
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,
- );
-
+ // 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();
@@ -6148 +8425 @@ pub async fn read_mpu(
// mpu.set_clock_source(mpu6050_dmp::clock_source::ClockSource::Xgyro).unwrap();
let mut fifo: [u8; 28] = [0; 28];
- let mut databuf_yaw: [f32; 10] = [0.0; 10];
- let mut databuf_pitch: [f32; 10] = [0.0; 10];
- let mut databuf_roll: [f32; 10] = [0.0; 10];
- let mut i = 0;
// set up the interrupt so we receive data from the internal mpu6050 dmp,
// combining gyro and accel
- mpu.interrupt_fifo_oflow_en().await.unwrap();
mpu.enable_fifo().await.unwrap();
+ mpu.interrupt_data_ready_en().await.unwrap();
+
loop {
+ // block until we get an interrupt from the MPU line
ext.wait_for_rising_edge().await;
- // info!("Received new data");
+ // read the combined data from the MPU fifo. It sends 28 byte packets, of which the first
+ // 16 are the quaternion 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);
- 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;
- // write the big endian repr of the sensor angles to UART for
- // plot representation in the telemetry receiving device.
- // let yaw_bytes = yaw_deg.to_ne_bytes();
- // let pitch_bytes = pitch_deg.to_ne_bytes();
- // let roll_bytes = roll_deg.to_ne_bytes();
-
- // tmtry.blocking_write(&yaw_bytes).unwrap();
- // tmtry.blocking_write(&pitch_bytes).unwrap();
- // tmtry.blocking_write(&roll_bytes).unwrap();
- // tmtry.write(&[255]).await.unwrap();
+ // obtain the first 16 quaternion packets
+ let quaternion_packet = Quaternion::from_bytes(&fifo[..16]).unwrap().normalize();
+ // send quaternion value to sync channel
+ channel.send(quaternion_packet);
- // info!(
- // " Angles [deg]: yaw={}, pitch={}, roll={}",
- // yaw_deg, pitch_deg, roll_deg
- // );
- info!(
- " Angles [quat]: w={}, x={}, y={}, z={}",
- quat.w, quat.x, quat.y, quat.z
- );
-
- mpu.interrupt_read_clear().await.unwrap();
+ // clear the pending interrupt and wait for the next
mpu.reset_fifo().await.unwrap();
+ mpu.interrupt_read_clear().await.unwrap();
}
}
