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-05-02
- Revision:
- 15:17bdadc6aa9c
- Parent:
- 13:90465220b75f
- Child:
- 16:4ab2eac7be21
File content as of revision 15:17bdadc6aa9c:
/** ****************************************************************************** * @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 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))) #define SINCN_MAX 3 #define DECIMATION_MAX 64 /* Variables -----------------------------------------------------------------*/ uint32_t DivideConst = 0; int64_t SubConst = 0; uint32_t sinc1[DECIMATION_MAX]; uint32_t sinc2[DECIMATION_MAX*2]; uint32_t sinc3[DECIMATION_MAX*3]; uint32_t sinc4[DECIMATION_MAX*4]; uint32_t sinc[DECIMATION_MAX*5]; int64_t Z = 0; uint32_t coef[5][DECIMATION_MAX]; // Max sinc 5 with Param->Decimation = 128. #ifdef USE_LUT int32_t lut[256][DECIMATION_MAX / 8][SINCN_MAX]; #endif /* Functions -----------------------------------------------------------------*/ int64_t filterTable(uint8_t *data, uint8_t SincN, 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 */]); #ifdef USE_LUT inline int64_t filterTable(uint8_t *data, uint8_t SincN, uint8_t decimation, uint8_t channels) { uint8_t c, d; uint16_t data_index = 0; int64_t F = 0; for (d = 0; d < (decimation >> 3); d++) { c = data[data_index]; F += lut[c][d][SincN]; data_index += channels; } return F; } #else 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; } #endif 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; #ifdef USE_LUT /* Look-Up Table. */ uint16_t c, d, s; for (s = 0; s < SINCN_MAX; s++) { uint32_t *coef_p = &coef[s][0]; for (c = 0; c < 256; c++) for (d = 0; d < DECIMATION_MAX / 8; d++) lut[c][d][s] = ((c >> 7) ) * coef_p[d * 8 ] + ((c >> 6) & 0x01) * coef_p[d * 8 + 1] + ((c >> 5) & 0x01) * coef_p[d * 8 + 2] + ((c >> 4) & 0x01) * coef_p[d * 8 + 3] + ((c >> 3) & 0x01) * coef_p[d * 8 + 4] + ((c >> 2) & 0x01) * coef_p[d * 8 + 5] + ((c >> 1) & 0x01) * coef_p[d * 8 + 6] + ((c ) & 0x01) * coef_p[d * 8 + 7]; } #endif } 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; }