Il y avait des problèmes dans la libraire...
Dependencies: ST_FREQUENCY_DIVIDER ST_I2S USBDEVICE
Fork of X_NUCLEO_CCA02M1 by
Middlewares/OpenPDM2PCM/OpenPDMFilter.c
- Committer:
- davide.aliprandi@st.com
- Date:
- 2017-04-21
- Revision:
- 2:9f389fd8fb2e
- Child:
- 6:9b8bc842aeb3
File content as of revision 2:9f389fd8fb2e:
/** ****************************************************************************** * @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>© 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; }