Diff: Middlewares/OpenPDM2PCM/OpenPDMFilter.c

Revision:

2:9f389fd8fb2e

Child:

6:9b8bc842aeb3

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Middlewares/OpenPDM2PCM/OpenPDMFilter.c	Fri Apr 21 10:33:08 2017 +0200
@@ -0,0 +1,306 @@
+/**
+******************************************************************************
+* @file    OpenPDMFilter.h
+* @author  CL
+* @version V1.0.0
+* @date    9-September-2015
+* @brief   Open PDM audio software decoding Library.   
+*          This Library is used to decode and reconstruct the audio signal
+*          produced by ST MEMS microphone (MP45Dxxx, MP34Dxxx). 
+******************************************************************************
+* @attention
+*
+* <h2><center>&copy; COPYRIGHT 2013 STMicroelectronics</center></h2>
+*
+* Licensed under MCD-ST Image SW License Agreement V2, (the "License");
+* You may not use this file except in compliance with the License.
+* You may obtain a copy of the License at:
+*
+*        http://www.st.com/software_license_agreement_image_v2
+*
+* Unless required by applicable law or agreed to in writing, software 
+* distributed under the License is distributed on an "AS IS" BASIS, 
+* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+* See the License for the specific language governing permissions and
+* limitations under the License.
+*
+******************************************************************************
+*/
+
+
+/* Includes ------------------------------------------------------------------*/
+
+#include "OpenPDMFilter.h"
+
+
+/* Definitions ---------------------------------------------------------------*/
+
+#define maxDecFactor 128
+#define maxVol 64
+#define FilterGain 16;
+#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)))
+
+
+/* Variables -----------------------------------------------------------------*/
+
+uint32_t coef[5][maxDecFactor]; // Max sinc 5 with decimation 128
+uint32_t DivideConst = 0;
+int64_t SubConst = 0;
+uint32_t sinc1[maxDecFactor];
+uint32_t sinc2[maxDecFactor*2];
+uint32_t sinc3[maxDecFactor*3];
+uint32_t sinc4[maxDecFactor*4];
+uint32_t sinc[maxDecFactor*5];              
+int64_t Z = 0;
+uint16_t app;
+
+
+/* Functions -----------------------------------------------------------------*/
+
+int64_t filterTable(uint8_t *data, uint8_t table, TPDMFilter_InitStruct *Param);
+
+void convolve(uint32_t Signal[/* SignalLen */], unsigned short SignalLen,
+              uint32_t Kernel[/* KernelLen */], unsigned short KernelLen,
+              uint32_t Result[/* SignalLen + KernelLen - 1 */]);
+
+int64_t filterTable(uint8_t *data, uint8_t table, TPDMFilter_InitStruct *Param)
+{
+  uint16_t contatore = 0;
+  int64_t F = 0;
+  uint8_t c;
+  uint8_t i;
+  uint16_t internalBit = Param->bit[table];
+  uint16_t internalBit_copy = internalBit;
+  uint16_t In_Mic = Param->In_MicChannels;
+
+  c = data[0];
+  for(i=0; i<Param->Decimation;i++)
+  {  
+    if (c & (1<<((7-internalBit))))
+      F += coef[table][i];  
+    internalBit++;  
+    if(internalBit==8)
+    {
+      contatore+=In_Mic;
+      internalBit = 0;
+      c = data[contatore];
+    }
+  }
+  Param->bit[table] = internalBit;
+  Param->byte = contatore;
+
+  return F;
+}
+
+void Open_PDM_Filter_Init(TPDMFilter_InitStruct *Param)
+{
+  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++)
+  {
+    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;
+  }
+  
+  if(Param->HP_HZ!=0)
+  {
+    Param->HP_ALFA = (uint16_t)(Param->Fs*256 / (2*3.14*Param->HP_HZ +  Param->Fs));
+  }else
+  {
+    Param->HP_ALFA = 0;
+  }
+  
+  switch(Param->SincN)
+  {
+  case 1:
+    Param->FilterLen = Param->Decimation;
+    
+    for(i=0;i<Param->Decimation;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];
+      }
+    }
+    
+    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;
+}
+
+void convolve(uint32_t Signal[/* SignalLen */], unsigned short SignalLen,
+              uint32_t Kernel[/* KernelLen */], unsigned short KernelLen,
+              uint32_t Result[/* SignalLen + KernelLen - 1 */])
+{
+  uint16_t n;
+  for (n = 0; n < SignalLen + KernelLen - 1; n++)
+  {
+    unsigned short kmin, kmax, k;
+    
+    Result[n] = 0;
+    
+    kmin = (n >= KernelLen - 1) ? n - (KernelLen - 1) : 0;
+    kmax = (n < SignalLen - 1) ? n : SignalLen - 1;
+    
+    for (k = kmin; k <= kmax; k++)
+    {
+      Result[n] += Signal[k] * Kernel[n - k];
+    }
+  }
+}
+
+void Open_PDM_Filter(uint8_t* data, uint16_t* dataOut, uint16_t MicGain, TPDMFilter_InitStruct *Param)
+{
+  uint32_t i;
+  int64_t OldOut, OldIn, OldZ;       
+  OldOut=Param->OldOut;
+  OldIn=Param->OldIn;
+  OldZ=Param->OldZ;
+  
+  for (i = 0; i < Param->Fs/1000; i++) 
+  {
+    switch(Param->SincN)
+    {
+    case 1:
+      Z = filterTable(data,0, Param);
+      break;
+    case 2:
+      Z = Param->Coef[0] + filterTable(data,1, Param);
+      Param->Coef[0] = filterTable(data,0, Param);
+      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);
+      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);
+      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);
+      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-app)/8 * Param->In_MicChannels;
+    data+=Param->byte;
+    //		data+=Param->Decimation/8*Param->In_MicChannels;				
+  }        
+  Param->OldOut=OldOut;
+  Param->OldIn=OldIn;
+  Param->OldZ=OldZ;
+}