553 lines
14 KiB
C
553 lines
14 KiB
C
/* A simple approach to FMradio.
|
|
*
|
|
* The code was addapted from Marco Cornero Aug'06
|
|
* for Acotes project. This version ensures to have
|
|
* the same result that the stream one for plain C.
|
|
*
|
|
* Author: David Rodenas-Pico, BSC
|
|
* Creation Date: March 2007
|
|
*/
|
|
|
|
|
|
#include <assert.h>
|
|
#include <math.h>
|
|
#include <stdio.h>
|
|
#include <stdlib.h>
|
|
#include <string.h>
|
|
|
|
|
|
#define KSMPS 1024
|
|
#define FM_MAX 5
|
|
#define FM_LIMIT (65536/2)-1
|
|
|
|
#define __avl_proc_ 8
|
|
|
|
|
|
/* *********************************************** complex operations */
|
|
|
|
typedef struct
|
|
{
|
|
float real;
|
|
float imag;
|
|
} complex;
|
|
|
|
#define M_PI 3.14159265358979323846 /* pi */
|
|
#define GAIN_LMR 2000
|
|
|
|
#define complex_conj(s, d) \
|
|
{ \
|
|
(d)->real = (s)->real; \
|
|
(d)->imag = -1 * (s)->imag; \
|
|
}
|
|
|
|
#define complex_arg(x) \
|
|
atan2((x)->imag, (x)->real)
|
|
|
|
#define complex_mul(a, b, d) \
|
|
{ \
|
|
(d)->real = (a)->real * (b)->real - ((a)->imag * (b)->imag); \
|
|
(d)->imag = (a)->real * (b)->imag + (a)->imag * (b)->real; \
|
|
}
|
|
|
|
|
|
/* *********************************************** auxiliar functions */
|
|
|
|
short
|
|
dac_cast_trunc_and_normalize_to_short(float f)
|
|
{
|
|
short x;
|
|
f = (f / FM_MAX)*FM_LIMIT;
|
|
x = f;
|
|
return x;
|
|
}
|
|
|
|
#if 0
|
|
/* *********************************************** file filters */
|
|
|
|
void
|
|
file_to_stream(FILE* file, stream_t* output)
|
|
{
|
|
int i;
|
|
int room;
|
|
float value;
|
|
|
|
room= stream_room(output);
|
|
for (i= 0; i < room; i++) {
|
|
if (fread(&value, sizeof(value), 1, file)) {
|
|
stream_push(output, &value);
|
|
}
|
|
}
|
|
}
|
|
|
|
void
|
|
stream_to_file(stream_t* input, FILE* file)
|
|
{
|
|
int i;
|
|
int count;
|
|
float value;
|
|
short res_int;
|
|
|
|
count= stream_count(input);
|
|
for (i= 0; i < count; i++)
|
|
{
|
|
stream_pop(input, &value);
|
|
//fwrite(&value, sizeof(value), 1, file);
|
|
res_int= dac_cast_trunc_and_normalize_to_short(value);
|
|
fwrite(&res_int, sizeof(res_int), 1, file);
|
|
}
|
|
}
|
|
#endif
|
|
|
|
|
|
/* *********************************************** fm_quad_demod */
|
|
|
|
typedef struct {
|
|
float history[2];
|
|
} fm_quad_demod_filter;
|
|
|
|
void
|
|
fm_quad_demod_init (fm_quad_demod_filter* filter)
|
|
{
|
|
filter->history[0]= 0;
|
|
filter->history[1]= 0;
|
|
}
|
|
|
|
void
|
|
fm_quad_demod (fm_quad_demod_filter* filter, float i1, float i2, float* result)
|
|
{
|
|
complex x_N;
|
|
complex x_N_1;
|
|
complex x_N_1_conj;
|
|
complex y_N;
|
|
float demod, d_gain = 0.5;
|
|
|
|
/*
|
|
*
|
|
* y(n) = angle(x(n)*conj(x(n-1))
|
|
*
|
|
*/
|
|
|
|
/* read two complex data */
|
|
x_N.real = i2; x_N.imag = i1;
|
|
x_N_1.real = filter->history[1]; x_N_1.imag = filter->history[0];
|
|
|
|
/* compute */
|
|
complex_conj(& x_N_1, & x_N_1_conj);
|
|
complex_mul(& x_N_1_conj, & x_N, & y_N);
|
|
demod = d_gain * complex_arg(& y_N);
|
|
|
|
filter->history[0]= i1;
|
|
filter->history[1]= i2;
|
|
|
|
*result= demod;
|
|
}
|
|
|
|
|
|
|
|
/* *********************************************** multiply_square */
|
|
|
|
void
|
|
multiply_square(float i1, float i2, float* result)
|
|
{
|
|
*result= GAIN_LMR * i1 * i2 * i2;
|
|
}
|
|
|
|
|
|
/* *********************************************** ntaps_filter */
|
|
|
|
typedef struct ntaps_filter_conf
|
|
{
|
|
double* coeff;
|
|
int decimation;
|
|
int taps;
|
|
int next;
|
|
} ntaps_filter_conf;
|
|
|
|
#define WIN_HAMMING 0
|
|
#define WIN_HANNING 1
|
|
#define WIN_BLACKMAN 2
|
|
|
|
void
|
|
compute_window(unsigned int ntaps, double *taps, unsigned int type)
|
|
{
|
|
int n;
|
|
int M = ntaps - 1; // filter order
|
|
|
|
if(type == WIN_HAMMING)
|
|
{
|
|
for (n = 0; n < ntaps; n++)
|
|
taps[n] = 0.54 - 0.46 * cos ((2 * M_PI * n) / M);
|
|
}
|
|
else if(type == WIN_HANNING)
|
|
{
|
|
for (n = 0; n < ntaps; n++)
|
|
taps[n] = 0.5 - 0.5 * cos ((2 * M_PI * n) / M);
|
|
}
|
|
else if(type == WIN_BLACKMAN)
|
|
{
|
|
for (n = 0; n < ntaps; n++)
|
|
taps[n] = 0.42 - 0.50 * cos ((2*M_PI * n) / (M-1)) - 0.08 * cos ((4*M_PI * n) / (M-1));
|
|
}
|
|
}
|
|
|
|
int
|
|
compute_ntaps (float sampling_freq,
|
|
float transition_width,
|
|
int window_type)
|
|
{
|
|
/* Mormalized transition width */
|
|
float delta_f = transition_width / sampling_freq;
|
|
|
|
float width_factor[3] = { 3.3, 3.1, 5.5 };
|
|
|
|
/* compute number of taps required for given transition width */
|
|
int ntaps = (int) (width_factor[window_type]/ delta_f + 0.5);
|
|
|
|
if ((ntaps & 1) == 0)
|
|
ntaps++;
|
|
|
|
return ntaps;
|
|
}
|
|
|
|
void
|
|
ntaps_filter_ffd_init(ntaps_filter_conf *conf,
|
|
double cutoff_freq,
|
|
double transition_band,
|
|
double gain,
|
|
int decimation,
|
|
double sampling_rate,
|
|
int window_type)
|
|
{
|
|
/* Taken from the GNU software radio .. */
|
|
|
|
int n;
|
|
int i;
|
|
unsigned int ntaps;
|
|
double* w;
|
|
int M;
|
|
double fwT0;
|
|
double fmax;
|
|
|
|
ntaps = compute_ntaps(sampling_rate, transition_band, window_type);
|
|
conf->coeff = malloc(ntaps * sizeof(double));
|
|
w = malloc(ntaps * sizeof(double));
|
|
conf->taps = ntaps;
|
|
conf->decimation = decimation;
|
|
|
|
compute_window(ntaps, w, window_type);
|
|
|
|
M = (ntaps - 1) / 2;
|
|
|
|
fwT0 = 2 * M_PI * cutoff_freq / sampling_rate;
|
|
|
|
for (n=0; n<ntaps; n++)
|
|
conf->coeff[n]=0.0;
|
|
|
|
for (n = -M; n <= M; n++) {
|
|
if (n == 0)
|
|
conf->coeff[n + M] = fwT0 / M_PI * w[n + M];
|
|
else
|
|
conf->coeff[n + M] = sin (n * fwT0) / (n * M_PI) * w[n + M];
|
|
}
|
|
|
|
fmax = conf->coeff[0 + M];
|
|
|
|
for (n = 1; n <= M; n++)
|
|
fmax += 2 * conf->coeff[n + M];
|
|
|
|
gain /= fmax; // normalize
|
|
|
|
for (i = 0; i < ntaps; i++)
|
|
conf->coeff[i] *= gain;
|
|
|
|
free(w);
|
|
}
|
|
|
|
|
|
void
|
|
ntaps_filter_ffd (ntaps_filter_conf* conf, int input_size,
|
|
float *input, float* result)
|
|
{
|
|
float sum;
|
|
int i;
|
|
|
|
sum = 0.0;
|
|
for (i = 0; i < conf->taps; i++)
|
|
{
|
|
sum += input [i] * conf->coeff[conf->taps - i - 1];
|
|
}
|
|
|
|
*result = sum;
|
|
}
|
|
|
|
|
|
/* *********************************************** stereo_sum */
|
|
|
|
void
|
|
stereo_sum(float data_spm, float data_smm, float* left, float* right)
|
|
{
|
|
*left = (data_spm+data_smm);
|
|
*right= (data_spm-data_smm);
|
|
}
|
|
|
|
|
|
/* *********************************************** subctract */
|
|
|
|
void
|
|
subctract(float i1, float i2, float* result)
|
|
{
|
|
*result= i1 - i2;
|
|
}
|
|
|
|
|
|
/* *********************************************** program options */
|
|
|
|
int
|
|
read_opts(int argc, char* argv[],
|
|
FILE** input_file,
|
|
FILE** output_file,
|
|
FILE** text_file,
|
|
int* final_audio_frequency)
|
|
{
|
|
char input_filename[4096];
|
|
char output_filename[4096];
|
|
char text_filename[4096];
|
|
|
|
|
|
/* arguments parsing */
|
|
if ((argc < 1) || (argc > 3))
|
|
{
|
|
printf("Usage: fm_rcv [sample_file_name [audio_frequency]]\n");
|
|
return 1;
|
|
}
|
|
if (argc == 1)
|
|
{
|
|
strcpy(input_filename, "input.dat");
|
|
}
|
|
else
|
|
{
|
|
strcpy(input_filename, argv[1]);
|
|
}
|
|
sprintf(output_filename,"%s.raw",argv[0]);
|
|
sprintf(text_filename,"%s.txt",argv[0]);
|
|
if(argc == 3)
|
|
*final_audio_frequency = atoi(argv[2]);
|
|
|
|
|
|
*input_file= fopen(input_filename, "r");
|
|
if (*input_file == NULL) {
|
|
printf("Cannot open input file %s\n", input_filename);
|
|
return 1;
|
|
}
|
|
*output_file= fopen(output_filename, "w");
|
|
if (*output_file == NULL) {
|
|
printf("Cannot open output file %s\n", output_filename);
|
|
return 1;
|
|
}
|
|
*text_file= fopen(text_filename, "w");
|
|
if (*text_file == NULL) {
|
|
printf("Cannot open output file %s\n", text_filename);
|
|
return 1;
|
|
}
|
|
|
|
|
|
return 0;
|
|
}
|
|
|
|
|
|
|
|
/* *********************************************** main */
|
|
|
|
int
|
|
main(int argc, char* argv[])
|
|
{
|
|
ntaps_filter_conf lp_2_conf;
|
|
ntaps_filter_conf lp_11_conf, lp_12_conf;
|
|
ntaps_filter_conf lp_21_conf, lp_22_conf;
|
|
ntaps_filter_conf lp_3_conf;
|
|
fm_quad_demod_filter fm_qd_conf;
|
|
|
|
int final_audio_frequency = 64*KSMPS;
|
|
float input_rate = 512 * KSMPS;
|
|
|
|
float inout_ratio;
|
|
|
|
FILE* input_file;
|
|
FILE* output_file;
|
|
FILE* text_file;
|
|
|
|
float read_buffer[16];
|
|
|
|
float band_2;
|
|
float band_3;
|
|
float band_11;
|
|
float band_12;
|
|
float resume_1;
|
|
float band_21;
|
|
float band_22;
|
|
float resume_2;
|
|
|
|
float output1, output2;
|
|
short output_short[2];
|
|
|
|
int i=0;
|
|
int maxtaps = 0;
|
|
int maxtaps_minus_one;
|
|
|
|
if (read_opts(argc, argv,&input_file,&output_file,&text_file, &final_audio_frequency))
|
|
{
|
|
return 1;
|
|
}
|
|
|
|
inout_ratio = ((int) input_rate)/final_audio_frequency;
|
|
|
|
ntaps_filter_ffd_init(&lp_2_conf, 15000, 4000, 0.5, inout_ratio, input_rate, WIN_HANNING);
|
|
ntaps_filter_ffd_init(&lp_11_conf, 53000, 4000, 1, 1, input_rate, WIN_HANNING);
|
|
ntaps_filter_ffd_init(&lp_12_conf, 23000, 4000, 1, 1, input_rate, WIN_HANNING);
|
|
ntaps_filter_ffd_init(&lp_21_conf, 21000, 2000, 1, 1, input_rate, WIN_HANNING);
|
|
ntaps_filter_ffd_init(&lp_22_conf, 17000, 2000, 1, 1, input_rate, WIN_HANNING);
|
|
ntaps_filter_ffd_init(&lp_3_conf, 15000, 4000, 1.0, inout_ratio, input_rate, WIN_HANNING);
|
|
fm_quad_demod_init (&fm_qd_conf);
|
|
|
|
if (maxtaps < lp_11_conf.taps) maxtaps = lp_11_conf.taps;
|
|
if (maxtaps < lp_12_conf.taps) maxtaps = lp_12_conf.taps;
|
|
if (maxtaps < lp_21_conf.taps) maxtaps = lp_21_conf.taps;
|
|
if (maxtaps < lp_22_conf.taps) maxtaps = lp_22_conf.taps;
|
|
if (maxtaps < lp_2_conf.taps) maxtaps = lp_2_conf.taps;
|
|
maxtaps_minus_one = maxtaps - 1;
|
|
|
|
float ffd_buffer;
|
|
float fm_qd_buffer;
|
|
int iter = 0;
|
|
float view8[8];
|
|
|
|
int maxtaps_plus_seven = maxtaps + 7;
|
|
|
|
int diff_11 = maxtaps - lp_11_conf.taps;
|
|
int diff_12 = maxtaps - lp_12_conf.taps;
|
|
int diff_21 = maxtaps - lp_21_conf.taps;
|
|
int diff_22 = maxtaps - lp_22_conf.taps;
|
|
int diff_2 = maxtaps_plus_seven - lp_2_conf.taps;
|
|
|
|
float view_11[maxtaps];
|
|
float view_12[maxtaps];
|
|
float view_21[maxtaps];
|
|
float view_22[maxtaps];
|
|
float view_2[maxtaps_plus_seven];
|
|
float view_3[lp_3_conf.taps];
|
|
int lp_3_taps_minus_eight = lp_3_conf.taps - 8;
|
|
|
|
float fm_qd_buffer_pre[maxtaps];
|
|
|
|
|
|
#pragma omp parallel num_threads (1) default (none) \
|
|
shared (lp_3_taps_minus_eight, maxtaps, maxtaps_minus_one, maxtaps_plus_seven, view_11, view_12, view_21, view_22, view_2, view_3) \
|
|
shared (diff_11, diff_12, diff_21, diff_22, diff_2, view8, iter, ffd_buffer, fm_qd_buffer, fm_qd_buffer_pre) \
|
|
shared (input_file, output_file, text_file, lp_2_conf, lp_11_conf, lp_12_conf, lp_21_conf, lp_22_conf, lp_3_conf, fm_qd_conf) \
|
|
private (read_buffer, band_2, band_3, band_11, band_12, resume_1, band_21, band_22, resume_2, output1, output2, output_short, i)
|
|
{
|
|
#pragma omp single
|
|
{
|
|
|
|
/* Implement pre operations (add enough zeros at beginning of
|
|
stream). */
|
|
#pragma omp task output (fm_qd_buffer << fm_qd_buffer_pre[maxtaps_minus_one]) private (i)
|
|
for (i = 0; i < maxtaps_minus_one; ++i)
|
|
fm_qd_buffer_pre[i] = 0;
|
|
|
|
#pragma omp task output (ffd_buffer << view_3[lp_3_taps_minus_eight]) private (i)
|
|
for (i = 0; i < lp_3_taps_minus_eight; ++i)
|
|
view_3[i] = 0;
|
|
|
|
|
|
while ((16 == fread (read_buffer, sizeof(float), 16, input_file)))
|
|
{
|
|
|
|
#pragma omp task firstprivate (read_buffer) output (fm_qd_buffer << view8[8]) shared (fm_qd_conf) private (i)
|
|
for (i = 0; i < 8; i++)
|
|
fm_quad_demod (&fm_qd_conf, read_buffer[2*i], read_buffer[2*i + 1], &view8[i]);
|
|
|
|
for (i = 0; i < 8; i++)
|
|
{
|
|
#pragma omp task input (fm_qd_buffer >> view_11[1]) output (band_11) \
|
|
shared (lp_11_conf, diff_11) num_threads (3)
|
|
ntaps_filter_ffd (&lp_11_conf, 1, &view_11[diff_11], &band_11);
|
|
|
|
#pragma omp task input (fm_qd_buffer >> view_12[1]) output (band_12) \
|
|
shared (lp_12_conf, diff_12) num_threads (3)
|
|
ntaps_filter_ffd (&lp_12_conf, 1, &view_12[diff_12], &band_12);
|
|
|
|
|
|
#pragma omp task input (fm_qd_buffer >> view_21[1]) output (band_21) \
|
|
shared (lp_21_conf, diff_21) num_threads (6)
|
|
ntaps_filter_ffd (&lp_21_conf, 1, &view_21[diff_21], &band_21);
|
|
|
|
#pragma omp task input (fm_qd_buffer >> view_22[1]) output (band_22) \
|
|
shared (lp_22_conf, diff_22) num_threads (6)
|
|
ntaps_filter_ffd (&lp_22_conf, 1, &view_22[diff_22], &band_22);
|
|
|
|
|
|
#pragma omp task input (band_11, band_12, band_21, band_22) output (ffd_buffer) \
|
|
private (resume_1, resume_2)
|
|
{
|
|
subctract (band_11, band_12, &resume_1);
|
|
subctract (band_21, band_22, &resume_2);
|
|
multiply_square (resume_1, resume_2, &ffd_buffer);
|
|
}
|
|
}
|
|
|
|
#if __avl_proc_ > 8
|
|
#if __avl_proc_ > 16
|
|
#pragma omp task input (fm_qd_buffer >> view_2[8]) output (band_2) \
|
|
shared (lp_2_conf, diff_2)
|
|
ntaps_filter_ffd (&lp_2_conf, 8, &view_2[diff_2], &band_2);
|
|
|
|
#pragma omp task input (ffd_buffer >> view_3[8]) output (band_3) \
|
|
shared (lp_3_conf)
|
|
ntaps_filter_ffd (&lp_3_conf, 8, view_3, &band_3);
|
|
#else
|
|
#pragma omp task input (fm_qd_buffer >> view_2[8], ffd_buffer >> view_3[8]) \
|
|
output (band_2, band_3) \
|
|
shared (lp_2_conf, lp_3_conf, diff_2)
|
|
{
|
|
ntaps_filter_ffd (&lp_2_conf, 8, &view_2[diff_2], &band_2);
|
|
ntaps_filter_ffd (&lp_3_conf, 8, view_3, &band_3);
|
|
}
|
|
#endif
|
|
#pragma omp task input (band_2, band_3) \
|
|
shared (output_file, text_file) private (output_short, output1, output2)
|
|
{
|
|
stereo_sum (band_2, band_3, &output1, &output2);
|
|
output_short[0] = dac_cast_trunc_and_normalize_to_short (output1);
|
|
output_short[1] = dac_cast_trunc_and_normalize_to_short (output2);
|
|
fwrite (output_short, sizeof(short), 2, output_file);
|
|
//fprintf (text_file, "%-10.5f %-10.5f\n", output1, output2);
|
|
//fprintf (text_file, "%-10.5f %-10.5f \t %10d %10d\n", output1, output2, output_short[0], output_short[1]);
|
|
}
|
|
#else
|
|
#pragma omp task input (fm_qd_buffer >> view_2[8], ffd_buffer >> view_3[8]) \
|
|
shared (output_file, text_file, lp_2_conf, lp_3_conf, diff_2) \
|
|
private (output_short, output1, output2, band_2, band_3)
|
|
{
|
|
ntaps_filter_ffd (&lp_2_conf, 8, &view_2[diff_2], &band_2);
|
|
ntaps_filter_ffd (&lp_3_conf, 8, view_3, &band_3);
|
|
stereo_sum (band_2, band_3, &output1, &output2);
|
|
output_short[0] = dac_cast_trunc_and_normalize_to_short (output1);
|
|
output_short[1] = dac_cast_trunc_and_normalize_to_short (output2);
|
|
fwrite (output_short, sizeof(short), 2, output_file);
|
|
//fprintf (text_file, "%-10.5f %-10.5f\n", output1, output2);
|
|
//fprintf (text_file, "%-10.5f %-10.5f \t %10d %10d\n", output1, output2, output_short[0], output_short[1]);
|
|
}
|
|
|
|
#endif
|
|
++iter;
|
|
}
|
|
}
|
|
}
|
|
fclose (input_file);
|
|
fclose (output_file);
|
|
fclose (text_file);
|
|
|
|
return 0;
|
|
}
|