void vor_float_demod_run ( float sample) {
const float ti = i * vfd_te;
// phase error re-injection
vfd_ref_phi -= vfd_ref_alpha * vfd_ref_err;
// local oscillator phase
const float vfd_ref_phase = 2. * M_PI * vfd_ref_freq * ti + vfd_ref_phi;
// local oscillator signal
const float vfd_ref_local_sig = sin(vfd_ref_phase);
// get REF signal by bandpassing input signal
const float vfd_ref_sig = vor_float_filter_bp_ref(sample);
// multiply input signal by local oscillator signal
const float vfd_ref_y = vfd_ref_sig * vfd_ref_local_sig;
// get phase error by low passing the result of the multiplication.
vfd_ref_err = vor_float_filter_lp_ref(vfd_ref_y);
// filter 30 REF before decimating it
const float vfd_ref_err_decim = vor_float_filter_lp_decim(vfd_ref_err);
// get VAR signal by bandpassing input signal
// const float vfd_var_sig = vor_float_filter_bp_var(sample);
vfd_var_sig = vor_float_filter_bp_var(sample);
if (decim >= vfd_DECIM) {
decim = 0;
vfd_var_phi -= vfd_var_alpha * vfd_var_err;
const float vfd_var_phase = 2. * M_PI * vfd_var_freq * ti + vfd_var_phi;
const float vfd_var_local_sig = -sin( vfd_var_phase);
const float vfd_var_y = vfd_var_sig * vfd_var_local_sig;
vfd_var_err = vor_float_filter_lp_var(vfd_var_y);
vfd_fm_phi -= vfd_fm_alpha * vfd_fm_err;
const float vfd_fm_phase = 2. * M_PI * vfd_fm_freq * ti + vfd_fm_phi;
// const float vfd_fm_local_sig = -sin( vfd_fm_phase );
vfd_fm_local_sig = -sin( vfd_fm_phase );
const float vfd_fm_y = vfd_ref_err_decim * vfd_fm_local_sig;
vfd_fm_err = vor_float_filter_lp_fm(vfd_fm_y);
vfd_qdr = vfd_var_phi - vfd_fm_phi;
}
i++;
decim++;
}
int main(int argc, char** argv) {
vor_audio_read_wav("signal_VOR_BF_50_200dB.wav");
int i;
float te = 1/29880.;
for (i=0; i<nb_samples; i++) {
float t = i * te;
// float yi_f = vor_float_filter_bp_var(float_buf[i]);
// int32_t yi_i = vor_int_filter_bp_var(adc_buf[i]);
float yi_f = vor_float_filter_bp_ref(float_buf[i]);
int32_t yi_i = vor_int_filter_bp_ref(adc_buf[i]);
printf("%f\t%f\t%f\t%d\t%d\t%f\n",
t, float_buf[i], yi_f, adc_buf[i], yi_i, (float)yi_i / (float)VIF_SFACT / (1<<5));
}
return 0;
}
