Thumbnail

steew/gilgamesh.git

Clone URL: https://git.buni.party/steew/gilgamesh.git

Info Tree Commits Branches Tags Show Grep
commit 91f69a34d9664436cc66a1ad2be4cad19651f0b3 Author: Daniel Pérez <steew@psi.my.domain> Date: Sat Jan 17 20:09:23 2026 +0000 Send quaternions instead of ypr diff --git a/fw/RustFM/src/main.rs b/fw/RustFM/src/main.rs index 4f488b1..072f861 100644 --- a/fw/RustFM/src/main.rs +++ b/fw/RustFM/src/main.rs @@ -317 +318 @@    mod mpu;  mod rf; +mod status;   -use embassy_sync::{ - blocking_mutex::raw::{CriticalSectionRawMutex, ThreadModeRawMutex}, - rwlock::RwLock, -}; +use defmt::trace; +use embassy_stm32::time::Hertz; +use embassy_sync::blocking_mutex::raw::ThreadModeRawMutex;   -use heapless::pool::arc::Arc; +use embassy_sync::mutex::Mutex;  use mpu::read_mpu;  use mpu::BUFFERED_QUATERNIONS;  use mpu6050_dmp::quaternion::Quaternion;  use rf::transmit; +use status::SystemStatus;    use embassy_executor::Spawner;  use embassy_stm32::{ @@ -286 +298 @@ use embassy_stm32::{  use embassy_sync::channel::Channel;    use crate::mpu::telemetry_sender; +use crate::status::status_leds; +use crate::status::SharedStatus;  use embassy_time::Timer;    use {defmt_rtt as _, panic_probe as _}; @@ -4023 +4331 @@ bind_interrupts!(struct Irqs {    #[embassy_executor::main]  async fn main(spawner: Spawner) { + trace! {"Starting system up..."}; let mut config = Config::default();   config.rcc.hse = Some(rcc::Hse { freq: embassy_stm32::time::Hertz(12_000_000), mode: rcc::HseMode::Oscillator, }); + + config.rcc.sys = rcc::Sysclk::PLL1_R; + config.rcc.pll = Some(rcc::Pll { + source: rcc::PllSource::HSE, + prediv: rcc::PllPreDiv::DIV2, + mul: rcc::PllMul::MUL8, + divp: None, + divq: Some(rcc::PllQDiv::DIV2), // PLL1_Q clock (32 / 2 * 6 / 2), used for RNG + divr: Some(rcc::PllRDiv::DIV2), // sysclk 48Mhz clock (32 / 2 * 6 / 2) + }); + let p = embassy_stm32::init(config); + let mut i2c_config = i2c::Config::default(); + i2c_config.frequency = Hertz::khz(400); + i2c_config.gpio_speed = Speed::VeryHigh;   - // 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(), + p.I2C1, p.PA9, p.PA10, Irqs, p.DMA1_CH6, p.DMA1_CH7, i2c_config, ); let _lsb_pin = Output::new(p.PA11, Level::Low, Speed::Low);   @@ -766 +877 @@ async fn main(spawner: Spawner) { // 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(); + static SHARED_STATUS: SharedStatus = Mutex::new(SystemStatus::FAIL);   let tmtry_uart = Uart::new( p.USART3, @@ -8731 +9926 @@ async fn main(spawner: Spawner) { usart::Config::default(), ) .unwrap(); - tmtry_uart.set_baudrate(9600).unwrap(); + tmtry_uart.set_baudrate(115200).unwrap(); + + trace! {"Initializing tasks..."};   // 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(), QUATERNION_CHANNEL.sender())) + .spawn(read_mpu(iic, imu_int, QUATERNION_CHANNEL.sender())) .unwrap(); // dedicated task for UART telemetry - spawner.spawn(telemetry_sender(tmtry_uart, QUATERNION_CHANNEL.receiver())); + spawner + .spawn(telemetry_sender(tmtry_uart, QUATERNION_CHANNEL.receiver())) + .unwrap(); // 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(); -} - -#[embassy_executor::task(pool_size = 2)] -async fn status_leds(p: Peri<'static, AnyPin>) { - let mut led = Output::new(p, Level::Low, Speed::Low); - - loop { - led.set_high(); - Timer::after_millis(500).await; - led.set_low(); - Timer::after_millis(500).await; - } + // spawner.spawn(status_leds(ok_pin.into())).unwrap(); + // Timer::after_millis(500).await; + // spawner.spawn(status_leds(fail_pin.into())).unwrap(); + spawner + .spawn(status_leds(ok_pin.into(), fail_pin.into(), &SHARED_STATUS)) + .unwrap();  } diff --git a/fw/RustFM/src/mpu.rs b/fw/RustFM/src/mpu.rs index 124359c..3e8163d 100644 --- a/fw/RustFM/src/mpu.rs +++ b/fw/RustFM/src/mpu.rs @@ -14 +14 @@ -use cortex_m::prelude::_embedded_hal_blocking_serial_Write; +use defmt::trace;  use embassy_stm32::{ exti::ExtiInput, i2c::{I2c, Master}, @@ -632 +615 @@ use embassy_stm32::{ usart::Uart,  };  use embassy_sync::{ - blocking_mutex::raw::{CriticalSectionRawMutex, NoopRawMutex, ThreadModeRawMutex}, - channel::{Channel, Receiver, Sender}, - rwlock::RwLock, + blocking_mutex::raw::ThreadModeRawMutex, + channel::{Receiver, Sender},  }; -use embassy_time::{Delay, WithTimeout}; -use heapless::pool::arc::Arc; +use embassy_time::{Delay, Instant};  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 -} -  pub const BUFFERED_QUATERNIONS: usize = 5;    #[embassy_executor::task] @@ -3923 +2231 @@ pub async fn telemetry_sender( mut telemetry_port: Uart<'static, Async>, channel: Receiver<'static, ThreadModeRawMutex, Quaternion, BUFFERED_QUATERNIONS>,  ) { + let mut previous: Instant = Instant::now(); + let mut now: Instant; + 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; + let next_quaternion_value = channel.receive().await.normalize(); + // let ypr_format = YawPitchRoll::from(next_quaternion_value); + + now = Instant::now(); + let elapsed = now - previous; + defmt::info!("Time elapsed since last data: {} ms", elapsed.as_millis()); + previous = now; + + let w = &next_quaternion_value.w; + let x = &next_quaternion_value.x; + let y = &next_quaternion_value.y; + let z = &next_quaternion_value.z; + + let _send_result_w = telemetry_port.write(&w.to_le_bytes()).await; + let _send_result_x = telemetry_port.write(&x.to_le_bytes()).await; + let _send_result_y = telemetry_port.write(&y.to_le_bytes()).await; + let _send_result_z = telemetry_port.write(&z.to_le_bytes()).await; + + telemetry_port.flush().await.unwrap(); + defmt::info! {"Data: {} {} {} {}", w, x, y, z}; }  }   @@ -656 +567 @@ pub async fn read_mpu( mut ext: ExtiInput<'static>, channel: Sender<'static, ThreadModeRawMutex, Quaternion, BUFFERED_QUATERNIONS>,  ) { + trace! {"Entering MPU thread"}; let mut mpu = Mpu6050::new(iic, mpu6050_dmp::address::Address::default()) .await .unwrap(); @@ -7233 +6433 @@ pub async fn read_mpu( mpu.initialize_dmp(&mut Delay).await.unwrap();   // Configure calibration parameters - // let _calibration_params = CalibrationParameters::new( + // let calibration_params = CalibrationParameters::new( // mpu6050_dmp::accel::AccelFullScale::G2, // mpu6050_dmp::gyro::GyroFullScale::Deg2000, // mpu6050_dmp::calibration::ReferenceGravity::ZN, // ); - // info!("Calibrating Sensor"); + // trace!("Calibrating Sensor"); // mpu // .calibrate(&mut Delay, &calibration_params) // .await // .unwrap(); - // info!("Sensor Calibrated"); + // trace!("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) + mpu.set_sample_rate_divider(0).await.unwrap(); + mpu.set_digital_lowpass_filter(mpu6050_dmp::config::DigitalLowPassFilter::Filter0) .await .unwrap();   - // mpu.set_clock_source(mpu6050_dmp::clock_source::ClockSource::Xgyro).unwrap(); let mut fifo: [u8; 28] = [0; 28];   // set up the interrupt so we receive data from the internal mpu6050 dmp, // combining gyro and accel mpu.enable_fifo().await.unwrap(); - mpu.interrupt_data_ready_en().await.unwrap(); + trace! {"Enabling FIFO interrupt"}; + mpu.interrupt_fifo_oflow_en().await.unwrap();   loop { // block until we get an interrupt from the MPU line @@ -1097 +1017 @@ pub async fn read_mpu( // 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); + channel.send(quaternion_packet).await;   // clear the pending interrupt and wait for the next mpu.reset_fifo().await.unwrap(); diff --git a/fw/RustFM/src/rf.rs b/fw/RustFM/src/rf.rs index 2da8b02..5bcbea9 100644 --- a/fw/RustFM/src/rf.rs +++ b/fw/RustFM/src/rf.rs @@ -711 +711 @@ 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(); + // txpin.set_low(); + // Timer::after_millis(1).await; + // txpin.set_high(); + // Timer::after_millis(1).await; + // txpin.set_low();   loop { txpin.toggle(); diff --git a/fw/RustFM/src/status.rs b/fw/RustFM/src/status.rs new file mode 100644 index 0000000..aba5c0c --- /dev/null +++ b/fw/RustFM/src/status.rs @@ -00 +132 @@ +use embassy_stm32::{ + gpio::{AnyPin, Level, Output, Speed}, + Peri, +}; +use embassy_sync::{blocking_mutex::raw::ThreadModeRawMutex, mutex::Mutex}; +use embassy_time::Timer; + +pub enum SystemStatus { + OK, + FAIL, +} + +pub type SharedStatus = Mutex<ThreadModeRawMutex, SystemStatus>; + +#[embassy_executor::task] +pub async fn status_leds( + led_ok_pin: Peri<'static, AnyPin>, + led_fail_pin: Peri<'static, AnyPin>, + status: &'static SharedStatus, +) { + let mut led_ok = Output::new(led_ok_pin, Level::Low, Speed::Low); + let mut led_fail = Output::new(led_fail_pin, Level::Low, Speed::Low); + + loop { + led_ok.set_high(); + led_fail.set_low(); + Timer::after_millis(500).await; + led_ok.set_low(); + led_fail.set_high(); + Timer::after_millis(500).await; + } +} diff --git a/fw/RustFM/src/status.rs~ b/fw/RustFM/src/status.rs~ new file mode 100644 index 0000000..08ef457 --- /dev/null +++ b/fw/RustFM/src/status.rs~ @@ -00 +140 @@ +use embassy_stm32::{ + gpio::{AnyPin, Level, Output, Speed}, + Peri, +}; +use embassy_sync::{blocking_mutex::raw::ThreadModeRawMutex, mutex::Mutex}; +use embassy_time::Timer; + + +pub enum SystemStatus { + OK, + FAIL +} + +pub type SharedStatus = Mutex<ThreadModeRawMutex, SystemStatus>; + +#[embassy_executor::task] +pub async fn status_leds(led_ok_pin: Peri<'static, AnyPin>, led_fail_pin: Peri<'static, AnyPin>, status: &'static SharedStatus) { + let mut led_ok = Output::new(led_ok_pin, Level::Low, Speed::Low); + let led_fail = Output::new(led_fail_pin, Level::Low, Speed::Low); + + loop { + { + let mutex_lock = status.lock().await; + let status_lock = mutex_lock; + match mutex_lock { + OK => { + + }, + FAIL => { + + } + } + + } + led_ok.set_high(); + Timer::after_millis(500).await; + led_ok.set_low(); + Timer::after_millis(500).await; + } +}