88 lines
2.4 KiB
C
88 lines
2.4 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;
|
|
|
|
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);
|
|
|
|
agc_crcf agc = agc_crcf_create(); // create object
|
|
agc_crcf_set_bandwidth(agc,1e-3f); // set loop filter bandwidth
|
|
|
|
// output complex exponential
|
|
float complex x;
|
|
|
|
FILE* output = fopen("output.raw", "w");
|
|
FILE* input = fopen("middle_beacon.raw", "r");
|
|
|
|
float loop_bw = 0.3;
|
|
float min_freq = -1.0;
|
|
float max_freq = 1.0;
|
|
// Set the damping factor for a critically damped system
|
|
float damping = sqrtf(2.0f) / 2.0f;
|
|
float denom = (1.0 + 2.0 * damping * loop_bw + loop_bw * loop_bw);
|
|
float alpha = (4 * damping * loop_bw) / denom;
|
|
float beta = (4 * loop_bw * loop_bw) / denom;
|
|
|
|
|
|
float loop_freq = 0.0;
|
|
float loop_phase = 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 = cos(-loop_phase) + I * sin(-loop_phase);
|
|
|
|
float complex input_sample = (in[0] + I * in[1]);
|
|
float complex downmix_sample = input_sample * x;
|
|
|
|
agc_crcf_execute(agc, downmix_sample, &downmix_sample);
|
|
|
|
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);
|
|
|
|
float error = i_sample * r_sample;
|
|
|
|
loop_freq += beta * error;
|
|
loop_phase += loop_freq + alpha * error;
|
|
|
|
while(loop_phase > (2 * M_PI))
|
|
loop_phase -= 2 * M_PI;
|
|
while(loop_phase < (-2 * M_PI))
|
|
loop_phase += 2 * M_PI;
|
|
|
|
if(loop_freq > max_freq)
|
|
loop_freq = max_freq;
|
|
else if(loop_freq < min_freq)
|
|
loop_freq = min_freq;
|
|
|
|
float buffer[] = {creal(output_sample), cimag(output_sample)};
|
|
fwrite(buffer, sizeof(float), 2, output);
|
|
}
|
|
|
|
// destroy nco object
|
|
nco_crcf_destroy(downmix);
|
|
|
|
|
|
fclose(output);
|
|
fclose(input);
|
|
|
|
|
|
return 0;
|
|
}
|