00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018 #ifndef _VrCOMPLExFIRFILTER_H_
00019 #define _VrCOMPLExFIRFILTER_H_
00020
00021 #include <VrDecimatingSigProc.h>
00022
00023 #if defined (ENABLE_MMX)
00024 #include <VrMMX.h>
00025 #endif
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036
00037
00038 template<class iType>
00039 class VrComplexFIRfilter : public VrDecimatingSigProc<iType,VrComplex> {
00040 protected:
00041 int *numTaps, num_ch;
00042 VrComplex** taps;
00043 VrComplex *phase_corr_incr, *phase_offset;
00044 long time;
00045 float *center_freq, *gain;
00046 void buildFilter_complex(int);
00047 #if defined (ENABLE_MMX)
00048 mmxTaps** processedTaps;
00049 #endif
00050 public:
00051 virtual const char *name() { return "VrComplexFIRfilter"; }
00052 virtual int work(VrSampleRange output, void *o[],
00053 VrSampleRange inputs[], void *i[]);
00054
00055 virtual void initialize();
00056 int setCenter_Freq(int, float);
00057 int setPhase_Offset(VrComplex);
00058 int setCenter_Freq(float);
00059 int setNumber_Taps(int,int);
00060 int setNumber_Taps(int);
00061 VrComplexFIRfilter(int n, int d, const int t[], const float f[],
00062 const float g[]);
00063 VrComplexFIRfilter(int d, int t,float f, float g);
00064 ~VrComplexFIRfilter();
00065 };
00066
00067 template<class iType> int
00068 VrComplexFIRfilter<iType>::work(VrSampleRange output, void *ao[],
00069 VrSampleRange inputs[], void *ai[])
00070 {
00071 iType **i = (iType **)ai;
00072 VrComplex **o = (VrComplex **)ao;
00073 VrComplex result = 0;
00074 int ch_num = 0;
00075 unsigned int size = output.size;
00076
00077 VrComplex phase_correction[num_ch];
00078 for(ch_num=0;ch_num<num_ch; ch_num++) {
00079 phase_correction[ch_num]=pow(phase_corr_incr[ch_num],(int) output.index) * phase_offset[ch_num];
00080 }
00081
00082 for (;size>0;size--,i[0]+=decimation) {
00083 result = 0;
00084
00085 for (ch_num =0; ch_num<num_ch; ch_num++){
00086
00087 iType *inputArray = i[0]+history-numTaps[ch_num];
00088
00089 #if defined (ENABLE_MMX)
00090 if(processedTaps[ch_num]->mmxReady())
00091 result = processedTaps[ch_num]->mmxCVDProduct(inputArray);
00092 else
00093 #endif
00094 {
00095 VrComplex *taps_tmp = taps[ch_num];
00096 int j = 0;
00097 #if 0
00098 int n = numTaps[ch_num] & ~0x3;
00099 for (j = 0; j < n; j += 4){
00100 result += taps_tmp[j + 0] * inputArray[j + 0];
00101 result += taps_tmp[j + 1] * inputArray[j + 1];
00102 result += taps_tmp[j + 2] * inputArray[j + 2];
00103 result += taps_tmp[j + 3] * inputArray[j + 3];
00104 }
00105 #endif
00106 for (; j < numTaps[ch_num]; j++)
00107 result += taps_tmp[j] * inputArray[j];
00108
00109 }
00110
00111
00112 if (center_freq[ch_num] != 0.0) {
00113 phase_correction[ch_num] *= phase_corr_incr[ch_num];
00114 result *= phase_correction[ch_num];
00115 }
00116 *o[ch_num]++=result;
00117 }
00118 }
00119 return output.size;
00120 }
00121
00122 template<class iType> void
00123 VrComplexFIRfilter<iType>::buildFilter_complex(int ch){
00124
00125
00126 double inSampFreq;
00127 int index;
00128 float N = numTaps[ch];
00129 float M = N-1;
00130
00131 inSampFreq = getInputSamplingFrequencyN(0);
00132 time = 0;
00133
00134 if (center_freq[ch] == 0.0) {
00135 for ( index=0 ; index < numTaps[ch] ; index++) {
00136 taps[ch][index] = gain[ch]*VrComplex((0.54-0.46*cos(2*M_PI*index/(M))));
00137 }
00138 } else {
00139 float arg = 2*M_PI*center_freq[ch] / inSampFreq;
00140 for ( index=0 ; index < numTaps[ch] ; index++) {
00141 taps[ch][index] = VrComplex(gain[ch]*cos(arg*index)*(0.54-0.46*cos(2*M_PI*index/(M))),
00142 gain[ch]*(-1)*sin(arg*index)*(0.54-0.46*cos(2*M_PI*index/(M))));
00143 }
00144 phase_corr_incr[ch] = VrComplex(cos(arg*(float)decimation),
00145 (-1)*sin(arg*(float)decimation));
00146 }
00147 #if defined (ENABLE_MMX)
00148 if(processedTaps[ch]!=NULL)
00149 delete processedTaps[ch];
00150 processedTaps[ch]=new mmxTaps(taps[ch],numTaps[ch]);
00151 #endif
00152 }
00153
00154 template<class iType>
00155 VrComplexFIRfilter<iType>::VrComplexFIRfilter(int n, int dec, const int t[],
00156 const float freq[], const float g[])
00157 :VrDecimatingSigProc<iType,VrComplex>(n,dec), num_ch(n)
00158 {
00159 numTaps=new int[num_ch];
00160 phase_corr_incr = new VrComplex[num_ch];
00161 phase_offset = new VrComplex[num_ch];
00162 center_freq = new float[num_ch];
00163 gain = new float[num_ch];
00164 taps = new (VrComplex *[num_ch]);
00165
00166 #if defined (ENABLE_MMX)
00167 processedTaps = new (mmxTaps *[num_ch]);
00168 #endif
00169
00170 for (int i=0; i<num_ch; i++){
00171 numTaps[i] = t[i];
00172 phase_corr_incr[i] = VrComplex(1,0);
00173 phase_offset[i] = VrComplex(1,0);
00174 center_freq[i] = freq[i];
00175 gain[i] = g[i];
00176 #if defined (ENABLE_MMX)
00177 processedTaps[i]=NULL;
00178 #endif
00179 }
00180 }
00181
00182 template<class iType>
00183 VrComplexFIRfilter<iType>::VrComplexFIRfilter(int dec,int t,float freq, float g)
00184 :VrDecimatingSigProc<iType,VrComplex>(1,dec), num_ch(1)
00185 {
00186 numTaps=new int[num_ch];
00187 phase_corr_incr = new VrComplex[num_ch];
00188 phase_offset = new VrComplex[num_ch];
00189 center_freq = new float[num_ch];
00190 gain = new float[num_ch];
00191 taps = new (VrComplex *[num_ch]);
00192
00193 #if defined (ENABLE_MMX)
00194 processedTaps = new (mmxTaps *[num_ch]);
00195 #endif
00196
00197 numTaps[0] = t;
00198 phase_corr_incr[0] = VrComplex(1,0);
00199 phase_offset[0] = VrComplex(1,0);
00200 center_freq[0] = freq;
00201 gain[0] = g;
00202 #if defined (ENABLE_MMX)
00203 processedTaps[0]=NULL;
00204 #endif
00205 }
00206
00207 template<class iType>
00208 void VrComplexFIRfilter<iType>::initialize()
00209 {
00210 for (int i=0; i<num_ch; i++){
00211 taps[i]=new VrComplex[numTaps[i]];
00212 buildFilter_complex(i);
00213 }
00214
00215 int max_numTaps = 0;
00216 for (int i=0; i<num_ch; i++){
00217 if (numTaps[i] > max_numTaps) max_numTaps = numTaps[i];
00218 }
00219 history=max_numTaps;
00220 }
00221
00222 template<class iType>
00223 int VrComplexFIRfilter<iType>::setCenter_Freq(int ch, float cf)
00224 {
00225 center_freq[ch] = cf;
00226 buildFilter_complex(ch);
00227 return 1;
00228 }
00229
00230 template<class iType>
00231 int VrComplexFIRfilter<iType>::setCenter_Freq(float cf)
00232 {
00233 return setCenter_Freq(0,cf);
00234 }
00235
00236 template<class iType>
00237 int VrComplexFIRfilter<iType>::setPhase_Offset(VrComplex f)
00238 {
00239 phase_offset[0]=f;
00240 }
00241
00242 template<class iType>
00243 int VrComplexFIRfilter<iType>::setNumber_Taps(int ch, int numT)
00244 {
00245 numTaps[ch] = numT;
00246 delete taps[ch];
00247 taps[ch]=new VrComplex[numTaps[ch]];
00248
00249 int max_numTaps = 0;
00250 for (int i=0; i<num_ch; i++){
00251 if (numTaps[i] > max_numTaps) max_numTaps = numTaps[i];
00252 }
00253 if(history<(unsigned int) max_numTaps) {
00254 history=(unsigned int) max_numTaps;
00255 setup_upstream();
00256 }
00257
00258 buildFilter_complex(ch);
00259 return 1;
00260 }
00261
00262 template<class iType>
00263 int VrComplexFIRfilter<iType>::setNumber_Taps(int numT)
00264 {
00265 return setNumber_Taps(0, numT);
00266 }
00267
00268 template<class iType>
00269 VrComplexFIRfilter<iType>::~VrComplexFIRfilter()
00270 {
00271 for (int i=0; i<num_ch; i++){
00272 delete taps[i];
00273 #if defined (ENABLE_MMX)
00274 if(processedTaps[i]!=NULL)
00275 delete processedTaps[i];
00276 #endif
00277 }
00278 delete numTaps;
00279 delete [] phase_corr_incr;
00280 delete center_freq;
00281 delete gain;
00282 delete taps;
00283 }
00284 #endif