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

3G开发

开发平台:

Visual C++

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