diff --git a/src/ad5940.rs b/src/ad5940.rs
index eddd74c..6dbab8f 100644
--- a/src/ad5940.rs
+++ b/src/ad5940.rs
@@ -127,6 +127,77 @@ impl AD5940 {
         Ok(())
     }
 
+    pub async fn afecon(&mut self, ctr: ConfigurationRegister, state: bool) {
+        let reg = self.read_reg_32(Register::AFECON).await.unwrap();
+        let mut reg = ConfigurationRegister::from_bits_truncate(reg);
+
+        let flags = [
+            ConfigurationRegister::DACBUFEN,
+            ConfigurationRegister::DACREFEN,
+            ConfigurationRegister::SINC2EN,
+            ConfigurationRegister::DFTEN,
+            ConfigurationRegister::WAVEGENEN,
+            ConfigurationRegister::TEMPCONVEN,
+            ConfigurationRegister::TEMPSENSEN,
+            ConfigurationRegister::TIAEN,
+            ConfigurationRegister::INAMPEN,
+            ConfigurationRegister::EXBUFEN,
+            ConfigurationRegister::ADCCONVEN,
+            ConfigurationRegister::ADCEN,
+            ConfigurationRegister::DACEN,
+            ConfigurationRegister::HSREFDIS,
+        ];
+
+        for &flag in flags.iter() {
+            if ctr.contains(flag) {
+                reg = if state {
+                    reg | flag
+                } else {
+                    reg & !flag
+                };
+            }
+        }
+
+        self.write_reg_32(Register::AFECON, reg.bits()).await.unwrap();
+    }
+
+    pub async fn swcon(&mut self, ctr: SwitchMatrixConfigurationRegister) {
+        let reg = self.read_reg_32(Register::SWCON).await.unwrap();
+        let mut reg = SwitchMatrixConfigurationRegister::from_bits_truncate(reg);
+
+        // NMUXCON
+        if (ctr & SwitchMatrixConfigurationRegister::NMUXCON_MSK) == SwitchMatrixConfigurationRegister::NMUXCON_N2 {
+            reg &= !SwitchMatrixConfigurationRegister::NMUXCON_MSK;
+            reg |= SwitchMatrixConfigurationRegister::NMUXCON_N2;
+        } else if (ctr & SwitchMatrixConfigurationRegister::NMUXCON_MSK) == SwitchMatrixConfigurationRegister::NMUXCON_N5 {
+            reg &= !SwitchMatrixConfigurationRegister::NMUXCON_MSK;
+            reg |= SwitchMatrixConfigurationRegister::NMUXCON_N5;
+        };
+
+        // PMUXCON
+        if (ctr & SwitchMatrixConfigurationRegister::PMUXCON_MSK) == SwitchMatrixConfigurationRegister::PMUXCON_P2 {
+            reg &= !SwitchMatrixConfigurationRegister::PMUXCON_MSK;
+            reg |= SwitchMatrixConfigurationRegister::PMUXCON_P2;
+        } else if (ctr & SwitchMatrixConfigurationRegister::PMUXCON_MSK) == SwitchMatrixConfigurationRegister::PMUXCON_P11 {
+            reg &= !SwitchMatrixConfigurationRegister::PMUXCON_MSK;
+            reg |= SwitchMatrixConfigurationRegister::PMUXCON_P11;
+        }
+
+        // DMUXCON
+        if ctr.contains(SwitchMatrixConfigurationRegister::DMUXCON_D5) {
+            reg &= !SwitchMatrixConfigurationRegister::DMUXCON_MSK;
+            reg |= SwitchMatrixConfigurationRegister::DMUXCON_D5;
+        }
+
+        self.write_reg_32(Register::SWCON, reg.bits()).await.unwrap();
+    }
+
+    pub async fn wgfcw(&mut self, frequency: u32) {
+        let sinefcw = (frequency as f64) * (1_073_741_824.0 / 16_000_000.0);
+        let sinefcw = 0x00FFFFFF & sinefcw as u32;
+        self.write_reg_32(Register::WGFCW, sinefcw).await.unwrap();
+    }
+
     pub async fn get_chipid(&mut self) -> Result<u16, Error> {
         self.read_reg_16(Register::CHIPID).await
     }
@@ -137,17 +208,14 @@ impl AD5940 {
 
     pub async fn init_temperature(&mut self) -> Result<(), Error> {
         // AFECON:
-        let mut config_afecon = ConfigurationRegister::reset.bits() | ConfigurationRegister::TEMPSENSEN.bits() | ConfigurationRegister::ADCEN.bits();
-        self.write_reg_32(Register::AFECON, config_afecon).await?;
+        self.afecon(ConfigurationRegister::TEMPSENSEN | ConfigurationRegister::ADCEN, true).await;
 
         // ADCCON
         let config_adccon = ADCConfigurationRegister::GNPGA_1_5.bits() | ADCConfigurationRegister::MUXSELN_TEMP.bits() | ADCConfigurationRegister::MUXSELP_TEMP.bits();
         self.write_reg_32(Register::ADCCON, config_adccon).await?;
 
         // AFECON - start conversion
-        config_afecon |= ConfigurationRegister::TEMPCONVEN.bits();
-        config_afecon |= ConfigurationRegister::ADCCONVEN.bits();
-        self.write_reg_32(Register::AFECON, config_afecon).await?;
+        self.afecon(ConfigurationRegister::TEMPCONVEN | ConfigurationRegister::ADCCONVEN, true).await;
 
         Ok(())
     }
@@ -165,13 +233,11 @@ impl AD5940 {
 
     pub async fn init_waveform(&mut self) -> Result<(), Error> {
         // Set frequency
-        let freq: u32 = 2000;
-        let sinefcw = (freq as f64) * (1_073_741_824.0 / 16_000_000.0);
-        let sinefcw = 0x00FFFFFF & sinefcw as u32;
-        self.write_reg_32(Register::WGFCW, sinefcw).await?;
+        let freq: u32 = 1000;
+        self.wgfcw(freq).await;
 
         // Init amplitude
-        let wg_amplitude = 1279;
+        let wg_amplitude = 2047; // 2047 is the maximum amplitude for a 12-bit DAC --> 1.62V peak-to-peak
         self.write_reg_32(Register::WGAMPLITUDE, wg_amplitude).await?;
 
         // WGCON, enable waveform generator and set to sinusoidal, NO DACCAL
@@ -179,22 +245,21 @@ impl AD5940 {
         self.write_reg_32(Register::WGCON, config_wgcon).await?;
 
         // SWCON - set up switch matrix
-        let mut swcon = SwitchMatrixConfigurationRegister::reset.bits();
-
-        swcon &= !SwitchMatrixConfigurationRegister::NMUXCON_MSK.bits();
-        swcon |= SwitchMatrixConfigurationRegister::NMUXCON_N2.bits();
-
-        swcon &= !SwitchMatrixConfigurationRegister::PMUXCON_MSK.bits();
-        swcon |= SwitchMatrixConfigurationRegister::PMUXCON_P11.bits();
-
-        swcon &= !SwitchMatrixConfigurationRegister::DMUXCON_MSK.bits();
-        swcon |= SwitchMatrixConfigurationRegister::DMUXCON_D5.bits();
-
-        self.write_reg_32(Register::SWCON, swcon).await?;
+        self.swcon(
+            SwitchMatrixConfigurationRegister::NMUXCON_N2
+            | SwitchMatrixConfigurationRegister::PMUXCON_P11
+            | SwitchMatrixConfigurationRegister::DMUXCON_D5).await;
 
         // AFECON:
-        let config_afecon = ConfigurationRegister::reset.bits() | ConfigurationRegister::DACREFEN.bits() | ConfigurationRegister::EXBUFEN.bits() | ConfigurationRegister::INAMPEN.bits() | ConfigurationRegister::DACEN.bits() | ConfigurationRegister::WAVEGENEN.bits();
-        self.write_reg_32(Register::AFECON, config_afecon).await?;
+        self.afecon(
+            ConfigurationRegister::DACREFEN
+            | ConfigurationRegister::EXBUFEN
+            | ConfigurationRegister::INAMPEN
+            | ConfigurationRegister::DACEN
+            | ConfigurationRegister::WAVEGENEN,
+            true,
+        )
+        .await;
 
         Ok(())
     }
diff --git a/src/ad5940_registers.rs b/src/ad5940_registers.rs
index a56e5bb..64fc7dc 100644
--- a/src/ad5940_registers.rs
+++ b/src/ad5940_registers.rs
@@ -2,8 +2,8 @@ use bitflags::bitflags;
 
 bitflags! {
     // Address 0x00002000, Reset: 0x00080000, Name: AFECON
+    #[derive(Clone, Copy)]
     pub struct ConfigurationRegister: u32 {
-        const reset = 0x00080000;
         const DACBUFEN = 1 << 21;
         const DACREFEN = 1 << 20;
         const SINC2EN = 1 << 16;
@@ -23,8 +23,9 @@ bitflags! {
 
 bitflags! {
     // Address 0x0000200C, Reset: 0x00000000, Name: SWCON
+    #[derive(Clone, Copy)]
+    #[derive(PartialEq)]
     pub struct SwitchMatrixConfigurationRegister: u32 {
-        const reset         = 0x0000_FFFF;
         const NMUXCON_MSK   = 0b1111 << 8;
         const NMUXCON_N2    = 0b0010 << 8; // N2 switch
         const NMUXCON_N5    = 0b0101 << 8; // N5 switch