File content as of revision 6:9b8bc842aeb3:

/**
******************************************************************************
* @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 */]);

inline int64_t filterTable(uint8_t *data, uint8_t table, TPDMFilter_InitStruct *Param)
{
  uint16_t counter = 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++)
  {  
    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];
    }
  }
  Param->bit[table] = internal_bit;
  Param->byte = counter;

  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;
}