sqr_pll_bp.cpp
上传用户:jtjnyq9001
上传日期:2014-11-21
资源大小:3974k
文件大小:6k
- //
- // File = sqr_pll_bp.cpp
- //
- //#include <stdlib.h>
- //#include <fstream.h>
- #include "parmfile.h"
- #include "misdefs.h"
- #include "model_error.h"
- #include "sqr_pll_bp.h"
- #include "butt_filt_iir.h"
- #include "model_graph.h"
- //#include "sinc.h"
- extern ParmFile *ParmInput;
- extern PracSimModel *ActiveModel;
- //======================================================
- BandpassSquaringPLL::BandpassSquaringPLL( char* instance_name,
- PracSimModel* outer_model,
- Signal<float>* fsig_input,
- Signal<float>* fsig_square_of_input,
- Signal<float>* fsig_phase_error,
- Signal<float>* fsig_filtered_error,
- Signal<float>* fsig_vco_output,
- Signal<float>* vco_freq_sig,
- Signal<float>* ref_phase_sig,
- Signal<float>* out_sig )
- :PracSimModel(instance_name,
- outer_model)
- {
- MODEL_NAME(BandpassSquaringPLL);
- //---------------------------------------
- // Read model config parms
- OPEN_PARM_BLOCK;
- GET_DOUBLE_PARM(K_Sub_D);
- GET_DOUBLE_PARM(K_Sub_0);
- GET_DOUBLE_PARM(Center_Freq_Hz);
- GET_BOOL_PARM(m_UsingDco);
- //--------------------------------------
- // Connect input and output signals
- fsig_Input = fsig_input;
- fsig_Phase_Error = fsig_phase_error;
- fsig_Filtered_Error = fsig_filtered_error;
- fsig_Osc_Output = fsig_vco_output;
- fsig_Osc_Freq = vco_freq_sig;
- fsig_Osc_Phase = ref_phase_sig;
- fsig_Output = out_sig;
- fsig_Square_Of_Input = fsig_square_of_input;
- MAKE_OUTPUT( fsig_Output );
- MAKE_OUTPUT( fsig_Phase_Error );
- MAKE_OUTPUT( fsig_Filtered_Error );
- MAKE_OUTPUT( fsig_Osc_Output );
- MAKE_OUTPUT( fsig_Osc_Freq );
- MAKE_OUTPUT( fsig_Osc_Phase );
- MAKE_OUTPUT( fsig_Square_Of_Input );
-
- MAKE_INPUT( fsig_Input );
- //----------------------------------
- // Compute derived parameters
- Omega_Sub_0 = TWO_PI * Center_Freq_Hz;
- //--------------------------------
- // create loop filter object
- char sub_name[50];
- strcpy(sub_name, GetInstanceName());
- strcat(sub_name, ":Filter ");
- Filter_Core = new ButterworthFilterByIir<float>( sub_name, this);
- }
- //======================================
- BandpassSquaringPLL::~BandpassSquaringPLL( void ){ };
- //=======================================
- void BandpassSquaringPLL::Initialize(void)
- {
- //------------------
- int block_size = fsig_Output->GetBlockSize();
- Samp_Intvl = fsig_Input->GetSampIntvl();
- Filter_Core->Initialize(block_size, Samp_Intvl);
- Osc_Output_Prev_Val = 0.0;
- OscOutput = 0;
- Phi_Sub_2 = 0;
- }
- //=======================================================
- int BandpassSquaringPLL::Execute()
- {
- // pointers for signal data
- float *fsOutput_ptr;
- float *fsPhaseError_ptr;
- float *fs_filtered_error_ptr;
- float *fsOscOutput_ptr;
- float *fsOscFreq_ptr;
- float *fsOscPhase_ptr;
- float *fsInput_ptr;
- float *fsSquareOfInput_ptr;
- float input_val;
- float osc_output_val;
- float filt_val;
- float inst_freq;
- double samp_intvl;
- double phase_error;
- double err_sum=0;
- double err_avg;
- int is;
- int block_size;
- //--------------------------------------------------------------
- // set up pointers to data buffers for input and output signals
- fsOutput_ptr = GET_OUTPUT_PTR( fsig_Output );
- fsPhaseError_ptr = GET_OUTPUT_PTR( fsig_Phase_Error );
- fs_filtered_error_ptr = GET_OUTPUT_PTR( fsig_Filtered_Error );
- fsOscOutput_ptr = GET_OUTPUT_PTR( fsig_Osc_Output );
- fsOscFreq_ptr = GET_OUTPUT_PTR( fsig_Osc_Freq );
- fsOscPhase_ptr = GET_OUTPUT_PTR( fsig_Osc_Phase );
- fsSquareOfInput_ptr = GET_OUTPUT_PTR( fsig_Square_Of_Input );
- fsInput_ptr = GET_INPUT_PTR( fsig_Input );
- //---------------------------------------------------------------
- samp_intvl = Samp_Intvl;
- osc_output_val = Osc_Output_Prev_Val;
- block_size = fsig_Input->GetValidBlockSize();
- fsig_Output->SetValidBlockSize(block_size);
- fsig_Phase_Error->SetValidBlockSize(block_size);
- fsig_Filtered_Error->SetValidBlockSize(block_size);
- fsig_Osc_Output->SetValidBlockSize(block_size);
- fsig_Osc_Freq->SetValidBlockSize(block_size);
- fsig_Osc_Phase->SetValidBlockSize(block_size);
- fsig_Square_Of_Input->SetValidBlockSize(block_size);
- //-------------------------------------------------------
- for (is=0; is<block_size; is++)
- {
- *fsOscOutput_ptr++ = osc_output_val;
- input_val = *fsInput_ptr;
- *fsSquareOfInput_ptr++ = input_val * input_val;
- phase_error = K_Sub_D * input_val * input_val * OscOutput;
- // phase_error = K_Sub_D * input_val * OscOutput;
- *fsPhaseError_ptr++ = phase_error;
- //--------------------------------
- // filter the error signal
- filt_val = Filter_Core->ProcessSample(phase_error);
- *fs_filtered_error_ptr++ = filt_val;
- //---------------------------------------
- // just for educational purposes,
- // compute the average of the filtered error signal
- err_sum += filt_val;
- //----------------------------------------
- // use filtered error signal to drive VCO
- inst_freq = Omega_Sub_0 + K_Sub_0 * filt_val;
- Phi_Sub_2 += inst_freq * samp_intvl;
- if(Phi_Sub_2 > PI) Phi_Sub_2 -= TWO_PI;
- if(m_UsingDco) // make discrete valued output
- {
- if( (Phi_Sub_2 < -PI_OVER_TWO) || (Phi_Sub_2 > PI_OVER_TWO))
- {
- OscOutput = -1;
- }
- else
- {
- OscOutput = 1;
- }
- }
- else // simulate analog VCO output
- {
- OscOutput = cos(Phi_Sub_2);
- }
- *fsOscPhase_ptr++ = Phi_Sub_2;
- *fsOscFreq_ptr++ = inst_freq/TWO_PI;
- *fsOutput_ptr++ = sin(Phi_Sub_2);
- fsInput_ptr++;
- }
- err_avg = err_sum / block_size;
- BasicResults << "avg PLL error = " << err_avg << endl;
- Osc_Output_Prev_Val = osc_output_val;
- return(_MES_AOK);
- }