Included impedance topic.

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"

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"

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<ThreadModeRawMutex, AppDriver, 256, 256, 64, 256>;
 type BufStorage = PacketBuffers<1024, 1024>;
@@ -34,6 +36,22 @@ use static_cell::ConstStaticCell;
 static PBUFS: ConstStaticCell<BufStorage> = ConstStaticCell::new(BufStorage::new());
 static STORAGE: AppStorage = AppStorage::new();
 
+pub struct Context {
+    pub unique_id: [u8; 12],
+    pub impedance: &'static Mutex<ThreadModeRawMutex, ImpedanceTest>,
+}
+
+pub struct SpawnCtx {
+    pub impedance: &'static Mutex<ThreadModeRawMutex, ImpedanceTest>,
+}
+
+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<CriticalSectionRawMutex, f32> = 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<ThreadModeRawMutex, bool> = Mutex::new(false);
+
+#[embassy_executor::task]
+pub async fn start_impedance_handler(context: SpawnCtx, header: VarHeader, rqst: StartImpedance, sender: Sender<AppTx>) {
+    info!("Start impedance measurement with interval {:?} ms.", rqst.interval_ms);
+
+    let mut impedance = context.impedance.lock().await;
+
+    if sender
+        .reply::<StartImpedanceEndpoint>(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::<ImpedanceTopic>(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
+}

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<Mutex<ThreadModeRawMutex, ImpedanceTest>> = 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);
+    }
+}
+

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<peripherals::USB>;
 });