Diff: Middlewares/OpenPDM2PCM/OpenPDMFilter.c

Revision:

16:4ab2eac7be21

Parent:

15:17bdadc6aa9c

Child:

20:9952bef19da1

--- a/Middlewares/OpenPDM2PCM/OpenPDMFilter.c	Tue May 02 18:06:58 2017 +0200
+++ b/Middlewares/OpenPDM2PCM/OpenPDMFilter.c	Wed May 03 18:10:29 2017 +0200
@@ -35,26 +35,24 @@
 
 /* Definitions ---------------------------------------------------------------*/
 
-#define maxVol 64
-#define FilterGain 16;
-#define RoundDiv(a, b) (((a)>0)?(((a)+(b)/2)/(b)):(((a)-(b)/2)/(b)))
+#define RoundDiv(a, b)    (((a)>0)?(((a)+(b)/2)/(b)):(((a)-(b)/2)/(b)))
 #define SaturaLH(N, L, H) (((N)<(L))?(L):(((N)>(H))?(H):(N)))
 
-#define SINCN_MAX 3
+#define VOLUME_MAX     64
+#define FILTER_GAIN    16
+
+#define SINCN_MAX       3
 #define DECIMATION_MAX 64
 
 
 /* Variables -----------------------------------------------------------------*/
 
-uint32_t DivideConst = 0;
-int64_t SubConst = 0;
+uint32_t div_const = 0;
+int64_t sub_const = 0;
+uint32_t sinc[DECIMATION_MAX * SINCN_MAX];
 uint32_t sinc1[DECIMATION_MAX];
-uint32_t sinc2[DECIMATION_MAX*2];
-uint32_t sinc3[DECIMATION_MAX*3];
-uint32_t sinc4[DECIMATION_MAX*4];
-uint32_t sinc[DECIMATION_MAX*5];              
-int64_t Z = 0;
-uint32_t coef[5][DECIMATION_MAX]; // Max sinc 5 with Param->Decimation = 128.
+uint32_t sinc2[DECIMATION_MAX * 2];
+uint32_t coef[SINCN_MAX][DECIMATION_MAX];
 #ifdef USE_LUT
 int32_t lut[256][DECIMATION_MAX / 8][SINCN_MAX];
 #endif
@@ -62,36 +60,47 @@
 
 /* Functions -----------------------------------------------------------------*/
 
-int64_t filterTable(uint8_t *data, uint8_t SincN, uint8_t decimation, uint8_t channels);
+int32_t filter_table(uint8_t *data, uint8_t sincn, uint8_t channels);
 
 void convolve(uint32_t Signal[/* SignalLen */], unsigned short SignalLen,
               uint32_t Kernel[/* KernelLen */], unsigned short KernelLen,
               uint32_t Result[/* SignalLen + KernelLen - 1 */]);
 
 #ifdef USE_LUT
-inline int64_t filterTable(uint8_t *data, uint8_t SincN, uint8_t decimation, uint8_t channels)
+int32_t filter_table_mono(uint8_t *data, uint8_t sincn)
+{
+  return (int32_t)
+    lut[data[0]][0][sincn] +
+    lut[data[1]][1][sincn] +
+    lut[data[2]][2][sincn] +
+    lut[data[3]][3][sincn] +
+    lut[data[4]][4][sincn] +
+    lut[data[5]][5][sincn] +
+    lut[data[6]][6][sincn] +
+    lut[data[7]][7][sincn];
+}
+int32_t filter_table_stereo(uint8_t *data, uint8_t sincn)
 {
-  uint8_t c, d;
-  uint16_t data_index = 0;
-  int64_t F = 0;
-
-  for (d = 0; d < (decimation >> 3); d++)
-  {
-    c = data[data_index];
-    F += lut[c][d][SincN];
-    data_index += channels;
-  }
-  return F;
+  return (int32_t)
+    lut[data[0]][0][sincn] +
+    lut[data[2]][1][sincn] +
+    lut[data[4]][2][sincn] +
+    lut[data[6]][3][sincn] +
+    lut[data[8]][4][sincn] +
+    lut[data[10]][5][sincn] +
+    lut[data[12]][6][sincn] +
+    lut[data[14]][7][sincn];
 }
+int32_t (* filter_tables[2]) (uint8_t *data, uint8_t sincn) = {filter_table_mono, filter_table_stereo};
 #else
-inline int64_t filterTable(uint8_t *data, uint8_t table, uint8_t decimation, uint8_t channels)
+int64_t filter_table(uint8_t *data, uint8_t sincn, uint8_t channels)
 {
   uint8_t c, i;
   uint16_t data_index = 0;
   uint32_t *coef_p = &coef[table][0];
   int64_t F = 0;
 
-  for (i = 0; i < decimation; i += 8)
+  for (i = 0; i < DECIMATION_MAX; i += 8)
   {
     c = data[data_index];
     F += ((c >> 7)       ) * coef_p[i    ] +
@@ -110,122 +119,39 @@
 
 void Open_PDM_Filter_Init(TPDMFilter_InitStruct *Param)
 {
-  uint16_t i,j;
+  uint16_t i, j;
   int64_t sum = 0;
-  Param->Coef[0] = Param->Coef[1] = Param->Coef[2] = Param->Coef[3] = Param->Coef[4] = 0;        
-  for(i=0;i<5;i++)
-  {
-    Param->bit[i]=0;
-  }
-  
-  for(i=0;i<Param->Decimation;i++)
+  for (i = 0; i < SINCN_MAX; i++)
   {
-    sinc1[i]=1;
-  }       
-  Param->OldOut = Param->OldIn = Param->OldZ = 0;
-  if(Param->LP_HZ!=0)
-  {
-    Param->LP_ALFA = (uint16_t)(Param->LP_HZ*256 / (Param->LP_HZ +  Param->Fs/(2*3.14)));
-  }else
-  {
-    Param->LP_ALFA = 0;
+    Param->Coef[i] = 0;
+    Param->bit[i] = 0;
   }
-  
-  if(Param->HP_HZ!=0)
-  {
-    Param->HP_ALFA = (uint16_t)(Param->Fs*256 / (2*3.14*Param->HP_HZ +  Param->Fs));
-  }else
+  for (i = 0; i < DECIMATION_MAX; i++)
   {
-    Param->HP_ALFA = 0;
+    sinc1[i] = 1;
   }
-  
-  switch(Param->SincN)
+
+  Param->OldOut = Param->OldIn = Param->OldZ = 0;
+  Param->LP_ALFA = (Param->LP_HZ != 0 ? (uint16_t) (Param->LP_HZ * 256 / (Param->LP_HZ + Param->Fs / (2 * 3.14))) : 0);
+  Param->HP_ALFA = (Param->HP_HZ != 0 ? (uint16_t) (Param->Fs * 256 / (2 * 3.14 * Param->HP_HZ + Param->Fs)) : 0);
+
+  Param->FilterLen = DECIMATION_MAX * 3;       
+  sinc[0] = 0;
+  sinc[DECIMATION_MAX * 3 - 1] = 0;      
+  convolve(sinc1, DECIMATION_MAX, sinc1, DECIMATION_MAX, sinc2);
+  convolve(sinc2, DECIMATION_MAX * 2 - 1, sinc1, DECIMATION_MAX, &sinc[1]);     
+  for(j = 0; j < 3; j++)
   {
-  case 1:
-    Param->FilterLen = Param->Decimation;
-    
-    for(i=0;i<Param->Decimation;i++)
+    for (i = 0; i < DECIMATION_MAX ;i++)
     {
-      coef[0][i]=1;
-      sum+= 1;
-    }
-    break;
-  case 2:
-    Param->FilterLen = Param->Decimation * 2;
-    
-    sinc[0] = 0;
-    
-    convolve(sinc1, Param->Decimation,sinc1,Param->Decimation,&sinc[1]);
-    
-    for(j=0;j<2;j++)
-    {
-      for(i=0;i<Param->Decimation;i++)
-      {
-        coef[j][i] = sinc[j*Param->Decimation+i];
-        sum+= sinc[j*Param->Decimation+i];
-      }
+      coef[j][i] = sinc[j * DECIMATION_MAX + i];
+      sum += sinc[j * DECIMATION_MAX + i];
     }
-    
-    break;
-  case 3:
-    Param->FilterLen = Param->Decimation * 3; 			
-    sinc[0] = 0;
-    sinc[Param->Decimation*3-1] = 0;			
-    convolve(sinc1, Param->Decimation,sinc1,Param->Decimation,sinc2);
-    convolve(sinc2, Param->Decimation*2-1,sinc1,Param->Decimation,&sinc[1]);			
-    for(j=0;j<3;j++)
-    {
-      for(i=0;i<Param->Decimation;i++)
-      {
-        coef[j][i] = sinc[j*Param->Decimation+i];
-        sum+= sinc[j*Param->Decimation+i];
-      }
-    }			
-    break;
-  case 4:
-    Param->FilterLen =  Param->Decimation * 4; 			
-    sinc[0] = 0;
-    sinc[1] = 0;
-    sinc[Param->Decimation*4-1] = 0;
-    convolve(sinc1, Param->Decimation,sinc1,Param->Decimation,sinc2);
-    convolve(sinc2, Param->Decimation*2-1,sinc1,Param->Decimation,sinc3);
-    convolve(sinc3, Param->Decimation*3-2,sinc1,Param->Decimation,&sinc[2]);			
-    for(j=0;j<4;j++)
-    {
-      for(i=0;i<Param->Decimation;i++)
-      {
-        coef[j][i] = sinc[j*Param->Decimation+i];
-        sum+= sinc[j*Param->Decimation+i];
-      }
-    }
-    
-    break;
-  case 5:
-    Param->FilterLen = Param->Decimation*5;  // Dec * 5 - 2			
-    sinc[0] = 0;
-    sinc[1] = 0;
-    sinc[Param->Decimation*5-2] = 0;
-    sinc[Param->Decimation*5-1] = 0;
-    
-    convolve(sinc1, Param->Decimation,sinc1,Param->Decimation,sinc2);
-    convolve(sinc2, Param->Decimation*2-1,sinc1,Param->Decimation,sinc3);
-    convolve(sinc3, Param->Decimation*3-2,sinc1,Param->Decimation,sinc4);
-    convolve(sinc4, Param->Decimation*4-3,sinc1,Param->Decimation,&sinc[2]);
-    
-    for(j=0;j<5;j++)
-    {
-      for(i=0;i<Param->Decimation;i++)
-      {
-        coef[j][i] = sinc[j*Param->Decimation+i];
-        sum+= sinc[j*Param->Decimation+i];
-      }
-    }			
-    break;
   }
-  SubConst = sum  / 2;
-  DivideConst = SubConst*maxVol/32768/FilterGain;
-  if (DivideConst == 0)
-    DivideConst = 1;
+
+  sub_const = sum >> 1;
+  div_const = sub_const * VOLUME_MAX / 32768 / FILTER_GAIN;
+  div_const = (div_const == 0 ? 1 : div_const);
 
 #ifdef USE_LUT
   /* Look-Up Table. */
@@ -268,72 +194,50 @@
   }
 }
 
-void Open_PDM_Filter(uint8_t* data, uint16_t* dataOut, uint16_t MicGain, TPDMFilter_InitStruct *Param)
+void Open_PDM_Filter(uint8_t* data, uint16_t* dataOut, uint16_t volume, TPDMFilter_InitStruct *Param)
 {
-  uint32_t i;
+  uint8_t i, data_out_index;
+  uint8_t decimation = DECIMATION_MAX;
+  uint8_t channels = Param->In_MicChannels;
+  uint8_t data_inc = ((decimation >> 3) * channels);
+  int64_t Z, Z0, Z1, Z2;
   int64_t OldOut, OldIn, OldZ;       
-  OldOut=Param->OldOut;
-  OldIn=Param->OldIn;
-  OldZ=Param->OldZ;
-  
-  for (i = 0; i < Param->Fs/1000; i++) 
+
+  OldOut = Param->OldOut;
+  OldIn = Param->OldIn;
+  OldZ = Param->OldZ;
+
+  for (i = 0, data_out_index = 0; i < Param->Fs / 1000; i++, data_out_index += channels) 
   {
-    switch(Param->SincN)
-    {
-    case 1:
-      Z = filterTable(data, 0, Param->Decimation, Param->In_MicChannels);
-      break;
-    case 2:
-      Z = Param->Coef[0] + filterTable(data, 1, Param->Decimation, Param->In_MicChannels);
-      Param->Coef[0] = filterTable(data, 0, Param->Decimation, Param->In_MicChannels);
-      break;
-    case 3:
-      Z = Param->Coef[1] + filterTable(data, 2, Param->Decimation, Param->In_MicChannels);
-      Param->Coef[1] = Param->Coef[0] + filterTable(data, 1, Param->Decimation, Param->In_MicChannels);
-      Param->Coef[0] = filterTable(data, 0, Param->Decimation, Param->In_MicChannels);
-      break;
-    case 4:
-      Z = Param->Coef[2] + filterTable(data, 3, Param->Decimation, Param->In_MicChannels);
-      Param->Coef[2] = Param->Coef[1] + filterTable(data, 2, Param->Decimation, Param->In_MicChannels);
-      Param->Coef[1] = Param->Coef[0] + filterTable(data, 1, Param->Decimation, Param->In_MicChannels);
-      Param->Coef[0] = filterTable(data, 0, Param->Decimation, Param->In_MicChannels);
-      break;			
-    case 5:
-      Z = Param->Coef[3] + filterTable(data, 4, Param->Decimation, Param->In_MicChannels);
-      Param->Coef[3] = Param->Coef[2] + filterTable(data, 3, Param->Decimation, Param->In_MicChannels);
-      Param->Coef[2] = Param->Coef[1] + filterTable(data, 2, Param->Decimation, Param->In_MicChannels);
-      Param->Coef[1] = Param->Coef[0] + filterTable(data, 1, Param->Decimation, Param->In_MicChannels);
-      Param->Coef[0] = filterTable(data, 0, Param->Decimation, Param->In_MicChannels);
-      break;	
-    }
-    
-    Z -= SubConst;
-    
-    if (Param->HP_ALFA!= 0)
-    {
-      OldOut = (Param->HP_ALFA * (OldOut + Z - OldIn))/256;
-      OldIn = Z;
-      Z = OldOut;
-    }
-    
-    if (Param->LP_ALFA != 0)
-    {
-      OldZ = ((256-Param->LP_ALFA)*OldZ+Param->LP_ALFA*Z)/256;
-      Z = OldZ;
-    }
-    
-    //                if(Param->SincN>=3){
-    //                  Z = RoundDiv(Z, DivideConst);
-    //                  Z *= MicGain;
-    //                }else{
-    Z *= MicGain;
-    Z = RoundDiv(Z, DivideConst);
-    //                }   
-    Z = SaturaLH(Z, -32700, 32700); // saturation		
-    dataOut[i*Param->Out_MicChannels] = Z ;                  
-    data += ((Param->Decimation / 8) * Param->In_MicChannels);
-  }        
-  Param->OldOut=OldOut;
-  Param->OldIn=OldIn;
-  Param->OldZ=OldZ;
+#ifdef USE_LUT
+    Z0 = filter_tables[channels - 1](data, 0);
+    Z1 = filter_tables[channels - 1](data, 1);
+    Z2 = filter_tables[channels - 1](data, 2);
+#else
+    Z0 = filter_table(data, 0, channels);
+    Z1 = filter_table(data, 1, channels);
+    Z2 = filter_table(data, 2, channels);
+#endif
+
+    Z = Param->Coef[1] + Z2 - sub_const;
+    Param->Coef[1] = Param->Coef[0] + Z1;
+    Param->Coef[0] = Z0;
+
+    OldOut = (Param->HP_ALFA * (OldOut + Z - OldIn)) >> 8;
+    OldIn = Z;
+    Z = OldOut;
+    OldZ = ((256 - Param->LP_ALFA) * OldZ + Param->LP_ALFA * Z) >> 8;
+    Z = OldZ;
+
+    Z *= volume;
+    Z = RoundDiv(Z, div_const);
+    Z = SaturaLH(Z, -32700, 32700);
+
+    dataOut[data_out_index] = Z;
+    data += data_inc;
+  }
+
+  Param->OldOut = OldOut;
+  Param->OldIn = OldIn;
+  Param->OldZ = OldZ;
 }