Added multi frequency measurement mode.

src/communication.rs

@@ -19,9 +19,9 @@ use postcard_rpc::{
     },
 };
 
-use bioz_icd_rs::{PingEndpoint, GetUniqueIdEndpoint, SetGreenLedEndpoint, StartImpedanceEndpoint, StopImpedanceEndpoint, StartImpedance, ImpedanceOutputTopic, ENDPOINT_LIST, TOPICS_IN_LIST, TOPICS_OUT_LIST};
+use bioz_icd_rs::{GetUniqueIdEndpoint, PingEndpoint, SetGreenLedEndpoint, SingleImpedanceOutput, SingleImpedanceOutputTopic, StartMultiImpedance, StartMultiImpedanceEndpoint, StartSingleImpedance, StartSingleImpedanceEndpoint, StopMultiImpedanceEndpoint, StopSingleImpedanceEndpoint, MultiImpedanceOutputTopic28, MultiImpedanceOutput28, ENDPOINT_LIST, TOPICS_IN_LIST, TOPICS_OUT_LIST};
 
-use crate::impedance::{ImpedanceSetupType, IMPEDANCE_CHANNEL};
+use crate::impedance::{ImpedanceSetupType, IMPEDANCE_CHANNEL_SINGLE, IMPEDANCE_CHANNEL_MULTI, RunningMode};
 
 // Postcard RPC types
 type AppDriver = usb::Driver<'static, peripherals::USB>;
@@ -63,13 +63,15 @@ define_dispatch! {
     endpoints: {
         list: ENDPOINT_LIST;
 
-        | EndpointTy                | kind      | handler                        |
-        | ----------                | ----      | -------                        |
-        | PingEndpoint              | blocking  | ping_handler                   |
-        | GetUniqueIdEndpoint       | blocking  | get_unique_id_handler          |
-        | SetGreenLedEndpoint       | async     | set_green_led_handler          |
-        | StartImpedanceEndpoint    | spawn     | start_single_impedance_handler |
-        | StopImpedanceEndpoint     | async     | stop_single_impedance_handler  |
+        | EndpointTy                    | kind      | handler                        |
+        | ----------                    | ----      | -------                        |
+        | PingEndpoint                  | blocking  | ping_handler                   |
+        | GetUniqueIdEndpoint           | blocking  | get_unique_id_handler          |
+        | SetGreenLedEndpoint           | async     | set_green_led_handler          |
+        | StartSingleImpedanceEndpoint  | spawn     | start_single_impedance_handler |
+        | StopSingleImpedanceEndpoint   | async     | stop_single_impedance_handler  |
+        | StartMultiImpedanceEndpoint   | spawn     | start_multi_impedance_handler   |
+        | StopMultiImpedanceEndpoint    | async     | stop_multi_impedance_handler    |
     };
     topics_in: {
         list: TOPICS_IN_LIST;
@@ -162,11 +164,11 @@ pub async fn set_green_led_handler(_context: &mut Context, _header: VarHeader, r
 static STOP: Signal<CriticalSectionRawMutex, ()> = Signal::new();
 
 #[embassy_executor::task]
-pub async fn start_single_impedance_handler(context: SpawnCtx, header: VarHeader, rqst: StartImpedance, sender: Sender<AppTx>) {
+pub async fn start_single_impedance_handler(context: SpawnCtx, header: VarHeader, rqst: StartSingleImpedance, sender: Sender<AppTx>) {
     info!("Start impedance measurement at {:?} Hz.", rqst.sinus_frequency);
 
     // Mark the impedance setup as running
-    context.impedance_setup.lock().await.running = true;
+    context.impedance_setup.lock().await.running_mode = RunningMode::SingleFrequency;
 
     // Init the sequencer
     context.impedance_setup.lock().await.init_single_frequency_measurement(rqst.sinus_frequency, rqst.dft_number).await;
@@ -174,7 +176,7 @@ pub async fn start_single_impedance_handler(context: SpawnCtx, header: VarHeader
     context.impedance_setup.lock().await.start_measurement().await;
 
     if sender
-        .reply::<StartImpedanceEndpoint>(header.seq_no, &())
+        .reply::<StartSingleImpedanceEndpoint>(header.seq_no, &())
         .await
         .is_err()
     {
@@ -185,7 +187,7 @@ pub async fn start_single_impedance_handler(context: SpawnCtx, header: VarHeader
     let mut seq: u8 = 0;
     loop {
         let stop_fut = STOP.wait();
-        let recv_fut = IMPEDANCE_CHANNEL.receive();
+        let recv_fut = IMPEDANCE_CHANNEL_SINGLE.receive();
 
         match select(stop_fut, recv_fut).await {
             Either::First(_) => {
@@ -194,7 +196,7 @@ pub async fn start_single_impedance_handler(context: SpawnCtx, header: VarHeader
             }
             Either::Second(msg) => {
                 if sender
-                    .publish::<ImpedanceOutputTopic>(seq.into(), &msg)
+                    .publish::<SingleImpedanceOutputTopic>(seq.into(), &msg)
                     .await
                     .is_err()
                     {
@@ -206,16 +208,75 @@ pub async fn start_single_impedance_handler(context: SpawnCtx, header: VarHeader
         }
     }
 
-    context.impedance_setup.lock().await.running = false;
+    context.impedance_setup.lock().await.running_mode = RunningMode::None;
     info!("Impedance measurement stopped.");
     STOP.reset();
 }
 
 pub async fn stop_single_impedance_handler(context: &mut Context, _header: VarHeader, _rqst: ()) -> bool {
     info!("Stop impedance measurement");
-    let was_busy = context.impedance_setup.lock().await.running;
-    if was_busy {
+    let was_busy = context.impedance_setup.lock().await.running_mode;
+    if was_busy == RunningMode::SingleFrequency || was_busy == RunningMode::MultiFrequency {
         STOP.signal(());
     }
-    was_busy
+    was_busy == RunningMode::SingleFrequency || was_busy == RunningMode::MultiFrequency
+}
+
+#[embassy_executor::task]
+pub async fn start_multi_impedance_handler(context: SpawnCtx, header: VarHeader, rqst: StartMultiImpedance, sender: Sender<AppTx>) {
+    // Mark the impedance setup as running
+    context.impedance_setup.lock().await.running_mode = RunningMode::MultiFrequency;
+    
+    // Init the sequencer
+    context.impedance_setup.lock().await.init_multi_frequency_measurement(rqst.dft_number, rqst.number_of_points).await;
+    // Trigger the sequencer
+    context.impedance_setup.lock().await.start_measurement().await;
+
+    if sender
+        .reply::<StartMultiImpedanceEndpoint>(header.seq_no, &())
+        .await
+        .is_err()
+    {
+        defmt::error!("Failed to reply, stopping accel");
+        return;
+    }
+
+    info!("Start multi impedance measurement.");
+
+    let mut seq: u8 = 0;
+    loop {
+        let stop_fut = STOP.wait();
+        let recv_fut = IMPEDANCE_CHANNEL_MULTI.receive();
+
+        match select(stop_fut, recv_fut).await {
+            Either::First(_) => {
+                info!("Stop signal received.");
+                break;
+            }
+            Either::Second(msg) => {
+                if sender
+                    .publish::<MultiImpedanceOutputTopic28>(seq.into(), &msg)
+                    .await
+                    .is_err()
+                    {
+                        defmt::error!("Topic send error!");
+                        break;
+                    }
+                seq = seq.wrapping_add(1);
+            }
+        }
+    }
+
+    context.impedance_setup.lock().await.running_mode = RunningMode::None;
+    info!("Impedance measurement stopped.");
+    STOP.reset();
+}
+
+pub async fn stop_multi_impedance_handler(context: &mut Context, _header: VarHeader, _rqst: ()) -> bool {
+    info!("Stop impedance measurement");
+    let was_busy = context.impedance_setup.lock().await.    running_mode;
+    if was_busy == RunningMode::SingleFrequency || was_busy == RunningMode::MultiFrequency {
+        STOP.signal(());
+    } 
+    was_busy == RunningMode::SingleFrequency || was_busy == RunningMode::MultiFrequency
 }