From ffe9a806f01a5f9ef5115473c897d2e7629617b4 Mon Sep 17 00:00:00 2001
From: KubaPro010 <kuba.pro010@gmail.com>
Date: Sun, 23 Mar 2025 09:45:30 +0100
Subject: [PATCH] add pll

---
 lib/filters.c | 35 +++++++++++++++++++++++++++++++++++
 lib/filters.h | 15 ++++++++++++++-
 src/fm95.c    |  5 ++++-
 3 files changed, 53 insertions(+), 2 deletions(-)

diff --git a/lib/filters.c b/lib/filters.c
index 9d6021f..c013069 100644
--- a/lib/filters.c
+++ b/lib/filters.c
@@ -12,4 +12,39 @@ float apply_preemphasis(ResistorCapacitor *filter, float sample) {
 
 float hard_clip(float sample, float threshold) {
     return fmaxf(-threshold, fminf(threshold, sample));
+}
+
+void init_pll(PLL *pll, float output_freq, float reference_freq, float loop_filter_bandwidth, int quadrature_mode, int sample_rate) {
+    pll->phase = 0.0f;
+    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->sample_rate = sample_rate;
+    pll->quadrature_mode = quadrature_mode;
+}
+
+float apply_pll(PLL *pll, float ref_sample, float input_sample) {
+    float phase_error;
+    if (pll->quadrature_mode) {
+        phase_error = ref_sample * sinf(pll->phase) - input_sample * cosf(pll->phase);
+    } else {
+        phase_error = ref_sample * input_sample * sinf(pll->phase);
+    }
+    
+    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
diff --git a/lib/filters.h b/lib/filters.h
index 57faefe..f71c0d9 100644
--- a/lib/filters.h
+++ b/lib/filters.h
@@ -14,4 +14,17 @@ typedef struct
 void init_preemphasis(ResistorCapacitor *filter, float tau, float sample_rate);
 float apply_preemphasis(ResistorCapacitor *filter, float sample);
 
-float hard_clip(float sample, float threshold);
\ No newline at end of file
+float hard_clip(float sample, float threshold);
+
+typedef struct {
+    float phase;
+    float freq;
+    float ref_freq;
+    float loop_filter_state;
+    float kp;
+    float ki;
+    int sample_rate;
+    int quadrature_mode;
+} PLL;
+void init_pll(PLL *pll, float output_freq, float refrence_freq, float loop_filter_bandwidth, int quadrature_mode, int sample_rate);
+float apply_pll(PLL *pll, float ref_sample, float input_sample);
\ No newline at end of file
diff --git a/src/fm95.c b/src/fm95.c
index cd7a66b..75c2c46 100644
--- a/src/fm95.c
+++ b/src/fm95.c
@@ -417,6 +417,9 @@ int main(int argc, char **argv) {
     ResistorCapacitor preemp_l, preemp_r;
     init_preemphasis(&preemp_l, preemphasis_tau, sample_rate);
     init_preemphasis(&preemp_r, preemphasis_tau, sample_rate);
+
+    PLL rds2_pll;
+    init_pll(&rds2_pll, 66500, 19000, 100, 1, sample_rate);
     // #endregion
 
     signal(SIGINT, stop);
@@ -495,7 +498,7 @@ int main(int argc, char **argv) {
                 float rds_carrier = get_oscillator_sin_multiplier_ni(&osc, 3);
                 output[i] += (current_rds_in*rds_carrier)*RDS_VOLUME;
                 if(!sca_on) {
-                    float rds2_carrier_66 = get_oscillator_cos_sample(&rds2_osc);
+                    float rds2_carrier_66 = apply_pll(&rds2_pll, get_oscillator_sin_multiplier_ni(&osc, 1), get_oscillator_cos_sample(&rds2_osc));
                     output[i] += (current_rds2_in*rds2_carrier_66)*RDS2_VOLUME;
                 }
             }