commit b8fbd277449803d9f1531bc1eb6285e73b4c5057
Author: Daniel P. <daniel@steew.eu>
Date: Sat Nov 22 19:03:16 2025 +0000
segregate fmu code by behavior
diff --git a/fw/RustFM/src/main.rs b/fw/RustFM/src/main.rs
index 8b03cad..1499c5a 100644
--- a/fw/RustFM/src/main.rs
+++ b/fw/RustFM/src/main.rs
@@ -128 +129 @@
#![no_std]
#![no_main]
-use defmt::info;
+mod mpu;
+mod rf;
+
+use mpu::read_mpu;
+use rf::transmit;
+
use embassy_executor::Spawner;
use embassy_stm32::{
bind_interrupts,
exti::ExtiInput,
gpio::{AnyPin, Level, Output, Speed},
- i2c::{self, I2c, Master},
- mode::Async,
+ i2c::{self, I2c},
rcc::{self},
+ usart::{self, Uart},
Config, Peri,
};
-use embassy_time::{Delay, Timer};
+use embassy_time::Timer;
use {defmt_rtt as _, panic_probe as _};
-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>;
+ USART3 => usart::InterruptHandler<embassy_stm32::peripherals::USART3>;
});
#[embassy_executor::main]
@@ -608 +6122 @@ async fn main(spawner: Spawner) {
let imu_int = ExtiInput::new(p.PA12, p.EXTI12, embassy_stm32::gpio::Pull::Down);
+ let tmtry_uart = Uart::new(
+ p.USART3,
+ p.PC5,
+ p.PC4,
+ Irqs,
+ p.DMA1_CH2,
+ p.DMA1_CH3,
+ usart::Config::default(),
+ )
+ .unwrap();
+ tmtry_uart.set_baudrate(9600).unwrap();
+
spawner.spawn(transmit(txpin.into())).unwrap();
- spawner.spawn(read_mpu(iic, imu_int.into())).unwrap();
+ spawner
+ .spawn(read_mpu(iic, imu_int.into(), tmtry_uart.into()))
+ .unwrap();
spawner.spawn(status_leds(ok_pin.into())).unwrap();
Timer::after_millis(500).await;
@@ -7967 +943 @@ async fn status_leds(p: Peri<'static, AnyPin>) {
Timer::after_millis(500).await;
}
}
-
-#[embassy_executor::task]
-async fn transmit(p: Peri<'static, AnyPin>) {
- let mut txpin = Output::new(p, Level::Low, Speed::VeryHigh);
- txpin.set_low();
- Timer::after_millis(1).await;
- txpin.set_high();
- Timer::after_millis(1).await;
- txpin.set_low();
-
- loop {
- txpin.toggle();
- Timer::after_millis(1).await;
- }
-}
-
-#[embassy_executor::task]
-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;
- // 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);
- 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();
- }
-}
diff --git a/fw/RustFM/src/mpu.rs b/fw/RustFM/src/mpu.rs
new file mode 100644
index 0000000..dd6536a
--- /dev/null
+++ b/fw/RustFM/src/mpu.rs
@@ -00 +1108 @@
+use embassy_stm32::{
+ exti::ExtiInput,
+ i2c::{I2c, Master},
+ mode::Async,
+ usart::Uart,
+};
+use embassy_time::Delay;
+use mpu6050_dmp::{
+ calibration::CalibrationParameters, quaternion::Quaternion, sensor_async::Mpu6050,
+ yaw_pitch_roll::YawPitchRoll,
+};
+
+use defmt::info;
+
+// Holds the current body state
+struct MotionState {
+ // gyroscope readings
+ yaw: f32,
+ pitch: f32,
+ roll: f32,
+ // accelerometer readings
+ accel_x: f32,
+ accel_y: f32,
+ accel_z: f32,
+ // magnetometer readings
+
+}
+
+
+#[embassy_executor::task]
+pub async fn read_mpu(
+ iic: I2c<'static, Async, Master>,
+ mut ext: ExtiInput<'static>,
+ tmtry: Uart<'static, Async>,
+) {
+ 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_sample_rate_divider(9).await.unwrap();
+ mpu.set_digital_lowpass_filter(mpu6050_dmp::config::DigitalLowPassFilter::Filter1)
+ .await
+ .unwrap();
+
+ // 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();
+ loop {
+ ext.wait_for_rising_edge().await;
+ // 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);
+ 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();
+
+ // 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();
+ mpu.reset_fifo().await.unwrap();
+ }
+}
diff --git a/fw/RustFM/src/rf.rs b/fw/RustFM/src/rf.rs
new file mode 100644
index 0000000..2da8b02
--- /dev/null
+++ b/fw/RustFM/src/rf.rs
@@ -00 +120 @@
+use embassy_stm32::{
+ gpio::{AnyPin, Level, Output, Speed},
+ Peri,
+};
+use embassy_time::Timer;
+
+#[embassy_executor::task]
+pub async fn transmit(p: Peri<'static, AnyPin>) {
+ let mut txpin = Output::new(p, Level::Low, Speed::VeryHigh);
+ txpin.set_low();
+ Timer::after_millis(1).await;
+ txpin.set_high();
+ Timer::after_millis(1).await;
+ txpin.set_low();
+
+ loop {
+ txpin.toggle();
+ Timer::after_millis(1).await;
+ }
+}
