505 lines
12 KiB
C
505 lines
12 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
|
|
|
|
|
|
|
|
|
|
/* *********************************************** 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;
|
|
float* history;
|
|
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;
|
|
|
|
// init history
|
|
conf->next= 0;
|
|
conf->history= (float*)malloc(sizeof(float) * conf->taps);
|
|
for (n= 0; n < conf->taps; n++)
|
|
{
|
|
conf->history[n]= 0;
|
|
}
|
|
|
|
free(w);
|
|
}
|
|
|
|
|
|
void
|
|
ntaps_filter_ffd(ntaps_filter_conf* conf
|
|
, int input_size, float input[]
|
|
, float* result)
|
|
{
|
|
int i;
|
|
float sum;
|
|
|
|
assert(input_size == conf->decimation);
|
|
|
|
for (i= 0; i < conf->decimation; i++)
|
|
{
|
|
conf->history[conf->next]= input[i];
|
|
conf->next= (conf->next + 1) % conf->taps;
|
|
}
|
|
|
|
sum= 0.0;
|
|
for (i= 0; i < conf->taps; i++)
|
|
{
|
|
sum= sum
|
|
+ conf->history[(conf->next + i) % conf->taps]
|
|
* 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 fm_qd_buffer[8];
|
|
float ffd_buffer[8];
|
|
|
|
float band_2;
|
|
float band_3;
|
|
float band_11[8];
|
|
float band_12[8];
|
|
float resume_1[8];
|
|
float band_21[8];
|
|
float band_22[8];
|
|
float resume_2[8];
|
|
|
|
float output1, output2;
|
|
short output_short[2];
|
|
|
|
int i=0;
|
|
int count=0;
|
|
|
|
|
|
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);
|
|
|
|
|
|
#pragma omp parallel num_threads (2) default (none) \
|
|
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, fm_qd_buffer, ffd_buffer, band_2, band_3, band_11, band_12, resume_1, band_21, band_22, resume_2, output1, output2, output_short, i, count)
|
|
{
|
|
#pragma omp single
|
|
{
|
|
while (16 == fread (read_buffer, sizeof(float), 16, input_file))
|
|
{
|
|
|
|
#pragma omp task firstprivate (read_buffer) output (fm_qd_buffer) 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], &fm_qd_buffer[i]);
|
|
|
|
#pragma omp task input (fm_qd_buffer) output (band_11) shared (lp_11_conf) private (i) num_threads (2)
|
|
for (i = 0; i < 8; i++)
|
|
ntaps_filter_ffd (&lp_11_conf, 1, &fm_qd_buffer[i], &band_11[i]);
|
|
|
|
#pragma omp task input (fm_qd_buffer) output (band_12) shared (lp_12_conf) private (i)
|
|
for (i = 0; i < 8; i++)
|
|
ntaps_filter_ffd (&lp_12_conf, 1, &fm_qd_buffer[i], &band_12[i]);
|
|
|
|
#pragma omp task input (band_11, band_12) output (resume_1) private (i)
|
|
for (i = 0; i < 8; i++)
|
|
subctract (band_11[i], band_12[i], &resume_1[i]);
|
|
|
|
|
|
|
|
#pragma omp task input (fm_qd_buffer) output (band_21) shared (lp_21_conf) private (i)
|
|
for (i = 0; i < 8; i++)
|
|
ntaps_filter_ffd (&lp_21_conf, 1, &fm_qd_buffer[i], &band_21[i]);
|
|
|
|
#pragma omp task input (fm_qd_buffer) output (band_22) shared (lp_22_conf) private (i)
|
|
for (i = 0; i < 8; i++)
|
|
ntaps_filter_ffd (&lp_22_conf, 1, &fm_qd_buffer[i], &band_22[i]);
|
|
|
|
#pragma omp task input (band_21, band_22) output (resume_2) private (i)
|
|
for (i = 0; i < 8; i++)
|
|
subctract (band_21[i], band_22[i], &resume_2[i]);
|
|
|
|
#pragma omp task input (resume_1, resume_2) output (ffd_buffer) private (i)
|
|
for (i = 0; i < 8; i++)
|
|
multiply_square (resume_1[i], resume_2[i], &ffd_buffer[i]);
|
|
|
|
|
|
#pragma omp task input (fm_qd_buffer) output (band_2) shared (lp_2_conf)
|
|
ntaps_filter_ffd (&lp_2_conf, 8, fm_qd_buffer, &band_2);
|
|
|
|
#pragma omp task input (ffd_buffer) output (band_3) shared (lp_3_conf)
|
|
ntaps_filter_ffd (&lp_3_conf, 8, ffd_buffer, &band_3);
|
|
|
|
#pragma omp task input (band_2, band_3) output (output1, output2)
|
|
stereo_sum (band_2, band_3, &output1, &output2);
|
|
|
|
#pragma omp task input (output1, output2) shared (output_file, text_file) private (output_short)
|
|
{
|
|
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]);
|
|
}
|
|
}
|
|
}
|
|
}
|
|
fclose (input_file);
|
|
fclose (output_file);
|
|
fclose (text_file);
|
|
|
|
return 0;
|
|
}
|