qo100-trx-prototypes/costas/costas.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;
}