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 }
+}