From a654700ebad09e8559f50f51fbcdcadf4b872c2f Mon Sep 17 00:00:00 2001 From: KubaPro010 Date: Sun, 23 Mar 2025 09:59:05 +0100 Subject: [PATCH] pll? --- lib/filters.c | 22 +++++++++++----------- 1 file changed, 11 insertions(+), 11 deletions(-) diff --git a/lib/filters.c b/lib/filters.c index 1d81e36..b6d99ad 100644 --- a/lib/filters.c +++ b/lib/filters.c @@ -26,23 +26,23 @@ void init_pll(PLL *pll, float output_freq, float reference_freq, float loop_filt } float apply_pll(PLL *pll, float ref_sample, float input_sample) { - float pll_output = sin(pll->phase); - float phase_error; + + float output = sinf(pll->phase); + if (pll->quadrature_mode) { - float quadrature_output = sin(pll->phase + M_PI/2.0f); - phase_error = input_sample * quadrature_output; - } else { - phase_error = input_sample * pll_output; + output = sinf(pll->phase + (M_PI / 2.0f)); } + phase_error = ref_sample * input_sample; + pll->loop_filter_state += pll->ki * phase_error / pll->sample_rate; - float loop_filter_output = pll->loop_filter_state + pll->kp * phase_error; + float loop_output = pll->loop_filter_state + pll->kp * phase_error; - float phase_adjustment = loop_filter_output; - pll->phase += phase_adjustment; + float freq_adjustment = loop_output / (2.0f * M_PI); + float instantaneous_freq = pll->freq + freq_adjustment; - pll->phase += 2.0f * M_PI * pll->freq / pll->sample_rate; + pll->phase += 2.0f * M_PI * instantaneous_freq / pll->sample_rate; while (pll->phase >= 2.0f * M_PI) { pll->phase -= 2.0f * M_PI; @@ -51,5 +51,5 @@ float apply_pll(PLL *pll, float ref_sample, float input_sample) { pll->phase += 2.0f * M_PI; } - return pll_output; + return output; } \ No newline at end of file