97 lines
3.1 KiB
C
97 lines
3.1 KiB
C
#include <liquid/liquid.h>
|
|
#include <math.h>
|
|
#include <complex.h>
|
|
#include <stdio.h>
|
|
|
|
const float SAMPLINGRATE = 100000.0;
|
|
const float EXPECTED_CENTER = 25000.0;
|
|
|
|
// 15khz lowpass
|
|
float LOWPASS_COEFFS[37] = {
|
|
-0.0013484122464433312,-0.0015524507034569979,-0.0012626417446881533,
|
|
1.106597777377648e-18,0.0025128561537712812,0.005701862741261721,
|
|
0.007837646640837193,0.006523041054606438,-3.595113471383589e-18,
|
|
-0.011250422336161137,-0.02350831776857376,-0.03022352233529091,
|
|
-0.02406340278685093,6.529514774129222e-18,0.04183189943432808,
|
|
0.09489838778972626,0.14748811721801758,0.18619666993618011,
|
|
0.20043739676475525,0.18619666993618011,0.14748811721801758,
|
|
0.09489838778972626,0.04183189943432808,6.529514774129222e-18,
|
|
-0.02406340278685093,-0.03022352233529091,-0.02350831776857376,
|
|
-0.011250422336161137,-3.595113471383589e-18,0.006523041054606438,
|
|
0.007837646640837193,0.005701862741261721,0.0025128561537712812,
|
|
1.106597777377648e-18,-0.0012626417446881533,-0.0015524507034569979,
|
|
-0.0013484122464433312
|
|
};
|
|
|
|
int main() {
|
|
|
|
// create the NCO object
|
|
nco_crcf downmix = nco_crcf_create(LIQUID_NCO);
|
|
nco_crcf_set_phase(downmix, 0.0f);
|
|
float freq = -(2 * M_PI * EXPECTED_CENTER) / SAMPLINGRATE;
|
|
nco_crcf_set_frequency(downmix, freq);
|
|
|
|
nco_crcf costas_vco = nco_crcf_create(LIQUID_VCO);
|
|
nco_crcf_set_phase(costas_vco, 0.0f);
|
|
nco_crcf_set_frequency(costas_vco, 0);
|
|
|
|
|
|
firfilt_rrrf r_fir = firfilt_rrrf_create(LOWPASS_COEFFS, 37);
|
|
firfilt_rrrf i_fir = firfilt_rrrf_create(LOWPASS_COEFFS, 37);
|
|
|
|
// output complex exponential
|
|
float complex x;
|
|
|
|
FILE* output = fopen("output.raw", "w");
|
|
FILE* input = fopen("wobble.raw", "r");
|
|
|
|
float vco_freq = 0.0;
|
|
|
|
float in[2];
|
|
while(fread(in, sizeof(float), 2, input) == 2) {
|
|
// increment internal phase
|
|
nco_crcf_step(downmix);
|
|
// compute complex exponential
|
|
nco_crcf_cexpf(downmix, &x);
|
|
|
|
float complex y;
|
|
nco_crcf_step(costas_vco);
|
|
nco_crcf_cexpf(costas_vco, &y);
|
|
|
|
float complex input_sample = (in[0] + I * in[1]);
|
|
float complex downmix_sample = input_sample * x;
|
|
float complex costas_sample = downmix_sample * y;
|
|
float complex output_sample = input_sample * y;
|
|
|
|
float r_sample = creal(costas_sample);
|
|
float i_sample = cimag(costas_sample);
|
|
|
|
firfilt_rrrf_push(r_fir, r_sample); // push input sample
|
|
firfilt_rrrf_execute(r_fir,&r_sample); // compute output
|
|
|
|
firfilt_rrrf_push(i_fir, i_sample); // push input sample
|
|
firfilt_rrrf_execute(i_fir,&i_sample); // compute output
|
|
|
|
float error = i_sample * r_sample;
|
|
vco_freq += error * 0.1;
|
|
|
|
nco_crcf_set_frequency(costas_vco, -vco_freq);
|
|
|
|
float buffer[] = {creal(output_sample), cimag(output_sample)};
|
|
fwrite(buffer, sizeof(float), 2, output);
|
|
}
|
|
|
|
// destroy nco object
|
|
nco_crcf_destroy(downmix);
|
|
nco_crcf_destroy(costas_vco);
|
|
|
|
firfilt_rrrf_destroy(r_fir);
|
|
firfilt_rrrf_destroy(i_fir);
|
|
|
|
fclose(output);
|
|
fclose(input);
|
|
|
|
|
|
return 0;
|
|
}
|