Diff: Middlewares/OpenPDM2PCM/OpenPDMFilter.c

Revision:

20:9952bef19da1

Parent:

16:4ab2eac7be21

--- 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;
+}