From ed85c573239dc17780d06a5fa24b818a220f1542 Mon Sep 17 00:00:00 2001
From: KubaPro010 <kuba.pro010@gmail.com>
Date: Sun, 23 Mar 2025 09:52:55 +0100
Subject: [PATCH] redo pll

---
 lib/filters.c | 37 +++++++++++++++++--------------------
 1 file changed, 17 insertions(+), 20 deletions(-)

diff --git a/lib/filters.c b/lib/filters.c
index c013069..c7810e4 100644
--- a/lib/filters.c
+++ b/lib/filters.c
@@ -19,32 +19,29 @@ void init_pll(PLL *pll, float output_freq, float reference_freq, float loop_filt
     pll->freq = output_freq;
     pll->ref_freq = reference_freq;
     pll->loop_filter_state = 0.0f;
-    
-    float damping = 0.707;
-    pll->kp = 2.0f * damping * loop_filter_bandwidth;
-    pll->ki = loop_filter_bandwidth * loop_filter_bandwidth;
-    
+    pll->kp = 2.0f * M_PI * loop_filter_bandwidth;
+    pll->ki = 0.25f * pll->kp * pll->kp;
     pll->sample_rate = sample_rate;
     pll->quadrature_mode = quadrature_mode;
 }
 
 float apply_pll(PLL *pll, float ref_sample, float input_sample) {
-    float phase_error;
+    float phase_inc = 2.0f * M_PI * pll->freq / pll->sample_rate;
+    pll->phase += phase_inc;
+    if (pll->phase > 2.0f * M_PI) {
+        pll->phase -= 2.0f * M_PI;
+    }
+
+    float vco_out = sinf(pll->phase);
+    float phase_error = ref_sample * vco_out;
+
+    pll->loop_filter_state += pll->ki * phase_error;
+    float freq_offset = pll->kp * phase_error + pll->loop_filter_state;
+    pll->freq = pll->ref_freq + freq_offset;
+
     if (pll->quadrature_mode) {
-        phase_error = ref_sample * sinf(pll->phase) - input_sample * cosf(pll->phase);
+        return cosf(pll->phase);
     } else {
-        phase_error = ref_sample * input_sample * sinf(pll->phase);
+        return vco_out;
     }
-    
-    float filter_output = pll->kp * phase_error + pll->loop_filter_state;
-    pll->loop_filter_state += pll->ki * phase_error * (1/pll->sample_rate);
-    
-    pll->freq = pll->freq + filter_output;
-    
-    pll->phase += M_2PI * pll->freq * (1.0f/pll->sample_rate);
-    if (pll->phase > M_2PI) {
-        pll->phase -= M_2PI;
-    }
-    
-    return cosf(pll->phase);
 }
\ No newline at end of file