126 lines
3.8 KiB
C
126 lines
3.8 KiB
C
#include <stdlib.h>
|
|
#include <liquid/liquid.h>
|
|
#include <math.h>
|
|
#include <complex.h>
|
|
#include <stdio.h>
|
|
|
|
const float SAMPLINGRATE = 1000000.0;
|
|
const float FFT_LEN = 512;
|
|
|
|
int spectral_bin_to_fft_idx(int bin) {
|
|
if(bin == 0) {
|
|
return FFT_LEN/2;
|
|
} else if(bin > 0) {
|
|
return bin - 1;
|
|
} else {
|
|
return bin + FFT_LEN;
|
|
}
|
|
}
|
|
|
|
|
|
int main() {
|
|
FILE* output_csv = fopen("output.csv", "w");
|
|
FILE* output = fopen("output.raw", "w");
|
|
FILE* input = fopen("input.raw", "r");
|
|
|
|
nco_crcf correction = nco_crcf_create(LIQUID_NCO);
|
|
nco_crcf_set_phase(correction, 0.0f);
|
|
|
|
float complex * fft_in = (float complex*) malloc(FFT_LEN * sizeof(float complex));
|
|
float complex * fft_out = (float complex*) malloc(FFT_LEN * sizeof(float complex));
|
|
|
|
// create FFT plan
|
|
fftplan fft = fft_create_plan(FFT_LEN, fft_in, fft_out, LIQUID_FFT_FORWARD, 0);
|
|
|
|
int next_fft_in = 0;
|
|
int pos = 0;
|
|
float in[2];
|
|
|
|
uint64_t sample_count = 0;
|
|
|
|
while(fread(in, sizeof(float), 2, input) == 2) {
|
|
complex float cplx_in = (in[1] + I * in[0]);
|
|
sample_count++;
|
|
|
|
if(next_fft_in <= 0) {
|
|
fft_in[pos] = cplx_in;
|
|
pos += 1;
|
|
if(pos == FFT_LEN) {
|
|
pos = 0;
|
|
fft_execute(fft);
|
|
|
|
float fft_max = cabsf(fft_out[0]);
|
|
for(int i = 0; i < FFT_LEN; i++) {
|
|
float mag = cabsf(fft_out[i]);
|
|
if(mag > fft_max) {
|
|
fft_max = mag;
|
|
}
|
|
}
|
|
//printf("Min: %f Max: %f\n", fft_min, fft_max);
|
|
|
|
for(int bin = -FFT_LEN/2; bin < FFT_LEN/2; bin++) {
|
|
int idx = spectral_bin_to_fft_idx(bin);
|
|
float mag = cabsf(fft_out[idx]);
|
|
mag = mag / fft_max;
|
|
fprintf(output_csv, "%f;", mag);
|
|
}
|
|
|
|
printf("===========\n");
|
|
float max_levels = 0;
|
|
int max_center = 0;
|
|
for(int bin = -50; bin <= 50; bin++) {
|
|
int center_idx = spectral_bin_to_fft_idx(bin);
|
|
float center_val = cabsf(fft_out[center_idx]) / fft_max;
|
|
if(center_val > 0.25) {
|
|
printf("Found peak candidate at %d\n", bin);
|
|
int left_idx = spectral_bin_to_fft_idx(bin - 127);
|
|
int right_idx = spectral_bin_to_fft_idx(bin + 127);
|
|
float left_val = cabsf(fft_out[left_idx]) / fft_max;
|
|
float right_val = cabsf(fft_out[right_idx]) / fft_max;
|
|
|
|
if(center_val + left_val + right_val > max_levels) {
|
|
max_levels = center_val + left_val + right_val;
|
|
max_center = bin;
|
|
}
|
|
}
|
|
}
|
|
|
|
if(max_levels > 0.0) {
|
|
float center_freq = max_center * SAMPLINGRATE / FFT_LEN;
|
|
printf("Found center at %f\n", center_freq);
|
|
nco_crcf_set_frequency(correction, -(2 * M_PI * center_freq) / SAMPLINGRATE);
|
|
}
|
|
|
|
fprintf(output_csv, "\n");
|
|
|
|
next_fft_in = SAMPLINGRATE / 4;
|
|
}
|
|
} else {
|
|
next_fft_in--;
|
|
}
|
|
|
|
float complex x;
|
|
// increment internal phase
|
|
nco_crcf_step(correction);
|
|
// compute complex exponential
|
|
nco_crcf_cexpf(correction, &x);
|
|
|
|
float complex cplx_out = cplx_in * x;
|
|
|
|
float buffer[] = {creal(cplx_out), cimag(cplx_out)};
|
|
fwrite(buffer, sizeof(float), 2, output);
|
|
}
|
|
|
|
printf("Processed %fs\n", sample_count / SAMPLINGRATE);
|
|
|
|
fft_destroy_plan(fft);
|
|
free(fft_in);
|
|
free(fft_out);
|
|
|
|
fclose(output_csv);
|
|
fclose(output);
|
|
fclose(input);
|
|
|
|
return 0;
|
|
}
|