Quentin Roche / X_NUCLEO_CCA02M1

Il y avait des problèmes dans la libraire...

Dependencies:   ST_FREQUENCY_DIVIDER ST_I2S USBDEVICE

Fork of X_NUCLEO_CCA02M1 by ST

Middlewares/OpenPDM2PCM/OpenPDMFilter.c@15:17bdadc6aa9c, 2017-05-02 (annotated)

Committer:
davide.aliprandi@st.com
Date:
Tue May 02 18:06:58 2017 +0200
Revision:
15:17bdadc6aa9c
Parent:
13:90465220b75f
Child:
16:4ab2eac7be21
Improve performance with a LUT (now supports stereo@32KHz).

Who changed what in which revision?

UserRevisionLine numberNew contents of line
davide.aliprandi@st.com 2:9f389fd8fb2e 1 /**
davide.aliprandi@st.com 2:9f389fd8fb2e 2 ******************************************************************************
davide.aliprandi@st.com 2:9f389fd8fb2e 3 * @file OpenPDMFilter.h
davide.aliprandi@st.com 2:9f389fd8fb2e 4 * @author CL
davide.aliprandi@st.com 2:9f389fd8fb2e 5 * @version V1.0.0
davide.aliprandi@st.com 2:9f389fd8fb2e 6 * @date 9-September-2015
davide.aliprandi@st.com 2:9f389fd8fb2e 7 * @brief Open PDM audio software decoding Library.
davide.aliprandi@st.com 2:9f389fd8fb2e 8 * This Library is used to decode and reconstruct the audio signal
davide.aliprandi@st.com 2:9f389fd8fb2e 9 * produced by ST MEMS microphone (MP45Dxxx, MP34Dxxx).
davide.aliprandi@st.com 2:9f389fd8fb2e 10 ******************************************************************************
davide.aliprandi@st.com 2:9f389fd8fb2e 11 * @attention
davide.aliprandi@st.com 2:9f389fd8fb2e 12 *
davide.aliprandi@st.com 2:9f389fd8fb2e 13 * <h2><center>&copy; COPYRIGHT 2013 STMicroelectronics</center></h2>
davide.aliprandi@st.com 2:9f389fd8fb2e 14 *
davide.aliprandi@st.com 2:9f389fd8fb2e 15 * Licensed under MCD-ST Image SW License Agreement V2, (the "License");
davide.aliprandi@st.com 2:9f389fd8fb2e 16 * You may not use this file except in compliance with the License.
davide.aliprandi@st.com 2:9f389fd8fb2e 17 * You may obtain a copy of the License at:
davide.aliprandi@st.com 2:9f389fd8fb2e 18 *
davide.aliprandi@st.com 2:9f389fd8fb2e 19 * http://www.st.com/software_license_agreement_image_v2
davide.aliprandi@st.com 2:9f389fd8fb2e 20 *
davide.aliprandi@st.com 2:9f389fd8fb2e 21 * Unless required by applicable law or agreed to in writing, software
davide.aliprandi@st.com 2:9f389fd8fb2e 22 * distributed under the License is distributed on an "AS IS" BASIS,
davide.aliprandi@st.com 2:9f389fd8fb2e 23 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
davide.aliprandi@st.com 2:9f389fd8fb2e 24 * See the License for the specific language governing permissions and
davide.aliprandi@st.com 2:9f389fd8fb2e 25 * limitations under the License.
davide.aliprandi@st.com 2:9f389fd8fb2e 26 *
davide.aliprandi@st.com 2:9f389fd8fb2e 27 ******************************************************************************
davide.aliprandi@st.com 2:9f389fd8fb2e 28 */
davide.aliprandi@st.com 2:9f389fd8fb2e 29
davide.aliprandi@st.com 2:9f389fd8fb2e 30
davide.aliprandi@st.com 2:9f389fd8fb2e 31 /* Includes ------------------------------------------------------------------*/
davide.aliprandi@st.com 2:9f389fd8fb2e 32
davide.aliprandi@st.com 2:9f389fd8fb2e 33 #include "OpenPDMFilter.h"
davide.aliprandi@st.com 2:9f389fd8fb2e 34
davide.aliprandi@st.com 2:9f389fd8fb2e 35
davide.aliprandi@st.com 2:9f389fd8fb2e 36 /* Definitions ---------------------------------------------------------------*/
davide.aliprandi@st.com 2:9f389fd8fb2e 37
davide.aliprandi@st.com 2:9f389fd8fb2e 38 #define maxVol 64
davide.aliprandi@st.com 2:9f389fd8fb2e 39 #define FilterGain 16;
davide.aliprandi@st.com 2:9f389fd8fb2e 40 #define RoundDiv(a, b) (((a)>0)?(((a)+(b)/2)/(b)):(((a)-(b)/2)/(b)))
davide.aliprandi@st.com 2:9f389fd8fb2e 41 #define SaturaLH(N, L, H) (((N)<(L))?(L):(((N)>(H))?(H):(N)))
davide.aliprandi@st.com 2:9f389fd8fb2e 42
davide.aliprandi@st.com 15:17bdadc6aa9c 43 #define SINCN_MAX 3
davide.aliprandi@st.com 15:17bdadc6aa9c 44 #define DECIMATION_MAX 64
davide.aliprandi@st.com 15:17bdadc6aa9c 45
davide.aliprandi@st.com 2:9f389fd8fb2e 46
davide.aliprandi@st.com 2:9f389fd8fb2e 47 /* Variables -----------------------------------------------------------------*/
davide.aliprandi@st.com 2:9f389fd8fb2e 48
davide.aliprandi@st.com 2:9f389fd8fb2e 49 uint32_t DivideConst = 0;
davide.aliprandi@st.com 2:9f389fd8fb2e 50 int64_t SubConst = 0;
davide.aliprandi@st.com 15:17bdadc6aa9c 51 uint32_t sinc1[DECIMATION_MAX];
davide.aliprandi@st.com 15:17bdadc6aa9c 52 uint32_t sinc2[DECIMATION_MAX*2];
davide.aliprandi@st.com 15:17bdadc6aa9c 53 uint32_t sinc3[DECIMATION_MAX*3];
davide.aliprandi@st.com 15:17bdadc6aa9c 54 uint32_t sinc4[DECIMATION_MAX*4];
davide.aliprandi@st.com 15:17bdadc6aa9c 55 uint32_t sinc[DECIMATION_MAX*5];
davide.aliprandi@st.com 2:9f389fd8fb2e 56 int64_t Z = 0;
davide.aliprandi@st.com 15:17bdadc6aa9c 57 uint32_t coef[5][DECIMATION_MAX]; // Max sinc 5 with Param->Decimation = 128.
davide.aliprandi@st.com 15:17bdadc6aa9c 58 #ifdef USE_LUT
davide.aliprandi@st.com 15:17bdadc6aa9c 59 int32_t lut[256][DECIMATION_MAX / 8][SINCN_MAX];
davide.aliprandi@st.com 15:17bdadc6aa9c 60 #endif
davide.aliprandi@st.com 2:9f389fd8fb2e 61
davide.aliprandi@st.com 2:9f389fd8fb2e 62
davide.aliprandi@st.com 2:9f389fd8fb2e 63 /* Functions -----------------------------------------------------------------*/
davide.aliprandi@st.com 2:9f389fd8fb2e 64
davide.aliprandi@st.com 15:17bdadc6aa9c 65 int64_t filterTable(uint8_t *data, uint8_t SincN, uint8_t decimation, uint8_t channels);
davide.aliprandi@st.com 2:9f389fd8fb2e 66
davide.aliprandi@st.com 2:9f389fd8fb2e 67 void convolve(uint32_t Signal[/* SignalLen */], unsigned short SignalLen,
davide.aliprandi@st.com 2:9f389fd8fb2e 68 uint32_t Kernel[/* KernelLen */], unsigned short KernelLen,
davide.aliprandi@st.com 2:9f389fd8fb2e 69 uint32_t Result[/* SignalLen + KernelLen - 1 */]);
davide.aliprandi@st.com 2:9f389fd8fb2e 70
davide.aliprandi@st.com 15:17bdadc6aa9c 71 #ifdef USE_LUT
davide.aliprandi@st.com 15:17bdadc6aa9c 72 inline int64_t filterTable(uint8_t *data, uint8_t SincN, uint8_t decimation, uint8_t channels)
davide.aliprandi@st.com 15:17bdadc6aa9c 73 {
davide.aliprandi@st.com 15:17bdadc6aa9c 74 uint8_t c, d;
davide.aliprandi@st.com 15:17bdadc6aa9c 75 uint16_t data_index = 0;
davide.aliprandi@st.com 15:17bdadc6aa9c 76 int64_t F = 0;
davide.aliprandi@st.com 15:17bdadc6aa9c 77
davide.aliprandi@st.com 15:17bdadc6aa9c 78 for (d = 0; d < (decimation >> 3); d++)
davide.aliprandi@st.com 15:17bdadc6aa9c 79 {
davide.aliprandi@st.com 15:17bdadc6aa9c 80 c = data[data_index];
davide.aliprandi@st.com 15:17bdadc6aa9c 81 F += lut[c][d][SincN];
davide.aliprandi@st.com 15:17bdadc6aa9c 82 data_index += channels;
davide.aliprandi@st.com 15:17bdadc6aa9c 83 }
davide.aliprandi@st.com 15:17bdadc6aa9c 84 return F;
davide.aliprandi@st.com 15:17bdadc6aa9c 85 }
davide.aliprandi@st.com 15:17bdadc6aa9c 86 #else
davide.aliprandi@st.com 11:b2f7f79026e4 87 inline int64_t filterTable(uint8_t *data, uint8_t table, uint8_t decimation, uint8_t channels)
davide.aliprandi@st.com 2:9f389fd8fb2e 88 {
Davidroid 13:90465220b75f 89 uint8_t c, i;
davide.aliprandi@st.com 11:b2f7f79026e4 90 uint16_t data_index = 0;
davide.aliprandi@st.com 11:b2f7f79026e4 91 uint32_t *coef_p = &coef[table][0];
davide.aliprandi@st.com 2:9f389fd8fb2e 92 int64_t F = 0;
davide.aliprandi@st.com 2:9f389fd8fb2e 93
davide.aliprandi@st.com 11:b2f7f79026e4 94 for (i = 0; i < decimation; i += 8)
davide.aliprandi@st.com 15:17bdadc6aa9c 95 {
davide.aliprandi@st.com 11:b2f7f79026e4 96 c = data[data_index];
davide.aliprandi@st.com 11:b2f7f79026e4 97 F += ((c >> 7) ) * coef_p[i ] +
davide.aliprandi@st.com 11:b2f7f79026e4 98 ((c >> 6) & 0x01) * coef_p[i + 1] +
davide.aliprandi@st.com 11:b2f7f79026e4 99 ((c >> 5) & 0x01) * coef_p[i + 2] +
davide.aliprandi@st.com 11:b2f7f79026e4 100 ((c >> 4) & 0x01) * coef_p[i + 3] +
davide.aliprandi@st.com 11:b2f7f79026e4 101 ((c >> 3) & 0x01) * coef_p[i + 4] +
davide.aliprandi@st.com 11:b2f7f79026e4 102 ((c >> 2) & 0x01) * coef_p[i + 5] +
davide.aliprandi@st.com 11:b2f7f79026e4 103 ((c >> 1) & 0x01) * coef_p[i + 6] +
davide.aliprandi@st.com 11:b2f7f79026e4 104 ((c ) & 0x01) * coef_p[i + 7];
davide.aliprandi@st.com 11:b2f7f79026e4 105 data_index += channels;
davide.aliprandi@st.com 2:9f389fd8fb2e 106 }
davide.aliprandi@st.com 2:9f389fd8fb2e 107 return F;
davide.aliprandi@st.com 2:9f389fd8fb2e 108 }
davide.aliprandi@st.com 15:17bdadc6aa9c 109 #endif
davide.aliprandi@st.com 2:9f389fd8fb2e 110
davide.aliprandi@st.com 2:9f389fd8fb2e 111 void Open_PDM_Filter_Init(TPDMFilter_InitStruct *Param)
davide.aliprandi@st.com 2:9f389fd8fb2e 112 {
davide.aliprandi@st.com 2:9f389fd8fb2e 113 uint16_t i,j;
davide.aliprandi@st.com 2:9f389fd8fb2e 114 int64_t sum = 0;
davide.aliprandi@st.com 2:9f389fd8fb2e 115 Param->Coef[0] = Param->Coef[1] = Param->Coef[2] = Param->Coef[3] = Param->Coef[4] = 0;
davide.aliprandi@st.com 2:9f389fd8fb2e 116 for(i=0;i<5;i++)
davide.aliprandi@st.com 2:9f389fd8fb2e 117 {
davide.aliprandi@st.com 2:9f389fd8fb2e 118 Param->bit[i]=0;
davide.aliprandi@st.com 2:9f389fd8fb2e 119 }
davide.aliprandi@st.com 2:9f389fd8fb2e 120
davide.aliprandi@st.com 2:9f389fd8fb2e 121 for(i=0;i<Param->Decimation;i++)
davide.aliprandi@st.com 2:9f389fd8fb2e 122 {
davide.aliprandi@st.com 2:9f389fd8fb2e 123 sinc1[i]=1;
davide.aliprandi@st.com 2:9f389fd8fb2e 124 }
davide.aliprandi@st.com 2:9f389fd8fb2e 125 Param->OldOut = Param->OldIn = Param->OldZ = 0;
davide.aliprandi@st.com 2:9f389fd8fb2e 126 if(Param->LP_HZ!=0)
davide.aliprandi@st.com 2:9f389fd8fb2e 127 {
davide.aliprandi@st.com 2:9f389fd8fb2e 128 Param->LP_ALFA = (uint16_t)(Param->LP_HZ*256 / (Param->LP_HZ + Param->Fs/(2*3.14)));
davide.aliprandi@st.com 2:9f389fd8fb2e 129 }else
davide.aliprandi@st.com 2:9f389fd8fb2e 130 {
davide.aliprandi@st.com 2:9f389fd8fb2e 131 Param->LP_ALFA = 0;
davide.aliprandi@st.com 2:9f389fd8fb2e 132 }
davide.aliprandi@st.com 2:9f389fd8fb2e 133
davide.aliprandi@st.com 2:9f389fd8fb2e 134 if(Param->HP_HZ!=0)
davide.aliprandi@st.com 2:9f389fd8fb2e 135 {
davide.aliprandi@st.com 2:9f389fd8fb2e 136 Param->HP_ALFA = (uint16_t)(Param->Fs*256 / (2*3.14*Param->HP_HZ + Param->Fs));
davide.aliprandi@st.com 2:9f389fd8fb2e 137 }else
davide.aliprandi@st.com 2:9f389fd8fb2e 138 {
davide.aliprandi@st.com 2:9f389fd8fb2e 139 Param->HP_ALFA = 0;
davide.aliprandi@st.com 2:9f389fd8fb2e 140 }
davide.aliprandi@st.com 2:9f389fd8fb2e 141
davide.aliprandi@st.com 2:9f389fd8fb2e 142 switch(Param->SincN)
davide.aliprandi@st.com 2:9f389fd8fb2e 143 {
davide.aliprandi@st.com 2:9f389fd8fb2e 144 case 1:
davide.aliprandi@st.com 2:9f389fd8fb2e 145 Param->FilterLen = Param->Decimation;
davide.aliprandi@st.com 2:9f389fd8fb2e 146
davide.aliprandi@st.com 2:9f389fd8fb2e 147 for(i=0;i<Param->Decimation;i++)
davide.aliprandi@st.com 2:9f389fd8fb2e 148 {
davide.aliprandi@st.com 2:9f389fd8fb2e 149 coef[0][i]=1;
davide.aliprandi@st.com 2:9f389fd8fb2e 150 sum+= 1;
davide.aliprandi@st.com 2:9f389fd8fb2e 151 }
davide.aliprandi@st.com 2:9f389fd8fb2e 152 break;
davide.aliprandi@st.com 2:9f389fd8fb2e 153 case 2:
davide.aliprandi@st.com 2:9f389fd8fb2e 154 Param->FilterLen = Param->Decimation * 2;
davide.aliprandi@st.com 2:9f389fd8fb2e 155
davide.aliprandi@st.com 2:9f389fd8fb2e 156 sinc[0] = 0;
davide.aliprandi@st.com 2:9f389fd8fb2e 157
davide.aliprandi@st.com 2:9f389fd8fb2e 158 convolve(sinc1, Param->Decimation,sinc1,Param->Decimation,&sinc[1]);
davide.aliprandi@st.com 2:9f389fd8fb2e 159
davide.aliprandi@st.com 2:9f389fd8fb2e 160 for(j=0;j<2;j++)
davide.aliprandi@st.com 2:9f389fd8fb2e 161 {
davide.aliprandi@st.com 2:9f389fd8fb2e 162 for(i=0;i<Param->Decimation;i++)
davide.aliprandi@st.com 2:9f389fd8fb2e 163 {
davide.aliprandi@st.com 2:9f389fd8fb2e 164 coef[j][i] = sinc[j*Param->Decimation+i];
davide.aliprandi@st.com 2:9f389fd8fb2e 165 sum+= sinc[j*Param->Decimation+i];
davide.aliprandi@st.com 2:9f389fd8fb2e 166 }
davide.aliprandi@st.com 2:9f389fd8fb2e 167 }
davide.aliprandi@st.com 2:9f389fd8fb2e 168
davide.aliprandi@st.com 2:9f389fd8fb2e 169 break;
davide.aliprandi@st.com 2:9f389fd8fb2e 170 case 3:
davide.aliprandi@st.com 2:9f389fd8fb2e 171 Param->FilterLen = Param->Decimation * 3;
davide.aliprandi@st.com 2:9f389fd8fb2e 172 sinc[0] = 0;
davide.aliprandi@st.com 2:9f389fd8fb2e 173 sinc[Param->Decimation*3-1] = 0;
davide.aliprandi@st.com 2:9f389fd8fb2e 174 convolve(sinc1, Param->Decimation,sinc1,Param->Decimation,sinc2);
davide.aliprandi@st.com 2:9f389fd8fb2e 175 convolve(sinc2, Param->Decimation*2-1,sinc1,Param->Decimation,&sinc[1]);
davide.aliprandi@st.com 2:9f389fd8fb2e 176 for(j=0;j<3;j++)
davide.aliprandi@st.com 2:9f389fd8fb2e 177 {
davide.aliprandi@st.com 2:9f389fd8fb2e 178 for(i=0;i<Param->Decimation;i++)
davide.aliprandi@st.com 2:9f389fd8fb2e 179 {
davide.aliprandi@st.com 2:9f389fd8fb2e 180 coef[j][i] = sinc[j*Param->Decimation+i];
davide.aliprandi@st.com 2:9f389fd8fb2e 181 sum+= sinc[j*Param->Decimation+i];
davide.aliprandi@st.com 2:9f389fd8fb2e 182 }
davide.aliprandi@st.com 2:9f389fd8fb2e 183 }
davide.aliprandi@st.com 2:9f389fd8fb2e 184 break;
davide.aliprandi@st.com 2:9f389fd8fb2e 185 case 4:
davide.aliprandi@st.com 2:9f389fd8fb2e 186 Param->FilterLen = Param->Decimation * 4;
davide.aliprandi@st.com 2:9f389fd8fb2e 187 sinc[0] = 0;
davide.aliprandi@st.com 2:9f389fd8fb2e 188 sinc[1] = 0;
davide.aliprandi@st.com 2:9f389fd8fb2e 189 sinc[Param->Decimation*4-1] = 0;
davide.aliprandi@st.com 2:9f389fd8fb2e 190 convolve(sinc1, Param->Decimation,sinc1,Param->Decimation,sinc2);
davide.aliprandi@st.com 2:9f389fd8fb2e 191 convolve(sinc2, Param->Decimation*2-1,sinc1,Param->Decimation,sinc3);
davide.aliprandi@st.com 2:9f389fd8fb2e 192 convolve(sinc3, Param->Decimation*3-2,sinc1,Param->Decimation,&sinc[2]);
davide.aliprandi@st.com 2:9f389fd8fb2e 193 for(j=0;j<4;j++)
davide.aliprandi@st.com 2:9f389fd8fb2e 194 {
davide.aliprandi@st.com 2:9f389fd8fb2e 195 for(i=0;i<Param->Decimation;i++)
davide.aliprandi@st.com 2:9f389fd8fb2e 196 {
davide.aliprandi@st.com 2:9f389fd8fb2e 197 coef[j][i] = sinc[j*Param->Decimation+i];
davide.aliprandi@st.com 2:9f389fd8fb2e 198 sum+= sinc[j*Param->Decimation+i];
davide.aliprandi@st.com 2:9f389fd8fb2e 199 }
davide.aliprandi@st.com 2:9f389fd8fb2e 200 }
davide.aliprandi@st.com 2:9f389fd8fb2e 201
davide.aliprandi@st.com 2:9f389fd8fb2e 202 break;
davide.aliprandi@st.com 2:9f389fd8fb2e 203 case 5:
davide.aliprandi@st.com 2:9f389fd8fb2e 204 Param->FilterLen = Param->Decimation*5; // Dec * 5 - 2
davide.aliprandi@st.com 2:9f389fd8fb2e 205 sinc[0] = 0;
davide.aliprandi@st.com 2:9f389fd8fb2e 206 sinc[1] = 0;
davide.aliprandi@st.com 2:9f389fd8fb2e 207 sinc[Param->Decimation*5-2] = 0;
davide.aliprandi@st.com 2:9f389fd8fb2e 208 sinc[Param->Decimation*5-1] = 0;
davide.aliprandi@st.com 2:9f389fd8fb2e 209
davide.aliprandi@st.com 2:9f389fd8fb2e 210 convolve(sinc1, Param->Decimation,sinc1,Param->Decimation,sinc2);
davide.aliprandi@st.com 2:9f389fd8fb2e 211 convolve(sinc2, Param->Decimation*2-1,sinc1,Param->Decimation,sinc3);
davide.aliprandi@st.com 2:9f389fd8fb2e 212 convolve(sinc3, Param->Decimation*3-2,sinc1,Param->Decimation,sinc4);
davide.aliprandi@st.com 2:9f389fd8fb2e 213 convolve(sinc4, Param->Decimation*4-3,sinc1,Param->Decimation,&sinc[2]);
davide.aliprandi@st.com 2:9f389fd8fb2e 214
davide.aliprandi@st.com 2:9f389fd8fb2e 215 for(j=0;j<5;j++)
davide.aliprandi@st.com 2:9f389fd8fb2e 216 {
davide.aliprandi@st.com 2:9f389fd8fb2e 217 for(i=0;i<Param->Decimation;i++)
davide.aliprandi@st.com 2:9f389fd8fb2e 218 {
davide.aliprandi@st.com 2:9f389fd8fb2e 219 coef[j][i] = sinc[j*Param->Decimation+i];
davide.aliprandi@st.com 2:9f389fd8fb2e 220 sum+= sinc[j*Param->Decimation+i];
davide.aliprandi@st.com 2:9f389fd8fb2e 221 }
davide.aliprandi@st.com 2:9f389fd8fb2e 222 }
davide.aliprandi@st.com 2:9f389fd8fb2e 223 break;
davide.aliprandi@st.com 2:9f389fd8fb2e 224 }
davide.aliprandi@st.com 2:9f389fd8fb2e 225 SubConst = sum / 2;
davide.aliprandi@st.com 2:9f389fd8fb2e 226 DivideConst = SubConst*maxVol/32768/FilterGain;
davide.aliprandi@st.com 15:17bdadc6aa9c 227 if (DivideConst == 0)
davide.aliprandi@st.com 15:17bdadc6aa9c 228 DivideConst = 1;
davide.aliprandi@st.com 15:17bdadc6aa9c 229
davide.aliprandi@st.com 15:17bdadc6aa9c 230 #ifdef USE_LUT
davide.aliprandi@st.com 15:17bdadc6aa9c 231 /* Look-Up Table. */
davide.aliprandi@st.com 15:17bdadc6aa9c 232 uint16_t c, d, s;
davide.aliprandi@st.com 15:17bdadc6aa9c 233 for (s = 0; s < SINCN_MAX; s++)
davide.aliprandi@st.com 15:17bdadc6aa9c 234 {
davide.aliprandi@st.com 15:17bdadc6aa9c 235 uint32_t *coef_p = &coef[s][0];
davide.aliprandi@st.com 15:17bdadc6aa9c 236 for (c = 0; c < 256; c++)
davide.aliprandi@st.com 15:17bdadc6aa9c 237 for (d = 0; d < DECIMATION_MAX / 8; d++)
davide.aliprandi@st.com 15:17bdadc6aa9c 238 lut[c][d][s] = ((c >> 7) ) * coef_p[d * 8 ] +
davide.aliprandi@st.com 15:17bdadc6aa9c 239 ((c >> 6) & 0x01) * coef_p[d * 8 + 1] +
davide.aliprandi@st.com 15:17bdadc6aa9c 240 ((c >> 5) & 0x01) * coef_p[d * 8 + 2] +
davide.aliprandi@st.com 15:17bdadc6aa9c 241 ((c >> 4) & 0x01) * coef_p[d * 8 + 3] +
davide.aliprandi@st.com 15:17bdadc6aa9c 242 ((c >> 3) & 0x01) * coef_p[d * 8 + 4] +
davide.aliprandi@st.com 15:17bdadc6aa9c 243 ((c >> 2) & 0x01) * coef_p[d * 8 + 5] +
davide.aliprandi@st.com 15:17bdadc6aa9c 244 ((c >> 1) & 0x01) * coef_p[d * 8 + 6] +
davide.aliprandi@st.com 15:17bdadc6aa9c 245 ((c ) & 0x01) * coef_p[d * 8 + 7];
davide.aliprandi@st.com 15:17bdadc6aa9c 246 }
davide.aliprandi@st.com 15:17bdadc6aa9c 247 #endif
davide.aliprandi@st.com 2:9f389fd8fb2e 248 }
davide.aliprandi@st.com 2:9f389fd8fb2e 249
davide.aliprandi@st.com 2:9f389fd8fb2e 250 void convolve(uint32_t Signal[/* SignalLen */], unsigned short SignalLen,
davide.aliprandi@st.com 2:9f389fd8fb2e 251 uint32_t Kernel[/* KernelLen */], unsigned short KernelLen,
davide.aliprandi@st.com 2:9f389fd8fb2e 252 uint32_t Result[/* SignalLen + KernelLen - 1 */])
davide.aliprandi@st.com 2:9f389fd8fb2e 253 {
davide.aliprandi@st.com 2:9f389fd8fb2e 254 uint16_t n;
davide.aliprandi@st.com 2:9f389fd8fb2e 255 for (n = 0; n < SignalLen + KernelLen - 1; n++)
davide.aliprandi@st.com 2:9f389fd8fb2e 256 {
davide.aliprandi@st.com 2:9f389fd8fb2e 257 unsigned short kmin, kmax, k;
davide.aliprandi@st.com 2:9f389fd8fb2e 258
davide.aliprandi@st.com 2:9f389fd8fb2e 259 Result[n] = 0;
davide.aliprandi@st.com 2:9f389fd8fb2e 260
davide.aliprandi@st.com 2:9f389fd8fb2e 261 kmin = (n >= KernelLen - 1) ? n - (KernelLen - 1) : 0;
davide.aliprandi@st.com 2:9f389fd8fb2e 262 kmax = (n < SignalLen - 1) ? n : SignalLen - 1;
davide.aliprandi@st.com 2:9f389fd8fb2e 263
davide.aliprandi@st.com 2:9f389fd8fb2e 264 for (k = kmin; k <= kmax; k++)
davide.aliprandi@st.com 2:9f389fd8fb2e 265 {
davide.aliprandi@st.com 2:9f389fd8fb2e 266 Result[n] += Signal[k] * Kernel[n - k];
davide.aliprandi@st.com 2:9f389fd8fb2e 267 }
davide.aliprandi@st.com 2:9f389fd8fb2e 268 }
davide.aliprandi@st.com 2:9f389fd8fb2e 269 }
davide.aliprandi@st.com 2:9f389fd8fb2e 270
davide.aliprandi@st.com 2:9f389fd8fb2e 271 void Open_PDM_Filter(uint8_t* data, uint16_t* dataOut, uint16_t MicGain, TPDMFilter_InitStruct *Param)
davide.aliprandi@st.com 2:9f389fd8fb2e 272 {
davide.aliprandi@st.com 2:9f389fd8fb2e 273 uint32_t i;
davide.aliprandi@st.com 2:9f389fd8fb2e 274 int64_t OldOut, OldIn, OldZ;
davide.aliprandi@st.com 2:9f389fd8fb2e 275 OldOut=Param->OldOut;
davide.aliprandi@st.com 2:9f389fd8fb2e 276 OldIn=Param->OldIn;
davide.aliprandi@st.com 2:9f389fd8fb2e 277 OldZ=Param->OldZ;
davide.aliprandi@st.com 2:9f389fd8fb2e 278
davide.aliprandi@st.com 2:9f389fd8fb2e 279 for (i = 0; i < Param->Fs/1000; i++)
davide.aliprandi@st.com 2:9f389fd8fb2e 280 {
davide.aliprandi@st.com 2:9f389fd8fb2e 281 switch(Param->SincN)
davide.aliprandi@st.com 2:9f389fd8fb2e 282 {
davide.aliprandi@st.com 2:9f389fd8fb2e 283 case 1:
davide.aliprandi@st.com 11:b2f7f79026e4 284 Z = filterTable(data, 0, Param->Decimation, Param->In_MicChannels);
davide.aliprandi@st.com 2:9f389fd8fb2e 285 break;
davide.aliprandi@st.com 2:9f389fd8fb2e 286 case 2:
davide.aliprandi@st.com 11:b2f7f79026e4 287 Z = Param->Coef[0] + filterTable(data, 1, Param->Decimation, Param->In_MicChannels);
davide.aliprandi@st.com 11:b2f7f79026e4 288 Param->Coef[0] = filterTable(data, 0, Param->Decimation, Param->In_MicChannels);
davide.aliprandi@st.com 2:9f389fd8fb2e 289 break;
davide.aliprandi@st.com 2:9f389fd8fb2e 290 case 3:
davide.aliprandi@st.com 11:b2f7f79026e4 291 Z = Param->Coef[1] + filterTable(data, 2, Param->Decimation, Param->In_MicChannels);
davide.aliprandi@st.com 11:b2f7f79026e4 292 Param->Coef[1] = Param->Coef[0] + filterTable(data, 1, Param->Decimation, Param->In_MicChannels);
davide.aliprandi@st.com 11:b2f7f79026e4 293 Param->Coef[0] = filterTable(data, 0, Param->Decimation, Param->In_MicChannels);
davide.aliprandi@st.com 2:9f389fd8fb2e 294 break;
davide.aliprandi@st.com 2:9f389fd8fb2e 295 case 4:
davide.aliprandi@st.com 11:b2f7f79026e4 296 Z = Param->Coef[2] + filterTable(data, 3, Param->Decimation, Param->In_MicChannels);
davide.aliprandi@st.com 11:b2f7f79026e4 297 Param->Coef[2] = Param->Coef[1] + filterTable(data, 2, Param->Decimation, Param->In_MicChannels);
davide.aliprandi@st.com 11:b2f7f79026e4 298 Param->Coef[1] = Param->Coef[0] + filterTable(data, 1, Param->Decimation, Param->In_MicChannels);
davide.aliprandi@st.com 11:b2f7f79026e4 299 Param->Coef[0] = filterTable(data, 0, Param->Decimation, Param->In_MicChannels);
davide.aliprandi@st.com 2:9f389fd8fb2e 300 break;
davide.aliprandi@st.com 2:9f389fd8fb2e 301 case 5:
davide.aliprandi@st.com 11:b2f7f79026e4 302 Z = Param->Coef[3] + filterTable(data, 4, Param->Decimation, Param->In_MicChannels);
davide.aliprandi@st.com 11:b2f7f79026e4 303 Param->Coef[3] = Param->Coef[2] + filterTable(data, 3, Param->Decimation, Param->In_MicChannels);
davide.aliprandi@st.com 11:b2f7f79026e4 304 Param->Coef[2] = Param->Coef[1] + filterTable(data, 2, Param->Decimation, Param->In_MicChannels);
davide.aliprandi@st.com 11:b2f7f79026e4 305 Param->Coef[1] = Param->Coef[0] + filterTable(data, 1, Param->Decimation, Param->In_MicChannels);
davide.aliprandi@st.com 11:b2f7f79026e4 306 Param->Coef[0] = filterTable(data, 0, Param->Decimation, Param->In_MicChannels);
davide.aliprandi@st.com 2:9f389fd8fb2e 307 break;
davide.aliprandi@st.com 2:9f389fd8fb2e 308 }
davide.aliprandi@st.com 2:9f389fd8fb2e 309
davide.aliprandi@st.com 11:b2f7f79026e4 310 Z -= SubConst;
davide.aliprandi@st.com 2:9f389fd8fb2e 311
davide.aliprandi@st.com 11:b2f7f79026e4 312 if (Param->HP_ALFA!= 0)
davide.aliprandi@st.com 2:9f389fd8fb2e 313 {
davide.aliprandi@st.com 2:9f389fd8fb2e 314 OldOut = (Param->HP_ALFA * (OldOut + Z - OldIn))/256;
davide.aliprandi@st.com 11:b2f7f79026e4 315 OldIn = Z;
davide.aliprandi@st.com 11:b2f7f79026e4 316 Z = OldOut;
davide.aliprandi@st.com 2:9f389fd8fb2e 317 }
davide.aliprandi@st.com 2:9f389fd8fb2e 318
davide.aliprandi@st.com 11:b2f7f79026e4 319 if (Param->LP_ALFA != 0)
davide.aliprandi@st.com 2:9f389fd8fb2e 320 {
davide.aliprandi@st.com 2:9f389fd8fb2e 321 OldZ = ((256-Param->LP_ALFA)*OldZ+Param->LP_ALFA*Z)/256;
davide.aliprandi@st.com 2:9f389fd8fb2e 322 Z = OldZ;
davide.aliprandi@st.com 2:9f389fd8fb2e 323 }
davide.aliprandi@st.com 2:9f389fd8fb2e 324
davide.aliprandi@st.com 2:9f389fd8fb2e 325 // if(Param->SincN>=3){
davide.aliprandi@st.com 2:9f389fd8fb2e 326 // Z = RoundDiv(Z, DivideConst);
davide.aliprandi@st.com 2:9f389fd8fb2e 327 // Z *= MicGain;
davide.aliprandi@st.com 2:9f389fd8fb2e 328 // }else{
davide.aliprandi@st.com 2:9f389fd8fb2e 329 Z *= MicGain;
davide.aliprandi@st.com 2:9f389fd8fb2e 330 Z = RoundDiv(Z, DivideConst);
davide.aliprandi@st.com 2:9f389fd8fb2e 331 // }
davide.aliprandi@st.com 2:9f389fd8fb2e 332 Z = SaturaLH(Z, -32700, 32700); // saturation
davide.aliprandi@st.com 2:9f389fd8fb2e 333 dataOut[i*Param->Out_MicChannels] = Z ;
davide.aliprandi@st.com 11:b2f7f79026e4 334 data += ((Param->Decimation / 8) * Param->In_MicChannels);
davide.aliprandi@st.com 2:9f389fd8fb2e 335 }
davide.aliprandi@st.com 2:9f389fd8fb2e 336 Param->OldOut=OldOut;
davide.aliprandi@st.com 2:9f389fd8fb2e 337 Param->OldIn=OldIn;
davide.aliprandi@st.com 2:9f389fd8fb2e 338 Param->OldZ=OldZ;
davide.aliprandi@st.com 2:9f389fd8fb2e 339 }