Il y avait des problèmes dans la libraire...
Dependencies: ST_FREQUENCY_DIVIDER ST_I2S USBDEVICE
Fork of X_NUCLEO_CCA02M1 by
Diff: Middlewares/OpenPDM2PCM/OpenPDMFilter.c
- Revision:
- 20:9952bef19da1
- Parent:
- 16:4ab2eac7be21
diff -r 1a061e306cc9 -r 9952bef19da1 Middlewares/OpenPDM2PCM/OpenPDMFilter.c --- a/Middlewares/OpenPDM2PCM/OpenPDMFilter.c Thu May 04 10:39:39 2017 +0000 +++ b/Middlewares/OpenPDM2PCM/OpenPDMFilter.c Fri May 05 11:34:10 2017 +0000 @@ -1,6 +1,6 @@ /** ****************************************************************************** -* @file OpenPDMFilter.h +* @file OpenPDMFilter.c * @author CL * @version V1.0.0 * @date 9-September-2015 @@ -33,41 +33,23 @@ #include "OpenPDMFilter.h" -/* Definitions ---------------------------------------------------------------*/ - -#define RoundDiv(a, b) (((a)>0)?(((a)+(b)/2)/(b)):(((a)-(b)/2)/(b))) -#define SaturaLH(N, L, H) (((N)<(L))?(L):(((N)>(H))?(H):(N))) - -#define VOLUME_MAX 64 -#define FILTER_GAIN 16 - -#define SINCN_MAX 3 -#define DECIMATION_MAX 64 - - /* Variables -----------------------------------------------------------------*/ uint32_t div_const = 0; int64_t sub_const = 0; -uint32_t sinc[DECIMATION_MAX * SINCN_MAX]; +uint32_t sinc[DECIMATION_MAX * SINCN]; uint32_t sinc1[DECIMATION_MAX]; uint32_t sinc2[DECIMATION_MAX * 2]; -uint32_t coef[SINCN_MAX][DECIMATION_MAX]; +uint32_t coef[SINCN][DECIMATION_MAX]; #ifdef USE_LUT -int32_t lut[256][DECIMATION_MAX / 8][SINCN_MAX]; +int32_t lut[256][DECIMATION_MAX / 8][SINCN]; #endif /* Functions -----------------------------------------------------------------*/ -int32_t filter_table(uint8_t *data, uint8_t sincn, uint8_t channels); - -void convolve(uint32_t Signal[/* SignalLen */], unsigned short SignalLen, - uint32_t Kernel[/* KernelLen */], unsigned short KernelLen, - uint32_t Result[/* SignalLen + KernelLen - 1 */]); - #ifdef USE_LUT -int32_t filter_table_mono(uint8_t *data, uint8_t sincn) +int32_t filter_table_mono_64(uint8_t *data, uint8_t sincn) { return (int32_t) lut[data[0]][0][sincn] + @@ -79,7 +61,7 @@ lut[data[6]][6][sincn] + lut[data[7]][7][sincn]; } -int32_t filter_table_stereo(uint8_t *data, uint8_t sincn) +int32_t filter_table_stereo_64(uint8_t *data, uint8_t sincn) { return (int32_t) lut[data[0]][0][sincn] + @@ -91,17 +73,59 @@ lut[data[12]][6][sincn] + lut[data[14]][7][sincn]; } -int32_t (* filter_tables[2]) (uint8_t *data, uint8_t sincn) = {filter_table_mono, filter_table_stereo}; +int32_t filter_table_mono_128(uint8_t *data, uint8_t sincn) +{ + return (int32_t) + lut[data[0]][0][sincn] + + lut[data[1]][1][sincn] + + lut[data[2]][2][sincn] + + lut[data[3]][3][sincn] + + lut[data[4]][4][sincn] + + lut[data[5]][5][sincn] + + lut[data[6]][6][sincn] + + lut[data[7]][7][sincn] + + lut[data[8]][8][sincn] + + lut[data[9]][9][sincn] + + lut[data[10]][10][sincn] + + lut[data[11]][11][sincn] + + lut[data[12]][12][sincn] + + lut[data[13]][13][sincn] + + lut[data[14]][14][sincn] + + lut[data[15]][15][sincn]; +} +int32_t filter_table_stereo_128(uint8_t *data, uint8_t sincn) +{ + return (int32_t) + lut[data[0]][0][sincn] + + lut[data[2]][1][sincn] + + lut[data[4]][2][sincn] + + lut[data[6]][3][sincn] + + lut[data[8]][4][sincn] + + lut[data[10]][5][sincn] + + lut[data[12]][6][sincn] + + lut[data[14]][7][sincn] + + lut[data[16]][8][sincn] + + lut[data[18]][9][sincn] + + lut[data[20]][10][sincn] + + lut[data[22]][11][sincn] + + lut[data[24]][12][sincn] + + lut[data[26]][13][sincn] + + lut[data[28]][14][sincn] + + lut[data[30]][15][sincn]; +} +int32_t (* filter_tables_64[2]) (uint8_t *data, uint8_t sincn) = {filter_table_mono_64, filter_table_stereo_64}; +int32_t (* filter_tables_128[2]) (uint8_t *data, uint8_t sincn) = {filter_table_mono_128, filter_table_stereo_128}; #else -int64_t filter_table(uint8_t *data, uint8_t sincn, uint8_t channels) +int32_t filter_table(uint8_t *data, uint8_t sincn, TPDMFilter_InitStruct *param) { uint8_t c, i; uint16_t data_index = 0; - uint32_t *coef_p = &coef[table][0]; - int64_t F = 0; + uint32_t *coef_p = &coef[sincn][0]; + int32_t F = 0; + uint8_t decimation = param->Decimation; + uint8_t channels = param->In_MicChannels; - for (i = 0; i < DECIMATION_MAX; i += 8) - { + for (i = 0; i < decimation; i += 8) { c = data[data_index]; F += ((c >> 7) ) * coef_p[i ] + ((c >> 6) & 0x01) * coef_p[i + 1] + @@ -117,50 +141,70 @@ } #endif +void convolve(uint32_t Signal[/* SignalLen */], unsigned short SignalLen, + uint32_t Kernel[/* KernelLen */], unsigned short KernelLen, + uint32_t Result[/* SignalLen + KernelLen - 1 */]) +{ + uint16_t n; + + for (n = 0; n < SignalLen + KernelLen - 1; n++) + { + unsigned short kmin, kmax, k; + + Result[n] = 0; + + kmin = (n >= KernelLen - 1) ? n - (KernelLen - 1) : 0; + kmax = (n < SignalLen - 1) ? n : SignalLen - 1; + + for (k = kmin; k <= kmax; k++) { + Result[n] += Signal[k] * Kernel[n - k]; + } + } +} + void Open_PDM_Filter_Init(TPDMFilter_InitStruct *Param) { uint16_t i, j; int64_t sum = 0; - for (i = 0; i < SINCN_MAX; i++) - { + + uint8_t decimation = Param->Decimation; + + for (i = 0; i < SINCN; i++) { Param->Coef[i] = 0; Param->bit[i] = 0; } - for (i = 0; i < DECIMATION_MAX; i++) - { + for (i = 0; i < decimation; i++) { sinc1[i] = 1; } Param->OldOut = Param->OldIn = Param->OldZ = 0; - Param->LP_ALFA = (Param->LP_HZ != 0 ? (uint16_t) (Param->LP_HZ * 256 / (Param->LP_HZ + Param->Fs / (2 * 3.14))) : 0); - Param->HP_ALFA = (Param->HP_HZ != 0 ? (uint16_t) (Param->Fs * 256 / (2 * 3.14 * Param->HP_HZ + Param->Fs)) : 0); + Param->LP_ALFA = (Param->LP_HZ != 0 ? (uint16_t) (Param->LP_HZ * 256 / (Param->LP_HZ + Param->Fs / (2 * 3.14159))) : 0); + Param->HP_ALFA = (Param->HP_HZ != 0 ? (uint16_t) (Param->Fs * 256 / (2 * 3.14159 * Param->HP_HZ + Param->Fs)) : 0); - Param->FilterLen = DECIMATION_MAX * 3; + Param->FilterLen = decimation * SINCN; sinc[0] = 0; - sinc[DECIMATION_MAX * 3 - 1] = 0; - convolve(sinc1, DECIMATION_MAX, sinc1, DECIMATION_MAX, sinc2); - convolve(sinc2, DECIMATION_MAX * 2 - 1, sinc1, DECIMATION_MAX, &sinc[1]); - for(j = 0; j < 3; j++) - { - for (i = 0; i < DECIMATION_MAX ;i++) - { - coef[j][i] = sinc[j * DECIMATION_MAX + i]; - sum += sinc[j * DECIMATION_MAX + i]; + sinc[decimation * SINCN - 1] = 0; + convolve(sinc1, decimation, sinc1, decimation, sinc2); + convolve(sinc2, decimation * 2 - 1, sinc1, decimation, &sinc[1]); + for(j = 0; j < SINCN; j++) { + for (i = 0; i < decimation; i++) { + coef[j][i] = sinc[j * decimation + i]; + sum += sinc[j * decimation + i]; } } sub_const = sum >> 1; - div_const = sub_const * VOLUME_MAX / 32768 / FILTER_GAIN; + div_const = sub_const * Param->MaxVolume / 32768 / FILTER_GAIN; div_const = (div_const == 0 ? 1 : div_const); #ifdef USE_LUT /* Look-Up Table. */ uint16_t c, d, s; - for (s = 0; s < SINCN_MAX; s++) + for (s = 0; s < SINCN; s++) { uint32_t *coef_p = &coef[s][0]; for (c = 0; c < 256; c++) - for (d = 0; d < DECIMATION_MAX / 8; d++) + for (d = 0; d < decimation / 8; d++) lut[c][d][s] = ((c >> 7) ) * coef_p[d * 8 ] + ((c >> 6) & 0x01) * coef_p[d * 8 + 1] + ((c >> 5) & 0x01) * coef_p[d * 8 + 2] + @@ -173,50 +217,31 @@ #endif } -void convolve(uint32_t Signal[/* SignalLen */], unsigned short SignalLen, - uint32_t Kernel[/* KernelLen */], unsigned short KernelLen, - uint32_t Result[/* SignalLen + KernelLen - 1 */]) -{ - uint16_t n; - for (n = 0; n < SignalLen + KernelLen - 1; n++) - { - unsigned short kmin, kmax, k; - - Result[n] = 0; - - kmin = (n >= KernelLen - 1) ? n - (KernelLen - 1) : 0; - kmax = (n < SignalLen - 1) ? n : SignalLen - 1; - - for (k = kmin; k <= kmax; k++) - { - Result[n] += Signal[k] * Kernel[n - k]; - } - } -} - -void Open_PDM_Filter(uint8_t* data, uint16_t* dataOut, uint16_t volume, TPDMFilter_InitStruct *Param) +void Open_PDM_Filter_64(uint8_t* data, uint16_t* dataOut, uint16_t volume, TPDMFilter_InitStruct *Param) { uint8_t i, data_out_index; - uint8_t decimation = DECIMATION_MAX; uint8_t channels = Param->In_MicChannels; - uint8_t data_inc = ((decimation >> 3) * channels); + uint8_t data_inc = ((DECIMATION_MAX >> 4) * channels); int64_t Z, Z0, Z1, Z2; - int64_t OldOut, OldIn, OldZ; + int64_t OldOut, OldIn, OldZ; OldOut = Param->OldOut; OldIn = Param->OldIn; OldZ = Param->OldZ; - for (i = 0, data_out_index = 0; i < Param->Fs / 1000; i++, data_out_index += channels) - { +#ifdef USE_LUT + uint8_t j = channels - 1; +#endif + + for (i = 0, data_out_index = 0; i < Param->Fs / 1000; i++, data_out_index += channels) { #ifdef USE_LUT - Z0 = filter_tables[channels - 1](data, 0); - Z1 = filter_tables[channels - 1](data, 1); - Z2 = filter_tables[channels - 1](data, 2); + Z0 = filter_tables_64[j](data, 0); + Z1 = filter_tables_64[j](data, 1); + Z2 = filter_tables_64[j](data, 2); #else - Z0 = filter_table(data, 0, channels); - Z1 = filter_table(data, 1, channels); - Z2 = filter_table(data, 2, channels); + Z0 = filter_table(data, 0, Param); + Z1 = filter_table(data, 1, Param); + Z2 = filter_table(data, 2, Param); #endif Z = Param->Coef[1] + Z2 - sub_const; @@ -225,11 +250,9 @@ OldOut = (Param->HP_ALFA * (OldOut + Z - OldIn)) >> 8; OldIn = Z; - Z = OldOut; - OldZ = ((256 - Param->LP_ALFA) * OldZ + Param->LP_ALFA * Z) >> 8; - Z = OldZ; + OldZ = ((256 - Param->LP_ALFA) * OldZ + Param->LP_ALFA * OldOut) >> 8; - Z *= volume; + Z = OldZ * volume; Z = RoundDiv(Z, div_const); Z = SaturaLH(Z, -32700, 32700); @@ -241,3 +264,51 @@ Param->OldIn = OldIn; Param->OldZ = OldZ; } + +void Open_PDM_Filter_128(uint8_t* data, uint16_t* dataOut, uint16_t volume, TPDMFilter_InitStruct *Param) +{ + uint8_t i, data_out_index; + uint8_t channels = Param->In_MicChannels; + uint8_t data_inc = ((DECIMATION_MAX >> 3) * channels); + int64_t Z, Z0, Z1, Z2; + int64_t OldOut, OldIn, OldZ; + + OldOut = Param->OldOut; + OldIn = Param->OldIn; + OldZ = Param->OldZ; + +#ifdef USE_LUT + uint8_t j = channels - 1; +#endif + + for (i = 0, data_out_index = 0; i < Param->Fs / 1000; i++, data_out_index += channels) { +#ifdef USE_LUT + Z0 = filter_tables_128[j](data, 0); + Z1 = filter_tables_128[j](data, 1); + Z2 = filter_tables_128[j](data, 2); +#else + Z0 = filter_table(data, 0, Param); + Z1 = filter_table(data, 1, Param); + Z2 = filter_table(data, 2, Param); +#endif + + Z = Param->Coef[1] + Z2 - sub_const; + Param->Coef[1] = Param->Coef[0] + Z1; + Param->Coef[0] = Z0; + + OldOut = (Param->HP_ALFA * (OldOut + Z - OldIn)) >> 8; + OldIn = Z; + OldZ = ((256 - Param->LP_ALFA) * OldZ + Param->LP_ALFA * OldOut) >> 8; + + Z = OldZ * volume; + Z = RoundDiv(Z, div_const); + Z = SaturaLH(Z, -32700, 32700); + + dataOut[data_out_index] = Z; + data += data_inc; + } + + Param->OldOut = OldOut; + Param->OldIn = OldIn; + Param->OldZ = OldZ; +}