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@6:9b8bc842aeb3, 2017-04-21 (annotated)

Committer:
davide.aliprandi@st.com
Date:
Fri Apr 21 19:39:27 2017 +0200
Revision:
6:9b8bc842aeb3
Parent:
2:9f389fd8fb2e
Child:
11:b2f7f79026e4
Closed PDM2PCM library removed.

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 maxDecFactor 128
davide.aliprandi@st.com 2:9f389fd8fb2e 39 #define maxVol 64
davide.aliprandi@st.com 2:9f389fd8fb2e 40 #define FilterGain 16;
davide.aliprandi@st.com 2:9f389fd8fb2e 41 #define RoundDiv(a, b) (((a)>0)?(((a)+(b)/2)/(b)):(((a)-(b)/2)/(b)))
davide.aliprandi@st.com 2:9f389fd8fb2e 42 #define SaturaLH(N, L, H) (((N)<(L))?(L):(((N)>(H))?(H):(N)))
davide.aliprandi@st.com 2:9f389fd8fb2e 43
davide.aliprandi@st.com 2:9f389fd8fb2e 44
davide.aliprandi@st.com 2:9f389fd8fb2e 45 /* Variables -----------------------------------------------------------------*/
davide.aliprandi@st.com 2:9f389fd8fb2e 46
davide.aliprandi@st.com 2:9f389fd8fb2e 47 uint32_t coef[5][maxDecFactor]; // Max sinc 5 with decimation 128
davide.aliprandi@st.com 2:9f389fd8fb2e 48 uint32_t DivideConst = 0;
davide.aliprandi@st.com 2:9f389fd8fb2e 49 int64_t SubConst = 0;
davide.aliprandi@st.com 2:9f389fd8fb2e 50 uint32_t sinc1[maxDecFactor];
davide.aliprandi@st.com 2:9f389fd8fb2e 51 uint32_t sinc2[maxDecFactor*2];
davide.aliprandi@st.com 2:9f389fd8fb2e 52 uint32_t sinc3[maxDecFactor*3];
davide.aliprandi@st.com 2:9f389fd8fb2e 53 uint32_t sinc4[maxDecFactor*4];
davide.aliprandi@st.com 2:9f389fd8fb2e 54 uint32_t sinc[maxDecFactor*5];
davide.aliprandi@st.com 2:9f389fd8fb2e 55 int64_t Z = 0;
davide.aliprandi@st.com 2:9f389fd8fb2e 56 uint16_t app;
davide.aliprandi@st.com 2:9f389fd8fb2e 57
davide.aliprandi@st.com 2:9f389fd8fb2e 58
davide.aliprandi@st.com 2:9f389fd8fb2e 59 /* Functions -----------------------------------------------------------------*/
davide.aliprandi@st.com 2:9f389fd8fb2e 60
davide.aliprandi@st.com 2:9f389fd8fb2e 61 int64_t filterTable(uint8_t *data, uint8_t table, TPDMFilter_InitStruct *Param);
davide.aliprandi@st.com 2:9f389fd8fb2e 62
davide.aliprandi@st.com 2:9f389fd8fb2e 63 void convolve(uint32_t Signal[/* SignalLen */], unsigned short SignalLen,
davide.aliprandi@st.com 2:9f389fd8fb2e 64 uint32_t Kernel[/* KernelLen */], unsigned short KernelLen,
davide.aliprandi@st.com 2:9f389fd8fb2e 65 uint32_t Result[/* SignalLen + KernelLen - 1 */]);
davide.aliprandi@st.com 2:9f389fd8fb2e 66
davide.aliprandi@st.com 6:9b8bc842aeb3 67 inline int64_t filterTable(uint8_t *data, uint8_t table, TPDMFilter_InitStruct *Param)
davide.aliprandi@st.com 2:9f389fd8fb2e 68 {
davide.aliprandi@st.com 6:9b8bc842aeb3 69 uint16_t counter = 0;
davide.aliprandi@st.com 2:9f389fd8fb2e 70 int64_t F = 0;
davide.aliprandi@st.com 2:9f389fd8fb2e 71 uint8_t c;
davide.aliprandi@st.com 2:9f389fd8fb2e 72 uint8_t i;
davide.aliprandi@st.com 6:9b8bc842aeb3 73 uint16_t internal_bit = Param->bit[table];
davide.aliprandi@st.com 2:9f389fd8fb2e 74 uint16_t In_Mic = Param->In_MicChannels;
davide.aliprandi@st.com 2:9f389fd8fb2e 75
davide.aliprandi@st.com 2:9f389fd8fb2e 76 c = data[0];
davide.aliprandi@st.com 6:9b8bc842aeb3 77 for (i=0; i<Param->Decimation; i++)
davide.aliprandi@st.com 2:9f389fd8fb2e 78 {
davide.aliprandi@st.com 6:9b8bc842aeb3 79 if (c & (1<<((7-internal_bit))))
davide.aliprandi@st.com 2:9f389fd8fb2e 80 F += coef[table][i];
davide.aliprandi@st.com 6:9b8bc842aeb3 81 internal_bit++;
davide.aliprandi@st.com 6:9b8bc842aeb3 82 if (internal_bit==8)
davide.aliprandi@st.com 2:9f389fd8fb2e 83 {
davide.aliprandi@st.com 6:9b8bc842aeb3 84 counter += In_Mic;
davide.aliprandi@st.com 6:9b8bc842aeb3 85 internal_bit = 0;
davide.aliprandi@st.com 6:9b8bc842aeb3 86 c = data[counter];
davide.aliprandi@st.com 2:9f389fd8fb2e 87 }
davide.aliprandi@st.com 2:9f389fd8fb2e 88 }
davide.aliprandi@st.com 6:9b8bc842aeb3 89 Param->bit[table] = internal_bit;
davide.aliprandi@st.com 6:9b8bc842aeb3 90 Param->byte = counter;
davide.aliprandi@st.com 2:9f389fd8fb2e 91
davide.aliprandi@st.com 2:9f389fd8fb2e 92 return F;
davide.aliprandi@st.com 2:9f389fd8fb2e 93 }
davide.aliprandi@st.com 2:9f389fd8fb2e 94
davide.aliprandi@st.com 2:9f389fd8fb2e 95 void Open_PDM_Filter_Init(TPDMFilter_InitStruct *Param)
davide.aliprandi@st.com 2:9f389fd8fb2e 96 {
davide.aliprandi@st.com 2:9f389fd8fb2e 97 uint16_t i,j;
davide.aliprandi@st.com 2:9f389fd8fb2e 98 int64_t sum = 0;
davide.aliprandi@st.com 2:9f389fd8fb2e 99 Param->Coef[0] = Param->Coef[1] = Param->Coef[2] = Param->Coef[3] = Param->Coef[4] = 0;
davide.aliprandi@st.com 2:9f389fd8fb2e 100 for(i=0;i<5;i++)
davide.aliprandi@st.com 2:9f389fd8fb2e 101 {
davide.aliprandi@st.com 2:9f389fd8fb2e 102 Param->bit[i]=0;
davide.aliprandi@st.com 2:9f389fd8fb2e 103 }
davide.aliprandi@st.com 2:9f389fd8fb2e 104
davide.aliprandi@st.com 2:9f389fd8fb2e 105 for(i=0;i<Param->Decimation;i++)
davide.aliprandi@st.com 2:9f389fd8fb2e 106 {
davide.aliprandi@st.com 2:9f389fd8fb2e 107 sinc1[i]=1;
davide.aliprandi@st.com 2:9f389fd8fb2e 108 }
davide.aliprandi@st.com 2:9f389fd8fb2e 109 Param->OldOut = Param->OldIn = Param->OldZ = 0;
davide.aliprandi@st.com 2:9f389fd8fb2e 110 if(Param->LP_HZ!=0)
davide.aliprandi@st.com 2:9f389fd8fb2e 111 {
davide.aliprandi@st.com 2:9f389fd8fb2e 112 Param->LP_ALFA = (uint16_t)(Param->LP_HZ*256 / (Param->LP_HZ + Param->Fs/(2*3.14)));
davide.aliprandi@st.com 2:9f389fd8fb2e 113 }else
davide.aliprandi@st.com 2:9f389fd8fb2e 114 {
davide.aliprandi@st.com 2:9f389fd8fb2e 115 Param->LP_ALFA = 0;
davide.aliprandi@st.com 2:9f389fd8fb2e 116 }
davide.aliprandi@st.com 2:9f389fd8fb2e 117
davide.aliprandi@st.com 2:9f389fd8fb2e 118 if(Param->HP_HZ!=0)
davide.aliprandi@st.com 2:9f389fd8fb2e 119 {
davide.aliprandi@st.com 2:9f389fd8fb2e 120 Param->HP_ALFA = (uint16_t)(Param->Fs*256 / (2*3.14*Param->HP_HZ + Param->Fs));
davide.aliprandi@st.com 2:9f389fd8fb2e 121 }else
davide.aliprandi@st.com 2:9f389fd8fb2e 122 {
davide.aliprandi@st.com 2:9f389fd8fb2e 123 Param->HP_ALFA = 0;
davide.aliprandi@st.com 2:9f389fd8fb2e 124 }
davide.aliprandi@st.com 2:9f389fd8fb2e 125
davide.aliprandi@st.com 2:9f389fd8fb2e 126 switch(Param->SincN)
davide.aliprandi@st.com 2:9f389fd8fb2e 127 {
davide.aliprandi@st.com 2:9f389fd8fb2e 128 case 1:
davide.aliprandi@st.com 2:9f389fd8fb2e 129 Param->FilterLen = Param->Decimation;
davide.aliprandi@st.com 2:9f389fd8fb2e 130
davide.aliprandi@st.com 2:9f389fd8fb2e 131 for(i=0;i<Param->Decimation;i++)
davide.aliprandi@st.com 2:9f389fd8fb2e 132 {
davide.aliprandi@st.com 2:9f389fd8fb2e 133 coef[0][i]=1;
davide.aliprandi@st.com 2:9f389fd8fb2e 134 sum+= 1;
davide.aliprandi@st.com 2:9f389fd8fb2e 135 }
davide.aliprandi@st.com 2:9f389fd8fb2e 136 break;
davide.aliprandi@st.com 2:9f389fd8fb2e 137 case 2:
davide.aliprandi@st.com 2:9f389fd8fb2e 138 Param->FilterLen = Param->Decimation * 2;
davide.aliprandi@st.com 2:9f389fd8fb2e 139
davide.aliprandi@st.com 2:9f389fd8fb2e 140 sinc[0] = 0;
davide.aliprandi@st.com 2:9f389fd8fb2e 141
davide.aliprandi@st.com 2:9f389fd8fb2e 142 convolve(sinc1, Param->Decimation,sinc1,Param->Decimation,&sinc[1]);
davide.aliprandi@st.com 2:9f389fd8fb2e 143
davide.aliprandi@st.com 2:9f389fd8fb2e 144 for(j=0;j<2;j++)
davide.aliprandi@st.com 2:9f389fd8fb2e 145 {
davide.aliprandi@st.com 2:9f389fd8fb2e 146 for(i=0;i<Param->Decimation;i++)
davide.aliprandi@st.com 2:9f389fd8fb2e 147 {
davide.aliprandi@st.com 2:9f389fd8fb2e 148 coef[j][i] = sinc[j*Param->Decimation+i];
davide.aliprandi@st.com 2:9f389fd8fb2e 149 sum+= sinc[j*Param->Decimation+i];
davide.aliprandi@st.com 2:9f389fd8fb2e 150 }
davide.aliprandi@st.com 2:9f389fd8fb2e 151 }
davide.aliprandi@st.com 2:9f389fd8fb2e 152
davide.aliprandi@st.com 2:9f389fd8fb2e 153 break;
davide.aliprandi@st.com 2:9f389fd8fb2e 154 case 3:
davide.aliprandi@st.com 2:9f389fd8fb2e 155 Param->FilterLen = Param->Decimation * 3;
davide.aliprandi@st.com 2:9f389fd8fb2e 156 sinc[0] = 0;
davide.aliprandi@st.com 2:9f389fd8fb2e 157 sinc[Param->Decimation*3-1] = 0;
davide.aliprandi@st.com 2:9f389fd8fb2e 158 convolve(sinc1, Param->Decimation,sinc1,Param->Decimation,sinc2);
davide.aliprandi@st.com 2:9f389fd8fb2e 159 convolve(sinc2, Param->Decimation*2-1,sinc1,Param->Decimation,&sinc[1]);
davide.aliprandi@st.com 2:9f389fd8fb2e 160 for(j=0;j<3;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 break;
davide.aliprandi@st.com 2:9f389fd8fb2e 169 case 4:
davide.aliprandi@st.com 2:9f389fd8fb2e 170 Param->FilterLen = Param->Decimation * 4;
davide.aliprandi@st.com 2:9f389fd8fb2e 171 sinc[0] = 0;
davide.aliprandi@st.com 2:9f389fd8fb2e 172 sinc[1] = 0;
davide.aliprandi@st.com 2:9f389fd8fb2e 173 sinc[Param->Decimation*4-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,sinc3);
davide.aliprandi@st.com 2:9f389fd8fb2e 176 convolve(sinc3, Param->Decimation*3-2,sinc1,Param->Decimation,&sinc[2]);
davide.aliprandi@st.com 2:9f389fd8fb2e 177 for(j=0;j<4;j++)
davide.aliprandi@st.com 2:9f389fd8fb2e 178 {
davide.aliprandi@st.com 2:9f389fd8fb2e 179 for(i=0;i<Param->Decimation;i++)
davide.aliprandi@st.com 2:9f389fd8fb2e 180 {
davide.aliprandi@st.com 2:9f389fd8fb2e 181 coef[j][i] = sinc[j*Param->Decimation+i];
davide.aliprandi@st.com 2:9f389fd8fb2e 182 sum+= sinc[j*Param->Decimation+i];
davide.aliprandi@st.com 2:9f389fd8fb2e 183 }
davide.aliprandi@st.com 2:9f389fd8fb2e 184 }
davide.aliprandi@st.com 2:9f389fd8fb2e 185
davide.aliprandi@st.com 2:9f389fd8fb2e 186 break;
davide.aliprandi@st.com 2:9f389fd8fb2e 187 case 5:
davide.aliprandi@st.com 2:9f389fd8fb2e 188 Param->FilterLen = Param->Decimation*5; // Dec * 5 - 2
davide.aliprandi@st.com 2:9f389fd8fb2e 189 sinc[0] = 0;
davide.aliprandi@st.com 2:9f389fd8fb2e 190 sinc[1] = 0;
davide.aliprandi@st.com 2:9f389fd8fb2e 191 sinc[Param->Decimation*5-2] = 0;
davide.aliprandi@st.com 2:9f389fd8fb2e 192 sinc[Param->Decimation*5-1] = 0;
davide.aliprandi@st.com 2:9f389fd8fb2e 193
davide.aliprandi@st.com 2:9f389fd8fb2e 194 convolve(sinc1, Param->Decimation,sinc1,Param->Decimation,sinc2);
davide.aliprandi@st.com 2:9f389fd8fb2e 195 convolve(sinc2, Param->Decimation*2-1,sinc1,Param->Decimation,sinc3);
davide.aliprandi@st.com 2:9f389fd8fb2e 196 convolve(sinc3, Param->Decimation*3-2,sinc1,Param->Decimation,sinc4);
davide.aliprandi@st.com 2:9f389fd8fb2e 197 convolve(sinc4, Param->Decimation*4-3,sinc1,Param->Decimation,&sinc[2]);
davide.aliprandi@st.com 2:9f389fd8fb2e 198
davide.aliprandi@st.com 2:9f389fd8fb2e 199 for(j=0;j<5;j++)
davide.aliprandi@st.com 2:9f389fd8fb2e 200 {
davide.aliprandi@st.com 2:9f389fd8fb2e 201 for(i=0;i<Param->Decimation;i++)
davide.aliprandi@st.com 2:9f389fd8fb2e 202 {
davide.aliprandi@st.com 2:9f389fd8fb2e 203 coef[j][i] = sinc[j*Param->Decimation+i];
davide.aliprandi@st.com 2:9f389fd8fb2e 204 sum+= sinc[j*Param->Decimation+i];
davide.aliprandi@st.com 2:9f389fd8fb2e 205 }
davide.aliprandi@st.com 2:9f389fd8fb2e 206 }
davide.aliprandi@st.com 2:9f389fd8fb2e 207 break;
davide.aliprandi@st.com 2:9f389fd8fb2e 208 }
davide.aliprandi@st.com 2:9f389fd8fb2e 209 SubConst = sum / 2;
davide.aliprandi@st.com 2:9f389fd8fb2e 210 DivideConst = SubConst*maxVol/32768/FilterGain;
davide.aliprandi@st.com 2:9f389fd8fb2e 211 if(DivideConst == 0 ) DivideConst = 1;
davide.aliprandi@st.com 2:9f389fd8fb2e 212 }
davide.aliprandi@st.com 2:9f389fd8fb2e 213
davide.aliprandi@st.com 2:9f389fd8fb2e 214 void convolve(uint32_t Signal[/* SignalLen */], unsigned short SignalLen,
davide.aliprandi@st.com 2:9f389fd8fb2e 215 uint32_t Kernel[/* KernelLen */], unsigned short KernelLen,
davide.aliprandi@st.com 2:9f389fd8fb2e 216 uint32_t Result[/* SignalLen + KernelLen - 1 */])
davide.aliprandi@st.com 2:9f389fd8fb2e 217 {
davide.aliprandi@st.com 2:9f389fd8fb2e 218 uint16_t n;
davide.aliprandi@st.com 2:9f389fd8fb2e 219 for (n = 0; n < SignalLen + KernelLen - 1; n++)
davide.aliprandi@st.com 2:9f389fd8fb2e 220 {
davide.aliprandi@st.com 2:9f389fd8fb2e 221 unsigned short kmin, kmax, k;
davide.aliprandi@st.com 2:9f389fd8fb2e 222
davide.aliprandi@st.com 2:9f389fd8fb2e 223 Result[n] = 0;
davide.aliprandi@st.com 2:9f389fd8fb2e 224
davide.aliprandi@st.com 2:9f389fd8fb2e 225 kmin = (n >= KernelLen - 1) ? n - (KernelLen - 1) : 0;
davide.aliprandi@st.com 2:9f389fd8fb2e 226 kmax = (n < SignalLen - 1) ? n : SignalLen - 1;
davide.aliprandi@st.com 2:9f389fd8fb2e 227
davide.aliprandi@st.com 2:9f389fd8fb2e 228 for (k = kmin; k <= kmax; k++)
davide.aliprandi@st.com 2:9f389fd8fb2e 229 {
davide.aliprandi@st.com 2:9f389fd8fb2e 230 Result[n] += Signal[k] * Kernel[n - k];
davide.aliprandi@st.com 2:9f389fd8fb2e 231 }
davide.aliprandi@st.com 2:9f389fd8fb2e 232 }
davide.aliprandi@st.com 2:9f389fd8fb2e 233 }
davide.aliprandi@st.com 2:9f389fd8fb2e 234
davide.aliprandi@st.com 2:9f389fd8fb2e 235 void Open_PDM_Filter(uint8_t* data, uint16_t* dataOut, uint16_t MicGain, TPDMFilter_InitStruct *Param)
davide.aliprandi@st.com 2:9f389fd8fb2e 236 {
davide.aliprandi@st.com 2:9f389fd8fb2e 237 uint32_t i;
davide.aliprandi@st.com 2:9f389fd8fb2e 238 int64_t OldOut, OldIn, OldZ;
davide.aliprandi@st.com 2:9f389fd8fb2e 239 OldOut=Param->OldOut;
davide.aliprandi@st.com 2:9f389fd8fb2e 240 OldIn=Param->OldIn;
davide.aliprandi@st.com 2:9f389fd8fb2e 241 OldZ=Param->OldZ;
davide.aliprandi@st.com 2:9f389fd8fb2e 242
davide.aliprandi@st.com 2:9f389fd8fb2e 243 for (i = 0; i < Param->Fs/1000; i++)
davide.aliprandi@st.com 2:9f389fd8fb2e 244 {
davide.aliprandi@st.com 2:9f389fd8fb2e 245 switch(Param->SincN)
davide.aliprandi@st.com 2:9f389fd8fb2e 246 {
davide.aliprandi@st.com 2:9f389fd8fb2e 247 case 1:
davide.aliprandi@st.com 2:9f389fd8fb2e 248 Z = filterTable(data,0, Param);
davide.aliprandi@st.com 2:9f389fd8fb2e 249 break;
davide.aliprandi@st.com 2:9f389fd8fb2e 250 case 2:
davide.aliprandi@st.com 2:9f389fd8fb2e 251 Z = Param->Coef[0] + filterTable(data,1, Param);
davide.aliprandi@st.com 2:9f389fd8fb2e 252 Param->Coef[0] = filterTable(data,0, Param);
davide.aliprandi@st.com 2:9f389fd8fb2e 253 break;
davide.aliprandi@st.com 2:9f389fd8fb2e 254 case 3:
davide.aliprandi@st.com 2:9f389fd8fb2e 255 Z = Param->Coef[1] + filterTable(data,2, Param);
davide.aliprandi@st.com 2:9f389fd8fb2e 256 Param->Coef[1] = Param->Coef[0] + filterTable(data,1, Param);
davide.aliprandi@st.com 2:9f389fd8fb2e 257 Param->Coef[0] = filterTable(data,0, Param);
davide.aliprandi@st.com 2:9f389fd8fb2e 258 break;
davide.aliprandi@st.com 2:9f389fd8fb2e 259 case 4:
davide.aliprandi@st.com 2:9f389fd8fb2e 260 Z = Param->Coef[2] + filterTable(data,3, Param);
davide.aliprandi@st.com 2:9f389fd8fb2e 261 Param->Coef[2] = Param->Coef[1] + filterTable(data,2, Param);
davide.aliprandi@st.com 2:9f389fd8fb2e 262 Param->Coef[1] = Param->Coef[0] + filterTable(data,1, Param);
davide.aliprandi@st.com 2:9f389fd8fb2e 263 Param->Coef[0] = filterTable(data,0, Param);
davide.aliprandi@st.com 2:9f389fd8fb2e 264 break;
davide.aliprandi@st.com 2:9f389fd8fb2e 265 case 5:
davide.aliprandi@st.com 2:9f389fd8fb2e 266 Z = Param->Coef[3] + filterTable(data,4, Param);
davide.aliprandi@st.com 2:9f389fd8fb2e 267 Param->Coef[3] = Param->Coef[2] + filterTable(data,3, Param);
davide.aliprandi@st.com 2:9f389fd8fb2e 268 Param->Coef[2] = Param->Coef[1] + filterTable(data,2, Param);
davide.aliprandi@st.com 2:9f389fd8fb2e 269 Param->Coef[1] = Param->Coef[0] + filterTable(data,1, Param);
davide.aliprandi@st.com 2:9f389fd8fb2e 270 Param->Coef[0] = filterTable(data,0, Param);
davide.aliprandi@st.com 2:9f389fd8fb2e 271 break;
davide.aliprandi@st.com 2:9f389fd8fb2e 272 }
davide.aliprandi@st.com 2:9f389fd8fb2e 273
davide.aliprandi@st.com 2:9f389fd8fb2e 274 Z-=SubConst;
davide.aliprandi@st.com 2:9f389fd8fb2e 275
davide.aliprandi@st.com 2:9f389fd8fb2e 276 if(Param->HP_ALFA!= 0)
davide.aliprandi@st.com 2:9f389fd8fb2e 277 {
davide.aliprandi@st.com 2:9f389fd8fb2e 278 OldOut = (Param->HP_ALFA * (OldOut + Z - OldIn))/256;
davide.aliprandi@st.com 2:9f389fd8fb2e 279 OldIn=Z;
davide.aliprandi@st.com 2:9f389fd8fb2e 280 Z=OldOut;
davide.aliprandi@st.com 2:9f389fd8fb2e 281 }
davide.aliprandi@st.com 2:9f389fd8fb2e 282
davide.aliprandi@st.com 2:9f389fd8fb2e 283 if(Param->LP_ALFA != 0)
davide.aliprandi@st.com 2:9f389fd8fb2e 284 {
davide.aliprandi@st.com 2:9f389fd8fb2e 285 OldZ = ((256-Param->LP_ALFA)*OldZ+Param->LP_ALFA*Z)/256;
davide.aliprandi@st.com 2:9f389fd8fb2e 286 Z = OldZ;
davide.aliprandi@st.com 2:9f389fd8fb2e 287 }
davide.aliprandi@st.com 2:9f389fd8fb2e 288
davide.aliprandi@st.com 2:9f389fd8fb2e 289 // if(Param->SincN>=3){
davide.aliprandi@st.com 2:9f389fd8fb2e 290 // Z = RoundDiv(Z, DivideConst);
davide.aliprandi@st.com 2:9f389fd8fb2e 291 // Z *= MicGain;
davide.aliprandi@st.com 2:9f389fd8fb2e 292 // }else{
davide.aliprandi@st.com 2:9f389fd8fb2e 293 Z *= MicGain;
davide.aliprandi@st.com 2:9f389fd8fb2e 294 Z = RoundDiv(Z, DivideConst);
davide.aliprandi@st.com 2:9f389fd8fb2e 295 // }
davide.aliprandi@st.com 2:9f389fd8fb2e 296 Z = SaturaLH(Z, -32700, 32700); // saturation
davide.aliprandi@st.com 2:9f389fd8fb2e 297 dataOut[i*Param->Out_MicChannels] = Z ;
davide.aliprandi@st.com 2:9f389fd8fb2e 298 // data+=(Param->Decimation-app)/8 * Param->In_MicChannels;
davide.aliprandi@st.com 2:9f389fd8fb2e 299 data+=Param->byte;
davide.aliprandi@st.com 2:9f389fd8fb2e 300 // data+=Param->Decimation/8*Param->In_MicChannels;
davide.aliprandi@st.com 2:9f389fd8fb2e 301 }
davide.aliprandi@st.com 2:9f389fd8fb2e 302 Param->OldOut=OldOut;
davide.aliprandi@st.com 2:9f389fd8fb2e 303 Param->OldIn=OldIn;
davide.aliprandi@st.com 2:9f389fd8fb2e 304 Param->OldZ=OldZ;
davide.aliprandi@st.com 2:9f389fd8fb2e 305 }