From 75663d2562e4c6dc453cb451ed104b8fafb87a4a Mon Sep 17 00:00:00 2001
From: KubaPro010 <kuba.pro010@gmail.com>
Date: Sun, 23 Mar 2025 17:19:26 +0100
Subject: [PATCH] fix

---
 lib/filters.c | 16 ++++------------
 lib/filters.h |  7 ++-----
 src/fm95.c    |  5 +++--
 3 files changed, 9 insertions(+), 19 deletions(-)

diff --git a/lib/filters.c b/lib/filters.c
index d629587..e2c99b6 100644
--- a/lib/filters.c
+++ b/lib/filters.c
@@ -14,7 +14,7 @@ float hard_clip(float sample, float threshold) {
 	return fmaxf(-threshold, fminf(threshold, sample));
 }
 
-void init_pll(PLL *pll, int interpolation, int decimation, float freq, float loop_filter_bandwidth, int quadrature_mode, int sample_rate) {
+void init_pll(PLL *pll, float freq, float loop_filter_bandwidth, int quadrature_mode, int sample_rate) {
 	pll->phase = 0.0f;
 	pll->freq = freq;
 	pll->loop_filter_state = 0.0f;
@@ -22,21 +22,15 @@ void init_pll(PLL *pll, int interpolation, int decimation, float freq, float loo
 	pll->ki = 0.25f * pll->kp * pll->kp;
 	pll->sample_rate = sample_rate;
 	pll->quadrature_mode = quadrature_mode;
-	pll->last_output = 0.0f;
-	pll->interpolation = interpolation;
-	pll->decimation = decimation;
 }
 
-float apply_pll(PLL *pll, float ref_sample) {
+float apply_pll(PLL *pll, float ref_sample, float input_sample) {
 	float phase_error;
 
-	float vco_phase = pll->phase;
-	if(pll->quadrature_mode) vco_phase += M_PI/2.0f;
-
 	float vco_output = sinf(pll->phase);
 	if (pll->quadrature_mode) vco_output = sinf(pll->phase + (M_PI / 2.0f)); // 90 degrees
 
-	phase_error = ref_sample * pll->last_output;
+	phase_error = ref_sample * input_sample;
 
 	pll->loop_filter_state += pll->ki * phase_error / pll->sample_rate;
 	float loop_output = pll->loop_filter_state + pll->kp * phase_error;
@@ -53,7 +47,5 @@ float apply_pll(PLL *pll, float ref_sample) {
 		pll->phase += M_2PI;
 	}
 
-	pll->last_output = sinf((vco_phase*pll->interpolation)/pll->decimation);
-
-	return pll->last_output;
+	return vco_output;
 }
\ No newline at end of file
diff --git a/lib/filters.h b/lib/filters.h
index 7747e9f..e73d953 100644
--- a/lib/filters.h
+++ b/lib/filters.h
@@ -22,11 +22,8 @@ typedef struct {
 	float loop_filter_state;
 	float kp;
 	float ki;
-	float last_output;
-	int interpolation;
-	int decimation;
 	int sample_rate;
 	int quadrature_mode;
 } PLL;
-void init_pll(PLL *pll, int interpolation, int decimation, float freq, float loop_filter_bandwidth, int quadrature_mode, int sample_rate);
-float apply_pll(PLL *pll, float ref_sample);
\ No newline at end of file
+void init_pll(PLL *pll, float 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 956d976..fabc7f5 100644
--- a/src/fm95.c
+++ b/src/fm95.c
@@ -405,8 +405,9 @@ int main(int argc, char **argv) {
 	}
 
 	// #region Setup Filters/Modulaltors/Oscillators
-	Oscillator osc;
+	Oscillator osc, rds2_osc;
 	init_oscillator(&osc, polar_stereo ? 31250.0 : 19000, sample_rate);
+	init_oscillator(&rds2_osc, 66500, sample_rate);
 
 	FMModulator sca_mod;
 	init_fm_modulator(&sca_mod, sca_frequency, sca_deviation, sample_rate);
@@ -416,7 +417,7 @@ int main(int argc, char **argv) {
 	init_preemphasis(&preemp_r, preemphasis_tau, sample_rate);
 
 	PLL rds2_pll;
-	init_pll(&rds2_pll, 7, 2, 19000, 100, 1, sample_rate);
+	init_pll(&rds2_pll, 66500, 1, 1, sample_rate);
 	// #endregion
 
 	signal(SIGINT, stop);