sqr_pll_bp.cpp
上传用户:jtjnyq9001
上传日期:2014-11-21
资源大小:3974k
文件大小:6k
源码类别:

3G开发

开发平台:

Visual C++

  1. //
  2. //  File = sqr_pll_bp.cpp
  3. //
  4. //#include <stdlib.h>
  5. //#include <fstream.h>
  6. #include "parmfile.h"
  7. #include "misdefs.h"
  8. #include "model_error.h"
  9. #include "sqr_pll_bp.h"
  10. #include "butt_filt_iir.h"
  11. #include "model_graph.h"
  12. //#include "sinc.h"
  13. extern ParmFile *ParmInput;
  14. extern PracSimModel *ActiveModel;
  15. //======================================================
  16. BandpassSquaringPLL::BandpassSquaringPLL( char* instance_name,
  17.                         PracSimModel* outer_model,
  18.                         Signal<float>* fsig_input,
  19.                         Signal<float>* fsig_square_of_input,
  20.                         Signal<float>* fsig_phase_error,
  21.                         Signal<float>* fsig_filtered_error,
  22.                         Signal<float>* fsig_vco_output,
  23.                         Signal<float>* vco_freq_sig,
  24.                         Signal<float>* ref_phase_sig,
  25.                         Signal<float>* out_sig )
  26.                 :PracSimModel(instance_name,
  27.                               outer_model)
  28. {
  29.    MODEL_NAME(BandpassSquaringPLL);
  30.    //---------------------------------------
  31.    //  Read model config parms
  32.    OPEN_PARM_BLOCK;
  33.    GET_DOUBLE_PARM(K_Sub_D);
  34.    GET_DOUBLE_PARM(K_Sub_0);
  35.    GET_DOUBLE_PARM(Center_Freq_Hz);
  36.    GET_BOOL_PARM(m_UsingDco);
  37.    //--------------------------------------
  38.    //  Connect input and output signals
  39.    fsig_Input = fsig_input;
  40.    fsig_Phase_Error = fsig_phase_error;
  41.    fsig_Filtered_Error = fsig_filtered_error;
  42.    fsig_Osc_Output = fsig_vco_output;
  43.    fsig_Osc_Freq = vco_freq_sig;
  44.    fsig_Osc_Phase = ref_phase_sig;
  45.    fsig_Output = out_sig;
  46.    fsig_Square_Of_Input = fsig_square_of_input;
  47.    MAKE_OUTPUT( fsig_Output );
  48.    MAKE_OUTPUT( fsig_Phase_Error );
  49.    MAKE_OUTPUT( fsig_Filtered_Error );
  50.    MAKE_OUTPUT( fsig_Osc_Output );
  51.    MAKE_OUTPUT( fsig_Osc_Freq );
  52.    MAKE_OUTPUT( fsig_Osc_Phase );
  53.    MAKE_OUTPUT( fsig_Square_Of_Input );
  54.    
  55.    MAKE_INPUT( fsig_Input );
  56.    //----------------------------------
  57.    //  Compute derived parameters
  58.    Omega_Sub_0 = TWO_PI * Center_Freq_Hz;
  59.    //--------------------------------
  60.    // create loop filter object
  61.    char sub_name[50];
  62.    strcpy(sub_name, GetInstanceName());
  63.    strcat(sub_name, ":Filter");
  64.    Filter_Core = new ButterworthFilterByIir<float>( sub_name, this);
  65. }
  66. //======================================
  67. BandpassSquaringPLL::~BandpassSquaringPLL( void ){ };
  68. //=======================================
  69. void BandpassSquaringPLL::Initialize(void)
  70. {
  71.    //------------------
  72.    int block_size = fsig_Output->GetBlockSize();
  73.    Samp_Intvl = fsig_Input->GetSampIntvl();
  74.    Filter_Core->Initialize(block_size, Samp_Intvl);
  75.    Osc_Output_Prev_Val = 0.0;
  76.    OscOutput = 0;
  77.    Phi_Sub_2 = 0;
  78. }
  79. //=======================================================
  80. int BandpassSquaringPLL::Execute()
  81. {
  82.    // pointers for signal data
  83.    float *fsOutput_ptr;
  84.    float *fsPhaseError_ptr;
  85.    float *fs_filtered_error_ptr;
  86.    float *fsOscOutput_ptr;
  87.    float *fsOscFreq_ptr;
  88.    float *fsOscPhase_ptr;
  89.    float *fsInput_ptr;
  90.    float *fsSquareOfInput_ptr;
  91.    float input_val;
  92.    float osc_output_val;
  93.    float filt_val;
  94.    float inst_freq;
  95.    double samp_intvl;
  96.    double phase_error;
  97.    double err_sum=0;
  98.    double err_avg;
  99.    int is;
  100.    int block_size;
  101.    //--------------------------------------------------------------
  102.    // set up pointers to data buffers for input and output signals
  103.    fsOutput_ptr = GET_OUTPUT_PTR( fsig_Output );
  104.    fsPhaseError_ptr = GET_OUTPUT_PTR( fsig_Phase_Error );
  105.    fs_filtered_error_ptr = GET_OUTPUT_PTR( fsig_Filtered_Error );
  106.    fsOscOutput_ptr = GET_OUTPUT_PTR( fsig_Osc_Output );
  107.    fsOscFreq_ptr = GET_OUTPUT_PTR( fsig_Osc_Freq );
  108.    fsOscPhase_ptr = GET_OUTPUT_PTR( fsig_Osc_Phase );
  109.    fsSquareOfInput_ptr = GET_OUTPUT_PTR( fsig_Square_Of_Input );
  110.    fsInput_ptr = GET_INPUT_PTR( fsig_Input );
  111.    //---------------------------------------------------------------
  112.    samp_intvl = Samp_Intvl;
  113.    osc_output_val = Osc_Output_Prev_Val;
  114.    block_size = fsig_Input->GetValidBlockSize();
  115.    fsig_Output->SetValidBlockSize(block_size);
  116.    fsig_Phase_Error->SetValidBlockSize(block_size);
  117.    fsig_Filtered_Error->SetValidBlockSize(block_size);
  118.    fsig_Osc_Output->SetValidBlockSize(block_size);
  119.    fsig_Osc_Freq->SetValidBlockSize(block_size);
  120.    fsig_Osc_Phase->SetValidBlockSize(block_size);
  121.    fsig_Square_Of_Input->SetValidBlockSize(block_size);
  122.    //-------------------------------------------------------
  123.    for (is=0; is<block_size; is++)
  124.    {
  125.       *fsOscOutput_ptr++ = osc_output_val;
  126.       input_val = *fsInput_ptr;
  127.       *fsSquareOfInput_ptr++ = input_val * input_val;
  128.       phase_error = K_Sub_D * input_val * input_val * OscOutput;
  129. //      phase_error = K_Sub_D * input_val * OscOutput;
  130.       *fsPhaseError_ptr++ = phase_error;
  131.       //--------------------------------
  132.       //  filter the error signal
  133.       filt_val = Filter_Core->ProcessSample(phase_error);
  134.       *fs_filtered_error_ptr++ = filt_val;
  135.       //---------------------------------------
  136.       //  just for educational purposes, 
  137.       //  compute the average of the filtered error signal
  138.       err_sum += filt_val;
  139.       //----------------------------------------
  140.       // use filtered error signal to drive VCO
  141.       inst_freq = Omega_Sub_0 + K_Sub_0 * filt_val;
  142.       Phi_Sub_2 += inst_freq * samp_intvl;
  143.       if(Phi_Sub_2 > PI) Phi_Sub_2 -= TWO_PI;
  144.       if(m_UsingDco) // make discrete valued output
  145.       {
  146.          if( (Phi_Sub_2 < -PI_OVER_TWO) || (Phi_Sub_2 > PI_OVER_TWO))
  147.          {
  148.             OscOutput = -1;
  149.          }
  150.          else
  151.          {
  152.             OscOutput = 1;
  153.          }
  154.       }
  155.       else // simulate analog VCO output
  156.       {
  157.          OscOutput = cos(Phi_Sub_2);
  158.       }
  159.       *fsOscPhase_ptr++ = Phi_Sub_2;
  160.       *fsOscFreq_ptr++ = inst_freq/TWO_PI;
  161.       *fsOutput_ptr++ = sin(Phi_Sub_2);
  162.       fsInput_ptr++;
  163.    }
  164.    err_avg = err_sum / block_size;
  165.    BasicResults << "avg PLL error = " << err_avg << endl;
  166.    Osc_Output_Prev_Val = osc_output_val;
  167.   return(_MES_AOK);
  168. }