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@26:d67b45687c92, 2017-09-03 (annotated)

Committer:
qroche
Date:
Sun Sep 03 23:21:40 2017 +0000
Revision:
26:d67b45687c92
Parent:
20:9952bef19da1 
Il y avait des probl?mes dans la libraire...

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 ******************************************************************************
Davidroid 20:9952bef19da1 3 * @file OpenPDMFilter.c
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 /* Variables -----------------------------------------------------------------*/
davide.aliprandi@st.com 2:9f389fd8fb2e 37
davide.aliprandi@st.com 16:4ab2eac7be21 38 uint32_t div_const = 0;
davide.aliprandi@st.com 16:4ab2eac7be21 39 int64_t sub_const = 0;
Davidroid 20:9952bef19da1 40 uint32_t sinc[DECIMATION_MAX * SINCN];
davide.aliprandi@st.com 15:17bdadc6aa9c 41 uint32_t sinc1[DECIMATION_MAX];
davide.aliprandi@st.com 16:4ab2eac7be21 42 uint32_t sinc2[DECIMATION_MAX * 2];
Davidroid 20:9952bef19da1 43 uint32_t coef[SINCN][DECIMATION_MAX];
davide.aliprandi@st.com 15:17bdadc6aa9c 44 #ifdef USE_LUT
Davidroid 20:9952bef19da1 45 int32_t lut[256][DECIMATION_MAX / 8][SINCN];
davide.aliprandi@st.com 15:17bdadc6aa9c 46 #endif
davide.aliprandi@st.com 2:9f389fd8fb2e 47
davide.aliprandi@st.com 2:9f389fd8fb2e 48
davide.aliprandi@st.com 2:9f389fd8fb2e 49 /* Functions -----------------------------------------------------------------*/
davide.aliprandi@st.com 2:9f389fd8fb2e 50
davide.aliprandi@st.com 15:17bdadc6aa9c 51 #ifdef USE_LUT
Davidroid 20:9952bef19da1 52 int32_t filter_table_mono_64(uint8_t *data, uint8_t sincn)
davide.aliprandi@st.com 16:4ab2eac7be21 53 {
davide.aliprandi@st.com 16:4ab2eac7be21 54 return (int32_t)
davide.aliprandi@st.com 16:4ab2eac7be21 55 lut[data[0]][0][sincn] +
davide.aliprandi@st.com 16:4ab2eac7be21 56 lut[data[1]][1][sincn] +
davide.aliprandi@st.com 16:4ab2eac7be21 57 lut[data[2]][2][sincn] +
davide.aliprandi@st.com 16:4ab2eac7be21 58 lut[data[3]][3][sincn] +
davide.aliprandi@st.com 16:4ab2eac7be21 59 lut[data[4]][4][sincn] +
davide.aliprandi@st.com 16:4ab2eac7be21 60 lut[data[5]][5][sincn] +
davide.aliprandi@st.com 16:4ab2eac7be21 61 lut[data[6]][6][sincn] +
davide.aliprandi@st.com 16:4ab2eac7be21 62 lut[data[7]][7][sincn];
davide.aliprandi@st.com 16:4ab2eac7be21 63 }
Davidroid 20:9952bef19da1 64 int32_t filter_table_stereo_64(uint8_t *data, uint8_t sincn)
davide.aliprandi@st.com 15:17bdadc6aa9c 65 {
davide.aliprandi@st.com 16:4ab2eac7be21 66 return (int32_t)
davide.aliprandi@st.com 16:4ab2eac7be21 67 lut[data[0]][0][sincn] +
davide.aliprandi@st.com 16:4ab2eac7be21 68 lut[data[2]][1][sincn] +
davide.aliprandi@st.com 16:4ab2eac7be21 69 lut[data[4]][2][sincn] +
davide.aliprandi@st.com 16:4ab2eac7be21 70 lut[data[6]][3][sincn] +
davide.aliprandi@st.com 16:4ab2eac7be21 71 lut[data[8]][4][sincn] +
davide.aliprandi@st.com 16:4ab2eac7be21 72 lut[data[10]][5][sincn] +
davide.aliprandi@st.com 16:4ab2eac7be21 73 lut[data[12]][6][sincn] +
davide.aliprandi@st.com 16:4ab2eac7be21 74 lut[data[14]][7][sincn];
davide.aliprandi@st.com 15:17bdadc6aa9c 75 }
Davidroid 20:9952bef19da1 76 int32_t filter_table_mono_128(uint8_t *data, uint8_t sincn)
Davidroid 20:9952bef19da1 77 {
Davidroid 20:9952bef19da1 78 return (int32_t)
Davidroid 20:9952bef19da1 79 lut[data[0]][0][sincn] +
Davidroid 20:9952bef19da1 80 lut[data[1]][1][sincn] +
Davidroid 20:9952bef19da1 81 lut[data[2]][2][sincn] +
Davidroid 20:9952bef19da1 82 lut[data[3]][3][sincn] +
Davidroid 20:9952bef19da1 83 lut[data[4]][4][sincn] +
Davidroid 20:9952bef19da1 84 lut[data[5]][5][sincn] +
Davidroid 20:9952bef19da1 85 lut[data[6]][6][sincn] +
Davidroid 20:9952bef19da1 86 lut[data[7]][7][sincn] +
Davidroid 20:9952bef19da1 87 lut[data[8]][8][sincn] +
Davidroid 20:9952bef19da1 88 lut[data[9]][9][sincn] +
Davidroid 20:9952bef19da1 89 lut[data[10]][10][sincn] +
Davidroid 20:9952bef19da1 90 lut[data[11]][11][sincn] +
Davidroid 20:9952bef19da1 91 lut[data[12]][12][sincn] +
Davidroid 20:9952bef19da1 92 lut[data[13]][13][sincn] +
Davidroid 20:9952bef19da1 93 lut[data[14]][14][sincn] +
Davidroid 20:9952bef19da1 94 lut[data[15]][15][sincn];
Davidroid 20:9952bef19da1 95 }
Davidroid 20:9952bef19da1 96 int32_t filter_table_stereo_128(uint8_t *data, uint8_t sincn)
Davidroid 20:9952bef19da1 97 {
Davidroid 20:9952bef19da1 98 return (int32_t)
Davidroid 20:9952bef19da1 99 lut[data[0]][0][sincn] +
Davidroid 20:9952bef19da1 100 lut[data[2]][1][sincn] +
Davidroid 20:9952bef19da1 101 lut[data[4]][2][sincn] +
Davidroid 20:9952bef19da1 102 lut[data[6]][3][sincn] +
Davidroid 20:9952bef19da1 103 lut[data[8]][4][sincn] +
Davidroid 20:9952bef19da1 104 lut[data[10]][5][sincn] +
Davidroid 20:9952bef19da1 105 lut[data[12]][6][sincn] +
Davidroid 20:9952bef19da1 106 lut[data[14]][7][sincn] +
Davidroid 20:9952bef19da1 107 lut[data[16]][8][sincn] +
Davidroid 20:9952bef19da1 108 lut[data[18]][9][sincn] +
Davidroid 20:9952bef19da1 109 lut[data[20]][10][sincn] +
Davidroid 20:9952bef19da1 110 lut[data[22]][11][sincn] +
Davidroid 20:9952bef19da1 111 lut[data[24]][12][sincn] +
Davidroid 20:9952bef19da1 112 lut[data[26]][13][sincn] +
Davidroid 20:9952bef19da1 113 lut[data[28]][14][sincn] +
Davidroid 20:9952bef19da1 114 lut[data[30]][15][sincn];
Davidroid 20:9952bef19da1 115 }
Davidroid 20:9952bef19da1 116 int32_t (* filter_tables_64[2]) (uint8_t *data, uint8_t sincn) = {filter_table_mono_64, filter_table_stereo_64};
Davidroid 20:9952bef19da1 117 int32_t (* filter_tables_128[2]) (uint8_t *data, uint8_t sincn) = {filter_table_mono_128, filter_table_stereo_128};
davide.aliprandi@st.com 15:17bdadc6aa9c 118 #else
Davidroid 20:9952bef19da1 119 int32_t filter_table(uint8_t *data, uint8_t sincn, TPDMFilter_InitStruct *param)
davide.aliprandi@st.com 2:9f389fd8fb2e 120 {
Davidroid 13:90465220b75f 121 uint8_t c, i;
davide.aliprandi@st.com 11:b2f7f79026e4 122 uint16_t data_index = 0;
Davidroid 20:9952bef19da1 123 uint32_t *coef_p = &coef[sincn][0];
Davidroid 20:9952bef19da1 124 int32_t F = 0;
Davidroid 20:9952bef19da1 125 uint8_t decimation = param->Decimation;
Davidroid 20:9952bef19da1 126 uint8_t channels = param->In_MicChannels;
davide.aliprandi@st.com 2:9f389fd8fb2e 127
Davidroid 20:9952bef19da1 128 for (i = 0; i < decimation; i += 8) {
davide.aliprandi@st.com 11:b2f7f79026e4 129 c = data[data_index];
davide.aliprandi@st.com 11:b2f7f79026e4 130 F += ((c >> 7) ) * coef_p[i ] +
davide.aliprandi@st.com 11:b2f7f79026e4 131 ((c >> 6) & 0x01) * coef_p[i + 1] +
davide.aliprandi@st.com 11:b2f7f79026e4 132 ((c >> 5) & 0x01) * coef_p[i + 2] +
davide.aliprandi@st.com 11:b2f7f79026e4 133 ((c >> 4) & 0x01) * coef_p[i + 3] +
davide.aliprandi@st.com 11:b2f7f79026e4 134 ((c >> 3) & 0x01) * coef_p[i + 4] +
davide.aliprandi@st.com 11:b2f7f79026e4 135 ((c >> 2) & 0x01) * coef_p[i + 5] +
davide.aliprandi@st.com 11:b2f7f79026e4 136 ((c >> 1) & 0x01) * coef_p[i + 6] +
davide.aliprandi@st.com 11:b2f7f79026e4 137 ((c ) & 0x01) * coef_p[i + 7];
davide.aliprandi@st.com 11:b2f7f79026e4 138 data_index += channels;
davide.aliprandi@st.com 2:9f389fd8fb2e 139 }
davide.aliprandi@st.com 2:9f389fd8fb2e 140 return F;
davide.aliprandi@st.com 2:9f389fd8fb2e 141 }
davide.aliprandi@st.com 15:17bdadc6aa9c 142 #endif
davide.aliprandi@st.com 2:9f389fd8fb2e 143
Davidroid 20:9952bef19da1 144 void convolve(uint32_t Signal[/* SignalLen */], unsigned short SignalLen,
Davidroid 20:9952bef19da1 145 uint32_t Kernel[/* KernelLen */], unsigned short KernelLen,
Davidroid 20:9952bef19da1 146 uint32_t Result[/* SignalLen + KernelLen - 1 */])
Davidroid 20:9952bef19da1 147 {
Davidroid 20:9952bef19da1 148 uint16_t n;
Davidroid 20:9952bef19da1 149
Davidroid 20:9952bef19da1 150 for (n = 0; n < SignalLen + KernelLen - 1; n++)
Davidroid 20:9952bef19da1 151 {
Davidroid 20:9952bef19da1 152 unsigned short kmin, kmax, k;
Davidroid 20:9952bef19da1 153
Davidroid 20:9952bef19da1 154 Result[n] = 0;
Davidroid 20:9952bef19da1 155
Davidroid 20:9952bef19da1 156 kmin = (n >= KernelLen - 1) ? n - (KernelLen - 1) : 0;
Davidroid 20:9952bef19da1 157 kmax = (n < SignalLen - 1) ? n : SignalLen - 1;
Davidroid 20:9952bef19da1 158
Davidroid 20:9952bef19da1 159 for (k = kmin; k <= kmax; k++) {
Davidroid 20:9952bef19da1 160 Result[n] += Signal[k] * Kernel[n - k];
Davidroid 20:9952bef19da1 161 }
Davidroid 20:9952bef19da1 162 }
Davidroid 20:9952bef19da1 163 }
Davidroid 20:9952bef19da1 164
davide.aliprandi@st.com 2:9f389fd8fb2e 165 void Open_PDM_Filter_Init(TPDMFilter_InitStruct *Param)
davide.aliprandi@st.com 2:9f389fd8fb2e 166 {
davide.aliprandi@st.com 16:4ab2eac7be21 167 uint16_t i, j;
davide.aliprandi@st.com 2:9f389fd8fb2e 168 int64_t sum = 0;
Davidroid 20:9952bef19da1 169
Davidroid 20:9952bef19da1 170 uint8_t decimation = Param->Decimation;
Davidroid 20:9952bef19da1 171
Davidroid 20:9952bef19da1 172 for (i = 0; i < SINCN; i++) {
davide.aliprandi@st.com 16:4ab2eac7be21 173 Param->Coef[i] = 0;
davide.aliprandi@st.com 16:4ab2eac7be21 174 Param->bit[i] = 0;
davide.aliprandi@st.com 2:9f389fd8fb2e 175 }
Davidroid 20:9952bef19da1 176 for (i = 0; i < decimation; i++) {
davide.aliprandi@st.com 16:4ab2eac7be21 177 sinc1[i] = 1;
davide.aliprandi@st.com 2:9f389fd8fb2e 178 }
davide.aliprandi@st.com 16:4ab2eac7be21 179
davide.aliprandi@st.com 16:4ab2eac7be21 180 Param->OldOut = Param->OldIn = Param->OldZ = 0;
Davidroid 20:9952bef19da1 181 Param->LP_ALFA = (Param->LP_HZ != 0 ? (uint16_t) (Param->LP_HZ * 256 / (Param->LP_HZ + Param->Fs / (2 * 3.14159))) : 0);
Davidroid 20:9952bef19da1 182 Param->HP_ALFA = (Param->HP_HZ != 0 ? (uint16_t) (Param->Fs * 256 / (2 * 3.14159 * Param->HP_HZ + Param->Fs)) : 0);
davide.aliprandi@st.com 16:4ab2eac7be21 183
Davidroid 20:9952bef19da1 184 Param->FilterLen = decimation * SINCN;
davide.aliprandi@st.com 16:4ab2eac7be21 185 sinc[0] = 0;
Davidroid 20:9952bef19da1 186 sinc[decimation * SINCN - 1] = 0;
Davidroid 20:9952bef19da1 187 convolve(sinc1, decimation, sinc1, decimation, sinc2);
Davidroid 20:9952bef19da1 188 convolve(sinc2, decimation * 2 - 1, sinc1, decimation, &sinc[1]);
Davidroid 20:9952bef19da1 189 for(j = 0; j < SINCN; j++) {
Davidroid 20:9952bef19da1 190 for (i = 0; i < decimation; i++) {
Davidroid 20:9952bef19da1 191 coef[j][i] = sinc[j * decimation + i];
Davidroid 20:9952bef19da1 192 sum += sinc[j * decimation + i];
davide.aliprandi@st.com 2:9f389fd8fb2e 193 }
davide.aliprandi@st.com 2:9f389fd8fb2e 194 }
davide.aliprandi@st.com 16:4ab2eac7be21 195
davide.aliprandi@st.com 16:4ab2eac7be21 196 sub_const = sum >> 1;
Davidroid 20:9952bef19da1 197 div_const = sub_const * Param->MaxVolume / 32768 / FILTER_GAIN;
davide.aliprandi@st.com 16:4ab2eac7be21 198 div_const = (div_const == 0 ? 1 : div_const);
davide.aliprandi@st.com 15:17bdadc6aa9c 199
davide.aliprandi@st.com 15:17bdadc6aa9c 200 #ifdef USE_LUT
davide.aliprandi@st.com 15:17bdadc6aa9c 201 /* Look-Up Table. */
davide.aliprandi@st.com 15:17bdadc6aa9c 202 uint16_t c, d, s;
Davidroid 20:9952bef19da1 203 for (s = 0; s < SINCN; s++)
davide.aliprandi@st.com 15:17bdadc6aa9c 204 {
davide.aliprandi@st.com 15:17bdadc6aa9c 205 uint32_t *coef_p = &coef[s][0];
davide.aliprandi@st.com 15:17bdadc6aa9c 206 for (c = 0; c < 256; c++)
Davidroid 20:9952bef19da1 207 for (d = 0; d < decimation / 8; d++)
davide.aliprandi@st.com 15:17bdadc6aa9c 208 lut[c][d][s] = ((c >> 7) ) * coef_p[d * 8 ] +
davide.aliprandi@st.com 15:17bdadc6aa9c 209 ((c >> 6) & 0x01) * coef_p[d * 8 + 1] +
davide.aliprandi@st.com 15:17bdadc6aa9c 210 ((c >> 5) & 0x01) * coef_p[d * 8 + 2] +
davide.aliprandi@st.com 15:17bdadc6aa9c 211 ((c >> 4) & 0x01) * coef_p[d * 8 + 3] +
davide.aliprandi@st.com 15:17bdadc6aa9c 212 ((c >> 3) & 0x01) * coef_p[d * 8 + 4] +
davide.aliprandi@st.com 15:17bdadc6aa9c 213 ((c >> 2) & 0x01) * coef_p[d * 8 + 5] +
davide.aliprandi@st.com 15:17bdadc6aa9c 214 ((c >> 1) & 0x01) * coef_p[d * 8 + 6] +
davide.aliprandi@st.com 15:17bdadc6aa9c 215 ((c ) & 0x01) * coef_p[d * 8 + 7];
davide.aliprandi@st.com 15:17bdadc6aa9c 216 }
davide.aliprandi@st.com 15:17bdadc6aa9c 217 #endif
davide.aliprandi@st.com 2:9f389fd8fb2e 218 }
davide.aliprandi@st.com 2:9f389fd8fb2e 219
Davidroid 20:9952bef19da1 220 void Open_PDM_Filter_64(uint8_t* data, uint16_t* dataOut, uint16_t volume, TPDMFilter_InitStruct *Param)
davide.aliprandi@st.com 2:9f389fd8fb2e 221 {
davide.aliprandi@st.com 16:4ab2eac7be21 222 uint8_t i, data_out_index;
davide.aliprandi@st.com 16:4ab2eac7be21 223 uint8_t channels = Param->In_MicChannels;
Davidroid 20:9952bef19da1 224 uint8_t data_inc = ((DECIMATION_MAX >> 4) * channels);
davide.aliprandi@st.com 16:4ab2eac7be21 225 int64_t Z, Z0, Z1, Z2;
Davidroid 20:9952bef19da1 226 int64_t OldOut, OldIn, OldZ;
davide.aliprandi@st.com 16:4ab2eac7be21 227
davide.aliprandi@st.com 16:4ab2eac7be21 228 OldOut = Param->OldOut;
davide.aliprandi@st.com 16:4ab2eac7be21 229 OldIn = Param->OldIn;
davide.aliprandi@st.com 16:4ab2eac7be21 230 OldZ = Param->OldZ;
davide.aliprandi@st.com 16:4ab2eac7be21 231
Davidroid 20:9952bef19da1 232 #ifdef USE_LUT
Davidroid 20:9952bef19da1 233 uint8_t j = channels - 1;
Davidroid 20:9952bef19da1 234 #endif
Davidroid 20:9952bef19da1 235
Davidroid 20:9952bef19da1 236 for (i = 0, data_out_index = 0; i < Param->Fs / 1000; i++, data_out_index += channels) {
davide.aliprandi@st.com 16:4ab2eac7be21 237 #ifdef USE_LUT
Davidroid 20:9952bef19da1 238 Z0 = filter_tables_64[j](data, 0);
Davidroid 20:9952bef19da1 239 Z1 = filter_tables_64[j](data, 1);
Davidroid 20:9952bef19da1 240 Z2 = filter_tables_64[j](data, 2);
davide.aliprandi@st.com 16:4ab2eac7be21 241 #else
Davidroid 20:9952bef19da1 242 Z0 = filter_table(data, 0, Param);
Davidroid 20:9952bef19da1 243 Z1 = filter_table(data, 1, Param);
Davidroid 20:9952bef19da1 244 Z2 = filter_table(data, 2, Param);
davide.aliprandi@st.com 16:4ab2eac7be21 245 #endif
davide.aliprandi@st.com 16:4ab2eac7be21 246
davide.aliprandi@st.com 16:4ab2eac7be21 247 Z = Param->Coef[1] + Z2 - sub_const;
davide.aliprandi@st.com 16:4ab2eac7be21 248 Param->Coef[1] = Param->Coef[0] + Z1;
davide.aliprandi@st.com 16:4ab2eac7be21 249 Param->Coef[0] = Z0;
davide.aliprandi@st.com 16:4ab2eac7be21 250
davide.aliprandi@st.com 16:4ab2eac7be21 251 OldOut = (Param->HP_ALFA * (OldOut + Z - OldIn)) >> 8;
davide.aliprandi@st.com 16:4ab2eac7be21 252 OldIn = Z;
Davidroid 20:9952bef19da1 253 OldZ = ((256 - Param->LP_ALFA) * OldZ + Param->LP_ALFA * OldOut) >> 8;
davide.aliprandi@st.com 16:4ab2eac7be21 254
Davidroid 20:9952bef19da1 255 Z = OldZ * volume;
davide.aliprandi@st.com 16:4ab2eac7be21 256 Z = RoundDiv(Z, div_const);
davide.aliprandi@st.com 16:4ab2eac7be21 257 Z = SaturaLH(Z, -32700, 32700);
davide.aliprandi@st.com 16:4ab2eac7be21 258
davide.aliprandi@st.com 16:4ab2eac7be21 259 dataOut[data_out_index] = Z;
davide.aliprandi@st.com 16:4ab2eac7be21 260 data += data_inc;
davide.aliprandi@st.com 16:4ab2eac7be21 261 }
davide.aliprandi@st.com 16:4ab2eac7be21 262
davide.aliprandi@st.com 16:4ab2eac7be21 263 Param->OldOut = OldOut;
davide.aliprandi@st.com 16:4ab2eac7be21 264 Param->OldIn = OldIn;
davide.aliprandi@st.com 16:4ab2eac7be21 265 Param->OldZ = OldZ;
davide.aliprandi@st.com 2:9f389fd8fb2e 266 }
Davidroid 20:9952bef19da1 267
Davidroid 20:9952bef19da1 268 void Open_PDM_Filter_128(uint8_t* data, uint16_t* dataOut, uint16_t volume, TPDMFilter_InitStruct *Param)
Davidroid 20:9952bef19da1 269 {
Davidroid 20:9952bef19da1 270 uint8_t i, data_out_index;
Davidroid 20:9952bef19da1 271 uint8_t channels = Param->In_MicChannels;
Davidroid 20:9952bef19da1 272 uint8_t data_inc = ((DECIMATION_MAX >> 3) * channels);
Davidroid 20:9952bef19da1 273 int64_t Z, Z0, Z1, Z2;
Davidroid 20:9952bef19da1 274 int64_t OldOut, OldIn, OldZ;
Davidroid 20:9952bef19da1 275
Davidroid 20:9952bef19da1 276 OldOut = Param->OldOut;
Davidroid 20:9952bef19da1 277 OldIn = Param->OldIn;
Davidroid 20:9952bef19da1 278 OldZ = Param->OldZ;
Davidroid 20:9952bef19da1 279
Davidroid 20:9952bef19da1 280 #ifdef USE_LUT
Davidroid 20:9952bef19da1 281 uint8_t j = channels - 1;
Davidroid 20:9952bef19da1 282 #endif
Davidroid 20:9952bef19da1 283
Davidroid 20:9952bef19da1 284 for (i = 0, data_out_index = 0; i < Param->Fs / 1000; i++, data_out_index += channels) {
Davidroid 20:9952bef19da1 285 #ifdef USE_LUT
Davidroid 20:9952bef19da1 286 Z0 = filter_tables_128[j](data, 0);
Davidroid 20:9952bef19da1 287 Z1 = filter_tables_128[j](data, 1);
Davidroid 20:9952bef19da1 288 Z2 = filter_tables_128[j](data, 2);
Davidroid 20:9952bef19da1 289 #else
Davidroid 20:9952bef19da1 290 Z0 = filter_table(data, 0, Param);
Davidroid 20:9952bef19da1 291 Z1 = filter_table(data, 1, Param);
Davidroid 20:9952bef19da1 292 Z2 = filter_table(data, 2, Param);
Davidroid 20:9952bef19da1 293 #endif
Davidroid 20:9952bef19da1 294
Davidroid 20:9952bef19da1 295 Z = Param->Coef[1] + Z2 - sub_const;
Davidroid 20:9952bef19da1 296 Param->Coef[1] = Param->Coef[0] + Z1;
Davidroid 20:9952bef19da1 297 Param->Coef[0] = Z0;
Davidroid 20:9952bef19da1 298
Davidroid 20:9952bef19da1 299 OldOut = (Param->HP_ALFA * (OldOut + Z - OldIn)) >> 8;
Davidroid 20:9952bef19da1 300 OldIn = Z;
Davidroid 20:9952bef19da1 301 OldZ = ((256 - Param->LP_ALFA) * OldZ + Param->LP_ALFA * OldOut) >> 8;
Davidroid 20:9952bef19da1 302
Davidroid 20:9952bef19da1 303 Z = OldZ * volume;
Davidroid 20:9952bef19da1 304 Z = RoundDiv(Z, div_const);
Davidroid 20:9952bef19da1 305 Z = SaturaLH(Z, -32700, 32700);
Davidroid 20:9952bef19da1 306
Davidroid 20:9952bef19da1 307 dataOut[data_out_index] = Z;
Davidroid 20:9952bef19da1 308 data += data_inc;
Davidroid 20:9952bef19da1 309 }
Davidroid 20:9952bef19da1 310
Davidroid 20:9952bef19da1 311 Param->OldOut = OldOut;
Davidroid 20:9952bef19da1 312 Param->OldIn = OldIn;
Davidroid 20:9952bef19da1 313 Param->OldZ = OldZ;
Davidroid 20:9952bef19da1 314 }