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 gr_complex 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