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; }