00001
00006 #include "bpm/bpm_dsp.h"
00007
00008 int _expand_complex_polynomial( complex_t *w, int n, complex_t *a ) {
00009
00016 int i = 0, j = 0;
00017
00018 if ( !w || !a ) {
00019 bpm_error( "Invalid pointers in _expand_complex_polynomial", __FILE__, __LINE__ );
00020 return 1;
00021 }
00022
00023 a[0] = complex( 1., 0. );
00024 for ( i=0; i<n; i++ ) a[i+1] = complex( 0., 0. );
00025 for ( i=0; i<n; i++ ) {
00026 for ( j=n; j>=1; j-- ) a[j] = c_sum( c_mult( c_neg(w[i]), a[j] ), a[j-1]);
00027 a[0] = c_mult( a[0], c_neg( w[i] ) );
00028 }
00029
00030
00031 for ( i=0; i<n+1; i++ ) {
00032
00033 if ( fabs( c_imag( a[i]) ) > FILT_EPS ) {
00034 bpm_error( "Poles/zeros not complex conjugates", __FILE__, __LINE__ );
00035 return 1;
00036 }
00037 }
00038
00039 return 0;
00040 }
00041
00042
00043
00044 complex_t _eval_complex_polynomial( complex_t *a, int n, complex_t z ) {
00045
00046 complex_t p = complex( 0., 0. );
00047 int i = 0;
00048 for ( i=n; i>=0; i-- ) p = c_sum( c_mult( p, z ), a[i] );
00049
00050 return p;
00051 }
00052
00053
00054
00055
00056 int calculate_filter_coefficients( filter_t *f ) {
00057
00063 complex_t topco[MAXPZ+1], botco[MAXPZ+1];
00064 double theta = 0;
00065 int i, nz, np;
00066
00067 nz = f->cplane->nzeros;
00068 np = f->cplane->npoles;
00069
00070 if ( _expand_complex_polynomial( f->cplane->zero, nz, topco ) ) return 1;
00071 f->nxc = nz+1;
00072
00073 if ( _expand_complex_polynomial( f->cplane->pole, np, botco ) ) return 1;
00074 f->nyc = np+1;
00075
00076
00077 f->dc_gain = c_div( _eval_complex_polynomial( topco, nz, complex( 1., 0. ) ),
00078 _eval_complex_polynomial( botco, np, complex( 1., 0. ) ) );
00079
00080 theta = 2.*PI*0.5*( f->alpha1 + f->alpha2 );
00081 f->fc_gain = c_div( _eval_complex_polynomial( topco, nz, complex(cos(theta),sin(theta)) ),
00082 _eval_complex_polynomial( botco, np, complex(cos(theta),sin(theta)) ) );
00083
00084 f->hf_gain = c_div( _eval_complex_polynomial( topco, nz, complex( -1., 0.) ),
00085 _eval_complex_polynomial( botco, np, complex( -1., 0.) ) );
00086
00087
00088
00089 if ( f->options & LOWPASS ) f->gain = c_abs( f->dc_gain );
00090 else if( f->options & HIGHPASS ) f->gain = c_abs( f->hf_gain );
00091 else if( f->options & BANDPASS ) f->gain = c_abs( f->fc_gain );
00092 else if( f->options & BANDSTOP ) f->gain = c_abs( c_sqrt( c_mult( f->dc_gain,
00093 f->hf_gain ) ) );
00094 else f->gain = 1.;
00095
00096
00097 for ( i=0; i<f->nxc; i++ ) f->xc[i] = c_real( topco[i] ) / c_real( botco[f->nyc-1] );
00098 for ( i=0; i<f->nyc; i++ ) f->yc[i] = -c_real( botco[i] ) / c_real( botco[f->nyc-1] );
00099
00100 return 0;
00101 }