Created new impedance file for impedance_setup.

src/ad5940.rs

@@ -1,6 +1,5 @@
 use defmt::*;
 
-use embassy_embedded_hal::shared_bus::asynch::spi;
 use embassy_stm32::{gpio::Output, mode::Blocking, spi::Spi, spi::Error};
 use embassy_time::Timer;
 
@@ -410,6 +409,56 @@ impl AD5940 {
         }
     }
 
+    pub async fn sequencer_calculate_wait_time(&mut self, config: &DspConfig) -> Option<u32> {
+        let mut wait_time = 0;
+
+        let sinc3_table = [5, 4, 2];
+        let sinc2_table = [22, 44, 89, 178, 267, 533, 640, 667, 800, 889, 1067, 1333];
+
+        match config.dftin {
+            Some(DFTINSEL::Sinc2) => {
+                if let Some(sinc3osr) = config.sinc3osr {
+                    wait_time += sinc3_table[sinc3osr as usize];
+                } else {
+                    return None; // Sinc2 requires sinc3osr to be set
+                };
+
+                if let Some(sinc2osr) = config.sinc2osr {
+                    wait_time += sinc2_table[sinc2osr as usize];
+                } else {
+                    return None; // Sinc2 requires sinc2osr to be set
+                };
+            }
+            Some(DFTINSEL::GainOffset) => {
+                if let Some(sinc3osr) = config.sinc3osr {
+                    wait_time += sinc3_table[sinc3osr as usize];
+                } else {
+                    return None; // Sinc2 requires sinc3osr to be set
+                };
+            }
+            Some(DFTINSEL::AdcRaw) => {
+            }
+            None => {
+                return None;
+            }
+        }
+
+        // Calculate wait time based on DFTNUM
+        if let Some(dftnum) = config.dftnum {
+            let samples_per_dft = 1 << (dftnum as u32 + 2);
+            wait_time *= samples_per_dft;
+        } else {
+            return None; // DFTNUM must be set
+        }
+
+        // When ACLK = 16MHz, ADC samplerate is 800kHz
+        // When ACLK = 32MHz, ADC samplerate is 1.6MHz
+        // --> Always per ADC sample 20 cycles
+        wait_time *= 20;
+
+        Some(wait_time)
+    }
+
     pub async fn apply_switch_config(&mut self, config: SwitchConfig) -> Result<(), Error> {
         // SWCON
         let mut current = self.read_reg(Register::SWCON).await?;
@@ -444,7 +493,7 @@ impl AD5940 {
         Ok(())
     }
 
-    pub async fn apply_dsp_config(&mut self, config: DspConfig) -> Result<(), Error> {
+    pub async fn apply_dsp_config(&mut self, config: &DspConfig) -> Result<(), Error> {
         // ADCCON
         let mut current = self.read_reg(Register::ADCCON).await?;
 
@@ -454,7 +503,7 @@ impl AD5940 {
         }
         if let Some(muxselp) = config.muxselp {
             current &= !(0b11111);
-            current |= muxselp as u32;
+            current |= (muxselp as u32);
         }
 
         self.write_reg(Register::ADCCON, current).await?;
@@ -652,42 +701,6 @@ impl AD5940 {
 
         Ok(())
     }
-
-    pub async fn init_impedance(&mut self) -> Result<(), Error> {
-        // AFECON:
-        self.afecon(
-            AFECON::DACBUFEN
-            | AFECON::DACREFEN
-            | AFECON::SINC2EN
-            | AFECON::TIAEN
-            | AFECON::INAMPEN
-            | AFECON::EXBUFEN
-            | AFECON::DACEN,
-            true,
-        )
-        .await;
-
-        // Set DSP configuration
-        let dsp_config = DspConfig::default()
-            .adc_mux_n(MUXSELN::HsTiaNeg)
-            .adc_mux_p(MUXSELP::HsTiaPos)
-            .ctiacon(CTIACON::C32)
-            .rtiacon(RTIACON::R5k)
-            .sinc3osr(SINC3OSR::R5)
-            .sinc2osr(SINC2OSR::R178)
-            .adcsamplerate(ADCSAMPLERATE::R800Hz)
-            .dftin_sel(DFTINSEL::GainOffset)
-            .dftnum(DFTNUM::Num2048)
-            .hanning(true);
-
-        self.apply_dsp_config(dsp_config).await.unwrap();
-
-        // WGCON: set sinus output
-        let config_wgcon = WGCON::TYPESEL_SIN.bits();
-        self.write_reg(Register::WGCON, config_wgcon).await.unwrap();
-
-        Ok(())
-    }
 }
 
 #[allow(dead_code)]

src/ad5940_registers.rs

@@ -1,5 +1,6 @@
 use bitflags::bitflags;
 
+#[allow(dead_code)]
 pub trait Resettable {
     fn reset() -> Self;
     // fn msk() -> u32;
@@ -106,6 +107,7 @@ impl Resettable for DMUXCON {
 
 #[allow(dead_code)]
 #[repr(u32)]
+#[derive(Copy, Clone)]
 pub enum MUXSELN 
 {
     HsTiaNeg = 0b00001,
@@ -115,6 +117,7 @@ pub enum MUXSELN
 
 #[allow(dead_code)]
 #[repr(u32)]
+#[derive(Copy, Clone)]
 pub enum MUXSELP {
     HsTiaPos = 0b00001,
     AIN1    = 0b00101,
@@ -122,6 +125,7 @@ pub enum MUXSELP {
 
 #[allow(dead_code)]
 #[repr(u32)]
+#[derive(Copy, Clone)]
 pub enum DFTINSEL {
     Sinc2       = 0b00,
     GainOffset  = 0b01,
@@ -136,6 +140,7 @@ impl Resettable for DFTINSEL {
 
 #[allow(dead_code)]
 #[repr(u32)]
+#[derive(Copy, Clone)]
 pub enum DFTNUM {
     Num4       = 0b0000,
     Num8       = 0b0001,
@@ -159,6 +164,7 @@ impl Resettable for DFTNUM {
 }
 
 #[allow(dead_code)]
+#[derive(Copy, Clone)]
 pub enum CTIACON {
     C1 = 0,
     C2 = 1 << 0,
@@ -175,6 +181,7 @@ impl Resettable for CTIACON {
 }
 
 #[allow(dead_code)]
+#[derive(Copy, Clone)]
 pub enum RTIACON {
     R200 = 0b0000,
     R1k  = 0b0001,
@@ -194,6 +201,7 @@ impl Resettable for RTIACON {
 }
 
 #[allow(dead_code)]
+#[derive(Copy, Clone)]
 pub enum SINC3OSR {
     R5 = 0b00,
     R4 = 0b01,
@@ -207,6 +215,7 @@ impl Resettable for SINC3OSR {
 }
 
 #[allow(dead_code)]
+#[derive(Copy, Clone)]
 pub enum SINC2OSR {
     R22 = 0b0000,
     R44 = 0b0001,
@@ -229,8 +238,9 @@ impl Resettable for SINC2OSR {
 }
 
 #[allow(dead_code)]
+#[derive(Copy, Clone)]
 pub enum ADCSAMPLERATE {
-    R800Hz = 1,
+    R800kHz = 1,
     R1_6MHz = 0,
 }
 

src/communication.rs

@@ -1,7 +1,6 @@
 use defmt::info;
 
 use embassy_executor::Spawner;
-use embassy_time::{Ticker, Duration};
 use embassy_sync::mutex::Mutex;
 use embassy_sync::blocking_mutex::raw::{ThreadModeRawMutex, CriticalSectionRawMutex};
 use embassy_sync::signal::Signal;
@@ -20,9 +19,9 @@ use postcard_rpc::{
     },
 };
 
-use bioz_icd_rs::{PingEndpoint, GetUniqueIdEndpoint, SetGreenLedEndpoint, StartImpedanceEndpoint, StopImpedanceEndpoint, StartImpedance, Impedance, ImpedanceTopic, ENDPOINT_LIST, TOPICS_IN_LIST, TOPICS_OUT_LIST};
+use bioz_icd_rs::{PingEndpoint, GetUniqueIdEndpoint, SetGreenLedEndpoint, StartImpedanceEndpoint, StopImpedanceEndpoint, StartImpedance, ImpedanceOutputTopic, ENDPOINT_LIST, TOPICS_IN_LIST, TOPICS_OUT_LIST};
 
-use crate::impedance_test::{ImpedanceTest, IMPEDANCE_TEST, IMPEDANCE_CHANNEL};
+use crate::impedance::{IMPEDANCE_CHANNEL};
 
 // Postcard RPC types
 type AppDriver = usb::Driver<'static, peripherals::USB>;
@@ -38,17 +37,15 @@ static STORAGE: AppStorage = AppStorage::new();
 
 pub struct Context {
     pub unique_id: [u8; 12],
-    pub impedance: &'static Mutex<ThreadModeRawMutex, ImpedanceTest>,
 }
 
 pub struct SpawnCtx {
-    pub impedance: &'static Mutex<ThreadModeRawMutex, ImpedanceTest>,
 }
 
 impl SpawnContext for Context {
     type SpawnCtxt = SpawnCtx;
     fn spawn_ctxt(&mut self) -> Self::SpawnCtxt {
-        SpawnCtx { impedance: &self.impedance }
+        SpawnCtx {}
     }
 }
 
@@ -119,10 +116,8 @@ pub fn init_communication(usb_driver: Driver<'static, peripherals::USB>, spawner
     let pbufs = PBUFS.take();
     let config = usb_config();
 
-    let impedance_ref = IMPEDANCE_TEST.init(embassy_sync::mutex::Mutex::new(ImpedanceTest::new()));
     let context = Context {
         unique_id: *uid::uid(),
-        impedance: impedance_ref,
     };
 
     let (device, tx_impl, rx_impl) = STORAGE.init(usb_driver, config, pbufs.tx_buf.as_mut_slice());
@@ -159,13 +154,14 @@ pub async fn set_green_led_handler(_context: &mut Context, _header: VarHeader, r
     LED_FREQUENCY_SIGNAL.signal(rqst);
 }
 
+static RUNNING: Mutex<ThreadModeRawMutex, bool> = Mutex::new(false);
 static STOP: Mutex<ThreadModeRawMutex, bool> = Mutex::new(false);
 
 #[embassy_executor::task]
-pub async fn start_impedance_handler(context: SpawnCtx, header: VarHeader, rqst: StartImpedance, sender: Sender<AppTx>) {
+pub async fn start_impedance_handler(_context: SpawnCtx, header: VarHeader, rqst: StartImpedance, sender: Sender<AppTx>) {
     info!("Start impedance measurement with rate {:?} Hz.", rqst.update_frequency);
 
-    let mut impedance = context.impedance.lock().await;
+    *RUNNING.lock().await = true;
 
     if sender
         .reply::<StartImpedanceEndpoint>(header.seq_no, &())
@@ -176,28 +172,13 @@ pub async fn start_impedance_handler(context: SpawnCtx, header: VarHeader, rqst:
         return;
     }
 
-    let mut ticker = Ticker::every(Duration::from_hz(rqst.update_frequency.into()));
     let mut seq: u8 = 0;
     while !*STOP.lock().await {
-        // ticker.next().await;
-
-        // impedance.update(rqst.sinus_frequency);
-
-
-        // let msg = Impedance {
-        //     magnitude: impedance.magnitude,
-        //     phase: impedance.phase,
-        // };
 
         let msg = IMPEDANCE_CHANNEL.receive().await;
 
-        // let msg = Impedance {
-            // magnitude: data,
-            // phase: data,
-        // };
-
         if sender
-            .publish::<ImpedanceTopic>(seq.into(), &msg)
+            .publish::<ImpedanceOutputTopic>(seq.into(), &msg)
             .await
             .is_err()
         {
@@ -207,13 +188,14 @@ pub async fn start_impedance_handler(context: SpawnCtx, header: VarHeader, rqst:
         seq = seq.wrapping_add(1);
     }
 
+    *RUNNING.lock().await = false;
     info!("Impedance measurement stopped.");
     *STOP.lock().await = false; 
 }
 
-pub async fn stop_impedance_handler(context: &mut Context, _header: VarHeader, _rqst: ()) -> bool {
+pub async fn stop_impedance_handler(_context: &mut Context, _header: VarHeader, _rqst: ()) -> bool {
     info!("Stop impedance measurement");
-    let was_busy = context.impedance.try_lock().is_err();
+    let was_busy = *RUNNING.lock().await;
     if was_busy {
         *STOP.lock().await = true;
     }

src/impedance.rs

@@ -0,0 +1,134 @@
+use defmt::{info, error};
+
+use embassy_stm32::spi::Error;
+use embassy_sync::blocking_mutex::raw::ThreadModeRawMutex;
+use embassy_sync::channel::Channel;
+
+use crate::ad5940::*;
+use crate::ad5940_registers::*;
+
+use bioz_icd_rs::ImpedanceOutput;
+
+pub static IMPEDANCE_CHANNEL: Channel<ThreadModeRawMutex, ImpedanceOutput, 2000> = Channel::new();
+
+pub struct ImpedanceSetup {
+    ad5940: AD5940,
+}
+
+impl ImpedanceSetup {
+    pub fn new(ad5940: AD5940) -> Self {
+        ImpedanceSetup { ad5940 }
+    }
+        
+    pub async fn init(&mut self) -> Result<(), Error> {
+        // AFECON:
+        self.ad5940.afecon(
+            AFECON::DACBUFEN
+            | AFECON::DACREFEN
+            | AFECON::SINC2EN
+            | AFECON::TIAEN
+            | AFECON::INAMPEN
+            | AFECON::EXBUFEN
+            | AFECON::DACEN,
+            true,
+        )
+        .await;
+
+        // Set DSP configuration
+        let dsp_config = DspConfig::default()
+            .adc_mux_n(MUXSELN::HsTiaNeg)
+            .adc_mux_p(MUXSELP::HsTiaPos)
+            .ctiacon(CTIACON::C32)
+            .rtiacon(RTIACON::R5k)
+            .sinc3osr(SINC3OSR::R5)
+            .sinc2osr(SINC2OSR::R178)
+            .adcsamplerate(ADCSAMPLERATE::R800kHz)
+            .dftin_sel(DFTINSEL::GainOffset)
+            .dftnum(DFTNUM::Num2048)
+            .hanning(true);
+
+        self.ad5940.apply_dsp_config(&dsp_config).await.unwrap();
+
+        let test = self.ad5940.sequencer_calculate_wait_time(&dsp_config).await.unwrap();
+        info!("Wait time: {:?}", test);
+
+
+        // WGCON: set sinus output
+        let config_wgcon = WGCON::TYPESEL_SIN.bits();
+        self.ad5940.write_reg(Register::WGCON, config_wgcon).await.unwrap();
+
+        Ok(())
+    }
+
+    pub async fn init_measurement(&mut self) {
+        // 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;
+
+        self.ad5940.wgfcw(50000).await;
+        let wg_amplitude = 557; // 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
+        self.ad5940.afecon(AFECON::ADCCONVEN | AFECON::DFTEN, true).await;
+        // self.ad5940.sequencer_wait(16 * 102_400).await; // 0.75 second // 0,0512
+        self.ad5940.sequencer_wait(16 * 12_800).await; // 0.75 second // 0,0512
+        self.ad5940.sequencer_wait(16*20).await; // 0.75 second // 0,0512
+        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
+        self.ad5940.afecon(AFECON::ADCCONVEN | AFECON::DFTEN, true).await;
+        // self.ad5940.sequencer_wait(16 * 102_400).await; // 0.75 second
+        self.ad5940.sequencer_wait(16 * 12_800).await; // 0.75 second // 0,0512
+        self.ad5940.sequencer_wait(16*20).await; // 0.75 second // 0,0512
+        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; // 0.025 second
+        self.ad5940.write_reg(Register::SYNCEXTDEVICE, 0b111).await.unwrap();
+
+        self.ad5940.sequencer_enable(false).await;
+
+        // // Configure the sequencer cmd data sram
+        self.ad5940.cmddatacon().await;
+
+        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;
+    }
+
+    pub async fn start_measurement(&mut self) {
+        self.ad5940.sequencer_trigger(0).await;
+    }
+
+    pub async fn get_fifo_count(&mut self) ->  Result<u32, Error> {
+        self.ad5940.get_fifo_count().await
+    }
+
+    pub async fn read_fifo(&mut self, data: &mut [u32]) -> Result<(), Error> {
+        self.ad5940.read_fifo(data).await
+    }
+}

src/impedance_test.rs

@@ -1,36 +0,0 @@
-use bioz_icd_rs::Impedance;
-use embassy_sync::mutex::Mutex;
-use embassy_sync::blocking_mutex::raw::ThreadModeRawMutex;
-use embassy_sync::channel::Channel;
-
-use static_cell::StaticCell;
-
-use libm::{sinf, cosf};
-use core::f32::consts::PI;
-
-pub static IMPEDANCE_TEST: StaticCell<Mutex<ThreadModeRawMutex, ImpedanceTest>> = StaticCell::new();
-pub static IMPEDANCE_CHANNEL: Channel<ThreadModeRawMutex, Impedance, 2000> = Channel::new();
-pub struct ImpedanceTest {
-    time: embassy_time::Instant,
-    pub magnitude: f32,
-    pub phase: f32,
-}
-
-impl ImpedanceTest {
-    pub fn new() -> Self {
-        ImpedanceTest {
-            time: embassy_time::Instant::now(),
-            magnitude: 1.0,
-            phase: 0.0,
-        }
-    }
-
-    pub fn update(&mut self, frequency: f32) {
-        // let frequency = 1.0; // Hz, adjust as needed
-
-        let current_time = self.time.elapsed().as_millis() as f32 / 1000.0; // Convert to seconds
-        self.magnitude = cosf(2.0 * PI * frequency * current_time);
-        self.phase = sinf(2.0 * PI * frequency * current_time);
-    }
-}
-

src/main.rs

@@ -1,7 +1,7 @@
 #![no_std]
 #![no_main]
 
-use defmt::{info, error};
+use defmt::info;
 use embassy_executor::Spawner;
 use embassy_stm32::exti::ExtiInput;
 use embassy_time::{Timer, Duration};
@@ -12,10 +12,10 @@ use embassy_stm32::time::Hertz;
 
 use {defmt_rtt as _, panic_probe as _};
 
-use crate::ad5940::*;
+// use crate::ad5940::*;
-use crate::ad5940_registers::*;
+// use crate::ad5940_registers::*;
 
-use bioz_icd_rs::Impedance;
+use bioz_icd_rs::ImpedanceOutput;
 
 mod ad5940;
 use ad5940::AD5940;
@@ -34,9 +34,10 @@ use embassy_stm32::{bind_interrupts, peripherals, usb};
 mod communication;
 use communication::{init_communication, LED_FREQUENCY_SIGNAL};
 
-mod impedance_test;
+use impedance::IMPEDANCE_CHANNEL;
 
-use impedance_test::IMPEDANCE_CHANNEL;
+mod impedance;
+use impedance::ImpedanceSetup;
 
 bind_interrupts!(struct Irqs {
     USB_DRD_FS => usb::InterruptHandler<peripherals::USB>;
@@ -90,7 +91,9 @@ async fn main(spawner: Spawner) {
 
     // ad5940.init_temperature().await.unwrap();
     // ad5940.init_waveform().await.unwrap();
-    ad5940.init_impedance().await.unwrap();
+    let mut impedance_setup = ImpedanceSetup::new(ad5940);
+    impedance_setup.init().await.unwrap();
+    impedance_setup.init_measurement().await;
 
     // // Set up I2C for ADG2128
     // let i2c = i2c::I2c::new_blocking(
@@ -112,63 +115,6 @@ async fn main(spawner: Spawner) {
     // ad5940.write_reg_raw(0x0000_0004, 1 << 1).await.unwrap(); 
     // ad5940.write_reg_raw(0x0000_001C, 1 << 1).await.unwrap(); 
 
-    ad5940.write_reg(ad5940::Register::GP0CON, 0b10 << 4 | 0b10 << 2 | 0b10).await.unwrap();
-    ad5940.write_reg(ad5940::Register::SYNCEXTDEVICE, 0b111).await.unwrap();
-
-    // Sequencer test
-    ad5940.sequencer_enable(true).await;
-
-    ad5940.wgfcw(50000).await;
-    let wg_amplitude = 2047; // 2047 is the maximum amplitude for a 12-bit DAC --> 1.62V peak-to-peak
-    ad5940.write_reg(ad5940::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);
-    ad5940.apply_switch_config(switch_config).await.unwrap();
-    ad5940.afecon(AFECON::WAVEGENEN | AFECON::ADCEN, true).await;
-    ad5940.sequencer_wait(16*10).await; // 10 us
-    ad5940.afecon(AFECON::ADCCONVEN | AFECON::DFTEN, true).await;
-    // ad5940.sequencer_wait(16 * 102_400).await; // 0.75 second // 0,0512
-    ad5940.sequencer_wait(16 * 12_800).await; // 0.75 second // 0,0512
-    ad5940.sequencer_wait(16*20).await; // 0.75 second // 0,0512
-    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);
-    ad5940.apply_switch_config(switch_config).await.unwrap();
-    ad5940.afecon(AFECON::WAVEGENEN | AFECON::ADCEN, true).await;
-    ad5940.sequencer_wait(16*10).await; // 10 us
-    ad5940.afecon(AFECON::ADCCONVEN | AFECON::DFTEN, true).await;
-    // ad5940.sequencer_wait(16 * 102_400).await; // 0.75 second
-    ad5940.sequencer_wait(16 * 12_800).await; // 0.75 second // 0,0512
-    ad5940.sequencer_wait(16*20).await; // 0.75 second // 0,0512
-    ad5940.afecon(AFECON::WAVEGENEN | AFECON:: ADCEN | AFECON::ADCCONVEN | AFECON::DFTEN, false).await;
-
-    // Toggle leds
-    ad5940.write_reg(ad5940::Register::SYNCEXTDEVICE, 0b010).await.unwrap();
-    ad5940.sequencer_wait(16 * 1_000).await; // 0.025 second
-    ad5940.write_reg(ad5940::Register::SYNCEXTDEVICE, 0b111).await.unwrap();
-
-    ad5940.sequencer_enable(false).await;
-
-    // // Configure the sequencer cmd data sram
-    ad5940.cmddatacon().await;
-
-    let start_address = 0;
-    ad5940.sequencer_cmd_write(start_address).await;
-
-    ad5940.sequencer_info_configure(0, ad5940.seq_len, start_address).await;
-
     // Create USB driver and start postcard-rpc server
     let driver = Driver::new(p.USB, Irqs, p.PA12, p.PA11);
     init_communication(driver, spawner);
@@ -177,11 +123,11 @@ async fn main(spawner: Spawner) {
     // spawner.must_spawn(green_led(led));
 
     // Trigger the sequencer
-    ad5940.sequencer_trigger(0).await;
+    impedance_setup.start_measurement().await;
 
     // Set up interrupt at GPIO for AD5940
     let ad5940_gpio_0 = ExtiInput::new(p.PC8, p.EXTI8, embassy_stm32::gpio::Pull::Up);
-    spawner.must_spawn(ad5940_readout_task(ad5940_gpio_0, ad5940));
+    spawner.must_spawn(impedance_setup_readout_task(ad5940_gpio_0, impedance_setup));
 
     loop {
         // Read chip id
@@ -223,29 +169,30 @@ async fn green_led(mut led: Output<'static>) {
 }
 
 #[embassy_executor::task]
-async fn ad5940_readout_task(mut pin: ExtiInput<'static>, mut ad5940: AD5940) {
+async fn impedance_setup_readout_task(mut pin: ExtiInput<'static>, mut impedance_setup: ImpedanceSetup) {
     loop {
         // Wait untill sequence is done
         pin.wait_for_rising_edge().await;
 
-        // Trigger the sequencer agina
+        // Trigger the sequencer again
-        ad5940.sequencer_trigger(0).await;
+        impedance_setup.start_measurement().await;
 
         // Read the FIFO count
-        let count = ad5940.get_fifo_count().await.unwrap();
+        let count = impedance_setup.get_fifo_count().await.unwrap();
         // info!("FIFOCNTSTA: {}", count);
 
         if count >= 4 {
             let mut data: [u32; 4] = [0; 4];
 
-            ad5940.read_fifo(data.as_mut_slice()).await.unwrap();
+            impedance_setup.read_fifo(data.as_mut_slice()).await.unwrap();
 
             let result = calculate_impedance(data);
 
-            // You’ll need to implement your own logging or send this over serial in embedded
-            info!("Impedance: Magnitude = {} Ω, Phase = {} rad", result.magnitude, result.phase);
             
-            let data = Impedance {
+            // Log
+            // info!("Impedance: Magnitude = {} Ω, Phase = {} rad", result.magnitude, result.phase);
+
+            let data = ImpedanceOutput {
                 magnitude: result.magnitude,
                 phase: result.phase,
             };