diff --git a/firmware/command_ctrl.ino b/firmware/command_ctrl.ino
index 912035e..f5d304c 100644
--- a/firmware/command_ctrl.ino
+++ b/firmware/command_ctrl.ino
@@ -228,7 +228,8 @@ void cc_startMeasurement()
 
     if (freq < 8000)
     {
-      setWaveformFrequency((int)freq);
+      setWaveformFrequency(freq);
+      setWaveform(WAVEFORM_SINUS);
       enableWaveformOutput();
       si5351.output_enable(SI5351_CLK0, 0); // disable clock output 0
     } else {
@@ -305,7 +306,7 @@ void cc_enableClk(void)
   {
     if (start_freq < 8000)
     {
-      setWaveformFrequency((int)start_freq);
+      setWaveformFrequency(start_freq);
       enableWaveformOutput();
       si5351.output_enable(SI5351_CLK0, 0);
     } else {
diff --git a/firmware/firmware.ino b/firmware/firmware.ino
index 2d45e85..a3555f7 100644
--- a/firmware/firmware.ino
+++ b/firmware/firmware.ino
@@ -13,6 +13,7 @@
 
 extern "C" {
   #include "globals.h"
+  #include "waveforms.h"
 }
 
 /*****************************************************************************/
@@ -95,7 +96,8 @@ void loop()
   pollWaveformGenerator();
 
   //delay(100);
-  delay(10);
+  //delay(10);
+  delayMicroseconds(MAIN_LOOP_DELAY_US);
 }
 
 /*****************************************************************************/
diff --git a/firmware/globals.h b/firmware/globals.h
index c279220..3b8a4fc 100644
--- a/firmware/globals.h
+++ b/firmware/globals.h
@@ -3,6 +3,8 @@
 
 #define MEAS_LOOP_CNT 20
 
+#define MAIN_LOOP_DELAY_US 63 // 1/63us = ~16kHz
+
 /*****************************************************************************/
 
 /* Message byte definitions: */
diff --git a/firmware/waveformgenerator.ino b/firmware/waveformgenerator.ino
index 74f5f02..bce820e 100644
--- a/firmware/waveformgenerator.ino
+++ b/firmware/waveformgenerator.ino
@@ -1,41 +1,46 @@
 
 #include "Waveforms.h"
 
-#define PWM_BIT_WIDTH 10
-#define PWM_PIN 5 // PWM-Pin für DAC0 auf dem Arduino Nano 328
+#define PWM_BIT_WIDTH 8
+#define PWM_PIN       5 // PWM-Pin für DAC0 auf dem Arduino Nano 328
 
-int wf_wave0 = 0;
-int wf_pos = 0;
-int wf_sample;
+uint8_t  wf_wave0 = 0;
+uint8_t  wf_pos   = 0;
+uint16_t wf_freq  = 0;
+
+unsigned long wf_sample_us  = 0;
+unsigned long wf_prevMicros = 0;
 
 bool wf_outputEnabled = true;  // Variable to control waveform output state
 
 void initWaveformGenerator()
 {
+  pinMode(PWM_PIN, OUTPUT);
   setWaveform(WAVEFORM_SINUS);
   // Call the function to set default frequency, here you might want to specify a default frequency
-  setWaveformFrequency(WAVEFORM_DEFAULT_FREQ_HZ); 
+  setWaveformFrequency(WAVEFORM_DEFAULT_FREQ_HZ);
 }
 
-void setWaveform(int waveform0)
+void setWaveform(uint8_t waveform0)
 {
   if (waveform0 >= 0 and waveform0 < WAVEFORM_MAXWAVEFORM_NUM)
   {
     // Set default waveforms
     wf_wave0 = waveform0;
+  } else {
+    wf_wave0 = WAVEFORM_SINUS;
   }
 }
 
-void setWaveformFrequency(int frequency)
+void setWaveformFrequency(uint16_t frequency)
 {
   if (frequency >= 1 and frequency < 8000)
   {
-    wf_sample = map(frequency, 0, (2^PWM_BIT_WIDTH)-1, 0, WAVEFORM_ONE_HZ_SAMPLE);
-    wf_sample = constrain(wf_sample, 0, WAVEFORM_ONE_HZ_SAMPLE);
+    wf_freq = frequency;
   } else {
-    wf_sample = map(WAVEFORM_DEFAULT_FREQ_HZ, 0, (2^PWM_BIT_WIDTH)-1, 0, WAVEFORM_ONE_HZ_SAMPLE);
-    wf_sample = constrain(wf_sample, 0, WAVEFORM_ONE_HZ_SAMPLE);
+    wf_freq = WAVEFORM_DEFAULT_FREQ_HZ;
   }
+  wf_sample_us = 1000000UL / ((unsigned long)wf_freq * WAVEFORM_MAX_SAMPLES_NUM);
 }
 
 void enableWaveformOutput()
@@ -50,14 +55,25 @@ void disableWaveformOutput()
 
 void pollWaveformGenerator()
 {
+
   if (wf_outputEnabled)
   {
-    analogWrite(PWM_PIN, waveformsTable[wf_wave0][wf_pos]);  // write the selected waveform on DAC0
-    
-    wf_pos++;
-    if (wf_pos == WAVEFORM_MAX_SAMPLES_NUM)  // Reset the counter to repeat the wave
-      wf_pos = 0;
-    
-    delayMicroseconds(wf_sample);  // Hold the sample value for the sample time
+    unsigned long currentMicros = micros();  // Aktuelle Zeit abrufen
+    if (currentMicros - wf_prevMicros >= wf_sample_us)
+    {
+        wf_prevMicros = currentMicros;
+
+        uint16_t sample = map(waveformsTable[wf_wave0][wf_pos], 0, 0xfff, 0, (1 << PWM_BIT_WIDTH)-1);
+        sample = constrain(sample, 0, (1 << PWM_BIT_WIDTH)-1);
+        analogWrite(PWM_PIN, sample);  // write the selected waveform on DAC0
+        // analogWrite(PWM_PIN, 128);  // write the selected waveform on DAC0
+
+        wf_pos++;
+        if (wf_pos == WAVEFORM_MAX_SAMPLES_NUM)  // Reset the counter to repeat the wave
+            wf_pos = 0;
+
+    }
+  } else {
+        analogWrite(PWM_PIN, 0);
   }
 }