Added 4-lead multi calibration.

src/communication.rs

@@ -20,7 +20,7 @@ use postcard_rpc::{
     },
 };
 
-use bioz_icd_rs::{GetUniqueIdEndpoint, MeasurementPointSet, MultiImpedanceOutputTopic, MultiImpedanceResult, MultiImpedanceStartRequest, PingEndpoint, SetGreenLedEndpoint, SingleImpedanceOutputTopic, SingleImpedanceStartRequest, StartMultiImpedanceEndpoint, StartSingleImpedanceEndpoint, StopMultiImpedanceEndpoint, StopSingleImpedanceEndpoint, ENDPOINT_LIST, TOPICS_IN_LIST, TOPICS_OUT_LIST};
+use bioz_icd_rs::{GetUniqueIdEndpoint, MeasurementPointSet, MultiImpedanceOutputTopic, MultiImpedanceResult, MultiImpedanceStartRequest, PingEndpoint, SetGreenLedEndpoint, SingleImpedanceOutputTopic, SingleImpedanceStartRequest, StartMultiImpedanceEndpoint, StartSingleImpedanceEndpoint, StopMultiImpedanceEndpoint, StopSingleImpedanceEndpoint, BioImpedanceLeadMode, ENDPOINT_LIST, TOPICS_IN_LIST, TOPICS_OUT_LIST};
 
 use crate::impedance::{ImpedanceSetupType, RunningMode, IMPEDANCE_CHANNEL_MULTI, IMPEDANCE_CHANNEL_SINGLE};
 
@@ -170,14 +170,14 @@ pub async fn start_single_impedance_handler(context: SpawnCtx, header: VarHeader
 
     // Mark the impedance setup as running
     match rqst.lead_mode {
-        bioz_icd_rs::BioImpedanceLeadMode::TwoLead => context.impedance_setup.lock().await.running_mode = RunningMode::SingleFrequency2Lead,
+        BioImpedanceLeadMode::TwoLead => context.impedance_setup.lock().await.running_mode = RunningMode::SingleFrequency2Lead,
-        bioz_icd_rs::BioImpedanceLeadMode::FourLead => context.impedance_setup.lock().await.running_mode = RunningMode::SingleFrequency4Lead,
+        BioImpedanceLeadMode::FourLead => context.impedance_setup.lock().await.running_mode = RunningMode::SingleFrequency4Lead,
     }
 
     // Init the sequencer
     let init_impedance_result = match rqst.lead_mode {
-        bioz_icd_rs::BioImpedanceLeadMode::TwoLead => context.impedance_setup.lock().await.init_single_frequency_measurement(rqst.sinus_frequency, rqst.dft_number).await,
+        BioImpedanceLeadMode::TwoLead => context.impedance_setup.lock().await.init_single_frequency_measurement_2_lead(rqst.sinus_frequency, rqst.dft_number).await,
-        bioz_icd_rs::BioImpedanceLeadMode::FourLead => context.impedance_setup.lock().await.init_single_frequency_measurement_4_lead(rqst.sinus_frequency, rqst.dft_number).await,
+        BioImpedanceLeadMode::FourLead => context.impedance_setup.lock().await.init_single_frequency_measurement_4_lead(rqst.sinus_frequency, rqst.dft_number).await,
     };
     // Trigger the sequencer
     context.impedance_setup.lock().await.start_measurement().await;
@@ -223,26 +223,27 @@ pub async fn start_single_impedance_handler(context: SpawnCtx, header: VarHeader
 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_mode;
-    if was_busy == RunningMode::SingleFrequency2Lead || was_busy == RunningMode::SingleFrequency4Lead || was_busy == RunningMode::MultiFrequency(MeasurementPointSet::Eight) || was_busy == RunningMode::MultiFrequency(MeasurementPointSet::Eighteen) {
+    if was_busy == RunningMode::SingleFrequency2Lead || was_busy == RunningMode::SingleFrequency4Lead || was_busy == RunningMode::MultiFrequency2Lead(MeasurementPointSet::Eight) || was_busy == RunningMode::MultiFrequency2Lead(MeasurementPointSet::Eighteen) || was_busy == RunningMode::MultiFrequency4Lead(MeasurementPointSet::Eight) || was_busy == RunningMode::MultiFrequency4Lead(MeasurementPointSet::Eighteen) {
         STOP.signal(());
     }
-    was_busy == RunningMode::SingleFrequency2Lead || was_busy == RunningMode::SingleFrequency4Lead || was_busy == RunningMode::MultiFrequency(MeasurementPointSet::Eight) || was_busy == RunningMode::MultiFrequency(MeasurementPointSet::Eighteen)
+    was_busy == RunningMode::SingleFrequency2Lead || was_busy == RunningMode::SingleFrequency4Lead || was_busy == RunningMode::MultiFrequency2Lead(MeasurementPointSet::Eight) || was_busy == RunningMode::MultiFrequency2Lead(MeasurementPointSet::Eighteen) || was_busy == RunningMode::MultiFrequency4Lead(MeasurementPointSet::Eight) || was_busy == RunningMode::MultiFrequency4Lead(MeasurementPointSet::Eighteen)
 }
 
 #[embassy_executor::task]
 pub async fn start_multi_impedance_handler(context: SpawnCtx, header: VarHeader, rqst: MultiImpedanceStartRequest, sender: Sender<AppTx>) {
     // Mark the impedance setup as running
-    context.impedance_setup.lock().await.running_mode = RunningMode::MultiFrequency(rqst.points);
+    
     
     // Init the sequencer
-    let response = match rqst.points {
+    let response = match (rqst.lead_mode, rqst.points) {
-        MeasurementPointSet::Eight => {
+        (BioImpedanceLeadMode::TwoLead, MeasurementPointSet::Eight) => {
+            context.impedance_setup.lock().await.running_mode = RunningMode::MultiFrequency2Lead(rqst.points);
             const SIZE: usize = 8;
             context
                 .impedance_setup
                 .lock()
                 .await
-                .init_multi_frequency_measurement::<SIZE>(rqst.points)
+                .init_multi_frequency_measurement_2_lead::<SIZE>(rqst.points)
                 .await
                 .map(|periods| MultiImpedanceResult {
                     points: rqst.points,
@@ -250,13 +251,46 @@ pub async fn start_multi_impedance_handler(context: SpawnCtx, header: VarHeader,
                     periods_per_dft_18: Vec::new(),
                 })
         }
-        MeasurementPointSet::Eighteen => {
+        (BioImpedanceLeadMode::TwoLead, MeasurementPointSet::Eighteen) => {
+            context.impedance_setup.lock().await.running_mode = RunningMode::MultiFrequency2Lead(rqst.points);
             const SIZE: usize = 18;
             context
                 .impedance_setup
                 .lock()
                 .await
-                .init_multi_frequency_measurement::<SIZE>(rqst.points)
+                .init_multi_frequency_measurement_2_lead::<SIZE>(rqst.points)
+                .await
+                .map(|periods| MultiImpedanceResult {
+                    points: rqst.points,
+                    periods_per_dft_8: Vec::new(),
+                    periods_per_dft_18: periods,
+                })
+        }
+        (BioImpedanceLeadMode::FourLead, MeasurementPointSet::Eight) => {
+            info!("Start multi impedance 4 lead 8 points");
+            context.impedance_setup.lock().await.running_mode = RunningMode::MultiFrequency4Lead(rqst.points);
+            const SIZE: usize = 8;
+            context
+                .impedance_setup
+                .lock()
+                .await
+                .init_multi_frequency_measurement_4_lead::<SIZE>(rqst.points)
+                .await
+                .map(|periods| MultiImpedanceResult {
+                    points: rqst.points,
+                    periods_per_dft_8: periods,
+                    periods_per_dft_18: Vec::new(),
+                })
+        }
+        (BioImpedanceLeadMode::FourLead, MeasurementPointSet::Eighteen) => {
+            info!("Start multi impedance 4 lead 18 points");
+            context.impedance_setup.lock().await.running_mode = RunningMode::MultiFrequency4Lead(rqst.points);
+            const SIZE: usize = 18;
+            context
+                .impedance_setup
+                .lock()
+                .await
+                .init_multi_frequency_measurement_4_lead::<SIZE>(rqst.points)
                 .await
                 .map(|periods| MultiImpedanceResult {
                     points: rqst.points,
@@ -312,8 +346,8 @@ pub async fn start_multi_impedance_handler(context: SpawnCtx, header: VarHeader,
 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::SingleFrequency2Lead || was_busy == RunningMode::SingleFrequency4Lead || was_busy == RunningMode::MultiFrequency(MeasurementPointSet::Eight) || was_busy == RunningMode::MultiFrequency(MeasurementPointSet::Eighteen) {
+    if was_busy == RunningMode::SingleFrequency2Lead || was_busy == RunningMode::SingleFrequency4Lead || was_busy == RunningMode::MultiFrequency2Lead(MeasurementPointSet::Eight) || was_busy == RunningMode::MultiFrequency2Lead(MeasurementPointSet::Eighteen) || was_busy == RunningMode::MultiFrequency4Lead(MeasurementPointSet::Eight) || was_busy == RunningMode::MultiFrequency4Lead(MeasurementPointSet::Eighteen) {
         STOP.signal(());
     } 
-    was_busy == RunningMode::SingleFrequency2Lead || was_busy == RunningMode::SingleFrequency4Lead || was_busy == RunningMode::MultiFrequency(MeasurementPointSet::Eight) || was_busy == RunningMode::MultiFrequency(MeasurementPointSet::Eighteen)
+    was_busy == RunningMode::SingleFrequency2Lead || was_busy == RunningMode::SingleFrequency4Lead || was_busy == RunningMode::MultiFrequency2Lead(MeasurementPointSet::Eight) || was_busy == RunningMode::MultiFrequency2Lead(MeasurementPointSet::Eighteen) || was_busy == RunningMode::MultiFrequency4Lead(MeasurementPointSet::Eight) || was_busy == RunningMode::MultiFrequency4Lead(MeasurementPointSet::Eighteen)
 }

src/impedance.rs

@@ -32,9 +32,18 @@ pub enum RunningMode {
     None,
     SingleFrequency2Lead,
     SingleFrequency4Lead,
-    MultiFrequency(MeasurementPointSet),
+    MultiFrequency2Lead(MeasurementPointSet),
+    MultiFrequency4Lead(MeasurementPointSet),
 }
 
+pub enum RtiaCalibrated {
+    None,
+    Single(RtiaCalibrationResult),
+    Vec8(Vec<RtiaCalibrationResult, 8>),
+    Vec18(Vec<RtiaCalibrationResult, 18>),
+}
+
+#[derive(Debug, Clone, Copy, PartialEq)]
 pub struct RtiaCalibrationResult {
     pub magnitude: f32,
     pub phase: f32,
@@ -44,12 +53,12 @@ pub struct ImpedanceSetup {
     ad5940: AD5940,
     dsp_config: Option<DspConfig>,
     pub running_mode: RunningMode,
-    pub rtia_calibrated: Option<RtiaCalibrationResult>,
+    pub rtia_calibrated: RtiaCalibrated,
 }
 
 impl ImpedanceSetup {
     pub fn new(ad5940: AD5940) -> Self {
-        ImpedanceSetup { ad5940, dsp_config: None, running_mode: RunningMode::None, rtia_calibrated: None }
+        ImpedanceSetup { ad5940, dsp_config: None, running_mode: RunningMode::None, rtia_calibrated: RtiaCalibrated::None }
     }
         
     pub async fn init(&mut self) -> Result<(), Error> {
@@ -200,7 +209,51 @@ impl ImpedanceSetup {
         Ok(calibration_result)
     }
 
-    pub async fn init_single_frequency_measurement(&mut self, frequency: u32, dft_number: IcdDftNum) -> Result<f32, ImpedanceInitError> {
+    async fn calibrate_rtia_multiple<const N: usize>(&mut self, number_of_points: MeasurementPointSet) -> Result<Vec<RtiaCalibrationResult, N>, ImpedanceInitError> {
+        let mut results = Vec::<RtiaCalibrationResult, N>::new();
+        
+        // Set DFT number based on the frequency
+        for &frequency in number_of_points.values() {
+            // Determine wait time
+            let selected_dft_num = match frequency {
+                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,
+            };
+
+            let wait_time: u32;
+            
+            if let Some(dsp_config) = &mut self.dsp_config {
+                dsp_config
+                    .dftnum(selected_dft_num);
+                
+                // Update DFTNUM
+                let mut current =  self.ad5940.read_reg(Register::DFTCON).await.unwrap();
+                current = DFTNUM::apply(current, selected_dft_num as u32);
+                self.ad5940.write_reg(Register::DFTCON, current).await.unwrap();
+
+                wait_time = self.ad5940.sequencer_calculate_wait_time(&dsp_config).await.unwrap();
+
+                // Calibrate Rtia at the given frequency
+                let calibration_result = self.calibrate_rtia(frequency, wait_time).await.unwrap();
+
+                results.push(calibration_result).unwrap();
+            } else {
+                error!("DSP configuration not set, cannot calculate wait time");
+                return Err(ImpedanceInitError::DSPNotSet);
+            }
+        }
+
+        Ok(results)
+    }
+
+    pub async fn init_single_frequency_measurement_2_lead(&mut self, frequency: u32, dft_number: IcdDftNum) -> Result<f32, ImpedanceInitError> {
         // Reset FIFO
         self.ad5940.clear_and_enable_fifo().await.unwrap();
 
@@ -306,7 +359,7 @@ impl ImpedanceSetup {
         // Calibrate Rtia
         match self.calibrate_rtia(frequency, wait_time).await {
             Ok(calibration_result) => {
-                self.rtia_calibrated = Some(calibration_result);
+                self.rtia_calibrated = RtiaCalibrated::Single(calibration_result);
             },
             Err(e) => {
                 error!("Rtia calibration failed: {:?}", e);
@@ -376,7 +429,7 @@ impl ImpedanceSetup {
         Ok(sinus_periods_per_dft)
     }
 
-    pub async fn init_multi_frequency_measurement<const N: usize>(&mut self, number_of_points: MeasurementPointSet) -> Result<heapless::Vec<f32, N>, ImpedanceInitError> {
+    pub async fn init_multi_frequency_measurement_2_lead<const N: usize>(&mut self, number_of_points: MeasurementPointSet) -> Result<heapless::Vec<f32, N>, ImpedanceInitError> {
         // Create vector to store the periods per DFT for each frequency
         let mut periods_per_dft_vec = heapless::Vec::<f32, N>::new();
 
@@ -481,6 +534,131 @@ impl ImpedanceSetup {
         Ok(periods_per_dft_vec)
     }
 
+    pub async fn init_multi_frequency_measurement_4_lead<const N: usize>(&mut self, number_of_points: MeasurementPointSet) -> Result<heapless::Vec<f32, N>, ImpedanceInitError> {
+        // Create vector to store the periods per DFT for each frequency
+        let mut periods_per_dft_vec = heapless::Vec::<f32, N>::new();
+
+        // Calibrate Rtia
+        match number_of_points {
+            MeasurementPointSet::Eight => {
+                let results = self.calibrate_rtia_multiple::<8>(number_of_points).await.unwrap();
+                self.rtia_calibrated = RtiaCalibrated::Vec8(results);
+            },
+            MeasurementPointSet::Eighteen => {
+                let results = self.calibrate_rtia_multiple::<18>(number_of_points).await.unwrap();
+                self.rtia_calibrated = RtiaCalibrated::Vec18(results);
+            },
+        }
+
+        // Reset FIFO
+        self.ad5940.clear_and_enable_fifo().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();
+
+        // Configure sequencer
+        self.ad5940.sequencer_enable(true).await;
+
+        // Set DFT number based on the frequency
+        for &frequency in number_of_points.values() {    
+            // Determine wait time
+            let selected_dft_num = match frequency {
+                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,
+            };
+
+            let wait_time: u32;
+            
+            if let Some(dsp_config) = &mut self.dsp_config {
+                dsp_config
+                    .dftnum(selected_dft_num);
+                
+                // Update DFTNUM
+                let mut current =  self.ad5940.read_reg(Register::DFTCON).await.unwrap();
+                current = DFTNUM::apply(current, selected_dft_num as u32);
+                self.ad5940.write_reg(Register::DFTCON, current).await.unwrap();
+
+                wait_time = self.ad5940.sequencer_calculate_wait_time(&dsp_config).await.unwrap();
+                let periods_per_dft = wait_time as f32 / dsp_config.fsys.unwrap() as f32 * frequency as f32;
+                
+                periods_per_dft_vec.push(periods_per_dft).unwrap();
+                info!("{}Hz: Sinus periods per DFT: {}", frequency, periods_per_dft);
+
+            } else {
+                error!("DSP configuration not set, cannot calculate wait time");
+                return Err(ImpedanceInitError::DSPNotSet);
+            }
+            
+            // Configure switches
+            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();
+            
+            // Set frequency    
+            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();
+
+            // Voltage measurement
+            self.dsp_config.as_mut().unwrap()
+                .adc_mux_n(MUXSELN::AIN3)
+                .adc_mux_p(MUXSELP::AIN2);
+            // self.dsp_config.as_mut().unwrap()
+                // .adc_mux_n(MUXSELN::AIN1)
+                // .adc_mux_p(MUXSELP::CE0);
+            self.ad5940.apply_dsp_config(self.dsp_config.as_ref().unwrap()).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;
+
+            // Current measurement
+            self.dsp_config.as_mut().unwrap()
+                .adc_mux_n(MUXSELN::HsTiaNeg)
+                .adc_mux_p(MUXSELP::HsTiaPos);
+            self.ad5940.apply_dsp_config(self.dsp_config.as_ref().unwrap()).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;
+
+        Ok(periods_per_dft_vec)
+    }
+
     pub async fn start_measurement(&mut self) {
         self.ad5940.sequencer_trigger(0).await;
     }
@@ -507,7 +685,7 @@ pub async fn impedance_setup_readout_task(mut pin: ExtiInput<'static>, impedance
         let mut impedance_setup = impedance_setup.lock().await;
 
         // Trigger the sequencer again
-        if impedance_setup.running_mode == RunningMode::SingleFrequency2Lead || impedance_setup.running_mode == RunningMode::SingleFrequency4Lead || impedance_setup.running_mode == RunningMode::MultiFrequency(MeasurementPointSet::Eight) || impedance_setup.running_mode == RunningMode::MultiFrequency(MeasurementPointSet::Eighteen) {
+        if impedance_setup.running_mode == RunningMode::SingleFrequency2Lead || impedance_setup.running_mode == RunningMode::SingleFrequency4Lead || impedance_setup.running_mode == RunningMode::MultiFrequency2Lead(MeasurementPointSet::Eight) || impedance_setup.running_mode == RunningMode::MultiFrequency2Lead(MeasurementPointSet::Eighteen) || impedance_setup.running_mode == RunningMode::MultiFrequency4Lead(MeasurementPointSet::Eight) || impedance_setup.running_mode == RunningMode::MultiFrequency4Lead(MeasurementPointSet::Eighteen) {
             impedance_setup.start_measurement().await;
         }
 
@@ -525,7 +703,7 @@ pub async fn impedance_setup_readout_task(mut pin: ExtiInput<'static>, impedance
 
                     impedance_setup.read_fifo(data.as_mut_slice()).await.unwrap();
 
-                    let result = calculate_impedance(data);
+                    let result = calculate_impedance_2_lead(data);
                     
                     // Log
                     // info!("Impedance: Magnitude = {} Ω, Phase = {} rad", result.magnitude, result.phase);
@@ -543,54 +721,25 @@ pub async fn impedance_setup_readout_task(mut pin: ExtiInput<'static>, impedance
                     let mut data: [u32; 4] = [0; 4];
 
                     impedance_setup.read_fifo(data.as_mut_slice()).await.unwrap();
-
+                    match impedance_setup.rtia_calibrated {
-                    let mut dft_volt = Complex::new(
+                        RtiaCalibrated::Single(ref cal) => {
-                        sign_extend_18bit(data[0]) as f32,
+                            let result = calculate_impedance_4_lead(data, cal);
-                        sign_extend_18bit(data[1]) as f32
-                    );
-
-                    let mut dft_curr = Complex::new(
-                        sign_extend_18bit(data[2]) as f32,
-                        sign_extend_18bit(data[3]) as f32
-                    );
-                    
-                    // Current is measured inverted in rtia
-                    dft_curr.re *= -1.0;
-                    dft_curr.im *= -1.0;
-
-                    // Impedance imaginary part is measured inverted
-                    dft_volt.im *= -1.0;
-                    dft_curr.im *= -1.0;
-
-                    let (volt_mag, volt_phase) = dft_volt.to_polar();
-                    let (curr_mag, curr_phase) = dft_curr.to_polar();
-
-                    // info!("Volt Phase = {} V, Volt Phase = {} rad", volt_phase, curr_phase);
-            
-                    match impedance_setup.rtia_calibrated.as_ref() {
-                        None => {
-                            error!("Rtia not calibrated, cannot compute impedance");
-                            continue;
-                        },
-                        Some(cal) => {
-                            let rtia_mag = cal.magnitude;
-                            let rtia_phase = cal.phase;
-
-                            // Calculate impedance using calibrated Rtia
-                            let magnitude = volt_mag / curr_mag * rtia_mag;
-                            let phase = volt_phase - curr_phase + rtia_phase;
 
                             let data = SingleImpedanceOutput {
-                                magnitude,
+                                magnitude: result.magnitude,
-                                phase
+                                phase: result.phase
                             };
 
                             IMPEDANCE_CHANNEL_SINGLE.try_send(data).ok();
                         }
+                        _ => {
+                            error!("Rtia not (correctly) calibrated, cannot compute impedance");
+                            continue;
+                        },
                     }
                 }
             }
-            RunningMode::MultiFrequency(points) => {
+            RunningMode::MultiFrequency2Lead(points) => {
                 // Each frequency point produces 4 samples (DFT real/imag for Rcal and Rz)
                 let required_count = points.len() * 4;
                 
@@ -611,7 +760,7 @@ pub async fn impedance_setup_readout_task(mut pin: ExtiInput<'static>, impedance
 
                     // Take 4 samples per frequency point
                     for chunk in data_slice.chunks(4) {
-                        let result = calculate_impedance(chunk.try_into().unwrap());
+                        let result = calculate_impedance_2_lead(chunk.try_into().unwrap());
 
                         match points {
                             MeasurementPointSet::Eight => {
@@ -629,6 +778,57 @@ pub async fn impedance_setup_readout_task(mut pin: ExtiInput<'static>, impedance
                 }
                 continue;
             }
+            RunningMode::MultiFrequency4Lead(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(),
+                    };
+                    
+                    match (&points, &impedance_setup.rtia_calibrated) {
+                        (MeasurementPointSet::Eight, RtiaCalibrated::Vec8(cal_vec)) => {
+                            // Take 4 samples per frequency point
+                            for (i, chunk) in data_slice.chunks(4).enumerate() {
+                                let cal = &cal_vec[i];
+                                let result = calculate_impedance_4_lead(chunk.try_into().unwrap(), cal);
+
+                                impedance_output.magnitudes_8.push(result.magnitude).ok();
+                                impedance_output.phases_8.push(result.phase).ok();
+                            }
+                        },
+                        (MeasurementPointSet::Eighteen, RtiaCalibrated::Vec18(cal_vec)) => {
+                            // Take 4 samples per frequency point
+                            for (i, chunk) in data_slice.chunks(4).enumerate() {
+                                let cal = &cal_vec[i];
+                                let result = calculate_impedance_4_lead(chunk.try_into().unwrap(), cal);
+
+                                impedance_output.magnitudes_18.push(result.magnitude).ok();
+                                impedance_output.phases_18.push(result.phase).ok();
+                            }
+                        },
+                        _ => {
+                            error!("Rtia not (correctly) calibrated, cannot compute impedance");
+                            continue;
+                        }
+                    }
+
+                    IMPEDANCE_CHANNEL_MULTI.try_send(impedance_output).ok();
+                }
+                continue;
+              
+            }
         }
     }
 }
@@ -645,7 +845,7 @@ fn sign_extend_18bit(val: u32) -> i32 {
 }
 
 /// Calculate magnitude and phase of Rz using Rcal reference
-pub fn calculate_impedance(data: [u32; 4]) -> ImpedanceResult {
+pub fn calculate_impedance_2_lead(data: [u32; 4]) -> ImpedanceResult {
     let mut dft_rcal = Complex::new(
         sign_extend_18bit(data[0]) as f32, 
         sign_extend_18bit(data[1]) as f32
@@ -675,3 +875,37 @@ pub fn calculate_impedance(data: [u32; 4]) -> ImpedanceResult {
 
     ImpedanceResult { magnitude, phase }
 }
+
+/// Calculate magnitude and phase of Rz using the calibrated Rtia
+pub fn calculate_impedance_4_lead(data: [u32; 4], rtia: &RtiaCalibrationResult) -> ImpedanceResult {
+    let mut dft_volt = Complex::new(
+        sign_extend_18bit(data[0]) as f32, 
+        sign_extend_18bit(data[1]) as f32
+    );
+    let mut dft_curr = Complex::new(
+        sign_extend_18bit(data[2]) as f32, 
+        sign_extend_18bit(data[3]) as f32
+    );
+
+    // Current is measured inverted in rtia
+    dft_curr.re *= -1.0;
+    dft_curr.im *= -1.0;
+
+    // Impedance imaginary part is measured inverted
+    dft_volt.im *= -1.0;
+    dft_curr.im *= -1.0;
+
+    let (volt_mag, volt_phase) = dft_volt.to_polar();
+    let (curr_mag, curr_phase) = dft_curr.to_polar();
+
+    // Calculate impedance using calibrated Rtia
+    let magnitude = volt_mag / curr_mag * rtia.magnitude;
+    let phase = volt_phase - curr_phase + rtia.phase;
+
+    let data = ImpedanceResult {
+        magnitude,
+        phase
+    };
+
+    data
+}