00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018 #ifndef _VRSYNCFILTER_H_
00019 #define _VRSYNCFILTER_H_
00020
00021 #include <VrPulse.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
00039
00040 #define PRECISION_BITS 12
00041 #define BOZO_HISTORY 1000
00042
00043 template<class iType>
00044 class VrSyncFilter : public VrPulse<iType,VrComplex> {
00045 protected:
00046 int *num_taps, num_ch, bump;
00047 VrComplex** taps;
00048 VrComplex *phase_corr_incr, phase_bump, *phase_offset;
00049 long time;
00050 long differential_offset, delta_differential_offset;
00051 int symbol_boundary, symbol_period, pointer_incr, old_output_loc, new_output_loc;
00052 float *center_freq, *gain;
00053 void buildFilter_complex(int);
00054 #if defined (ENABLE_MMX)
00055 mmxTaps** processedTaps;
00056 #endif
00057 public:
00058 virtual const char *name() { return "VrSyncFilter"; }
00059 virtual int work(VrSampleRange output, void *o[],
00060 VrSampleRange inputs[], void *i[]);
00061 virtual void initialize();
00062 int setCenter_Freq(int, float);
00063 int setCenter_Freq(float);
00064 int setPhase_Offset(VrComplex);
00065 int setNumber_Taps(int,int);
00066 int setNumber_Taps(int);
00067
00068 float getSymbol_Period();
00069 int setSymbol_Period(float);
00070 int setSymbol_Timing(float);
00071 void start_oversampling(int, int,int,VrComplex*,int*) {};
00072 virtual int forecast(VrSampleRange output,
00073 VrSampleRange inputs[]);
00074 VrSyncFilter(int n, float d, const int t[], const float f[],
00075 const float g[]);
00076 VrSyncFilter(float d, int t,float f, float g);
00077 ~VrSyncFilter();
00078 int version() { return 1; };
00079 };
00080
00081 template<class iType> int
00082 VrSyncFilter<iType>::work(VrSampleRange output, void *ao[],
00083 VrSampleRange inputs[], void *ai[])
00084 {
00085 iType **i = (iType **)ai;
00086 VrComplex **o = (VrComplex **)ao;
00087 unsigned int size = output.size;
00088 VrComplex result = 0;
00089 int ch_num = 0;
00090 VrComplex *taps_tmp;
00091
00092 VrComplex phase_correction[num_ch];
00093
00094 for(ch_num=0;ch_num<num_ch; ch_num++) {
00095 phase_correction[ch_num]=pow(phase_corr_incr[ch_num],(int) output.index) * phase_offset[ch_num];
00096 }
00097
00098 while (size--) {
00099 symbol_boundary += symbol_period;
00100 new_output_loc = (symbol_boundary + (symbol_period >> 1)) >> PRECISION_BITS;
00101
00102
00103 pointer_incr = new_output_loc - old_output_loc;
00104 if (pointer_incr <0) {
00105
00106 pointer_incr += (1 << (32-PRECISION_BITS));
00107 }
00108 old_output_loc = new_output_loc;
00109
00110 if (pointer_incr != decimation) {
00111 differential_offset += pointer_incr-decimation;
00112
00113 }
00114
00115 if (bump != 0) {
00116 delta_differential_offset = bump >> PRECISION_BITS;
00117 differential_offset += delta_differential_offset;
00118 phase_correction[0] *= phase_bump;
00119 phase_bump = VrComplex(1,0);
00120 printf ("setting offset %ld\n", differential_offset);
00121
00122 cout << "Bumping pointer " << (float) (bump) / (float)(1<<PRECISION_BITS) << endl;
00123 bump =0;
00124 }
00125 result = 0;
00126
00127 for (ch_num =0; ch_num<num_ch; ch_num++){
00128
00129 iType *inputptr = &i[0][0];
00130
00131 #if defined (ENABLE_MMX)
00132 if(processedTaps[ch_num]->mmxReady())
00133 result = processedTaps[ch_num]->mmxCVDProduct(inputptr);
00134 else
00135 #endif
00136 {
00137 taps_tmp = taps[ch_num];
00138 for (int j=0; j < num_taps[ch_num]; j++)
00139 result += *taps_tmp++ * *inputptr++;
00140 }
00141
00142
00143 phase_correction[ch_num] *= phase_corr_incr[ch_num];
00144 result *= phase_correction[ch_num];
00145 *o[ch_num]++ = result;
00146 }
00147 i[0] += pointer_incr;
00148 }
00149 return output.size;
00150 }
00151
00152 template<class iType> void
00153 VrSyncFilter<iType>::buildFilter_complex(int ch){
00154 int inSampFreq;
00155 int index;
00156 float N = num_taps[ch];
00157 float M = N-1;
00158
00159 inSampFreq = (int)getInputSamplingFrequencyN(0);
00160 if (center_freq[ch] == 0.0){
00161
00162
00163 for ( index=0 ; index < num_taps[ch] ; index++)
00164 taps[ch][index] = gain[ch]*VrComplex((0.54-0.46*cos(2*M_PI*index/(M))));
00165 } else {
00166 float arg = 2*M_PI*center_freq[ch] / (float)inSampFreq;
00167 for ( index=0 ; index < num_taps[ch] ; index++)
00168 taps[ch][index] = VrComplex(gain[ch]*cos(arg*index)*(0.54-0.46*cos(2*M_PI*index/(M))),
00169 gain[ch]*(-1)*sin(arg*index)*(0.54-0.46*cos(2*M_PI*index/(M))));
00170 phase_corr_incr[ch] = VrComplex(cos(arg*(float)symbol_period/(float)(1<<PRECISION_BITS)),
00171 (-1)*sin(arg*(float)symbol_period/(float)(1<<PRECISION_BITS)));
00172 }
00173 #if defined (ENABLE_MMX)
00174 if(processedTaps[ch]!=NULL)
00175 delete processedTaps[ch];
00176 processedTaps[ch]=new mmxTaps(taps[ch],num_taps[ch]);
00177 #endif
00178 }
00179
00180 template<class iType>
00181 VrSyncFilter<iType>::VrSyncFilter(int n, float per, const int t[],
00182 const float freq[], const float g[])
00183 :VrPulse<iType,VrComplex>(n, (int) per), num_ch(n)
00184 {
00185
00186 differential_offset = 0;
00187 num_taps=new int[num_ch];
00188 phase_corr_incr = new VrComplex[num_ch];
00189 phase_offset = new VrComplex[num_ch];
00190 center_freq = new float[num_ch];
00191 gain = new float[num_ch];
00192 taps = new (VrComplex *[num_ch]);
00193 symbol_period = (int) (per * (float)(1 << PRECISION_BITS));
00194
00195 #if defined (ENABLE_MMX)
00196 processedTaps = new (mmxTaps *[num_ch]);
00197 #endif
00198
00199 for (int i=0; i<num_ch; i++){
00200 num_taps[i] = t[i];
00201 phase_corr_incr[i] = VrComplex(1,0);
00202 phase_offset[i] = VrComplex(1,0);
00203 center_freq[i] = freq[i];
00204 gain[i] = g[i];
00205 #if defined (ENABLE_MMX)
00206 processedTaps[i]=NULL;
00207 #endif
00208 }
00209
00210 }
00211
00212 template<class iType>
00213 VrSyncFilter<iType>::VrSyncFilter(float per,int t,float freq, float g)
00214 :VrPulse<iType,VrComplex>(1,(int) per), num_ch(1)
00215 {
00216 differential_offset = 0;
00217 num_taps=new int[num_ch];
00218 phase_corr_incr = new VrComplex[num_ch];
00219 phase_offset = new VrComplex[num_ch];
00220 center_freq = new float[num_ch];
00221 gain = new float[num_ch];
00222 taps = new (VrComplex *[num_ch]);
00223 symbol_period = (int) (per * (float)(1 << PRECISION_BITS));
00224 symbol_boundary = 0;
00225 old_output_loc = (symbol_period >>1) >> PRECISION_BITS;
00226 bump = 0;
00227
00228 #if defined (ENABLE_MMX)
00229 processedTaps = new (mmxTaps *[num_ch]);
00230 processedTaps[0]=NULL;
00231 #endif
00232 num_taps[0] = t;
00233 phase_corr_incr[0] = VrComplex(1,0);
00234 phase_offset[0] = VrComplex(1,0);
00235 center_freq[0] = freq;
00236 gain[0] = g;
00237 }
00238
00239 template<class iType>
00240 void VrSyncFilter<iType>::initialize()
00241 {
00242 for (int i=0; i<num_ch; i++){
00243 taps[i]=new VrComplex[num_taps[i]];
00244 buildFilter_complex(i);
00245 }
00246
00247 int max_num_taps = 0;
00248 for (int i=0; i<num_ch; i++)
00249 if (num_taps[i] > max_num_taps) max_num_taps = num_taps[i];
00250 history = max_num_taps + BOZO_HISTORY;
00251 differential_offset = 0;
00252 delta_differential_offset=0;
00253 }
00254
00255 template<class iType>
00256 int VrSyncFilter<iType>::setCenter_Freq(int ch, float cf)
00257 {
00258 center_freq[ch] = cf;
00259 buildFilter_complex(ch);
00260 return 1;
00261 }
00262
00263 template<class iType>
00264 int VrSyncFilter<iType>::setCenter_Freq(float cf)
00265 {
00266 return setCenter_Freq(0,cf);
00267 }
00268
00269 template<class iType>
00270 int VrSyncFilter<iType>::setPhase_Offset(VrComplex f)
00271 {
00272 phase_offset[0]=f;
00273 return 1;
00274 }
00275
00276 template<class iType>
00277 int VrSyncFilter<iType>::setNumber_Taps(int ch, int numT)
00278 {
00279 num_taps[ch] = numT;
00280 delete taps[ch];
00281 taps[ch]=new VrComplex[num_taps[ch]];
00282
00283
00284 int max_num_taps = 0;
00285 for (int i=0; i<num_ch; i++){
00286 if (num_taps[i] > max_num_taps) max_num_taps = num_taps[i];
00287 }
00288
00289 history = max_num_taps + BOZO_HISTORY;
00290
00291 buildFilter_complex(ch);
00292 return 1;
00293 }
00294
00295 template<class iType>
00296 int VrSyncFilter<iType>::setNumber_Taps(int numT)
00297 {
00298 return setNumber_Taps(0, numT);
00299 }
00300
00301 template<class iType>
00302 float VrSyncFilter<iType>::getSymbol_Period()
00303 {
00304
00305 return (float)symbol_period /(float)(1 << PRECISION_BITS) / (float)getInputSamplingFrequencyN(0);
00306 }
00307
00308 template<class iType>
00309 int VrSyncFilter<iType>::setSymbol_Period(float period)
00310 {
00311 double arg = 2*M_PI*center_freq[0] / (double)getInputSamplingFrequencyN(0);
00312
00313
00314 symbol_period = (int)(period * (float)(1 << PRECISION_BITS) * (float)getInputSamplingFrequencyN(0));
00315
00316 phase_corr_incr[0] = VrComplex(cos(arg*(double)symbol_period/(double)(1<<PRECISION_BITS)),
00317 (-1)*sin(arg*(double)symbol_period/(double)(1<<PRECISION_BITS)));
00318
00319 cout << " Set period to " << (double) symbol_period / (double)(1 << PRECISION_BITS) << endl;
00320 return 1;
00321 }
00322
00323 template<class iType>
00324 int VrSyncFilter<iType>::setSymbol_Timing(float fraction)
00325 {
00326 double arg = 2*M_PI*center_freq[0] / (double)getInputSamplingFrequencyN(0);
00327
00328
00329
00330 bump = (int)( fraction * (float) symbol_period);
00331 phase_bump = VrComplex(cos(arg*(double)bump/(double)(1<<PRECISION_BITS)),
00332 (-1)*sin(arg*(double)bump/(double)(1<<PRECISION_BITS)));
00333 return 1;
00334 }
00335
00336 template<class iType> int
00337 VrSyncFilter<iType>::forecast(VrSampleRange output,
00338 VrSampleRange inputs[]) {
00339
00340 if (!is_synced(output.index)) return -1;
00341
00342 for(unsigned int i=0;i<numberInputs;i++) {
00343 inputs[i].index=output.index*decimation+ differential_offset;
00344
00345 inputs[i].size=output.size*decimation + history-1;
00346 }
00347 return 0;
00348 }
00349
00350 template<class iType>
00351 VrSyncFilter<iType>::~VrSyncFilter()
00352 {
00353
00354 for (int i=0; i<num_ch; i++){
00355 delete taps[i];
00356 #if defined (ENABLE_MMX)
00357 if(processedTaps[i]!=NULL)
00358 delete processedTaps[i];
00359 #endif
00360 }
00361 delete num_taps;
00362 delete [] phase_corr_incr;
00363 delete [] phase_offset;
00364 delete center_freq;
00365 delete gain;
00366 delete taps;
00367 }
00368 #endif