lasp/lasp/c/lasp_decimation.c

222 lines
6.0 KiB
C

// lasp_decimation.c
//
// Author: J.A. de Jong -ASCEE
//
// Description:
// Implementation of the decimator
//////////////////////////////////////////////////////////////////////
#include "lasp_decimation.h"
#include "lasp_filterbank.h"
#include "lasp_tracer.h"
#include "lasp_alloc.h"
#include "lasp_dfifo.h"
// The Maximum number of taps in a decimation filter
#define DEC_FILTER_MAX_TAPS (128)
// The FFT length
#define DEC_FFT_LEN (1024)
typedef struct {
DEC_FAC df;
us dec_fac;
us ntaps;
d h[DEC_FILTER_MAX_TAPS];
} DecFilter;
static __thread DecFilter DecFilters[] = {
{DEC_FAC_4,4,128,
#include "dec_filter_4.c"
}
};
typedef struct Decimator_s {
us nchannels;
us dec_fac;
FilterBank** fbs;
dFifo* output_fifo;
} Decimator;
Decimator* Decimator_create(us nchannels,DEC_FAC df) {
fsTRACE(15);
/* Find the filter */
const us nfilters = sizeof(DecFilters)/sizeof(DecFilter);
DecFilter* filter = DecFilters;
bool found = false;
for(us filterno = 0;filterno < nfilters; filterno++) {
if(filter->df == df) {
TRACE(15,"Found filter");
found = true;
break;
}
filter++;
}
if(!found) {
WARN("Decimation factor not found in list of filters");
return NULL;
}
/* Create the filterbanks */
Decimator* dec = a_malloc(sizeof(Decimator));
dec->fbs = a_malloc(sizeof(FilterBank*)*nchannels);
dec->nchannels = nchannels;
dec->dec_fac = filter->dec_fac;
dmat h = dmat_foreign_data(filter->ntaps,1,filter->h,false);
for(us channelno=0;channelno<nchannels;channelno++) {
dec->fbs[channelno] = FilterBank_create(&h,DEC_FFT_LEN);
}
dmat_free(&h);
/* Create input and output fifo's */
dec->output_fifo = dFifo_create(nchannels,DEC_FFT_LEN);
feTRACE(15);
return dec;
}
static void lasp_downsample(dmat* downsampled,
const dmat* filtered,
us dec_fac) {
fsTRACE(15);
dbgassert(downsampled && filtered,NULLPTRDEREF);
dbgassert(filtered->n_rows/dec_fac == downsampled->n_rows,
"Incompatible number of rows");
dbgassert(downsampled->n_cols == filtered->n_cols,
"Incompatible number of rows");
dbgassert(dec_fac> 0,"Invalid dec_fac");
for(us col=0;col<downsampled->n_cols;col++) {
/* Low-level BLAS copy. */
d_copy(getdmatval(downsampled,0,col),
getdmatval(filtered,0,col),
downsampled->n_rows, /* number */
1, /* incx out */
dec_fac); /* incx in */
}
check_overflow_xmat(*downsampled);
check_overflow_xmat(*filtered);
feTRACE(15);
}
dmat Decimator_decimate(Decimator* dec,const dmat* samples) {
fsTRACE(15);
dbgassert(dec && samples,NULLPTRDEREF);
const us nchannels = dec->nchannels;
const us dec_fac = dec->dec_fac;
dbgassert(samples->n_cols == nchannels,"Invalid number of "
"channels in samples");
dbgassert(samples->n_rows > 0,"Number of rows should be >0")
/* Not downsampled, but filtered result */
dmat filtered;
/* Filter each channel and store result in filtered. In first
* iteration the right size for filtered is allocated. */
for(us chan=0;chan<nchannels;chan++) {
vd samples_channel = dmat_column((dmat*)samples,
chan);
/* Low-pass filter stuff */
dmat filtered_res = FilterBank_filter(dec->fbs[chan],
&samples_channel);
dbgassert(filtered_res.n_cols == 1,"Bug in FilterBank");
vd_free(&samples_channel);
if(filtered_res.n_rows > 0) {
if(chan==0) {
/* Allocate space for result */
filtered = dmat_alloc(filtered_res.n_rows,
nchannels);
}
dmat filtered_col = dmat_submat(&filtered,
0,chan,
filtered_res.n_rows,
1);
dbgassert(filtered_res.n_rows == filtered_col.n_rows,
"Not all FilterBank's have same output number"
" of rows!");
dmat_copy_rows(&filtered_col,
&filtered_res,
0,0,filtered_res.n_rows);
dmat_free(&filtered_res);
dmat_free(&filtered_col);
}
else {
filtered = dmat_alloc(0, nchannels);
}
}
if(filtered.n_rows > 0) {
/* Push filtered result into output fifo */
dFifo_push(dec->output_fifo,
&filtered);
}
dmat_free(&filtered);
/* Now, downsample stuff */
dmat downsampled;
uVARTRACE(15,dec_fac);
us fifo_size = dFifo_size(dec->output_fifo);
if((fifo_size / dec_fac) > 0) {
filtered = dmat_alloc((fifo_size/dec_fac)*dec_fac,
nchannels);
int nsamples = dFifo_pop(dec->output_fifo,
&filtered,0);
dbgassert((us) nsamples % dec_fac == 0 && nsamples > 0,
"BUG");
dbgassert((us) nsamples == (fifo_size/dec_fac)*dec_fac,"BUG");
downsampled = dmat_alloc(nsamples/dec_fac,nchannels);
/* Do the downsampling work */
lasp_downsample(&downsampled,&filtered,dec_fac);
dmat_free(&filtered);
}
else {
TRACE(15,"Empty downsampled");
downsampled = dmat_alloc(0,0);
}
feTRACE(15);
/* return filtered; */
return downsampled;
}
void Decimator_free(Decimator* dec) {
fsTRACE(15);
dbgassert(dec,NULLPTRDEREF);
dFifo_free(dec->output_fifo);
for(us chan=0;chan<dec->nchannels;chan++) {
FilterBank_free(dec->fbs[chan]);
}
a_free(dec->fbs);
a_free(dec);
feTRACE(15);
}
//////////////////////////////////////////////////////////////////////