开通博客赚积分
发布资源赚积分
分类

源码开发语言/平台
当前位置：首页 > 源码/资料 > 通讯/手机编程 > 3G开发 > 查看源码
costas_loop.cpp
资源名称：simulation_wireless_c++.rar　[点击查看]
上传用户：jtjnyq9001
上传日期：2014-11-21
资源大小：3974k
文件大小：6k
源码类别：

3G开发

开发平台：

Visual C++

costas_loop.cpp：源码内容
  1. //
  2. //  File = costas_loop.cpp
  3. //
  5. //#include <stdlib.h>
  6. //#include <fstream.h>
  7. #include "parmfile.h"
  8. #include "misdefs.h"
  9. #include "model_error.h"
  10. #include "costas_loop.h"
  11. #include "butt_filt_iir.h"
  12. #include "model_graph.h"
  13. //#include "sinc.h"
  14. extern ParmFile *ParmInput;
  15. extern PracSimModel *ActiveModel;
  17. //======================================================
  19. CostasLoop::CostasLoop( char* instance_name,
  20.                         PracSimModel* outer_model,
  21.                         Signal<float>* fsig_input,
  22.                         Signal<float>* fsig_phase_error,
  23.                         Signal<float>* fsig_filtered_error,
  24.                         Signal<float>* fsig_vco_output,
  25.                         Signal<float>* vco_freq_sig,
  26.                         Signal<float>* ref_phase_sig,
  27.                         Signal<float>* out_sig )
  28.                 :PracSimModel(instance_name,
  29.                               outer_model)
  30. {
  31.    MODEL_NAME(CostasLoop);
  33.    //---------------------------------------
  34.    //  Read model config parms
  36.    OPEN_PARM_BLOCK;
  38.    GET_DOUBLE_PARM(K_Sub_D);
  39.    GET_DOUBLE_PARM(K_Sub_0);
  40.    GET_DOUBLE_PARM(Center_Freq_Hz);
  41.    GET_BOOL_PARM(m_UsingDco);
  42.    //GET_DOUBLE_PARM(Scaler_Divisor);
  43.    Scaler_Divisor = 1.0;
  45.    //--------------------------------------
  46.    //  Connect input and output signals
  48.    fsig_Input = fsig_input;
  49.    fsig_Phase_Error = fsig_phase_error;
  50.    fsig_Filtered_Error = fsig_filtered_error;
  51.    fsig_Osc_Output = fsig_vco_output;
  52.    fsig_Osc_Freq = vco_freq_sig;
  53.    fsig_Osc_Phase = ref_phase_sig;
  54.    fsig_Output = out_sig;
  56.    MAKE_OUTPUT( fsig_Output );
  57.    MAKE_OUTPUT( fsig_Phase_Error );
  58.    MAKE_OUTPUT( fsig_Filtered_Error );
  59.    MAKE_OUTPUT( fsig_Osc_Output );
  60.    MAKE_OUTPUT( fsig_Osc_Freq );
  61.    MAKE_OUTPUT( fsig_Osc_Phase );
  62.    MAKE_INPUT( fsig_Input );
  64.    //----------------------------------
  65.    //  Compute derived parameters
  67.    Omega_Sub_0 = TWO_PI * Center_Freq_Hz;
  68.    //--------------------------------
  69.    // create loop filter object
  70.    char sub_name[50];
  71.    strcpy(sub_name, GetInstanceName());
  72.    strcat(sub_name, ":I_Filter");
  73.    I_Filter_Core = new ButterworthFilterByIir<float>( sub_name, this);
  75.    strcpy(sub_name, GetInstanceName());
  76.    strcat(sub_name, ":Q_Filter");
  77.    Q_Filter_Core = new ButterworthFilterByIir<float>( sub_name, this);
  79.    strcpy(sub_name, GetInstanceName());
  80.    strcat(sub_name, ":Prod_Filter");
  81.    Prod_Filter_Core = new ButterworthFilterByIir<float>( sub_name, this);
  82. }
  84. //======================================
  85. CostasLoop::~CostasLoop( void ){ };
  87. //=======================================
  88. void CostasLoop::Initialize(void)
  89. {
  90.    //------------------
  91.    int block_size = fsig_Output->GetBlockSize();
  92.    Samp_Intvl = fsig_Input->GetSampIntvl();
  93.    I_Filter_Core->Initialize(block_size, Samp_Intvl);
  94.    Q_Filter_Core->Initialize(block_size, Samp_Intvl);
  95.    Prod_Filter_Core->Initialize(block_size, Samp_Intvl);
  96.    Osc_Output_Prev_Val = 0.0;
  97.    I_OscOutput = 0;
  98.    Q_OscOutput = 0;
  99.    Phi_Sub_2 = 0;
  100.    Phi_Divided = 0;
  102. }
  103. //=======================================================
  104. int CostasLoop::Execute()
  105. {
  106.    // pointers for signal data
  108.    float *fsOutput_ptr;
  109.    float *fsPhaseError_ptr;
  110.    float *fs_filtered_error_ptr;
  111.    float *fsOscOutput_ptr;
  112.    float *fsOscFreq_ptr;
  113.    float *fsOscPhase_ptr;
  114.    float *fsInput_ptr;
  116.    float input_val;
  117.    float osc_output_val;
  118.    float i_filt_val;
  119.    float q_filt_val;
  120.    float inst_freq;
  122.    double samp_intvl;
  123.    double error_val;
  124.    double filt_val;
  125.    double i_arm_product;
  126.    double q_arm_product;
  127.    double err_sum=0;
  128.    double err_avg;
  129.    int is;
  130.    int block_size;
  133.    //--------------------------------------------------------------
  134.    // set up pointers to data buffers for input and output signals
  136.    fsOutput_ptr = GET_OUTPUT_PTR( fsig_Output );
  137.    fsPhaseError_ptr = GET_OUTPUT_PTR( fsig_Phase_Error );
  138.    fs_filtered_error_ptr = GET_OUTPUT_PTR( fsig_Filtered_Error );
  140.    fsOscOutput_ptr = GET_OUTPUT_PTR( fsig_Osc_Output );
  141.    fsOscFreq_ptr = GET_OUTPUT_PTR( fsig_Osc_Freq );
  142.    fsOscPhase_ptr = GET_OUTPUT_PTR( fsig_Osc_Phase );
  143.    fsInput_ptr = GET_INPUT_PTR( fsig_Input );
  144.    //---------------------------------------------------------------
  146.    samp_intvl = Samp_Intvl;
  147.    osc_output_val = Osc_Output_Prev_Val;
  149.    block_size = fsig_Input->GetValidBlockSize();
  151.    fsig_Output->SetValidBlockSize(block_size);
  152.    fsig_Phase_Error->SetValidBlockSize(block_size);
  153.    fsig_Filtered_Error->SetValidBlockSize(block_size);
  154.    fsig_Osc_Output->SetValidBlockSize(block_size);
  155.    fsig_Osc_Freq->SetValidBlockSize(block_size);
  156.    fsig_Osc_Phase->SetValidBlockSize(block_size);
  158.    //-------------------------------------------------------
  160.    for (is=0; is<block_size; is++)
  161.    {
  162.       *fsOscOutput_ptr++ = osc_output_val;
  163.       input_val = *fsInput_ptr;
  164.       i_arm_product = K_Sub_D * input_val * I_OscOutput;
  165.       q_arm_product = K_Sub_D * input_val * Q_OscOutput;
  166.       *fsPhaseError_ptr++ = i_arm_product;
  168.       //--------------------------------
  169.       //  filter the arm signals
  171.       i_filt_val = I_Filter_Core->ProcessSample(i_arm_product);
  172.       *fs_filtered_error_ptr++ = i_filt_val;
  173.       q_filt_val = Q_Filter_Core->ProcessSample(q_arm_product);
  175.       //----------------------------------------
  176.       //  multiply arm signals
  178.       error_val = i_filt_val * q_filt_val;
  180.       //----------------------------------------
  181.       //  filter the result
  183.       filt_val = Prod_Filter_Core->ProcessSample(error_val);
  185.       //----------------------------------------
  186.       // use filtered error signal to drive VCO
  188.       inst_freq = Omega_Sub_0 + K_Sub_0 * filt_val;
  189.       Phi_Sub_2 += inst_freq * samp_intvl;
  190.       if(Phi_Sub_2 > PI) Phi_Sub_2 -= TWO_PI;
  192.       I_OscOutput = -sin(Phi_Sub_2);
  193.       Q_OscOutput = -cos(Phi_Sub_2);
  195.       *fsOscPhase_ptr++ = Phi_Sub_2;
  196.       *fsOscFreq_ptr++ = inst_freq/TWO_PI;
  198.       Phi_Divided += inst_freq * samp_intvl/Scaler_Divisor;
  199.       if(Phi_Divided > PI) Phi_Divided -= TWO_PI;
  200.       *fsOutput_ptr++ = sin(Phi_Divided);
  202.       fsInput_ptr++;
  203.    }
  204.    err_avg = err_sum / block_size;
  205.    BasicResults << "avg PLL error = " << err_avg << endl;
  206.    Osc_Output_Prev_Val = osc_output_val;
  208.   return(_MES_AOK);
  209. }