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@16:4ab2eac7be21, 2017-05-03 (annotated)

Committer:
davide.aliprandi@st.com
Date:
Wed May 03 18:10:29 2017 +0200
Revision:
16:4ab2eac7be21
Parent:
15:17bdadc6aa9c
Child:
20:9952bef19da1
Improving performances with LUT; keeping only default configuration with SincN = 3.

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 16:4ab2eac7be21 38 #define RoundDiv(a, b) (((a)>0)?(((a)+(b)/2)/(b)):(((a)-(b)/2)/(b)))
davide.aliprandi@st.com 2:9f389fd8fb2e 39 #define SaturaLH(N, L, H) (((N)<(L))?(L):(((N)>(H))?(H):(N)))
davide.aliprandi@st.com 2:9f389fd8fb2e 40
davide.aliprandi@st.com 16:4ab2eac7be21 41 #define VOLUME_MAX 64
davide.aliprandi@st.com 16:4ab2eac7be21 42 #define FILTER_GAIN 16
davide.aliprandi@st.com 16:4ab2eac7be21 43
davide.aliprandi@st.com 16:4ab2eac7be21 44 #define SINCN_MAX 3
davide.aliprandi@st.com 15:17bdadc6aa9c 45 #define DECIMATION_MAX 64
davide.aliprandi@st.com 15:17bdadc6aa9c 46
davide.aliprandi@st.com 2:9f389fd8fb2e 47
davide.aliprandi@st.com 2:9f389fd8fb2e 48 /* Variables -----------------------------------------------------------------*/
davide.aliprandi@st.com 2:9f389fd8fb2e 49
davide.aliprandi@st.com 16:4ab2eac7be21 50 uint32_t div_const = 0;
davide.aliprandi@st.com 16:4ab2eac7be21 51 int64_t sub_const = 0;
davide.aliprandi@st.com 16:4ab2eac7be21 52 uint32_t sinc[DECIMATION_MAX * SINCN_MAX];
davide.aliprandi@st.com 15:17bdadc6aa9c 53 uint32_t sinc1[DECIMATION_MAX];
davide.aliprandi@st.com 16:4ab2eac7be21 54 uint32_t sinc2[DECIMATION_MAX * 2];
davide.aliprandi@st.com 16:4ab2eac7be21 55 uint32_t coef[SINCN_MAX][DECIMATION_MAX];
davide.aliprandi@st.com 15:17bdadc6aa9c 56 #ifdef USE_LUT
davide.aliprandi@st.com 15:17bdadc6aa9c 57 int32_t lut[256][DECIMATION_MAX / 8][SINCN_MAX];
davide.aliprandi@st.com 15:17bdadc6aa9c 58 #endif
davide.aliprandi@st.com 2:9f389fd8fb2e 59
davide.aliprandi@st.com 2:9f389fd8fb2e 60
davide.aliprandi@st.com 2:9f389fd8fb2e 61 /* Functions -----------------------------------------------------------------*/
davide.aliprandi@st.com 2:9f389fd8fb2e 62
davide.aliprandi@st.com 16:4ab2eac7be21 63 int32_t filter_table(uint8_t *data, uint8_t sincn, uint8_t channels);
davide.aliprandi@st.com 2:9f389fd8fb2e 64
davide.aliprandi@st.com 2:9f389fd8fb2e 65 void convolve(uint32_t Signal[/* SignalLen */], unsigned short SignalLen,
davide.aliprandi@st.com 2:9f389fd8fb2e 66 uint32_t Kernel[/* KernelLen */], unsigned short KernelLen,
davide.aliprandi@st.com 2:9f389fd8fb2e 67 uint32_t Result[/* SignalLen + KernelLen - 1 */]);
davide.aliprandi@st.com 2:9f389fd8fb2e 68
davide.aliprandi@st.com 15:17bdadc6aa9c 69 #ifdef USE_LUT
davide.aliprandi@st.com 16:4ab2eac7be21 70 int32_t filter_table_mono(uint8_t *data, uint8_t sincn)
davide.aliprandi@st.com 16:4ab2eac7be21 71 {
davide.aliprandi@st.com 16:4ab2eac7be21 72 return (int32_t)
davide.aliprandi@st.com 16:4ab2eac7be21 73 lut[data[0]][0][sincn] +
davide.aliprandi@st.com 16:4ab2eac7be21 74 lut[data[1]][1][sincn] +
davide.aliprandi@st.com 16:4ab2eac7be21 75 lut[data[2]][2][sincn] +
davide.aliprandi@st.com 16:4ab2eac7be21 76 lut[data[3]][3][sincn] +
davide.aliprandi@st.com 16:4ab2eac7be21 77 lut[data[4]][4][sincn] +
davide.aliprandi@st.com 16:4ab2eac7be21 78 lut[data[5]][5][sincn] +
davide.aliprandi@st.com 16:4ab2eac7be21 79 lut[data[6]][6][sincn] +
davide.aliprandi@st.com 16:4ab2eac7be21 80 lut[data[7]][7][sincn];
davide.aliprandi@st.com 16:4ab2eac7be21 81 }
davide.aliprandi@st.com 16:4ab2eac7be21 82 int32_t filter_table_stereo(uint8_t *data, uint8_t sincn)
davide.aliprandi@st.com 15:17bdadc6aa9c 83 {
davide.aliprandi@st.com 16:4ab2eac7be21 84 return (int32_t)
davide.aliprandi@st.com 16:4ab2eac7be21 85 lut[data[0]][0][sincn] +
davide.aliprandi@st.com 16:4ab2eac7be21 86 lut[data[2]][1][sincn] +
davide.aliprandi@st.com 16:4ab2eac7be21 87 lut[data[4]][2][sincn] +
davide.aliprandi@st.com 16:4ab2eac7be21 88 lut[data[6]][3][sincn] +
davide.aliprandi@st.com 16:4ab2eac7be21 89 lut[data[8]][4][sincn] +
davide.aliprandi@st.com 16:4ab2eac7be21 90 lut[data[10]][5][sincn] +
davide.aliprandi@st.com 16:4ab2eac7be21 91 lut[data[12]][6][sincn] +
davide.aliprandi@st.com 16:4ab2eac7be21 92 lut[data[14]][7][sincn];
davide.aliprandi@st.com 15:17bdadc6aa9c 93 }
davide.aliprandi@st.com 16:4ab2eac7be21 94 int32_t (* filter_tables[2]) (uint8_t *data, uint8_t sincn) = {filter_table_mono, filter_table_stereo};
davide.aliprandi@st.com 15:17bdadc6aa9c 95 #else
davide.aliprandi@st.com 16:4ab2eac7be21 96 int64_t filter_table(uint8_t *data, uint8_t sincn, uint8_t channels)
davide.aliprandi@st.com 2:9f389fd8fb2e 97 {
Davidroid 13:90465220b75f 98 uint8_t c, i;
davide.aliprandi@st.com 11:b2f7f79026e4 99 uint16_t data_index = 0;
davide.aliprandi@st.com 11:b2f7f79026e4 100 uint32_t *coef_p = &coef[table][0];
davide.aliprandi@st.com 2:9f389fd8fb2e 101 int64_t F = 0;
davide.aliprandi@st.com 2:9f389fd8fb2e 102
davide.aliprandi@st.com 16:4ab2eac7be21 103 for (i = 0; i < DECIMATION_MAX; i += 8)
davide.aliprandi@st.com 15:17bdadc6aa9c 104 {
davide.aliprandi@st.com 11:b2f7f79026e4 105 c = data[data_index];
davide.aliprandi@st.com 11:b2f7f79026e4 106 F += ((c >> 7) ) * coef_p[i ] +
davide.aliprandi@st.com 11:b2f7f79026e4 107 ((c >> 6) & 0x01) * coef_p[i + 1] +
davide.aliprandi@st.com 11:b2f7f79026e4 108 ((c >> 5) & 0x01) * coef_p[i + 2] +
davide.aliprandi@st.com 11:b2f7f79026e4 109 ((c >> 4) & 0x01) * coef_p[i + 3] +
davide.aliprandi@st.com 11:b2f7f79026e4 110 ((c >> 3) & 0x01) * coef_p[i + 4] +
davide.aliprandi@st.com 11:b2f7f79026e4 111 ((c >> 2) & 0x01) * coef_p[i + 5] +
davide.aliprandi@st.com 11:b2f7f79026e4 112 ((c >> 1) & 0x01) * coef_p[i + 6] +
davide.aliprandi@st.com 11:b2f7f79026e4 113 ((c ) & 0x01) * coef_p[i + 7];
davide.aliprandi@st.com 11:b2f7f79026e4 114 data_index += channels;
davide.aliprandi@st.com 2:9f389fd8fb2e 115 }
davide.aliprandi@st.com 2:9f389fd8fb2e 116 return F;
davide.aliprandi@st.com 2:9f389fd8fb2e 117 }
davide.aliprandi@st.com 15:17bdadc6aa9c 118 #endif
davide.aliprandi@st.com 2:9f389fd8fb2e 119
davide.aliprandi@st.com 2:9f389fd8fb2e 120 void Open_PDM_Filter_Init(TPDMFilter_InitStruct *Param)
davide.aliprandi@st.com 2:9f389fd8fb2e 121 {
davide.aliprandi@st.com 16:4ab2eac7be21 122 uint16_t i, j;
davide.aliprandi@st.com 2:9f389fd8fb2e 123 int64_t sum = 0;
davide.aliprandi@st.com 16:4ab2eac7be21 124 for (i = 0; i < SINCN_MAX; i++)
davide.aliprandi@st.com 2:9f389fd8fb2e 125 {
davide.aliprandi@st.com 16:4ab2eac7be21 126 Param->Coef[i] = 0;
davide.aliprandi@st.com 16:4ab2eac7be21 127 Param->bit[i] = 0;
davide.aliprandi@st.com 2:9f389fd8fb2e 128 }
davide.aliprandi@st.com 16:4ab2eac7be21 129 for (i = 0; i < DECIMATION_MAX; i++)
davide.aliprandi@st.com 2:9f389fd8fb2e 130 {
davide.aliprandi@st.com 16:4ab2eac7be21 131 sinc1[i] = 1;
davide.aliprandi@st.com 2:9f389fd8fb2e 132 }
davide.aliprandi@st.com 16:4ab2eac7be21 133
davide.aliprandi@st.com 16:4ab2eac7be21 134 Param->OldOut = Param->OldIn = Param->OldZ = 0;
davide.aliprandi@st.com 16:4ab2eac7be21 135 Param->LP_ALFA = (Param->LP_HZ != 0 ? (uint16_t) (Param->LP_HZ * 256 / (Param->LP_HZ + Param->Fs / (2 * 3.14))) : 0);
davide.aliprandi@st.com 16:4ab2eac7be21 136 Param->HP_ALFA = (Param->HP_HZ != 0 ? (uint16_t) (Param->Fs * 256 / (2 * 3.14 * Param->HP_HZ + Param->Fs)) : 0);
davide.aliprandi@st.com 16:4ab2eac7be21 137
davide.aliprandi@st.com 16:4ab2eac7be21 138 Param->FilterLen = DECIMATION_MAX * 3;
davide.aliprandi@st.com 16:4ab2eac7be21 139 sinc[0] = 0;
davide.aliprandi@st.com 16:4ab2eac7be21 140 sinc[DECIMATION_MAX * 3 - 1] = 0;
davide.aliprandi@st.com 16:4ab2eac7be21 141 convolve(sinc1, DECIMATION_MAX, sinc1, DECIMATION_MAX, sinc2);
davide.aliprandi@st.com 16:4ab2eac7be21 142 convolve(sinc2, DECIMATION_MAX * 2 - 1, sinc1, DECIMATION_MAX, &sinc[1]);
davide.aliprandi@st.com 16:4ab2eac7be21 143 for(j = 0; j < 3; j++)
davide.aliprandi@st.com 2:9f389fd8fb2e 144 {
davide.aliprandi@st.com 16:4ab2eac7be21 145 for (i = 0; i < DECIMATION_MAX ;i++)
davide.aliprandi@st.com 2:9f389fd8fb2e 146 {
davide.aliprandi@st.com 16:4ab2eac7be21 147 coef[j][i] = sinc[j * DECIMATION_MAX + i];
davide.aliprandi@st.com 16:4ab2eac7be21 148 sum += sinc[j * DECIMATION_MAX + i];
davide.aliprandi@st.com 2:9f389fd8fb2e 149 }
davide.aliprandi@st.com 2:9f389fd8fb2e 150 }
davide.aliprandi@st.com 16:4ab2eac7be21 151
davide.aliprandi@st.com 16:4ab2eac7be21 152 sub_const = sum >> 1;
davide.aliprandi@st.com 16:4ab2eac7be21 153 div_const = sub_const * VOLUME_MAX / 32768 / FILTER_GAIN;
davide.aliprandi@st.com 16:4ab2eac7be21 154 div_const = (div_const == 0 ? 1 : div_const);
davide.aliprandi@st.com 15:17bdadc6aa9c 155
davide.aliprandi@st.com 15:17bdadc6aa9c 156 #ifdef USE_LUT
davide.aliprandi@st.com 15:17bdadc6aa9c 157 /* Look-Up Table. */
davide.aliprandi@st.com 15:17bdadc6aa9c 158 uint16_t c, d, s;
davide.aliprandi@st.com 15:17bdadc6aa9c 159 for (s = 0; s < SINCN_MAX; s++)
davide.aliprandi@st.com 15:17bdadc6aa9c 160 {
davide.aliprandi@st.com 15:17bdadc6aa9c 161 uint32_t *coef_p = &coef[s][0];
davide.aliprandi@st.com 15:17bdadc6aa9c 162 for (c = 0; c < 256; c++)
davide.aliprandi@st.com 15:17bdadc6aa9c 163 for (d = 0; d < DECIMATION_MAX / 8; d++)
davide.aliprandi@st.com 15:17bdadc6aa9c 164 lut[c][d][s] = ((c >> 7) ) * coef_p[d * 8 ] +
davide.aliprandi@st.com 15:17bdadc6aa9c 165 ((c >> 6) & 0x01) * coef_p[d * 8 + 1] +
davide.aliprandi@st.com 15:17bdadc6aa9c 166 ((c >> 5) & 0x01) * coef_p[d * 8 + 2] +
davide.aliprandi@st.com 15:17bdadc6aa9c 167 ((c >> 4) & 0x01) * coef_p[d * 8 + 3] +
davide.aliprandi@st.com 15:17bdadc6aa9c 168 ((c >> 3) & 0x01) * coef_p[d * 8 + 4] +
davide.aliprandi@st.com 15:17bdadc6aa9c 169 ((c >> 2) & 0x01) * coef_p[d * 8 + 5] +
davide.aliprandi@st.com 15:17bdadc6aa9c 170 ((c >> 1) & 0x01) * coef_p[d * 8 + 6] +
davide.aliprandi@st.com 15:17bdadc6aa9c 171 ((c ) & 0x01) * coef_p[d * 8 + 7];
davide.aliprandi@st.com 15:17bdadc6aa9c 172 }
davide.aliprandi@st.com 15:17bdadc6aa9c 173 #endif
davide.aliprandi@st.com 2:9f389fd8fb2e 174 }
davide.aliprandi@st.com 2:9f389fd8fb2e 175
davide.aliprandi@st.com 2:9f389fd8fb2e 176 void convolve(uint32_t Signal[/* SignalLen */], unsigned short SignalLen,
davide.aliprandi@st.com 2:9f389fd8fb2e 177 uint32_t Kernel[/* KernelLen */], unsigned short KernelLen,
davide.aliprandi@st.com 2:9f389fd8fb2e 178 uint32_t Result[/* SignalLen + KernelLen - 1 */])
davide.aliprandi@st.com 2:9f389fd8fb2e 179 {
davide.aliprandi@st.com 2:9f389fd8fb2e 180 uint16_t n;
davide.aliprandi@st.com 2:9f389fd8fb2e 181 for (n = 0; n < SignalLen + KernelLen - 1; n++)
davide.aliprandi@st.com 2:9f389fd8fb2e 182 {
davide.aliprandi@st.com 2:9f389fd8fb2e 183 unsigned short kmin, kmax, k;
davide.aliprandi@st.com 2:9f389fd8fb2e 184
davide.aliprandi@st.com 2:9f389fd8fb2e 185 Result[n] = 0;
davide.aliprandi@st.com 2:9f389fd8fb2e 186
davide.aliprandi@st.com 2:9f389fd8fb2e 187 kmin = (n >= KernelLen - 1) ? n - (KernelLen - 1) : 0;
davide.aliprandi@st.com 2:9f389fd8fb2e 188 kmax = (n < SignalLen - 1) ? n : SignalLen - 1;
davide.aliprandi@st.com 2:9f389fd8fb2e 189
davide.aliprandi@st.com 2:9f389fd8fb2e 190 for (k = kmin; k <= kmax; k++)
davide.aliprandi@st.com 2:9f389fd8fb2e 191 {
davide.aliprandi@st.com 2:9f389fd8fb2e 192 Result[n] += Signal[k] * Kernel[n - k];
davide.aliprandi@st.com 2:9f389fd8fb2e 193 }
davide.aliprandi@st.com 2:9f389fd8fb2e 194 }
davide.aliprandi@st.com 2:9f389fd8fb2e 195 }
davide.aliprandi@st.com 2:9f389fd8fb2e 196
davide.aliprandi@st.com 16:4ab2eac7be21 197 void Open_PDM_Filter(uint8_t* data, uint16_t* dataOut, uint16_t volume, TPDMFilter_InitStruct *Param)
davide.aliprandi@st.com 2:9f389fd8fb2e 198 {
davide.aliprandi@st.com 16:4ab2eac7be21 199 uint8_t i, data_out_index;
davide.aliprandi@st.com 16:4ab2eac7be21 200 uint8_t decimation = DECIMATION_MAX;
davide.aliprandi@st.com 16:4ab2eac7be21 201 uint8_t channels = Param->In_MicChannels;
davide.aliprandi@st.com 16:4ab2eac7be21 202 uint8_t data_inc = ((decimation >> 3) * channels);
davide.aliprandi@st.com 16:4ab2eac7be21 203 int64_t Z, Z0, Z1, Z2;
davide.aliprandi@st.com 2:9f389fd8fb2e 204 int64_t OldOut, OldIn, OldZ;
davide.aliprandi@st.com 16:4ab2eac7be21 205
davide.aliprandi@st.com 16:4ab2eac7be21 206 OldOut = Param->OldOut;
davide.aliprandi@st.com 16:4ab2eac7be21 207 OldIn = Param->OldIn;
davide.aliprandi@st.com 16:4ab2eac7be21 208 OldZ = Param->OldZ;
davide.aliprandi@st.com 16:4ab2eac7be21 209
davide.aliprandi@st.com 16:4ab2eac7be21 210 for (i = 0, data_out_index = 0; i < Param->Fs / 1000; i++, data_out_index += channels)
davide.aliprandi@st.com 2:9f389fd8fb2e 211 {
davide.aliprandi@st.com 16:4ab2eac7be21 212 #ifdef USE_LUT
davide.aliprandi@st.com 16:4ab2eac7be21 213 Z0 = filter_tables[channels - 1](data, 0);
davide.aliprandi@st.com 16:4ab2eac7be21 214 Z1 = filter_tables[channels - 1](data, 1);
davide.aliprandi@st.com 16:4ab2eac7be21 215 Z2 = filter_tables[channels - 1](data, 2);
davide.aliprandi@st.com 16:4ab2eac7be21 216 #else
davide.aliprandi@st.com 16:4ab2eac7be21 217 Z0 = filter_table(data, 0, channels);
davide.aliprandi@st.com 16:4ab2eac7be21 218 Z1 = filter_table(data, 1, channels);
davide.aliprandi@st.com 16:4ab2eac7be21 219 Z2 = filter_table(data, 2, channels);
davide.aliprandi@st.com 16:4ab2eac7be21 220 #endif
davide.aliprandi@st.com 16:4ab2eac7be21 221
davide.aliprandi@st.com 16:4ab2eac7be21 222 Z = Param->Coef[1] + Z2 - sub_const;
davide.aliprandi@st.com 16:4ab2eac7be21 223 Param->Coef[1] = Param->Coef[0] + Z1;
davide.aliprandi@st.com 16:4ab2eac7be21 224 Param->Coef[0] = Z0;
davide.aliprandi@st.com 16:4ab2eac7be21 225
davide.aliprandi@st.com 16:4ab2eac7be21 226 OldOut = (Param->HP_ALFA * (OldOut + Z - OldIn)) >> 8;
davide.aliprandi@st.com 16:4ab2eac7be21 227 OldIn = Z;
davide.aliprandi@st.com 16:4ab2eac7be21 228 Z = OldOut;
davide.aliprandi@st.com 16:4ab2eac7be21 229 OldZ = ((256 - Param->LP_ALFA) * OldZ + Param->LP_ALFA * Z) >> 8;
davide.aliprandi@st.com 16:4ab2eac7be21 230 Z = OldZ;
davide.aliprandi@st.com 16:4ab2eac7be21 231
davide.aliprandi@st.com 16:4ab2eac7be21 232 Z *= volume;
davide.aliprandi@st.com 16:4ab2eac7be21 233 Z = RoundDiv(Z, div_const);
davide.aliprandi@st.com 16:4ab2eac7be21 234 Z = SaturaLH(Z, -32700, 32700);
davide.aliprandi@st.com 16:4ab2eac7be21 235
davide.aliprandi@st.com 16:4ab2eac7be21 236 dataOut[data_out_index] = Z;
davide.aliprandi@st.com 16:4ab2eac7be21 237 data += data_inc;
davide.aliprandi@st.com 16:4ab2eac7be21 238 }
davide.aliprandi@st.com 16:4ab2eac7be21 239
davide.aliprandi@st.com 16:4ab2eac7be21 240 Param->OldOut = OldOut;
davide.aliprandi@st.com 16:4ab2eac7be21 241 Param->OldIn = OldIn;
davide.aliprandi@st.com 16:4ab2eac7be21 242 Param->OldZ = OldZ;
davide.aliprandi@st.com 2:9f389fd8fb2e 243 }