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