mirror of
https://github.com/radio95-rnt/fm95.git
synced 2026-02-26 19:23:51 +01:00
how does this work?
This commit is contained in:
@@ -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 apply_pll(PLL *pll, float ref_sample, float input_sample) {
|
||||||
float phase_inc = 2.0f * M_PI * pll->freq / pll->sample_rate;
|
float pll_output = sin(pll->phase);
|
||||||
pll->phase += phase_inc;
|
|
||||||
if (pll->phase > 2.0f * M_PI) {
|
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;
|
pll->phase -= 2.0f * M_PI;
|
||||||
}
|
}
|
||||||
|
while (pll->phase < 0.0f) {
|
||||||
float vco_out = sinf(pll->phase);
|
pll->phase += 2.0f * M_PI;
|
||||||
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;
|
|
||||||
}
|
}
|
||||||
|
|
||||||
|
return pll_output;
|
||||||
}
|
}
|
||||||
Reference in New Issue
Block a user