File content as of revision 13:90465220b75f:

/**
******************************************************************************
* @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, 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, uint8_t decimation, 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)
  {  
    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;
  }
  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->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;
}