00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
#ifndef _GRCOSTASLOOP_H_
00024
#define _GRCOSTASLOOP_H_
00025
00026
#include <VrSigProc.h>
00027
#include <gr_fir.h>
00028
#include <gr_iir.h>
00029
#include <gr_firdes.h>
00030
#include <gr_nco.h>
00031
00032
template<
class iType,
class oType>
00033 class GrCostasLoop :
public VrSigProc
00034 {
00035
protected:
00036 gr_fir<iType,iType,float> *
ifilter;
00037 gr_fir<iType,iType,float> *
qfilter;
00038 gr_iir<double,double,double> *
loopfilter;
00039 vector<double>
fftaps;
00040 vector<double>
fbtaps;
00041 gr_nco<iType,oType> *
nco;
00042 unsigned int decimate;
00043 double sensitivity;
00044 double freq;
00045 double Fc;
00046 double arg;
00047 double argInc;
00048 double phi;
00049 double phase;
00050
virtual void initialize();
00051
public:
00052 virtual const char *
name() {
return "GrCostasLoop"; }
00053
virtual int work(
VrSampleRange output,
void *ao[],
00054
VrSampleRange inputs[],
void *ai[]);
00055 virtual ~GrCostasLoop()
00056 {
00057
delete ifilter;
00058
delete qfilter;
00059
delete loopfilter;
00060
delete nco;
00061 }
00062 GrCostasLoop(
int d,
double f,
double s,
double Fc)
00063 :
VrSigProc(1,sizeof(
iType),sizeof(
oType)),
00064
decimate(d),
sensitivity(s),
freq(f),Fc(Fc),
arg(0) { }
00065 };
00066
00067
template<
class iType,
class oType>
void
00068 GrCostasLoop<iType,oType>::initialize()
00069 {
00070
00071
double Fs =
getInputSamplingFrequencyN (0);
00072 vector<float> lpf_coeffs =
00073
gr_firdes::low_pass (1.0, Fs,
Fc,
Fc/10,gr_firdes::WIN_HAMMING,0);
00074
ifilter =
new gr_fir<iType,iType,float>(lpf_coeffs);
00075
qfilter =
new gr_fir<iType,iType,float>(lpf_coeffs);
00076
loopfilter =
new gr_iir<double,double,double>(
fftaps,
fbtaps);
00077
nco =
new gr_nco<float,float>();
00078
argInc = 2*M_PI*
freq*(1 / (
double)
getInputSamplingFrequencyN(0));
00079 }
00080
00081
template<
class iType,
class oType>
int
00082 GrCostasLoop<iType,oType>::work(
VrSampleRange output,
void *ao[],
00083
VrSampleRange inputs[],
void *ai[])
00084 {
00085
iType **i = (
iType**)ai;
00086
oType **o = (
iType**)ao;
00087
int size = output.
size/
decimate;
00088
00089
float alpha=0.01;
00090
float beta=0.01;
00091
iType i_path_fifo[size];
00092
iType q_path_fifo[size];
00093
iType i_filtered[size];
00094
iType q_filtered[size];
00095
iType phase_det_out[size];
00096
VrComplex phase_corr;
00097
00098
for (
int j = 0; j < size; j++)
00099 {
00100
00101
for(
int k = 0; k<
decimate;k++)
00102 {
00103 i_path_fifo[j*decimate+k] = i[0][j*decimate+k] *
nco->
get_phase().real();
00104 q_path_fifo[j*decimate+k] = i[0][j*decimate+k] * -
nco->
get_phase().imag();
00105
nco->
step();
00106 }
00107
00108 i_filtered[j]=
ifilter->
filter (i_path_fifo);
00109 q_filtered[j]=
qfilter->
filter (q_path_fifo);
00110
00111 phase_det_out[j] = atan2(q_filtered[j],i_filtered[j]);
00112
00113
00114
00115 phase_corr.
real(
cos(alpha*phase_det_out[j]));
00116 phase_corr.
imag(
sin(alpha*phase_det_out[j]));
00117
nco->rotate_phase(phase_corr);
00118
nco->delta_freq(beta*phase_det_out[j]);
00119 o[0][j] = 0;
00120 }
00121
00122
return output.
size;
00123 }
00124
00125
#endif