diff --git a/src/ad5940.rs b/src/ad5940.rs
index 09fe65b..6f8f74a 100644
--- a/src/ad5940.rs
+++ b/src/ad5940.rs
@@ -765,6 +765,17 @@ impl AD5940 {
         Ok((data >> 16) & 0x7FF)  
     }
 
+    pub async fn clear_and_enable_fifo(&mut self) -> Result<(), Error> {
+        let mut fifocon = self.read_reg(Register::FIFOCON).await?;
+        fifocon = DATAFIFOEN::apply(fifocon, DATAFIFOEN::FIFOisReset as u32); // Disable FIFO
+        self.write_reg(Register::FIFOCON, fifocon).await?;
+
+        fifocon = DATAFIFOEN::apply(fifocon, DATAFIFOEN::Normal as u32); // Enable FIFO
+        self.write_reg(Register::FIFOCON, fifocon).await?;
+
+        Ok(())
+    }
+
     pub async fn init_waveform(&mut self) -> Result<(), Error> {
         // Set frequency
         let freq: u32 = 1000;
diff --git a/src/communication.rs b/src/communication.rs
index 47e42ee..8077934 100644
--- a/src/communication.rs
+++ b/src/communication.rs
@@ -171,12 +171,12 @@ pub async fn start_single_impedance_handler(context: SpawnCtx, header: VarHeader
     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;
+    let init_impedance_result = context.impedance_setup.lock().await.init_single_frequency_measurement(rqst.sinus_frequency, rqst.dft_number).await;
     // Trigger the sequencer
     context.impedance_setup.lock().await.start_measurement().await;
 
     if sender
-        .reply::<StartSingleImpedanceEndpoint>(header.seq_no, &())
+        .reply::<StartSingleImpedanceEndpoint>(header.seq_no, &init_impedance_result)
         .await
         .is_err()
     {
diff --git a/src/impedance.rs b/src/impedance.rs
index a3b2a1a..66c2da6 100644
--- a/src/impedance.rs
+++ b/src/impedance.rs
@@ -12,7 +12,7 @@ use static_cell::StaticCell;
 use crate::ad5940::*;
 use crate::ad5940_registers::*;
 
-use bioz_icd_rs::{SingleImpedanceOutput, IcdDftNum};
+use bioz_icd_rs::{SingleImpedanceOutput, IcdDftNum, InitImpedanceError};
 use crate::icd_mapping::IntoDftnum;
 
 pub static IMPEDANCE_CHANNEL_SINGLE: Channel<ThreadModeRawMutex, SingleImpedanceOutput, 2000> = Channel::new();
@@ -96,7 +96,10 @@ impl ImpedanceSetup {
         Ok(())
     }
 
-    pub async fn init_single_frequency_measurement(&mut self, frequency: u32, dft_number: IcdDftNum) {
+    pub async fn init_single_frequency_measurement(&mut self, frequency: u32, dft_number: IcdDftNum) -> Result<f32, InitImpedanceError> {
+        // Reset FIFO
+        self.ad5940.clear_and_enable_fifo().await.unwrap();
+
         // 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();
@@ -106,12 +109,15 @@ impl ImpedanceSetup {
         self.ad5940.write_reg(Register::SYNCEXTDEVICE, 0b111).await.unwrap();
 
         // Calculate wait time between measurement start and stop
-        let mut wait_time = 0;
+        let wait_time;
+        let sinus_periods_per_dft;
         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);
+            sinus_periods_per_dft = wait_time as f32 / dsp_config.fsys.unwrap() as f32 * frequency as f32;
+            info!("Sinus periods per DFT: {}", sinus_periods_per_dft);
         } else {
             error!("DSP configuration not set, cannot calculate wait time");
+            return Err(InitImpedanceError::DSPNotSet);
         }
 
         // Configure sequencer
@@ -165,9 +171,14 @@ impl ImpedanceSetup {
         self.ad5940.sequencer_info_configure(0, self.ad5940.seq_len, start_address).await;
 
         self.start_measurement().await;
+
+        Ok(sinus_periods_per_dft)
     }
 
     pub async fn init_multi_frequency_measurement(&mut self, dft_number: IcdDftNum, number_of_points: NumberOfPoints) {
+        // Reset FIFO
+        self.ad5940.clear_and_enable_fifo().await.unwrap();
+
         // 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();
diff --git a/src/main.rs b/src/main.rs
index 4fadff8..558168c 100644
--- a/src/main.rs
+++ b/src/main.rs
@@ -1,7 +1,7 @@
 #![no_std]
 #![no_main]
 
-use defmt::info;
+use defmt::{info, warn};
 use embassy_executor::Spawner;
 use embassy_stm32::exti::ExtiInput;
 use embassy_sync::mutex::Mutex;
@@ -191,7 +191,7 @@ async fn impedance_setup_readout_task(mut pin: ExtiInput<'static>, impedance_set
 
         // Read the FIFO count
         let count = impedance_setup.get_fifo_count().await.unwrap();
-        info!("FIFOCNTSTA: {}", count);
+        // info!("FIFOCNTSTA: {}", count);
 
         match impedance_setup.running_mode {
             impedance::RunningMode::None => {
@@ -229,14 +229,20 @@ async fn impedance_setup_readout_task(mut pin: ExtiInput<'static>, impedance_set
 
                     // 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]]);
+                        let result = calculate_impedance(chunk.try_into().unwrap());
                         // 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);
+                    // for mags in impedance_output.magnitudes.iter() {
+                        // info!("Mag: {}", mags);
+                    // }
+
+                    // for phases in impedance_output.phases.iter() {
+                        // info!("Phase: {}", phases);
+                    // }
 
                     IMPEDANCE_CHANNEL_MULTI.try_send(impedance_output).ok();
                 }