Diff: Middlewares/OpenPDM2PCM/OpenPDMFilter.c

Revision:

11:b2f7f79026e4

Parent:

6:9b8bc842aeb3

Child:

13:90465220b75f

--- a/Middlewares/OpenPDM2PCM/OpenPDMFilter.c	Thu Apr 27 17:05:38 2017 +0200
+++ b/Middlewares/OpenPDM2PCM/OpenPDMFilter.c	Fri Apr 28 17:22:13 2017 +0200
@@ -58,37 +58,32 @@
 
 /* Functions -----------------------------------------------------------------*/
 
-int64_t filterTable(uint8_t *data, uint8_t table, TPDMFilter_InitStruct *Param);
+int64_t filterTable(uint8_t *data, uint8_t table, uint8_t decimation, 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 */]);
 
-inline int64_t filterTable(uint8_t *data, uint8_t table, TPDMFilter_InitStruct *Param)
+inline int64_t filterTable(uint8_t *data, uint8_t table, uint8_t decimation, uint8_t channels)
 {
-  uint16_t counter = 0;
+  uint8_t c, i, j;
+  uint16_t data_index = 0;
+  uint32_t *coef_p = &coef[table][0];
   int64_t F = 0;
-  uint8_t c;
-  uint8_t i;
-  uint16_t internal_bit = Param->bit[table];
-  uint16_t In_Mic = Param->In_MicChannels;
 
-  c = data[0];
-  for (i=0; i<Param->Decimation; i++)
+  for (i = 0; i < decimation; i += 8)
   {  
-    if (c & (1<<((7-internal_bit))))
-      F += coef[table][i];  
-    internal_bit++;  
-    if (internal_bit==8)
-    {
-      counter += In_Mic;
-      internal_bit = 0;
-      c = data[counter];
-    }
+    c = data[data_index];
+    F += ((c >> 7)       ) * coef_p[i    ] +
+         ((c >> 6) & 0x01) * coef_p[i + 1] +
+         ((c >> 5) & 0x01) * coef_p[i + 2] +
+         ((c >> 4) & 0x01) * coef_p[i + 3] +
+         ((c >> 3) & 0x01) * coef_p[i + 4] +
+         ((c >> 2) & 0x01) * coef_p[i + 5] +
+         ((c >> 1) & 0x01) * coef_p[i + 6] +
+         ((c     ) & 0x01) * coef_p[i + 7];
+    data_index += channels;
   }
-  Param->bit[table] = internal_bit;
-  Param->byte = counter;
-
   return F;
 }
 
@@ -245,42 +240,42 @@
     switch(Param->SincN)
     {
     case 1:
-      Z = filterTable(data,0, Param);
+      Z = filterTable(data, 0, Param->Decimation, Param->In_MicChannels);
       break;
     case 2:
-      Z = Param->Coef[0] + filterTable(data,1, Param);
-      Param->Coef[0] = filterTable(data,0, Param);
+      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);
-      Param->Coef[1] = Param->Coef[0] + filterTable(data,1, Param);
-      Param->Coef[0] = filterTable(data,0, Param);
+      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);
-      Param->Coef[2] = Param->Coef[1] + filterTable(data,2, Param);
-      Param->Coef[1] = Param->Coef[0] + filterTable(data,1, Param);
-      Param->Coef[0] = filterTable(data,0, Param);
+      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);
-      Param->Coef[3] = Param->Coef[2] + filterTable(data,3, Param);
-      Param->Coef[2] = Param->Coef[1] + filterTable(data,2, Param);
-      Param->Coef[1] = Param->Coef[0] + filterTable(data,1, Param);
-      Param->Coef[0] = filterTable(data,0, Param);
+      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;
+    Z -= SubConst;
     
-    if(Param->HP_ALFA!= 0)
+    if (Param->HP_ALFA!= 0)
     {
       OldOut = (Param->HP_ALFA * (OldOut + Z - OldIn))/256;
-      OldIn=Z;
-      Z=OldOut;
+      OldIn = Z;
+      Z = OldOut;
     }
     
-    if(Param->LP_ALFA != 0)
+    if (Param->LP_ALFA != 0)
     {
       OldZ = ((256-Param->LP_ALFA)*OldZ+Param->LP_ALFA*Z)/256;
       Z = OldZ;
@@ -295,9 +290,7 @@
     //                }   
     Z = SaturaLH(Z, -32700, 32700); // saturation		
     dataOut[i*Param->Out_MicChannels] = Z ;                  
-    //                data+=(Param->Decimation-app)/8 * Param->In_MicChannels;
-    data+=Param->byte;
-    //		data+=Param->Decimation/8*Param->In_MicChannels;				
+    data += ((Param->Decimation / 8) * Param->In_MicChannels);
   }        
   Param->OldOut=OldOut;
   Param->OldIn=OldIn;