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

Committer:
davide.aliprandi@st.com
Date:
Fri Apr 21 10:33:08 2017 +0200
Revision:
2:9f389fd8fb2e
Child:
6:9b8bc842aeb3
Aligned to ARM mbed codinmg style; OpenPDM2PCM middleware added.

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