Added fir filter code and functions for buffer conversion

1 files changed, 247 insertions, 0 deletions

diff --git a/FIR/firmath.c b/FIR/firmath.c
new file mode 100644
index 0000000..3fdf20b
--- /dev/null
+++ b/FIR/firmath.c
@@ -0,0 +1,247 @@
+//
+//  firmath.c
+//  FIR_rect_win
+//
+//  Created by Jacky Jack on 04/07/2021.
+//
+
+#include "firmath.h"
+
+/*
+void window_func_rect(double *w, int N1) {
+    int j;
+    
+    for (j=0;j<N1;j++) {
+        w[j] = 1.0;
+    }
+}
+*/
+
+void fir_init(fir_t *fir, uint32_t taps) {
+    int i;
+    for (i=0;i<taps;i++) {
+        fir->fir_coef[i] = (double)0.0;
+    }
+    fir->num_taps = taps;
+}
+
+
+void firw_init(firw_t *fir_win, uint32_t taps) {
+    int i;
+    for (i=0;i<taps;i++) {
+        fir_win->win_coef[i] = (double)0.0;
+    }
+    fir_win->num_taps = taps;
+}
+
+
+double math_sinx(double x) {
+    if (x>-1.0E-5 && x < 1.0E-5) return (1.0);
+    return sin(x)/x;
+}
+
+void fir_sinc_lp(fir_t *fir, double omega_c) {
+    int i;
+    double arg=0.0;
+    //should set 2fc?!
+    printf("omega_c=%f\n",omega_c);
+    for (i=0;i<fir->num_taps;i++) {
+        arg = (double)i-(double)(fir->num_taps-1)/2.0;
+        fir->fir_coef[i] = omega_c * math_sinx(omega_c*arg*M_PI);
+    }
+}
+
+void fir_sinc_hp(fir_t *fir, double omega_c) {
+    int i;
+    double arg=0.0;
+    int even = fir->num_taps%2 ? 0: 1;
+    
+    //should set 2fc?!
+    printf("omega_c=%f\n",omega_c);
+    if (even) {
+        for (i=0;i<fir->num_taps;i++) {
+            arg = (double)i-(double)(fir->num_taps-1)/2.0;
+            if (arg == 0.0) {
+                fir->fir_coef[i] = 0.0;
+            } else {
+                //from IOWA
+                fir->fir_coef[i] = cos(omega_c*arg*M_PI)/M_PI/arg + cos(arg*M_PI);
+            }
+        }
+    } else {
+        for (i=0;i<fir->num_taps;i++) {
+            arg = (double)i-(double)(fir->num_taps-1)/2.0;
+            fir->fir_coef[i] = math_sinx(arg*M_PI)-omega_c * math_sinx(omega_c*arg*M_PI);
+        }
+    }
+}
+
+void fir_sinc_bp(fir_t *fir, double omega_c1, double omega_c2) {
+    int i;
+    double arg=0.0;
+    //should set 2fc?!
+    printf("omega_c1=%f\n",omega_c1);
+    printf("omega_c2=%f\n",omega_c2);
+    for (i=0;i<fir->num_taps;i++) {
+        arg = (double)i-(double)(fir->num_taps-1)/2.0;
+        if (arg==0.0) {
+            fir->fir_coef[i] = 0.0;
+        } else {
+            //fir->fir_coef[i] = math_sinx(arg*M_PI) - omega_c2 * math_sinx(omega_c2*arg*M_PI) - omega_c1 * math_sinx(omega_c1*arg*M_PI);
+            fir->fir_coef[i] = (cos(omega_c1*arg*M_PI) - cos(omega_c2*arg*M_PI))/M_PI/arg;
+        }
+    }
+}
+
+void fir_sinc_bs(fir_t *fir, double omega_c1, double omega_c2) {
+    int i;
+    double arg=0.0;
+    //should set 2fc?!
+    printf("omega_c1=%f\n",omega_c1);
+    printf("omega_c2=%f\n",omega_c2);
+    for (i=0;i<fir->num_taps;i++) {
+        arg = (double)i-(double)(fir->num_taps-1)/2.0;
+        if (arg==0.0) {
+            fir->fir_coef[i] = 0.0;
+        } else {
+            //fir->fir_coef[i] = math_sinx(arg*M_PI) - omega_c2 * math_sinx(omega_c2*arg*M_PI) - omega_c1 * math_sinx(omega_c1*arg*M_PI);
+            fir->fir_coef[i] = math_sinx(arg*M_PI)  - omega_c2*math_sinx(omega_c2*arg*M_PI) - omega_c1*math_sinx(omega_c1*arg*M_PI);
+        }
+    }
+}
+
+void fir_convolute(fir_t *fir,firw_t *firw) {
+    int i=0;
+    for (i=0;i<fir->num_taps;i++) {
+        fir->fir_coef[i] = fir->fir_coef[i]*firw->win_coef[i];
+    }
+}
+
+
+void firw_rect(firw_t *fir_win) {
+    int i;
+    int n1 = fir_win->num_taps/2;
+    int even = fir_win->num_taps%2 ? 0: 1;
+    
+    if (even) n1+=1;
+    
+    for (i=0;i<n1;i++) {
+        fir_win->win_coef[i] = 1.0;
+    }
+    
+    for (i=0;i<n1;i++) {
+        fir_win->win_coef[fir_win->num_taps-1-i] =  fir_win->win_coef[i];
+    }
+    
+}
+
+
+void firw_hamming(firw_t *fir_win) {
+    int i;
+    double dN=0;
+    double arg=0;
+    //double p=0;
+    //double arg=0;
+    
+    int n1 = fir_win->num_taps/2;
+    int even = fir_win->num_taps%2 ? 0: 1;
+    if (!even) n1+=1;
+    dN = fir_win->num_taps +1;
+    
+    for (i=0;i<n1;i++) {
+        //arg = (double)i-(double)(fir_win->num_taps-1)/2.0;
+        //fir_win->win_coef[i] =  0.54+(1-0.54)*cos(2*M_PI*arg/n1);
+        
+        //fir_win->win_coef[i] =  0.54+(1-0.54)*cos(2*M_PI*i/n1);
+        arg = (double)i-(double)(fir_win->num_taps-1)/2.0;
+        fir_win->win_coef[i] = 0.54+0.46*cos(2*M_PI*arg/fir_win->num_taps);
+        
+    }
+    //fold
+    for (i=0;i<n1;i++) {
+        //arg = (double)i-(double)(fir_win->num_taps-1)/2.0;
+        //fir_win->win_coef[i] =  0.54+(1-0.54)*cos(2*M_PI*arg/n1);
+        
+        fir_win->win_coef[fir_win->num_taps-1-i] =  fir_win->win_coef[i];
+    }
+    
+}
+
+
+void firw_hanning(firw_t *fir_win) {
+    int i;
+    double dN=0;
+    double arg=0;
+    
+    int n1 = fir_win->num_taps/2;
+    int even = fir_win->num_taps%2 ? 0: 1;
+    if (!even) n1+=1;
+    dN = fir_win->num_taps +1;
+    
+    for (i=0;i<n1;i++) {
+        arg = (double)i-(double)(fir_win->num_taps-1)/2.0;
+        fir_win->win_coef[i] = 0.5+(1-0.5)*cos(2*M_PI*arg/fir_win->num_taps);
+        
+    }
+    //fold
+    for (i=0;i<n1;i++) {
+        fir_win->win_coef[fir_win->num_taps-1-i] =  fir_win->win_coef[i];
+    }
+}
+
+void firw_blackman(firw_t *firw) {
+    int i;
+    double dN=0;
+    double arg=0;
+    
+    int n1 = firw->num_taps/2;
+    int even = firw->num_taps%2 ? 0: 1;
+    if (!even) n1+=1;
+    dN = firw->num_taps +1;
+    
+    for (i=0;i<n1;i++) {
+        arg = (double)i-(double)(firw->num_taps-1)/2.0;
+        /*
+        firw->win_coef[i] = 0.42
+        +0.50*cos(2*M_PI*arg/(firw->num_taps))
+        +0.08*cos(4*M_PI*arg/(firw->num_taps));
+        */
+        firw->win_coef[i] = 0.42
+        +0.50*cos(2*M_PI*arg/(firw->num_taps-1))
+        +0.08*cos(4*M_PI*arg/(firw->num_taps-1));
+        
+    }
+    //fold
+    for (i=0;i<n1;i++) {
+        firw->win_coef[firw->num_taps-1-i] =  firw->win_coef[i];
+    }
+}
+
+void fir_print_matlab(fir_t *fir) {
+    int i;
+    printf("f=[\n");
+    for (i=0;i<fir->num_taps;i++) {
+        printf("%f\n",fir->fir_coef[i]);
+    }
+    printf("];\n");
+}
+
+void firw_print_matlab(firw_t *firw) {
+    int i;
+    printf("w=[\n");
+    for (i=0;i<firw->num_taps;i++) {
+        printf("%f\n",firw->win_coef[i]);
+    }
+    printf("];\n");
+}
+
+
+void fir_print_c(fir_t *fir) {
+    int i;
+    printf("double f=[\n");
+    for (i=0;i<fir->num_taps;i++) {
+        printf("%f,\n",fir->fir_coef[i]);
+    }
+    printf("];\n");
+}
+