mathDSP.c

00001 
00002 #include "Global.h"
00003 
00004 BAND_PASS_TYPE BandPassType;
00005 
00006 
00007 #define         L_PLC   3
00008 #define         L_DUP   3
00009 #define     DIV_CONST   768
00010 #define     DIV_CONST2  384
00011 #define      BUF_SIZE   64
00012 #define     HALFINT     16384
00013 
00014     int highPls = 0;
00015     int lowPls = 0;
00016     int BufInMovAverPls [67];
00017     int BufInMovAverMns [67];
00018     int BufInMovAverPls_2 [67];
00019     int BufInMovAverMns_2 [67];
00020     int hMovAver [67];
00021 
00022     int aPLC[L_PLC], bPLC[L_PLC], aDUP[L_DUP], bDUP[L_DUP]; 
00023     int aDUP_2[L_DUP] = {A0_HP, A1_HP, 0}, bDUP_2[L_DUP] = {0, B1_HP, 0};
00024 
00025 unsigned int Vibro_Filter_Aperture;
00026 unsigned int Vibro_2_CountIn;
00027 /******************************************************************************
00028 ** Function name:       init_VibroReduce
00029 **
00030 ** Descriptions:        Prepare coefficiennts and delay 
00031 **                      line for vibro reduce filter
00032 **
00033 ** parameters:           None
00034 ** Returned value:       None
00035 ** 
00036 ******************************************************************************/
00037 void init_VibroReduce()
00038 {
00039   unsigned int i; 
00040        __int64 coeff;
00041 //     ~22.9                    ~17600           768                               | 10000Hz = 10 KHz  |*|      ?????      |/|768000|
00042   Vibro_Filter_Aperture = Device_blk.Str.VB_N/DIV_CONST;    //e. real expression is DEVICE_SAMPLE_RATE_HZ*Device_blk.Str.VB_N/7680000
00043 //8832           определяется выше        384
00044    i = L_mult(Vibro_Filter_Aperture,DIV_CONST2);                //e. i до ближайшего целого.//умножение 2 16-ти разрядных знаковых чисел и получение 32-х разрядного числа
00045     //           ?
00046   if ((Device_blk.Str.VB_N - i)>DIV_CONST2) Vibro_Filter_Aperture++;  //проверка на переполнение L_mult()
00047 
00048   coeff = 0x7FFFFFFF/Vibro_Filter_Aperture;//насыщение при переполнении
00049 
00050   for ( i=0; i < Vibro_Filter_Aperture; i++)  /// не понятный цикл заполняющий три буфера заведенных выше...
00051   {
00052     BufInMovAverPls[i] = 0;  
00053       BufInMovAverMns[i] = 0;
00054     hMovAver[i]= coeff;
00055    }
00056    Vibro_2_CountIn = MULT_7680_12500/Vibro_Filter_Aperture;
00057    Vibro_2_CountIn++; //какойто счетчик
00058 }
00059 
00060 /******************************************************************************
00061 ** Function name:       VibroReduce
00062 **
00063 ** Descriptions:        Routine for reduce of vibro
00064 **
00065 ** parameters:          
00066 ** Returned value:       Filtered magnitude
00067 **  
00068 ******************************************************************************/
00069 int VibroReduce (int input)
00070 {
00071    static unsigned  kIn = 0;
00072           unsigned  s;          
00073            __int64  outMns = 0;
00074            __int64  outPls = 0;
00075    BufInMovAverPls[kIn] =  input;
00076      BufInMovAverMns[kIn] = -input;
00077     for (s=0; s<Vibro_Filter_Aperture; s++)
00078      {
00079       outPls += (__int64)hMovAver[s]*(__int64)BufInMovAverPls[s];
00080         outMns += (__int64)hMovAver[s]*(__int64)BufInMovAverMns[s];
00081      }
00082     highPls = (int)(outPls>>32);
00083     lowPls = (int)outPls;
00084 
00085      BufInMovAverPls_2[kIn] = (int)(outPls-(outMns>>32));
00086      BufInMovAverMns_2[kIn] = -BufInMovAverPls_2[kIn];
00087          outPls = 0;
00088            outMns = 0;
00089     for (s=0; s<Vibro_Filter_Aperture; s++)
00090      {
00091     outPls += (__int64)hMovAver[s]*(__int64)BufInMovAverPls_2[s];
00092       outMns += (__int64)hMovAver[s]*(__int64)BufInMovAverMns_2[s];
00093      }                                           
00094     kIn++;
00095     if (kIn>(Vibro_Filter_Aperture-1)) kIn = 0;
00096 
00097 
00098     return  (int)(outPls-(outMns>>32));  
00099 }
00100 /*#endif                                     
00101 int VibroReduce (int input)
00102 {
00103    static unsigned  k = 0;
00104    static __int64  out = 0, buf[67];
00105 
00106      out -= buf[k];
00107      buf[k] = (__int64)hMovAver[k] * (__int64)input;
00108      out += buf[k];
00109 
00110     if (k++ > (Vibro_Filter_Aperture-1)) k = 0;
00111 
00112 //  if ((int)out >> 16)
00113 //      return  (int)(out>>31)+1;
00114 //          else
00115         return  (int)(out>>31);
00116 }*/              
00117 /******************************************************************************
00118 ** Function name:       DUP_Filt
00119 **
00120 ** Descriptions:        Filter for dither frequency regulator
00121 **
00122 ** parameters:          
00123 ** Returned value:       Filtered magnitude
00124 ** 
00125 ******************************************************************************/
00126 int DUP_Filt (int input)
00127 {
00128 static unsigned int kIn = 0;
00129                 int ind;
00130                 __int64 temp = 0;
00131        unsigned int i;
00132 static int BufInDUP_1 [L_DUP] = {0,0,0};
00133 static int BufInDUP_2 [L_DUP] = {0,0,0};
00134 //static __int64 BufOutDUP[L_DUP] = {0,0,0};
00135 
00136     if (kIn>(L_DUP-1)) kIn = 0;
00137 
00138       BufInDUP_1[kIn] = input;
00139    ind = kIn;
00140     BufInDUP_2[kIn] = 0;
00141     for (i=0; i<L_DUP; i++)
00142     {
00143       temp += aDUP[i]*BufInDUP_1[ind];
00144       temp += bDUP[i]*BufInDUP_2[ind];
00145        if ((--ind) < 0) ind = L_DUP-1;
00146    }
00147      BufInDUP_2[kIn] =(int)(temp>>14);  //e.-----16----------- take into account that filter coefficients are divided on 2
00148 
00149 //-----------------------------2 section (HF-filtration)----------------------------------
00150   /*  BufOutDUP[kIn] = 0;
00151     for (i=0; i<L_DUP; i++)
00152     {
00153        BufOutDUP[kIn] += (__int64)aDUP_2[i]*BufInDUP_2[ind] + (__int64)bDUP_2[i]*BufOutDUP[ind];
00154        if ((--ind) < 0) ind = L_DUP-1;
00155     }
00156    BufOutDUP[kIn] >>= 30;     */
00157 
00158  return (BufInDUP_2[kIn++]);
00159 }
00160 
00161 
00162 
00163 
00164 //-------------------------PLC phase detector----------------------------------
00165 int PLC_PhaseDetFilt (int input)
00166 {
00167 static unsigned kIn = 0;
00168             int ind;
00169         __int64 temp = 0;
00170        unsigned i;
00171 static int BufInPLC_1 [L_PLC] = {0,0,0};
00172 static int BufInPLC_2 [L_PLC] = {0,0,0};
00173 static int BufOutPLC [L_PLC] = {0,0,0};
00174 
00175    if (kIn>(L_PLC-1)) kIn = 0;
00176 
00177    BufInPLC_1[kIn] = input;
00178    ind = kIn;
00179    // BufInPLC_2[kIn] = 0;
00180 
00181     for (i=0; i<L_PLC; i++)
00182     {
00183        temp += aPLC[i]*BufInPLC_1[ind];
00184        temp += bPLC[i]*BufInPLC_2[ind];
00185        if ((--ind) < 0) ind = L_PLC-1;
00186     }
00187      BufInPLC_2[kIn] =(int)(temp>>14);
00188 //-----------------------------2 section----------------------------------------
00189 //  BufOutPLC[kIn] = 0;
00190         temp = 0;
00191     for (i=0; i<L_PLC; i++)
00192     {
00193        temp += aPLC[i]*BufInPLC_2[ind];
00194          temp += bPLC[i]*BufOutPLC[ind];
00195        if ((--ind) < 0) ind = L_PLC-1;
00196     }
00197    BufOutPLC[kIn] =(int)(temp>>14);
00198 
00199  return (BufOutPLC[kIn++]);
00200 }
00201 /******************************************************************************
00202 ** Function name:       init_BandPass
00203 **
00204 ** Descriptions:        Initialization of IIR filters for PLC and DUP signals 
00205 **                      
00206 **
00207 ** parameters:           None
00208 ** Returned value:       None
00209 ** 
00210 ******************************************************************************/
00211 void init_BandPass(double CenterFreq, double BandWidth, BAND_PASS_TYPE FiltType)
00212 {
00213  double K, R, Cos_x_2, R_x_R; 
00214 
00215    R = 1.0 - 3.0 * BandWidth;
00216   R_x_R = R * R;
00217   Cos_x_2 = cos(2.0 * PI * CenterFreq) * 2.0;
00218   K = (1.0 - R * Cos_x_2 + R_x_R)/(2.0 - Cos_x_2);
00219  switch (FiltType)
00220  {
00221   case PLC:
00222    aPLC[0] = (int)((1.0 - K)*HALFINT);
00223    aPLC[1] = (int)(((K - R) * Cos_x_2)*HALFINT);
00224    aPLC[2] = (int)((R_x_R - K)*HALFINT);
00225    bPLC[0] = 0;
00226    bPLC[1] = (int)((R * Cos_x_2)*HALFINT);
00227    bPLC[2] = (int)((- R_x_R)*HALFINT);
00228   break;
00229   case DUP:
00230    aDUP[0] = (int)((1.0 - K)*HALFINT);
00231    aDUP[1] = (int)(((K - R) * Cos_x_2)*HALFINT);
00232    aDUP[2] = (int)((R_x_R - K)*HALFINT);
00233    bDUP[0] = 0;
00234    bDUP[1] = (int)((R * Cos_x_2)*HALFINT);
00235    bDUP[2] = (int)((- R_x_R)*HALFINT);   
00236   break;
00237  }
00238 }
00239 /******************************************************************************
00240 ** Function name:       HFO_MovAverFilt
00241 **
00242 ** Descriptions:        Moving average filter for ammplitude signal filtration 
00243 **                      
00244 **
00245 ** parameters:           None
00246 ** Returned value:       None
00247 ** 
00248 ******************************************************************************/
00249 int HFO_MovAverFilt (int Input)
00250 {   
00251    static __int64 smooth_HF = 0;
00252    static  int buffer_HF[BUF_SIZE];
00253    static unsigned i_HF = 0;
00254 
00255      smooth_HF -= buffer_HF[i_HF];
00256      buffer_HF[i_HF] = Input;
00257      smooth_HF += Input;
00258 
00259      i_HF++;
00260      i_HF &= (BUF_SIZE-1);
00261 
00262    return (smooth_HF>>22);  //shift on additional 6 bits for smoothing 2^6 = 64 
00263 }