attemp to use a pll

lib/filters.c

@@ -11,6 +11,37 @@ float apply_preemphasis(ResistorCapacitor *filter, float sample) {
 }
 
 float hard_clip(float sample, float threshold) {
-    // Branchless clipping
     return fmaxf(-threshold, fminf(threshold, sample));
+}
+
+void init_pll(PLL *pll, float reference_frequency, float target_frequency, float sample_rate) {
+    pll->reference_frequency = reference_frequency;
+    pll->target_frequency = target_frequency;
+    pll->multiplier = target_frequency / reference_frequency;
+    pll->sample_rate = sample_rate;
+    
+    pll->phase_error = 0.0f;
+    pll->loop_filter = 0.0f;
+    pll->vco_phase = 0.0f;
+    pll->vco_frequency = target_frequency;
+    
+    pll->loop_gain = 0.01f;
+    pll->filter_coefficient = 0.1f;
+}
+
+float update_pll(PLL *pll, float reference_signal) {
+    float vco_divided_phase = fmodf(pll->vco_phase / pll->multiplier, M_2PI);
+    float reference_phase = acosf(reference_signal); // Extract phase from reference signal
+    
+    pll->phase_error = sinf(reference_phase - vco_divided_phase);
+    
+    pll->loop_filter = pll->loop_filter * (1.0f - pll->filter_coefficient) + 
+                       pll->phase_error * pll->filter_coefficient * pll->loop_gain;
+    
+    pll->vco_frequency = pll->target_frequency + pll->loop_filter;
+    
+    float phase_increment = (M_2PI * pll->vco_frequency) / pll->sample_rate;
+    pll->vco_phase = fmodf(pll->vco_phase + phase_increment, M_2PI * pll->multiplier);
+    
+    return sinf(pll->vco_phase);
 }

lib/filters.h

@@ -3,6 +3,7 @@
 #include <string.h>
 #include <stdlib.h>
 #include "constants.h"
+#include "oscillator.h"
 
 typedef struct
 {
@@ -13,4 +14,21 @@ 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);
+float hard_clip(float sample, float threshold);
+
+typedef struct {
+    float phase_error;
+    float loop_filter;
+    float vco_phase;
+    float vco_frequency;
+
+    float reference_frequency;
+    float target_frequency;
+    float multiplier;
+    float sample_rate;
+    float loop_gain;
+    float filter_coefficient;
+} PLL;
+
+void init_pll(PLL *pll, float reference_frequency, float target_frequency, float sample_rate);
+float update_pll(PLL *pll, float reference_signal);

src/fm95.c

@@ -414,6 +414,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, 19000, 66500, sample_rate);
     // #endregion
 
     signal(SIGINT, stop);
@@ -491,7 +494,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_sin_multiplier_ni(&osc, 3.5f); // 66.5 KHz
+                    float rds2_carrier_66 = update_pll(&rds2_pll, get_oscillator_sin_multiplier_ni(&osc, 1));
                     output[i] += (current_rds2_in*rds2_carrier_66)*RDS2_VOLUME;
                 }
             }