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@11:b2f7f79026e4, 2017-04-28 (annotated)

Committer:
davide.aliprandi@st.com
Date:
Fri Apr 28 17:22:13 2017 +0200
Revision:
11:b2f7f79026e4
Parent:
6:9b8bc842aeb3
Child:
13:90465220b75f
OpenPDM2PCM library improved.

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