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

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

3G开发

开发平台：

Visual C++

coarse_delay_est.cpp：源码内容
  1. //
  2. //  File = coarse_delay_est.cpp
  3. //
  5. #include <stdlib.h>
  6. #include <complex>
  7. #include "parmfile.h"
  8. #include "coarse_delay_est.h"
  9. #include "misdefs.h"
  10. #include "model_graph.h"
  11. #include "dit_pino_T.h"
  12. #include "dit_nipo_T.h"
  14. extern ParmFile* ParmInput;
  15. extern int PassNumber;
  16. #ifdef _DEBUG
  17.   extern ofstream *DebugFile;
  18. #endif
  20. //======================================================
  22. CoarseDelayEstimator::CoarseDelayEstimator( char* instance_name,
  23.                                 PracSimModel* outer_model,
  24.                                 Signal<float>* in_sig,
  25.                                 Signal<float>* ref_sig,
  26.                                 Signal<float>* out_sig,
  27.                                 Control<bool>* dly_est_is_valid_cntl,
  28.                                 Control<float>* delay_at_max_corr,
  29.                                 Control<int>* samps_delay_at_max_corr)
  30.             :PracSimModel(instance_name,
  31.                           outer_model)
  32. {
  33.    In_Sig = in_sig;
  34.    Ref_Sig = ref_sig;
  35.    Out_Sig = out_sig;
  37.    Delay_At_Max_Corr = delay_at_max_corr;
  38.    Samps_Delay_At_Max_Corr = samps_delay_at_max_corr;
  39.    Dly_Est_Is_Valid_Cntl = dly_est_is_valid_cntl;
  40.    Dly_Est_Is_Valid_Cntl->SetValue(false);
  42.   OPEN_PARM_BLOCK;
  44.   GET_INT_PARM(Num_Corr_Passes);
  45.   GET_BOOL_PARM(Limited_Search_Window_Enab);
  46.   if(Limited_Search_Window_Enab)
  47.   {
  48.      GET_INT_PARM(Search_Window_Beg);
  49.      GET_INT_PARM(Search_Window_End);
  50.   }
  51.   GET_BOOL_PARM(Invert_Input_Sig_Enab);
  52.   GET_INT_PARM(Smoothing_Sidelobe_Len);
  54.   MAKE_OUTPUT(Out_Sig);
  55.   MAKE_INPUT(In_Sig);
  56.   MAKE_INPUT(Ref_Sig);
  58.   SAME_RATE(In_Sig, Out_Sig);
  59.   SAME_RATE(Ref_Sig, Out_Sig);
  61.   Spectrum_File = new ofstream("spectrum.txt", ios::out);
  62. }
  63. //======================================================
  64. CoarseDelayEstimator::~CoarseDelayEstimator( void ){ };
  65. //======================================================
  66. void CoarseDelayEstimator::Initialize(void)
  67. {
  68.    int dummy_size, i;
  69.    double tmp_nsexp;
  70.    double frac_part, int_part;
  71.    float sample=0;
  73.    Proc_Block_Size = Out_Sig->GetBlockSize();
  74.    Samp_Intvl = Out_Sig->GetSampIntvl();
  76.    In_Sig_Buf = new AuxSignalBuffer<float>(sample, Proc_Block_Size);
  77.    Ref_Sig_Buf = new AuxSignalBuffer<float>(sample, Proc_Block_Size);
  79.    tmp_nsexp = log10(double(Proc_Block_Size))/log10(2.0);
  80.    frac_part = modf(tmp_nsexp, &int_part);
  82.    if( frac_part != 0.0 )
  83.       {
  84.       Ns_Exp = int_part + 2;
  85.       }
  86.    else
  87.       {
  88.       Ns_Exp = int_part + 1;
  89.       }
  91.    Full_Corr_Size = pow(2.0, Ns_Exp);
  92.    X = new std::complex<float>[Full_Corr_Size];
  93.    Y = new std::complex<float>[Full_Corr_Size];
  95.    if(Limited_Search_Window_Enab)
  96.       {
  97.       if(Search_Window_Beg < 0)
  98.          {
  99.          Neg_Window_Beg = Full_Corr_Size + Search_Window_Beg;
  100.          Pos_Window_Beg = 0;
  101.          }
  102.       else
  103.          {
  104.          Neg_Window_Beg = Full_Corr_Size;
  105.          Pos_Window_Beg = Search_Window_Beg;
  106.          }
  107.       if(Search_Window_End >= 0)
  108.          {
  109.          Pos_Window_End = Search_Window_End;
  110.          Neg_Window_End = Full_Corr_Size-1;
  111.          }
  112.       else
  113.          {
  114.          Pos_Window_End = -1;
  115.          Neg_Window_End = Search_Window_End;
  116.          }
  117.       }
  118.    else
  119.       {
  120.       Neg_Window_Beg = Full_Corr_Size/2 + 1;
  121.       Neg_Window_End = Full_Corr_Size-1;
  122.       Pos_Window_Beg = 0;
  123.       Pos_Window_End = Full_Corr_Size/2 - 1;
  124.       }
  127.    dummy_size = Full_Corr_Size - Proc_Block_Size;
  128.    Zero_Array = new std::complex<float>[dummy_size];
  129.    for( i = 0; i < dummy_size; i++)
  130.    {
  131.       Zero_Array[i] = std::complex<float>(0.0, 0.0);
  132.    }
  133.    Size_Of_FComplex = sizeof(std::complex<float>);
  135.    Max_Corr = 0.0;
  136.    Max_Corr_Time = 0.0;
  138.   Diff_Response = new std::complex<float>[Full_Corr_Size];
  139.   for( i=0; i<Full_Corr_Size; i++)
  140.   {
  141.     Diff_Response[i] = 0.0;
  142.   }
  143.    Corr_Pass_Count = 0;
  144.    return;
  145. }
  146. //======================================================
  147. int CoarseDelayEstimator::Execute()
  148. {
  149.    using std::complex;
  150.    float *in_sig_ptr, *in_sig_buf_ptr;
  151.    float *ref_sig_ptr, *ref_sig_buf_ptr;
  152.    int in_sig_buf_count;
  153.    int ref_sig_buf_count;
  154.    float *out_sig_ptr;
  155.    float max_corr;
  156.    float out_mag, x_temp; // out_angle;
  157.    float max_corr_time;
  158.    int zero_size;
  159.    int samp, max_samp;
  160.    double deg_per_rad = 180.0/PI;
  161.    double phase_deg;
  163.    if(Corr_Pass_Count > Num_Corr_Passes) return(_MES_AOK);
  165.    //-------------------------------------------------------
  166.    //  Copy frequently accessed member vars into local vars
  167.    std::complex<float> *x = X;
  168.    std::complex<float> *y = Y;
  169.    int proc_block_size;
  170.    int in_sig_block_size;
  171.    int ref_sig_block_size;
  172.    int full_corr_size = Full_Corr_Size;
  173.    int size_of_fcomplex = Size_Of_FComplex;
  174.    int ns_exp = int(Ns_Exp);
  175.    int smoothing_sidelobe_len = Smoothing_Sidelobe_Len;
  176.    double samp_intvl = Samp_Intvl;
  177.    double phase_to_time;
  178.    double phase_rad;
  179.    double tau, tau_sum, tau_avg;
  180.    double denom, numer;
  181.    int regression_start, regression_stop;
  182.    double phase_sum;
  183.    double phase_avg;
  184.    double idx_sum, idx_avg;
  185.    std::complex<float> x_of_f[4096];
  186.    bool phase_has_wrapped;
  187.     float amp;
  189.    //---------------------------------------------------
  190.    // Get pointers for input and output
  192.    in_sig_ptr = GET_INPUT_PTR(In_Sig);
  193.    ref_sig_ptr = GET_INPUT_PTR(Ref_Sig);
  194.    out_sig_ptr = GET_OUTPUT_PTR(Out_Sig);
  195.    proc_block_size = Proc_Block_Size;
  196.    in_sig_block_size = In_Sig->GetValidBlockSize();
  197.    ref_sig_block_size = Ref_Sig->GetValidBlockSize();
  199.    //---------------------------------------------------
  200.    //  Check to see if there enough samples of input
  201.    //  signal and reference signal to perform correlation
  202.    //  If not, set valid block size for output to zero
  203.    //  and then exit from this model.
  205.    in_sig_buf_ptr = In_Sig_Buf->Load(  in_sig_ptr, 
  206.                                        in_sig_block_size, 
  207.                                        &in_sig_buf_count);
  209.    ref_sig_buf_ptr = Ref_Sig_Buf->Load(ref_sig_ptr, 
  210.                                        ref_sig_block_size,
  211.                                        &ref_sig_buf_count);
  213.    if( (in_sig_buf_count < proc_block_size)
  214.       || (ref_sig_buf_count < proc_block_size))
  215.    {
  216.       Out_Sig->SetValidBlockSize(0);
  217.       return(_MES_AOK);
  218.    }
  219.    
  221.    //----------------------------------------
  222.    zero_size = full_corr_size - proc_block_size;
  223.    phase_to_time = full_corr_size * samp_intvl/TWO_PI;
  224.    int i;
  225.    for( i=0; i<proc_block_size; i++){
  226.       x[i] = std::complex<float>( *in_sig_buf_ptr++, 0.0 );
  227.    }
  228.    In_Sig_Buf->Release(proc_block_size);
  230.    for( i=proc_block_size; i<full_corr_size; i++){
  231.       x[i] = 0.0;
  232.    }
  234.    if(Invert_Input_Sig_Enab){
  235.       for( i=0; i<proc_block_size; i++){
  236.          x[i] = -x[i];
  237.       }
  238.    }
  240.    for( i=0; i<proc_block_size; i++){
  241.       y[i] = std::complex<float>( *ref_sig_buf_ptr++, 0.0 );
  242.    }
  243.    Ref_Sig_Buf->Release(proc_block_size);
  245.    for( i=proc_block_size; i<full_corr_size; i++){
  246.       y[i] = 0.0;
  247.    }
  249.    FftDitNipo<float>(x, full_corr_size);
  250.    FftDitNipo<float>(y, full_corr_size);
  252.    // perform correlation
  253.    for(samp=0; samp < full_corr_size; samp++){
  254.       //x[samp] /= float(full_corr_size);
  255.       x[samp] *= std::conj<float>( y[samp] ) / float(full_corr_size);
  256.    }
  257.    //----------------------------------------------------------
  258.    if( (PassNumber >= 2) &&
  259.        (Corr_Pass_Count <= Num_Corr_Passes) ){
  260.      tau_sum = 0.0;
  261.      tau_avg = 0.0;
  263.       phase_has_wrapped = false;
  264.       for(samp=0; samp < full_corr_size; samp++){
  266.          Diff_Response[samp] += x[samp];
  268.          if(Corr_Pass_Count == Num_Corr_Passes){
  269.             amp = std::abs<float>(Diff_Response[samp]);
  270.             phase_deg = deg_per_rad * std::arg<float>(Diff_Response[samp]);
  271.             if(phase_deg > 100) phase_has_wrapped = true;
  272.             if( !phase_has_wrapped) regression_stop = samp;
  274.             phase_rad = PI * phase_deg/180.0;
  275.             tau = 0.0;
  276.             if(samp !=0)
  277.             {
  278.                tau = -phase_to_time * phase_rad/samp;
  279.             }
  280.             (*Spectrum_File) << samp << ", " << amp << ", " 
  281.                             << phase_deg << ", "
  282.                             << tau << endl;
  283.          }
  284.       }
  285.       if(Corr_Pass_Count == Num_Corr_Passes){
  286.          regression_start = 5;
  287.          if(regression_stop > 300) regression_stop = 300;
  288.          phase_sum = 0.0;
  289.          idx_sum = 0.0;
  290.          for(samp=regression_start; samp < regression_stop; samp++){
  291.             phase_rad = std::arg<float>(Diff_Response[samp]);
  292.             phase_sum += phase_rad;
  293.             idx_sum += samp;
  294.          }
  295.          numer = 0.0;
  296.          denom = 0.0;
  297.          idx_avg = idx_sum / (regression_stop - regression_start);
  298.          phase_avg = phase_sum / (regression_stop - regression_start);
  299.          for(samp=regression_start; samp < regression_stop; samp++){
  300.             phase_rad = std::arg<float>(Diff_Response[samp]);
  301.             numer += (samp-idx_avg)*(phase_rad - phase_avg);
  302.             denom += (samp-idx_avg)*(samp-idx_avg);
  303.          }
  304.          tau = -phase_to_time * numer / denom;
  305.          Spectrum_File->close();
  306.       }
  307.       Corr_Pass_Count++;
  308.    }
  309.    //----------------------------------------------------
  311.    IfftDitPino<float>(x, full_corr_size);
  313.    // fill the output buffer
  314.    //memcpy( out_sig_ptr, x, proc_block_size*size_of_fcomplex);
  315.    for(int ii=0; ii<proc_block_size; ii++){
  316.       out_sig_ptr[ii] = x[ii].real();
  317.    }
  319.    // Determine maximum correlation plus corresponding time
  320.    out_mag = 0.0;
  321.    for(samp=Neg_Window_Beg; samp <= Neg_Window_End; samp++){
  322.       x_temp = x[samp].real();
  323.       (*DebugFile) << samp << ", " << x_temp << endl;
  324.       if( x_temp > out_mag)
  325.       {
  326.          out_mag = x_temp;
  327.          max_corr = x[samp].real();
  328.          max_samp = samp;
  329.       }
  330.    }
  331.    for(samp=Pos_Window_Beg; samp <= Pos_Window_End; samp++)
  332.    {
  333.       x_temp = x[samp].real();
  334.       (*DebugFile) << samp << ", " << x_temp << endl;
  335.       if( x_temp > out_mag)
  336.       {
  337.          out_mag = x_temp;
  338.          max_corr = x[samp].real();
  339.          max_samp = samp;
  340.       }
  341.    }
  342.    (*DebugFile) << "max_samp = " << max_samp << endl;
  343.    if( max_samp > proc_block_size ) max_samp -= full_corr_size;
  344.    max_corr_time = max_samp*samp_intvl;
  345. //  if( (fabs(max_corr) < 1.0e-30) )
  346. //      {
  347. //      max_corr = 1.0;
  348. //      }
  349.    Delay_At_Max_Corr->SetValue(max_corr_time);
  351.    #ifdef _DEBUG
  352.       (*DebugFile) << "Correlator found delay as " << max_corr_time << endl;
  353.    #endif
  355.    Samps_Delay_At_Max_Corr->SetValue(max_samp);
  356.    Dly_Est_Is_Valid_Cntl->SetValue(true);
  358.    //---------------------
  359.    return(_MES_AOK);
  360. }