Included impedance topic.

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
 }