Moved readout to impedance.rs, changed frequency type to u32.

src/impedance.rs

@@ -1,20 +1,23 @@
 use defmt::{info, error};
 
 use embassy_stm32::spi::Error;
+use embassy_stm32::exti::ExtiInput;
 use embassy_sync::blocking_mutex::raw::ThreadModeRawMutex;
 use embassy_sync::channel::Channel;
 use embassy_sync::mutex::Mutex;
 
 use static_cell::StaticCell;
 
+use heapless::Vec;
+
 use crate::ad5940::*;
 use crate::ad5940_registers::*;
 
 use bioz_icd_rs::{SingleImpedanceOutput, IcdDftNum, ImpedanceInitError, MeasurementPointSet, MultiImpedanceOutput};
 use crate::icd_mapping::IntoDftnum;
 
-pub static IMPEDANCE_CHANNEL_SINGLE: Channel<ThreadModeRawMutex, SingleImpedanceOutput, 2000> = Channel::new();
-pub static IMPEDANCE_CHANNEL_MULTI: Channel<ThreadModeRawMutex, MultiImpedanceOutput, 10> = Channel::new();
+pub static IMPEDANCE_CHANNEL_SINGLE: Channel<ThreadModeRawMutex, SingleImpedanceOutput, 50> = Channel::new();
+pub static IMPEDANCE_CHANNEL_MULTI: Channel<ThreadModeRawMutex, MultiImpedanceOutput, 5> = Channel::new();
 
 pub type ImpedanceSetupType = Mutex<ThreadModeRawMutex, ImpedanceSetup>;
 pub static IMPEDANCE_SETUP: StaticCell<ImpedanceSetupType> = StaticCell::new();
@@ -191,14 +194,14 @@ impl ImpedanceSetup {
         for &frequency in number_of_points.values() {    
             // Determine wait time
             let selected_dft_num = match frequency {
-                f if f < 10.0 => DFTNUM::Num16384,
-                f if f < 100.0 => DFTNUM::Num16384,
-                f if f < 250.0 => DFTNUM::Num8192,
-                f if f < 1_000.0 => DFTNUM::Num4096,
-                f if f < 2_500.0 => DFTNUM::Num2048,
-                f if f < 10_000.0 => DFTNUM::Num1024,
-                f if f < 25_000.0 => DFTNUM::Num512,
-                f if f < 100_000.0 => DFTNUM::Num256,
+                f if f < 10 => DFTNUM::Num16384,
+                f if f < 100 => DFTNUM::Num16384,
+                f if f < 250 => DFTNUM::Num8192,
+                f if f < 1000 => DFTNUM::Num4096,
+                f if f < 2500 => DFTNUM::Num2048,
+                f if f < 10000 => DFTNUM::Num1024,
+                f if f < 25000 => DFTNUM::Num512,
+                f if f < 100000 => DFTNUM::Num256,
                 _ => DFTNUM::Num128,
             };
 
@@ -290,3 +293,145 @@ impl ImpedanceSetup {
         self.ad5940.read_fifo(data).await
     }
 }
+
+// Task to wait for the interrupt from the AD5940 and read the FIFO
+// Depending on the running mode, it will read either a single frequency or multiple frequencies
+// Result is sent via channel to the communication task which then sends it via USB to the host
+#[embassy_executor::task]
+pub async fn impedance_setup_readout_task(mut pin: ExtiInput<'static>, impedance_setup: &'static ImpedanceSetupType) {
+    loop {
+        // Wait untill sequence is done
+        pin.wait_for_rising_edge().await;
+
+        // Lock the impedance setup
+        let mut impedance_setup = impedance_setup.lock().await;
+
+        // Trigger the sequencer again
+        if impedance_setup.running_mode == RunningMode::SingleFrequency || impedance_setup.running_mode == RunningMode::MultiFrequency(MeasurementPointSet::Eight) || impedance_setup.running_mode == RunningMode::MultiFrequency(MeasurementPointSet::Eighteen) {
+            impedance_setup.start_measurement().await;
+        }
+
+        // Read the FIFO count
+        let count = impedance_setup.get_fifo_count().await.unwrap() as usize;
+        // info!("FIFOCNTSTA: {}", count);
+
+        match impedance_setup.running_mode {
+            RunningMode::None => {
+                continue; // Skip processing if not running
+            }
+            RunningMode::SingleFrequency => {
+                if count >= 4 {
+                    let mut data: [u32; 4] = [0; 4];
+
+                    impedance_setup.read_fifo(data.as_mut_slice()).await.unwrap();
+
+                    let result = calculate_impedance(data);
+                    
+                    // Log
+                    // info!("Impedance: Magnitude = {} Ω, Phase = {} rad", result.magnitude, result.phase);
+
+                    let data = SingleImpedanceOutput {
+                        magnitude: result.magnitude,
+                        phase: result.phase,
+                    };
+
+                    IMPEDANCE_CHANNEL_SINGLE.try_send(data).ok();
+                }
+            }
+
+            RunningMode::MultiFrequency(points) => {
+                // Each frequency point produces 4 samples (DFT real/imag for Rcal and Rz)
+                let required_count = points.len() * 4;
+                
+                if count >= required_count {
+                    // Use stack-allocated array
+                    let mut data: [u32; 72] = [0; 72]; // max needed size (18*4=72)
+                    let data_slice = &mut data[..required_count];
+                    impedance_setup.read_fifo(data_slice).await.unwrap();
+
+                    // Output structure
+                    let mut impedance_output = MultiImpedanceOutput {
+                        points,
+                        magnitudes_8: Vec::new(),
+                        phases_8: Vec::new(),
+                        magnitudes_18: Vec::new(),
+                        phases_18: Vec::new(),
+                    };
+
+                    // Take 4 samples per frequency point
+                    for chunk in data_slice.chunks(4) {
+                        let result = calculate_impedance(chunk.try_into().unwrap());
+
+                        match points {
+                            MeasurementPointSet::Eight => {
+                                impedance_output.magnitudes_8.push(result.magnitude).ok();
+                                impedance_output.phases_8.push(result.phase).ok();
+                            }
+                            MeasurementPointSet::Eighteen => {
+                                impedance_output.magnitudes_18.push(result.magnitude).ok();
+                                impedance_output.phases_18.push(result.phase).ok();
+                            }
+                        }
+                    }
+
+                    IMPEDANCE_CHANNEL_MULTI.try_send(impedance_output).ok();
+                }
+                continue;
+            }
+        }
+    }
+}
+
+extern crate libm;
+
+#[derive(Debug, Clone, Copy)]
+pub struct Complex {
+    real: i32,
+    imag: i32,
+}
+
+#[derive(Debug)]
+pub struct ImpedanceResult {
+    pub magnitude: f32,
+    pub phase: f32,
+}
+
+// Example Rcal value (Ohms)
+const RCAL_VAL: f32 = 1000.0;
+
+/// Convert raw 18-bit 2's complement value to signed i32
+fn sign_extend_18bit(val: u32) -> i32 {
+    ((val << 14) as i32) >> 14
+}
+
+/// Calculate magnitude and phase of Rz using Rcal reference
+pub fn calculate_impedance(data: [u32; 4]) -> ImpedanceResult {
+    let mut signed_data = [0i32; 4];
+    for (i, &val) in data.iter().enumerate() {
+        signed_data[i] = sign_extend_18bit(val);
+    }
+
+    let dft_rcal = Complex {
+        real: signed_data[0],
+        imag: signed_data[1],
+    };
+
+    let dft_rz = Complex {
+        real: signed_data[2],
+        imag: signed_data[3],
+    };
+
+    let rcal_mag = libm::sqrtf((dft_rcal.real as f32) * (dft_rcal.real as f32)
+                             + (dft_rcal.imag as f32) * (dft_rcal.imag as f32));
+
+    let rz_mag = libm::sqrtf((dft_rz.real as f32) * (dft_rz.real as f32)
+                           + (dft_rz.imag as f32) * (dft_rz.imag as f32));
+
+    let rcal_phase = libm::atan2f(-(dft_rcal.imag as f32), dft_rcal.real as f32);
+    let rz_phase = libm::atan2f(-(dft_rz.imag as f32), dft_rz.real as f32);
+
+    let magnitude = (rcal_mag / rz_mag) * RCAL_VAL;
+    let phase = rcal_phase - rz_phase;
+
+    ImpedanceResult { magnitude, phase }
+}

src/main.rs

@@ -11,15 +11,11 @@ use embassy_stm32::gpio::{Level, Output, Speed};
 use embassy_stm32::{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::{MeasurementPointSet, MultiImpedanceOutput, SingleImpedanceOutput};
-
 mod ad5940;
 use ad5940::AD5940;
 
@@ -37,10 +33,8 @@ use embassy_stm32::{bind_interrupts, peripherals, usb};
 mod communication;
 use communication::{init_communication, LED_FREQUENCY_SIGNAL};
 
-use impedance::{IMPEDANCE_CHANNEL_SINGLE, IMPEDANCE_CHANNEL_MULTI};
-
 mod impedance;
-use impedance::{ImpedanceSetup, ImpedanceSetupType, IMPEDANCE_SETUP};
+use impedance::{ImpedanceSetup, IMPEDANCE_SETUP, impedance_setup_readout_task};
 
 mod icd_mapping;
 
@@ -174,143 +168,3 @@ async fn green_led(mut led: Output<'static>) {
         led.toggle();
     }
 }
-
-#[embassy_executor::task]
-async fn impedance_setup_readout_task(mut pin: ExtiInput<'static>, impedance_setup: &'static ImpedanceSetupType) {
-    loop {
-        // Wait untill sequence is done
-        pin.wait_for_rising_edge().await;
-
-        // Lock the impedance setup
-        let mut impedance_setup = impedance_setup.lock().await;
-
-        // Trigger the sequencer again
-        if impedance_setup.running_mode == impedance::RunningMode::SingleFrequency || impedance_setup.running_mode == impedance::RunningMode::MultiFrequency(MeasurementPointSet::Eight) || impedance_setup.running_mode == impedance::RunningMode::MultiFrequency(MeasurementPointSet::Eighteen) {
-            impedance_setup.start_measurement().await;
-        }
-
-        // Read the FIFO count
-        let count = impedance_setup.get_fifo_count().await.unwrap() as usize;
-        // info!("FIFOCNTSTA: {}", count);
-
-        match impedance_setup.running_mode {
-            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();
-
-                    let result = calculate_impedance(data);
-                    
-                    // Log
-                    // info!("Impedance: Magnitude = {} Ω, Phase = {} rad", result.magnitude, result.phase);
-
-                    let data = SingleImpedanceOutput {
-                        magnitude: result.magnitude,
-                        phase: result.phase,
-                    };
-
-                    IMPEDANCE_CHANNEL_SINGLE.try_send(data).ok();
-                }
-            }
-
-            impedance::RunningMode::MultiFrequency(points) => {
-                // Each frequency point produces 4 samples (DFT real/imag for Rcal and Rz)
-                let required_count = points.len() * 4;
-                
-                if count >= required_count {
-                    // Use stack-allocated array
-                    let mut data: [u32; 72] = [0; 72]; // max needed size (18*4=72)
-                    let data_slice = &mut data[..required_count];
-                    impedance_setup.read_fifo(data_slice).await.unwrap();
-
-                    // Output structure
-                    let mut impedance_output = MultiImpedanceOutput {
-                        points,
-                        magnitudes_8: Vec::new(),
-                        phases_8: Vec::new(),
-                        magnitudes_18: Vec::new(),
-                        phases_18: Vec::new(),
-                    };
-
-                    // Take 4 samples per frequency point
-                    for chunk in data_slice.chunks(4) {
-                        let result = calculate_impedance(chunk.try_into().unwrap());
-
-                        match points {
-                            MeasurementPointSet::Eight => {
-                                impedance_output.magnitudes_8.push(result.magnitude).ok();
-                                impedance_output.phases_8.push(result.phase).ok();
-                            }
-                            MeasurementPointSet::Eighteen => {
-                                impedance_output.magnitudes_18.push(result.magnitude).ok();
-                                impedance_output.phases_18.push(result.phase).ok();
-                            }
-                        }
-                    }
-
-                    IMPEDANCE_CHANNEL_MULTI.try_send(impedance_output).ok();
-                }
-                continue;
-            }
-        }
-    }
-}
-
-
-extern crate libm;
-
-#[derive(Debug, Clone, Copy)]
-pub struct Complex {
-    real: i32,
-    imag: i32,
-}
-
-#[derive(Debug)]
-pub struct ImpedanceResult {
-    pub magnitude: f32,
-    pub phase: f32,
-}
-
-// Example Rcal value (Ohms)
-const RCAL_VAL: f32 = 1000.0;
-
-/// Convert raw 18-bit 2's complement value to signed i32
-fn sign_extend_18bit(val: u32) -> i32 {
-    ((val << 14) as i32) >> 14
-}
-
-/// Calculate magnitude and phase of Rz using Rcal reference
-pub fn calculate_impedance(data: [u32; 4]) -> ImpedanceResult {
-    let mut signed_data = [0i32; 4];
-    for (i, &val) in data.iter().enumerate() {
-        signed_data[i] = sign_extend_18bit(val);
-    }
-
-    let dft_rcal = Complex {
-        real: signed_data[0],
-        imag: signed_data[1],
-    };
-
-    let dft_rz = Complex {
-        real: signed_data[2],
-        imag: signed_data[3],
-    };
-
-    let rcal_mag = libm::sqrtf((dft_rcal.real as f32) * (dft_rcal.real as f32)
-                             + (dft_rcal.imag as f32) * (dft_rcal.imag as f32));
-
-    let rz_mag = libm::sqrtf((dft_rz.real as f32) * (dft_rz.real as f32)
-                           + (dft_rz.imag as f32) * (dft_rz.imag as f32));
-
-    let rcal_phase = libm::atan2f(-(dft_rcal.imag as f32), dft_rcal.real as f32);
-    let rz_phase = libm::atan2f(-(dft_rz.imag as f32), dft_rz.real as f32);
-
-    let magnitude = (rcal_mag / rz_mag) * RCAL_VAL;
-    let phase = rcal_phase - rz_phase;
-
-    ImpedanceResult { magnitude, phase }
-}