Added multi frequency measurement mode.

Cargo.lock

@@ -74,7 +74,7 @@ dependencies = [
  "embassy-time",
  "embassy-usb",
  "embedded-hal 1.0.0",
- "heapless 0.8.0",
+ "heapless 0.9.1",
  "libm",
  "panic-probe",
  "postcard-rpc",
@@ -85,6 +85,7 @@ dependencies = [
 name = "bioz-icd-rs"
 version = "0.1.0"
 dependencies = [
+ "heapless 0.9.1",
  "postcard-rpc",
  "postcard-schema",
  "serde",
@@ -714,6 +715,17 @@ dependencies = [
  "stable_deref_trait",
 ]
 
+[[package]]
+name = "heapless"
+version = "0.9.1"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "b1edcd5a338e64688fbdcb7531a846cfd3476a54784dcb918a0844682bc7ada5"
+dependencies = [
+ "hash32 0.3.1",
+ "serde",
+ "stable_deref_trait",
+]
+
 [[package]]
 name = "ident_case"
 version = "1.0.1"
@@ -830,9 +842,9 @@ dependencies = [
 
 [[package]]
 name = "postcard-rpc"
-version = "0.11.13"
+version = "0.11.15"
 source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "e1af23d87c9a8308bbfaae655ac770ec10517e6448fa5e0b50838a36e5d860b9"
+checksum = "c7e1944dfb9859e440511700c442edce3eacd5862f90f5a9997d004bd3553f3b"
 dependencies = [
  "defmt 0.3.100",
  "embassy-executor",
@@ -852,10 +864,11 @@ dependencies = [
 
 [[package]]
 name = "postcard-schema"
-version = "0.2.4"
+version = "0.2.5"
 source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "f5a13d8b1f8b3473b45c2c779b97c18c260ac6458eb045d4be75df8087784400"
+checksum = "9475666d89f42231a0a57da32d5f6ca7f9b5cd4c335ea1fe8f3278215b7a21ff"
 dependencies = [
+ "heapless 0.9.1",
  "postcard-derive",
  "serde",
 ]

Cargo.toml

@@ -7,7 +7,7 @@ edition = "2021"
 # Change stm32h563zi to your chip name, if necessary.
 embassy-stm32 = { version = "0.2.0", features = ["defmt", "stm32h533re", "memory-x", "time-driver-any", "exti", "unstable-pac"] }
 embassy-sync = { version = "0.6.0", features = ["defmt"] }
-embassy-executor = { version = "0.7.0", features = ["arch-cortex-m", "executor-thread", "defmt"] }
+embassy-executor = { version = "0.7.0", features = ["arch-cortex-m", "executor-thread",  "task-arena-size-32768", "defmt"] }
 embassy-time = { version = "0.4.0", features = ["defmt", "defmt-timestamp-uptime", "tick-hz-32_768"] }
 embassy-embedded-hal = { version = "0.4.0", features = ["defmt"]}
 # embassy-net = { version = "0.7.0", features = ["defmt", "tcp", "dhcpv4", "medium-ethernet", "proto-ipv6"] }
@@ -24,7 +24,7 @@ bioz-icd-rs = {path = "../bioz-icd-rs"}
 
 libm = { version = "0.2.15" }
 
-heapless = { version = "0.8.0" }
+heapless = { version = "0.9.1" }
 
 cortex-m = { version = "0.7.7", features = ["inline-asm", "critical-section-single-core"] }
 cortex-m-rt = "0.7.5"

src/ad5940.rs

@@ -204,7 +204,7 @@ pub struct AD5940 {
     rst: Output<'static>,
     seq_enabled: bool,
     pub seq_len: usize,
-    pub seq_buffer: [u32; 25],
+    pub seq_buffer: [u32; 500],
     pub seq_gen_db: LinearMap<Register, u32, 16>,
 }
 
@@ -216,7 +216,7 @@ impl AD5940 {
             rst,
             seq_enabled: false,
             seq_len: 0,
-            seq_buffer: [0; 25],
+            seq_buffer: [0; 500],
             seq_gen_db: LinearMap::new(),
         }
     }

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                        |
+        | EndpointTy                    | kind      | handler                        |
-        | ----------                | ----      | -------                        |
+        | ----------                    | ----      | -------                        |
-        | PingEndpoint              | blocking  | ping_handler                   |
+        | PingEndpoint                  | blocking  | ping_handler                   |
-        | GetUniqueIdEndpoint       | blocking  | get_unique_id_handler          |
+        | GetUniqueIdEndpoint           | blocking  | get_unique_id_handler          |
-        | SetGreenLedEndpoint       | async     | set_green_led_handler          |
+        | SetGreenLedEndpoint           | async     | set_green_led_handler          |
-        | StartImpedanceEndpoint    | spawn     | start_single_impedance_handler |
+        | StartSingleImpedanceEndpoint  | spawn     | start_single_impedance_handler |
-        | StopImpedanceEndpoint     | async     | stop_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;
+    let was_busy = context.impedance_setup.lock().await.running_mode;
-    if was_busy {
+    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
 }

src/impedance.rs

@@ -1,3 +1,5 @@
+use bioz_icd_rs::MultiImpedanceOutput28;
+use bioz_icd_rs::NumberOfPoints;
 use defmt::{info, error};
 
 use embassy_stm32::spi::Error;
@@ -10,23 +12,31 @@ use static_cell::StaticCell;
 use crate::ad5940::*;
 use crate::ad5940_registers::*;
 
-use bioz_icd_rs::{ImpedanceOutput, IcdDftNum};
+use bioz_icd_rs::{SingleImpedanceOutput, IcdDftNum};
 use crate::icd_mapping::IntoDftnum;
 
-pub static IMPEDANCE_CHANNEL: Channel<ThreadModeRawMutex, ImpedanceOutput, 2000> = Channel::new();
+pub static IMPEDANCE_CHANNEL_SINGLE: Channel<ThreadModeRawMutex, SingleImpedanceOutput, 2000> = Channel::new();
+pub static IMPEDANCE_CHANNEL_MULTI: Channel<ThreadModeRawMutex, MultiImpedanceOutput28, 50> = Channel::new();
 
 pub type ImpedanceSetupType = Mutex<ThreadModeRawMutex, ImpedanceSetup>;
 pub static IMPEDANCE_SETUP: StaticCell<ImpedanceSetupType> = StaticCell::new();
 
+#[derive(PartialEq, Copy, Clone)]
+pub enum RunningMode {
+    None,
+    SingleFrequency,
+    MultiFrequency,
+}
+
 pub struct ImpedanceSetup {
     ad5940: AD5940,
     dsp_config: Option<DspConfig>,
-    pub running: bool,
+    pub running_mode: RunningMode,
 }
 
 impl ImpedanceSetup {
     pub fn new(ad5940: AD5940) -> Self {
-        ImpedanceSetup { ad5940, dsp_config: None, running: false }
+        ImpedanceSetup { ad5940, dsp_config: None, running_mode: RunningMode::None }
     }
         
     pub async fn init(&mut self) -> Result<(), Error> {
@@ -157,7 +167,78 @@ impl ImpedanceSetup {
         self.start_measurement().await;
     }
 
+    pub async fn init_multi_frequency_measurement(&mut self, dft_number: IcdDftNum, number_of_points: NumberOfPoints) {
+        // Set DFT number
+        self.dsp_config.as_mut().unwrap().dftnum(dft_number.into_dftnum());
+        self.ad5940.apply_dsp_config(self.dsp_config.as_ref().unwrap()).await.unwrap();
 
+        // Configure GPIOs
+        self.ad5940.write_reg(Register::GP0CON, 0b10 << 4 |  0b10 << 2 | 0b10).await.unwrap();
+        self.ad5940.write_reg(Register::SYNCEXTDEVICE, 0b111).await.unwrap();
+
+        // Calculate wait time between measurement start and stop
+        let mut wait_time = 0;
+        if let Some(dsp_config) = &self.dsp_config {
+            wait_time = self.ad5940.sequencer_calculate_wait_time(dsp_config).await.unwrap();
+            // info!("Sinus periods per DFT: {}", wait_time as f32 / dsp_config.fsys.unwrap() as f32 * frequency as f32);
+        } else {
+            error!("DSP configuration not set, cannot calculate wait time");
+        }
+
+        // Configure sequencer
+        self.ad5940.sequencer_enable(true).await;
+
+        for &frequency in number_of_points.values() {        
+            self.ad5940.wgfcw(frequency as u32).await;
+            let wg_amplitude = 2047; // 2047 is the maximum amplitude for a 12-bit DAC --> 1.62V peak-to-peak
+            self.ad5940.write_reg(Register::WGAMPLITUDE, wg_amplitude).await.unwrap();
+
+            // Rcal
+            let switch_config = SwitchConfig::default()
+                .t9con(T9CON::T9Closed)
+                .tmuxcon(TMUXCON::TR1Closed)
+                .nmuxcon(NMUXCON::NR1Closed)
+                .pmuxcon(PMUXCON::PR0Closed)
+                .dmuxcon(DMUXCON::DR0Closed);
+            self.ad5940.apply_switch_config(switch_config).await.unwrap();
+            self.ad5940.afecon(AFECON::WAVEGENEN | AFECON::ADCEN, true).await;
+            self.ad5940.sequencer_wait(16*10).await; // 10 us based on SYSCLK = 16MHz
+            self.ad5940.afecon(AFECON::ADCCONVEN | AFECON::DFTEN, true).await;
+            self.ad5940.sequencer_wait(wait_time).await; // Determined above
+            self.ad5940.sequencer_wait(16*20).await; // 20 us based on SYSCLK = 16MHz
+            self.ad5940.afecon(AFECON::WAVEGENEN | AFECON:: ADCEN | AFECON::ADCCONVEN | AFECON::DFTEN, false).await;
+
+            // Rz
+            let switch_config = SwitchConfig::default()
+                .t9con(T9CON::T9Closed)
+                .tmuxcon(TMUXCON::T2Closed)
+                .nmuxcon(NMUXCON::N2Closed)
+                .pmuxcon(PMUXCON::P11Closed)
+                .dmuxcon(DMUXCON::D5Closed);
+            self.ad5940.apply_switch_config(switch_config).await.unwrap();
+            self.ad5940.afecon(AFECON::WAVEGENEN | AFECON::ADCEN, true).await;
+            self.ad5940.sequencer_wait(16*10).await; // 10 us based on SYSCLK = 16MHz
+            self.ad5940.afecon(AFECON::ADCCONVEN | AFECON::DFTEN, true).await;
+            self.ad5940.sequencer_wait(wait_time).await; // Determined above
+            self.ad5940.sequencer_wait(16*20).await; // 20 us based on SYSCLK = 16MHz
+            self.ad5940.afecon(AFECON::WAVEGENEN | AFECON:: ADCEN | AFECON::ADCCONVEN | AFECON::DFTEN, false).await;
+        }
+
+        // Toggle leds
+        self.ad5940.write_reg(Register::SYNCEXTDEVICE, 0b010).await.unwrap();
+        self.ad5940.sequencer_wait(16 * 1_000).await; // 1ms based on SYSCLK = 16MHz
+        self.ad5940.write_reg(Register::SYNCEXTDEVICE, 0b111).await.unwrap();
+
+        self.ad5940.sequencer_enable(false).await;
+
+        // Write sequence to SRAM
+        let start_address = 0;
+        self.ad5940.sequencer_cmd_write(start_address).await;
+
+        self.ad5940.sequencer_info_configure(0, self.ad5940.seq_len, start_address).await;
+
+        self.start_measurement().await;
+    }
 
     pub async fn start_measurement(&mut self) {
         self.ad5940.sequencer_trigger(0).await;

src/main.rs

@@ -11,12 +11,14 @@ use embassy_stm32::gpio::{Level, Output, Speed};
 use embassy_stm32::{i2c, spi, Config};
 use embassy_stm32::time::Hertz;
 
+use heapless::Vec;
+
 use {defmt_rtt as _, panic_probe as _};
 
 // use crate::ad5940::*;
 // use crate::ad5940_registers::*;
 
-use bioz_icd_rs::ImpedanceOutput;
+use bioz_icd_rs::{MultiImpedanceOutput28, SingleImpedanceOutput};
 
 mod ad5940;
 use ad5940::AD5940;
@@ -35,7 +37,7 @@ use embassy_stm32::{bind_interrupts, peripherals, usb};
 mod communication;
 use communication::{init_communication, LED_FREQUENCY_SIGNAL};
 
-use impedance::IMPEDANCE_CHANNEL;
+use impedance::{IMPEDANCE_CHANNEL_SINGLE, IMPEDANCE_CHANNEL_MULTI};
 
 mod impedance;
 use impedance::{ImpedanceSetup, ImpedanceSetupType, IMPEDANCE_SETUP};
@@ -183,31 +185,63 @@ async fn impedance_setup_readout_task(mut pin: ExtiInput<'static>, impedance_set
         let mut impedance_setup = impedance_setup.lock().await;
 
         // Trigger the sequencer again
-        if impedance_setup.running {
+        if impedance_setup.running_mode == impedance::RunningMode::SingleFrequency || impedance_setup.running_mode == impedance::RunningMode::MultiFrequency {
             impedance_setup.start_measurement().await;
         }
 
         // Read the FIFO count
         let count = impedance_setup.get_fifo_count().await.unwrap();
-        // info!("FIFOCNTSTA: {}", count);
+        info!("FIFOCNTSTA: {}", count);
 
-        if count >= 4 {
+        match impedance_setup.running_mode {
-            let mut data: [u32; 4] = [0; 4];
+            impedance::RunningMode::None => {
+                continue; // Skip processing if not running
+            }
+            impedance::RunningMode::SingleFrequency => {
+                if count >= 4 {
+                    let mut data: [u32; 4] = [0; 4];
 
-            impedance_setup.read_fifo(data.as_mut_slice()).await.unwrap();
+                    impedance_setup.read_fifo(data.as_mut_slice()).await.unwrap();
 
-            let result = calculate_impedance(data);
+                    let result = calculate_impedance(data);
                     
+                    // Log
+                    // info!("Impedance: Magnitude = {} Ω, Phase = {} rad", result.magnitude, result.phase);
 
-            // Log
+                    let data = SingleImpedanceOutput {
-            // info!("Impedance: Magnitude = {} Ω, Phase = {} rad", result.magnitude, result.phase);
+                        magnitude: result.magnitude,
+                        phase: result.phase,
+                    };
 
-            let data = ImpedanceOutput {
+                    IMPEDANCE_CHANNEL_SINGLE.try_send(data).ok();
-                magnitude: result.magnitude,
+                }
-                phase: result.phase,
+            }
-            };
             
-            IMPEDANCE_CHANNEL.try_send(data).ok();
+            impedance::RunningMode::MultiFrequency => {
+                if count >= 112 {
+                    let mut data = [0u32; 112];
+                    impedance_setup.read_fifo(data.as_mut_slice()).await.unwrap();
+
+                    let mut impedance_output = MultiImpedanceOutput28 {
+                        magnitudes: Vec::new(),
+                        phases: Vec::new(),
+                    };
+
+                    // Take 4 samples for each frequency point
+                    for chunk in data.chunks(4) {
+                        let result = calculate_impedance([chunk[0], chunk[1], chunk[2], chunk[3]]);
+                        // Store the results
+                        impedance_output.magnitudes.push(result.magnitude).ok();
+                        impedance_output.phases.push(result.phase).ok();
+                    }
+
+                    // Log
+                    // info!("Impedance: Magnitude = {} Ω, Phase = {} rad", result.magnitude, result.phase);
+
+                    IMPEDANCE_CHANNEL_MULTI.try_send(impedance_output).ok();
+                }
+                continue;
+            }
         }
     }
 }