radio95/fm95
0
1
Fork 0
mirror of https://github.com/radio95-rnt/fm95.git synced 2026-02-26 19:23:51 +01:00
Code Issues Activity

how does this work?

Browse Source
This commit is contained in:
KubaPro010
2025-03-23 09:55:24 +01:00
parent ed85c57323
commit d619cea8b1
1 changed files with 23 additions and 15 deletions
Download Patch File Download Diff File Expand all files Collapse all files

38
lib/filters.c
View File

@@ -26,22 +26,30 @@ 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 phase_inc = 2.0f * M_PI * pll->freq / pll->sample_rate;
pll->phase += phase_inc;
if (pll->phase > 2.0f * M_PI) {
float pll_output = sin(pll->phase);
float phase_error;
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;
}
pll->loop_filter_state += pll->ki * phase_error / pll->sample_rate;
float loop_filter_output = pll->loop_filter_state + pll->kp * phase_error;
float phase_adjustment = loop_filter_output;
pll->phase += phase_adjustment;
pll->phase += 2.0f * M_PI * pll->freq / pll->sample_rate;
while (pll->phase >= 2.0f * M_PI) {
pll->phase -= 2.0f * M_PI;
}
float vco_out = sinf(pll->phase);
float phase_error = ref_sample * vco_out;
pll->loop_filter_state += pll->ki * phase_error;
float freq_offset = pll->kp * phase_error + pll->loop_filter_state;
pll->freq = pll->ref_freq + freq_offset;
if (pll->quadrature_mode) {
return cosf(pll->phase);
} else {
return vco_out;
while (pll->phase < 0.0f) {
pll->phase += 2.0f * M_PI;
}
return pll_output;
}