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