00001 #include "bpm/bpm_dsp.h"
00002
00003
00004 void _shift_down( double *x, int n ) {
00005 int k=0;
00006 for( k=0; k<n-1; k++ ) x[k] = x[k+1];
00007 return;
00008 }
00009
00010
00011
00012 void _reset( double *x ) {
00013 int i = 0;
00014 for( i=0;i<=MAXPZ;i++) x[i] = 0.;
00015 }
00016
00017
00018
00019 int apply_filter( filter_t *f, doublewf_t *w ) {
00020
00021 int i = 0, k = 0;
00022
00023 if ( ! f || ! w ) {
00024 bpm_error( "Invalid pointers in apply_filter()", __FILE__, __LINE__ );
00025 return BPM_FAILURE;
00026 }
00027
00028
00029 _reset( f->xv );
00030 _reset( f->yv );
00031
00032 _reset( f->xv_ac );
00033 _reset( f->yv_ac );
00034
00035 if( f->options & FIR ) {
00036
00037
00038
00039 if ( f->options & ANTICAUSAL ) {
00040 for ( k=0; k<f->nxc_ac; k++ ) f->xv_ac[k] = w->wf[k] / f->gain;
00041 }
00042
00043 for ( i=0; i<f->ns; i++ ) {
00044
00045
00046
00047 if ( f->options & CAUSAL ) {
00048 _shift_down( f->xv, f->nxc );
00049 f->xv[f->nxc-1] = w->wf[i] / f->gain;
00050 f->wfbuffer[i] = 0.;
00051 for( k=0; k<f->nxc; k++ ) f->wfbuffer[i] += ( f->xc[k] * f->xv[k] );
00052 }
00053
00054 if ( f->options & ANTICAUSAL ) {
00055
00056
00057 if (i>0) _shift_down( f->xv_ac, f->nxc_ac );
00058
00059 f->xv_ac[f->nxc_ac-1] = ( ( i+f->nxc_ac <= f->ns ) ? w->wf[i+f->nxc_ac-1] / f->gain : 0. );
00060
00061 for ( k=1; k<f->nxc_ac; k++ ) f->wfbuffer[i] += ( f->xc_ac[k] * f->xv_ac[k] );
00062
00063 }
00064 }
00065
00066 } else {
00067
00068
00069 for ( i=0; i<f->ns; i++ ) {
00070
00071 _shift_down( f->xv, f->nxc );
00072 f->xv[f->nxc-1] = w->wf[i] / f->gain;
00073
00074 _shift_down( f->yv, f->nyc );
00075 f->yv[f->nyc-1] = 0.;
00076
00077
00078 for ( k=0; k < f->nxc; k++ ) f->yv[f->nyc-1] += ( f->xc[k] * f->xv[k] );
00079
00080
00081 for ( k=0; k < f->nyc-1; k++ ) f->yv[f->nyc-1] += ( f->yc[k] * f->yv[k] );
00082
00083 f->wfbuffer[i] = f->yv[f->nyc-1];
00084 }
00085
00086
00087 }
00088
00089
00090 for(i=0;i<f->ns;i++) w->wf[i] = f->wfbuffer[i];
00091
00092 return BPM_SUCCESS;
00093 }
00094
00095