experiment with the pll

lib/filters.c

@@ -20,17 +20,18 @@ void init_pll(PLL *pll, float freq, float loop_filter_bandwidth, int quadrature_
 	pll->loop_filter_state = 0.0f;
 	pll->kp = M_2PI * loop_filter_bandwidth;
 	pll->ki = 0.25f * pll->kp * pll->kp;
+	pll->last_output = 0.0f;
 	pll->sample_rate = sample_rate;
 	pll->quadrature_mode = quadrature_mode;
 }
 
-float apply_pll(PLL *pll, float ref_sample, float input_sample) {
+float apply_pll(PLL *pll, float ref_sample) {
 	float phase_error;
 
 	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 * input_sample;
+	phase_error = ref_sample * pll->last_output;
 
 	pll->loop_filter_state += pll->ki * phase_error / pll->sample_rate;
 	float loop_output = pll->loop_filter_state + pll->kp * phase_error;
@@ -47,5 +48,7 @@ float apply_pll(PLL *pll, float ref_sample, float input_sample) {
 		pll->phase += M_2PI;
 	}
 
+	pll->last_output = vco_output;
+
 	return vco_output;
 }

lib/filters.h

@@ -22,8 +22,9 @@ typedef struct {
 	float loop_filter_state;
 	float kp;
 	float ki;
+	float last_output;
 	int sample_rate;
 	int quadrature_mode;
 } PLL;
 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);
+float apply_pll(PLL *pll, float ref_sample);

src/fm95.c

@@ -405,9 +405,8 @@ int main(int argc, char **argv) {
 	}
 
 	// #region Setup Filters/Modulaltors/Oscillators
-	Oscillator osc, rds2_osc;
+	Oscillator 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);
@@ -417,7 +416,7 @@ int main(int argc, char **argv) {
 	init_preemphasis(&preemp_r, preemphasis_tau, sample_rate);
 
 	PLL rds2_pll;
-	init_pll(&rds2_pll, 66500, 1, 1, sample_rate);
+	init_pll(&rds2_pll, 66500, 1, 10000, sample_rate);
 	// #endregion
 
 	signal(SIGINT, stop);
@@ -496,7 +495,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 = apply_pll(&rds2_pll, get_oscillator_sin_multiplier_ni(&osc, 1), get_oscillator_sin_sample(&rds2_osc));
+					float rds2_carrier_66 = apply_pll(&rds2_pll, get_oscillator_sin_multiplier_ni(&osc, 1));
 					output[i] += (current_rds2_in*rds2_carrier_66)*RDS2_VOLUME;
 				}
 			}