From 69ac7c3f8a1a6a7b4483069129c600d216a6519f Mon Sep 17 00:00:00 2001
From: Kai Lauterbach <klaute@gmx.de>
Date: Mon, 12 Feb 2024 11:37:25 +0100
Subject: [PATCH] Added a function which allowes to set the PWM frequency

---
 firmware/command_ctrl.ino      |  2 +-
 firmware/waveformgenerator.ino | 60 ++++++++++++++++++++++++++++++++++
 2 files changed, 61 insertions(+), 1 deletion(-)

diff --git a/firmware/command_ctrl.ino b/firmware/command_ctrl.ino
index 29bc208..bc73bbb 100644
--- a/firmware/command_ctrl.ino
+++ b/firmware/command_ctrl.ino
@@ -252,7 +252,7 @@ void cc_startMeasurement()
       enableWaveformOutput();
       pollWaveformGenerator(); // manually poll the waveformgenerator
       si5351.output_enable(SI5351_CLK0, 0); // disable clock output 0
-      delay(1);
+      //delay(1);
     } else {
       disableWaveformOutput();
       si5351.set_freq((uint64_t)freq * 100, SI5351_PLL_FIXED, SI5351_CLK0);
diff --git a/firmware/waveformgenerator.ino b/firmware/waveformgenerator.ino
index 2f26d3e..ac84ac6 100644
--- a/firmware/waveformgenerator.ino
+++ b/firmware/waveformgenerator.ino
@@ -42,6 +42,11 @@ void setWaveformFrequency(uint16_t frequency)
     wf_freq = WAVEFORM_DEFAULT_FREQ_HZ;
   }
   wf_sample_us = 1000000UL / ((unsigned long)wf_freq * WAVEFORM_MAX_SAMPLES_NUM);
+  
+  if (frequency >= 30 && frequency <= 1000000)
+  {
+    setupPWMFrequency(PWM_PIN, frequency);
+  }
 }
 
 void setWaveformDC(uint8_t dc)
@@ -104,3 +109,58 @@ void pollWaveformGenerator()
     }
   }
 }
+
+void setupPWMFrequency(uint8_t pin, unsigned long frequency) {
+  byte prescalerbits = 0b001; // Voreinstellung: kein Prescaler (F_CPU)
+  unsigned long prescaler_value;
+  unsigned long ocr;
+
+  // Berechne den idealen Prescaler für die gewünschte Frequenz
+  prescaler_value = F_CPU / frequency;
+
+  // Finde den passenden Prescaler
+  if (prescaler_value < 65536UL) {
+    prescalerbits = 0b001; // Kein Prescaler (F_CPU)
+  }
+  else if ((prescaler_value >>= 3) < 65536UL) {
+    prescalerbits = 0b010; // Prescaler 8
+  }
+  else if ((prescaler_value >>= 3) < 65536UL) {
+    prescalerbits = 0b011; // Prescaler 64
+  }
+  else if ((prescaler_value >>= 2) < 65536UL) {
+    prescalerbits = 0b100; // Prescaler 256
+  }
+  else if ((prescaler_value >>= 2) < 65536UL) {
+    prescalerbits = 0b101; // Prescaler 1024
+  }
+  else {
+    prescalerbits = 0b101; // Prescaler 1024
+    frequency = F_CPU / (prescaler_value = 65535UL);
+  }
+
+  // Berechne den Wert für den Output Compare Register (OCR)
+  ocr = F_CPU / (prescaler_value * frequency) - 1;
+
+  // Timer konfigurieren
+  pinMode(pin, OUTPUT);
+  if (pin == 5 || pin == 6) {
+    TCCR0B &= ~0b111; // Prescaler löschen
+    TCCR0B |= prescalerbits; // Neuen Prescaler setzen
+    OCR0A = ocr; // Output Compare Register setzen
+    TCCR0A |= (1 << WGM01); // CTC-Modus aktivieren
+  }
+  else if (pin == 9 || pin == 10) {
+    TCCR1B &= ~0b111; // Prescaler löschen
+    TCCR1B |= prescalerbits; // Neuen Prescaler setzen
+    OCR1A = ocr; // Output Compare Register setzen
+    TCCR1A |= (1 << WGM11); // CTC-Modus aktivieren
+  }
+  else if (pin == 3 || pin == 11) {
+    TCCR2B &= ~0b111; // Prescaler löschen
+    TCCR2B |= prescalerbits; // Neuen Prescaler setzen
+    OCR2A = ocr; // Output Compare Register setzen
+    TCCR2A |= (1 << WGM21); // CTC-Modus aktivieren
+  }
+}
+