diff --git a/Cargo.lock b/Cargo.lock index 5aa22d9..ee4632d 100644 --- a/Cargo.lock +++ b/Cargo.lock @@ -74,6 +74,7 @@ dependencies = [ "embassy-time", "embassy-usb", "embedded-hal 1.0.0", + "libm", "panic-probe", "postcard-rpc", "static_cell", @@ -719,6 +720,12 @@ version = "1.0.1" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "b9e0384b61958566e926dc50660321d12159025e767c18e043daf26b70104c39" +[[package]] +name = "libm" +version = "0.2.15" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "f9fbbcab51052fe104eb5e5d351cf728d30a5be1fe14d9be8a3b097481fb97de" + [[package]] name = "litrs" version = "0.4.1" diff --git a/Cargo.toml b/Cargo.toml index c6ceab3..5b39d03 100644 --- a/Cargo.toml +++ b/Cargo.toml @@ -22,6 +22,8 @@ bitflags = "2.9.1" postcard-rpc = {version = "0.11.13", features = ["embassy-usb-0_4-server", "defmt"]} bioz-icd-rs = {path = "../bioz-icd-rs"} +libm = { version = "0.2.15" } + cortex-m = { version = "0.7.7", features = ["inline-asm", "critical-section-single-core"] } cortex-m-rt = "0.7.5" # embedded-hal = "0.2.6" diff --git a/src/communication.rs b/src/communication.rs index 129409a..2f3b88a 100644 --- a/src/communication.rs +++ b/src/communication.rs @@ -1,7 +1,10 @@ -use defmt::{info, error}; +use defmt::info; use embassy_executor::Spawner; -use embassy_sync::blocking_mutex::raw::ThreadModeRawMutex; +use embassy_time::{Ticker, Duration}; +use embassy_sync::mutex::Mutex; +use embassy_sync::blocking_mutex::raw::{ThreadModeRawMutex, CriticalSectionRawMutex}; +use embassy_sync::signal::Signal; use embassy_stm32::usb::Driver; use embassy_stm32::{peripherals, uid, usb}; @@ -10,19 +13,18 @@ use postcard_rpc::{ header::VarHeader, server::{ impls::embassy_usb_v0_4::{ - dispatch_impl::{WireRxBuf, WireRxImpl, WireSpawnImpl, WireStorage, WireTxImpl}, + dispatch_impl::{spawn_fn, WireRxBuf, WireRxImpl, WireSpawnImpl, WireStorage, WireTxImpl}, PacketBuffers, }, - Dispatch, Server, + Dispatch, Server, Sender, SpawnContext, }, }; -use bioz_icd_rs::{PingEndpoint, GetUniqueIdEndpoint, SetGreenLedEndpoint, ENDPOINT_LIST, TOPICS_IN_LIST, TOPICS_OUT_LIST}; +use bioz_icd_rs::{PingEndpoint, GetUniqueIdEndpoint, SetGreenLedEndpoint, StartImpedanceEndpoint, StopImpedanceEndpoint, StartImpedance, Impedance, ImpedanceTopic, ENDPOINT_LIST, TOPICS_IN_LIST, TOPICS_OUT_LIST}; -pub struct Context { - pub unique_id: [u8; 12], -} +use crate::impedance_test::{ImpedanceTest, IMPEDANCE_TEST}; +// Postcard RPC types type AppDriver = usb::Driver<'static, peripherals::USB>; type AppStorage = WireStorage; type BufStorage = PacketBuffers<1024, 1024>; @@ -34,6 +36,22 @@ use static_cell::ConstStaticCell; static PBUFS: ConstStaticCell = ConstStaticCell::new(BufStorage::new()); static STORAGE: AppStorage = AppStorage::new(); +pub struct Context { + pub unique_id: [u8; 12], + pub impedance: &'static Mutex, +} + +pub struct SpawnCtx { + pub impedance: &'static Mutex, +} + +impl SpawnContext for Context { + type SpawnCtxt = SpawnCtx; + fn spawn_ctxt(&mut self) -> Self::SpawnCtxt { + SpawnCtx { impedance: &self.impedance } + } +} + define_dispatch! { app: MyApp; spawn_fn: spawn_fn; @@ -49,6 +67,8 @@ define_dispatch! { | PingEndpoint | blocking | ping_handler | | GetUniqueIdEndpoint | blocking | get_unique_id_handler | | SetGreenLedEndpoint | async | set_green_led_handler | + | StartImpedanceEndpoint | spawn | start_impedance_handler | + | StopImpedanceEndpoint | async | stop_impedance_handler | }; topics_in: { list: TOPICS_IN_LIST; @@ -99,8 +119,10 @@ pub fn init_communication(usb_driver: Driver<'static, peripherals::USB>, spawner let pbufs = PBUFS.take(); let config = usb_config(); + let impedance_ref = IMPEDANCE_TEST.init(embassy_sync::mutex::Mutex::new(ImpedanceTest::new())); let context = Context { unique_id: *uid::uid(), + impedance: impedance_ref, }; let (device, tx_impl, rx_impl) = STORAGE.init(usb_driver, config, pbufs.tx_buf.as_mut_slice()); @@ -117,23 +139,75 @@ pub fn init_communication(usb_driver: Driver<'static, peripherals::USB>, spawner spawner.must_spawn(server_run(server)); } -fn ping_handler(_context: &mut Context, _header: VarHeader, rqst: u32) -> u32 { +// Functions + +pub fn ping_handler(_context: &mut Context, _header: VarHeader, rqst: u32) -> u32 { info!("ping"); rqst } -fn get_unique_id_handler(context: &mut Context, _header: VarHeader, _rqst: ()) -> [u8; 12] { +pub fn get_unique_id_handler(context: &mut Context, _header: VarHeader, _rqst: ()) -> [u8; 12] { info!("get_unique_id"); context.unique_id } -use embassy_sync::blocking_mutex::raw::CriticalSectionRawMutex; -use embassy_sync::signal::Signal; - pub static LED_FREQUENCY_SIGNAL: Signal = Signal::new(); -async fn set_green_led_handler(_context: &mut Context, _header: VarHeader, rqst: f32) { +pub async fn set_green_led_handler(_context: &mut Context, _header: VarHeader, rqst: f32) { info!("Set green led frequency to {:?} Hz", rqst); LED_FREQUENCY_SIGNAL.signal(rqst); +} + +static STOP: Mutex = Mutex::new(false); + +#[embassy_executor::task] +pub async fn start_impedance_handler(context: SpawnCtx, header: VarHeader, rqst: StartImpedance, sender: Sender) { + info!("Start impedance measurement with interval {:?} ms.", rqst.interval_ms); + + let mut impedance = context.impedance.lock().await; + + if sender + .reply::(header.seq_no, &()) + .await + .is_err() + { + defmt::error!("Failed to reply, stopping accel"); + return; + } + + let mut ticker = Ticker::every(Duration::from_millis(rqst.interval_ms.into())); + let mut seq: u8 = 0; + while !*STOP.lock().await { + ticker.next().await; + + impedance.update(); + + let msg = Impedance { + magnitude: impedance.magnitude, + phase: impedance.phase, + }; + + if sender + .publish::(seq.into(), &msg) + .await + .is_err() + { + defmt::error!("Topic send error!"); + break; + } + seq = seq.wrapping_add(1); + } + + info!("Impedance measurement stopped."); + *STOP.lock().await = false; +} + +pub async fn stop_impedance_handler(context: &mut Context, _header: VarHeader, _rqst: ()) -> bool { + info!("Stop impedance measurement"); + let was_busy = context.impedance.try_lock().is_err(); + if was_busy { + *STOP.lock().await = true; + } + was_busy } \ No newline at end of file diff --git a/src/impedance_test.rs b/src/impedance_test.rs new file mode 100644 index 0000000..3dad3da --- /dev/null +++ b/src/impedance_test.rs @@ -0,0 +1,34 @@ +use embassy_sync::mutex::Mutex; +use embassy_sync::blocking_mutex::raw::ThreadModeRawMutex; + +use static_cell::StaticCell; + +use libm::{sinf, cosf}; +use core::f32::consts::PI; + +pub static IMPEDANCE_TEST: StaticCell> = StaticCell::new(); + +pub struct ImpedanceTest { + time: embassy_time::Instant, + pub magnitude: f32, + pub phase: f32, +} + +impl ImpedanceTest { + pub fn new() -> Self { + ImpedanceTest { + time: embassy_time::Instant::now(), + magnitude: 1.0, + phase: 0.0, + } + } + + pub fn update(&mut self) { + let frequency = 1.0; // Hz, adjust as needed + + let current_time = self.time.elapsed().as_millis() as f32 / 1000.0; // Convert to seconds + self.magnitude = cosf(2.0 * PI * frequency * current_time); + self.phase = sinf(2.0 * PI * frequency * current_time); + } +} + diff --git a/src/main.rs b/src/main.rs index 9f4dc74..12307f0 100644 --- a/src/main.rs +++ b/src/main.rs @@ -30,6 +30,8 @@ use embassy_stm32::{bind_interrupts, peripherals, usb}; mod communication; use communication::{init_communication, LED_FREQUENCY_SIGNAL}; +mod impedance_test; + bind_interrupts!(struct Irqs { USB_DRD_FS => usb::InterruptHandler; });