summaryrefslogtreecommitdiff
path: root/cmd_fir.c
diff options
context:
space:
mode:
Diffstat (limited to 'cmd_fir.c')
-rw-r--r--cmd_fir.c621
1 files changed, 621 insertions, 0 deletions
diff --git a/cmd_fir.c b/cmd_fir.c
new file mode 100644
index 0000000..95e0b53
--- /dev/null
+++ b/cmd_fir.c
@@ -0,0 +1,621 @@
+#include "cmd_fir.h"
+
+#define pi 3.14159265
+
+typedef struct filter_t
+{
+ double fs,fc1,fc2,beta;
+ int n,n1;
+ int wtype;
+ int type;
+ int nflag;
+} filter_t;
+
+#define FILT_LP 1
+#define FILT_HP 2
+#define FILT_BP 3
+#define FILT_BS 4
+#define FILT_CHECK(X) (((X)>=FILT_LP)&&((X)<=FILT_BS))
+
+#define FILT_WIN_RECT 1
+#define FILT_WIN_HAMM 2
+#define FILT_WIN_HANN 3
+#define FILT_WIN_BLCK 4
+#define FILT_WIN_KAIS 5
+#define FILT_WIN_CHECK(X) (((X)>=FILT_WIN_RECT)&&((X)<=FILT_WIN_KAIS))
+
+void sincx1_(double fc, double *sinc_h, int n1, int parity);
+int filter_spec_(filter_t *filt, int type, double fs, double fc1, double fc2, int wtype, double beta, int n);
+void lowpass_highpass_(filter_t *filt, double *sinc_h);
+void window_func_(filter_t *filt, double *win);
+void filter_output_(filter_t *filt, double *h, double *hb);
+double bessel(double x);
+
+void filter_calc(filter_t *filt, double *hc, int hc_sz);
+
+
+
+int filter_spec_(filter_t *filt, int type, double fs, double fc1, double fc2, int wtype, double beta, int n)
+{
+
+ /*Check input params*/
+ //check sample rate
+ if (fs < 0)
+ {
+ ERROR("ERROR fs\n");
+ return -1;
+ }
+ filt->fs = fs;
+
+ //check filter type
+ if (!FILT_CHECK(type))
+ {
+ ENL();
+ return -1;
+ }
+ filt->type = type;
+
+ if ((filt->type==FILT_LP)||(filt->type==FILT_HP))
+ {
+ //PRINT("%f %f\n",fc1, fs);
+ filt->fc1 = fc1/fs; //fc2 not used
+ } else if ((type==FILT_BS)||(type==FILT_BP))
+ {
+ filt->fc1 = fc1/fs;
+ filt->fc2 = fc2/fs;
+ //PRINT("%f %f %f\n", fc1, fc2, fs);
+ } else
+ {
+ ENL();
+ return -1;
+ }
+
+ //window type used
+ if (FILT_WIN_CHECK(wtype))
+ {
+ if (wtype == FILT_WIN_KAIS)
+ {
+ //beta for Kaiser window
+ filt->beta = beta;
+ }
+ }
+ filt->wtype = wtype;
+
+ /* request filter length and determine whether it is odd or even
+ * for N even only N/2 coefficients need be calculated, and for N odd
+ * only (N+1)/2 because of the symmetry in h(n).
+ */
+
+ filt->n = n;
+ filt->n1 = n/2;
+ filt->nflag = 1;
+ if ((filt->n - 2*filt->n1)==1)
+ {
+ filt->nflag = 0;
+ filt->n1 = filt->n1+1;
+ }
+ return 0;
+}
+
+//sinc_h size as filt->n1 size
+void lowpass_highpass_(filter_t *filt, double *h)
+{
+ int j;
+
+ sincx1_(filt->fc1, h, filt->n1, filt->nflag);
+ if (filt->type == FILT_HP)
+ {
+ h[0]=1-h[0];
+ for (j=0;j<filt->n1;++j)
+ {
+ h[j] = -h[j];
+ //printf("%f ",sinc_h[j]);
+ }
+ }
+
+}
+
+
+void bandpass_bandstop_(filter_t *filt, double *sinc_h, double *h_bp)
+{
+ int j;
+
+ double fc;
+
+ fc = filt->fc2;
+ sincx1_(fc, sinc_h, filt->n1, filt->nflag);
+ for (j=0; j<filt->n1;++j)
+ {
+ h_bp[j] = sinc_h[j];
+ }
+
+ fc = filt->fc1;
+ sincx1_(fc, sinc_h, filt->n1, filt->nflag);
+ for (j=0;j<filt->n1;++j)
+ {
+ sinc_h[j] = h_bp[j] - sinc_h[j];
+ }
+
+ if (filt->type == FILT_BS)
+ {
+ sinc_h[0] = 1 - sinc_h[0];
+ for (j=1; j<filt->n1; ++j)
+ {
+ sinc_h[j] = -sinc_h[j];
+ }
+ }
+}
+
+/*----------------------------------------------------------------------
+* function to calculate sincx: 2FC * sin(nwc)/nwc
+*/
+void sincx1_(double fc, double *h, int n1, int parity)
+{
+ int i,j;
+ double omega, p;
+
+ omega=2*pi*fc;
+ h[0]=2*fc;
+ i=1;
+ if(parity==1)
+ { /* check for N even */
+ i=0;
+ }
+
+ for(j=i; j<n1; j++)
+ {
+ p=parity*0.5+j;
+ h[j]=2*fc*sin(omega*p)/(omega*p);
+ }
+}
+
+
+void window_func_(filter_t *filt, double *win)
+{
+ int j;
+ double bd,p,rm,bn,x1;
+
+ switch (filt->wtype)
+ {
+ case FILT_WIN_RECT:
+ for (j=0; j<filt->n1; ++j)
+ {
+ win[j]=1.0;
+ }
+ break;
+ case FILT_WIN_HAMM:
+ for (j=0; j<filt->n1; ++j)
+ {
+ p=filt->nflag*0.5+j;
+ win[j] = 0.54+(1-0.54)*cos(2*pi*p/filt->n);
+ }
+ break;
+ case FILT_WIN_HANN:
+ for (j=0; j<filt->n1; ++j)
+ {
+ p = filt->nflag*0.5 + j;
+ win[j] = 0.5+(1-0.5)*cos(2*pi*p/filt->n);
+ }
+ break;
+ case FILT_WIN_BLCK:
+ for (j=0; j<filt->n1; ++j)
+ {
+ p = filt->nflag*0.5 + j;
+ win[j] = 0.42
+ + 0.5*cos(2*pi*p/(filt->n-1))
+ + 0.08*cos(4*pi*p/(filt->n-1));
+ }
+ break;
+ case FILT_WIN_KAIS:
+ x1 = filt->beta;
+ bd = bessel(x1);
+ for (j=0; j<filt->n1; ++j)
+ {
+ p = filt->nflag*0.5 + j;
+ rm = 2*p/(filt->n);
+ rm = rm*rm;
+ x1 = filt->beta * sqrt(1-rm);
+ bn = bessel(x1);
+ win[j] = bn/bd;
+ }
+ break;
+ default:
+ printf("Unknown window type %d\n", filt->wtype);
+ }
+}
+
+/*
+*
+*/
+double bessel(double x)
+{
+ double y,t,t1,e,de,sde;
+ int i;
+
+ y=x/2;
+ t=1.E-08; e=1; de=1;
+ for(i=1; i <25; ++i){
+ t1=(double) i;
+ de=de*y/t1;
+ sde=de*de;
+ e=e+sde;
+ if((e*t-sde) > 0.0)
+ return(e);
+ }
+ ENL();
+ return 0.0;
+}
+
+
+void filter_output_(filter_t *filt, double *h, double *hb)
+{
+ int j;
+
+ double fs,fc,fc1,fc2;
+
+ fs = filt->fs;
+ fc = filt->fc1 * filt->fs;
+ fc1 = filt->fc1 * filt->fs;
+ fc2 = filt->fc2 * filt->fs;
+
+ switch(filt->type)
+ {
+ case FILT_LP:
+ printf("lowpass filter \n");
+ printf("sampling frequency:\t%f\n", fs);
+ printf("cutoff frequency:\t%f\n", fc);
+ break;
+ case FILT_HP:
+ printf("highpass filter \n");
+ printf("sampling frequency:\t%f\n", fs);
+ printf("cutoff frequency:\t%f\n", fc);
+ break;
+ case FILT_BP:
+ printf("bandpass filter \n");
+ printf("sampling frequency:\t%f\n", fs);
+ printf("cutoff frequencies:\t%f\t%f\n", fc1, fc2);
+ break;
+ case FILT_BS:
+ printf("bandstop filter \n");
+ printf("sampling frequency:\t%f\n", fs);
+ printf("cutoff frequencies:\t%f\t%f\n", fc1, fc2);
+ break;
+ }
+ printf("\n");
+
+ switch(filt->wtype)
+ {
+ case 1:
+ printf("Rectangular window \n");
+ break;
+ case 2:
+ printf("Hamming window \n");
+ break;
+ case 3:
+ printf("Hanning window \n");
+ break;
+ case 4:
+ printf("Blackman window \n");
+ break;
+ case 5:
+ printf("Kaiser window \n");
+ break;
+ }
+ printf("\n");
+
+ printf("Impulse response coefficients\n");
+ printf("\n");
+ for (j=0;j<filt->n1;++j)
+ {
+ printf("h[%2d",j);
+ printf("] =\t%15.5e\t",h[filt->n1-j-1]);
+ printf(" = h[%2d",filt->n-j-1);
+ printf("]\n");
+ hb[j]=h[filt->n1-j-1];
+ hb[filt->n-j-1]=hb[j];
+ }
+}
+
+void filter_calc(filter_t *filt, double *hc, int hc_sz)
+{
+ int i;
+ double *h=NULL,*w=NULL,*hb=NULL;
+ filter_t loc_filt;
+
+ memset(&loc_filt, 0, sizeof(filter_t));
+
+ if (filt == NULL)
+ {
+ ENL();
+ return;
+ }
+
+ if (hc == NULL)
+ {
+ ENL();
+ return;
+ }
+
+ if (hc_sz<=0)
+ {
+ ENL();
+ return;
+ }
+
+ //set internal structure value, precalculate coefficients
+ filter_spec_(&loc_filt, filt->type, filt->fs, filt->fc1, filt->fc2, filt->wtype, filt->beta, filt->n);
+
+ h = malloc(sizeof(double) * loc_filt.n1);
+ if (h==NULL)
+ {
+ ENL();
+ }
+ memset(h, 0, sizeof(double) * loc_filt.n1);
+
+ hb = malloc(sizeof(double) * loc_filt.n);
+ if (hb==NULL)
+ {
+ ENL();
+ }
+ memset(hb, 0, sizeof(double) * loc_filt.n);
+
+
+
+ w = malloc(sizeof(double) * loc_filt.n1);
+ if (w==NULL)
+ {
+ ENL();
+ }
+ memset(w, 0, sizeof(double) * loc_filt.n1);
+
+ if (
+ (loc_filt.type == FILT_LP)
+ || (loc_filt.type == FILT_HP)
+ )
+ {
+ lowpass_highpass_(&loc_filt, h);
+ } else if (
+ (loc_filt.type == FILT_BP)
+ || (loc_filt.type == FILT_BS)
+ )
+ {
+ bandpass_bandstop_(&loc_filt, h, hb);
+ } else
+ {
+ ENL();
+ }
+
+ window_func_(&loc_filt, w); /* compute the window function */
+ for(i=0; i < loc_filt.n1; i++)
+ {
+ h[i]=h[i]*w[i];
+ }
+ //filter_output_(&loc_filt, h, hb); /* print the filter coefficients */
+
+ for (i=0; i<loc_filt.n1; i++)
+ {
+ if (i<hc_sz)
+ {
+ hc[i] = h[loc_filt.n1-i-1];
+
+ }
+ if ((loc_filt.n-i-1)<hc_sz)
+ {
+ hc[loc_filt.n-i-1] = h[loc_filt.n1-i-1];
+ }
+ }
+ free(h);
+ free(w);
+ free(hb);
+
+ return;
+}
+
+
+void *cmd_fir(void *data)
+{
+ char *ret = NULL;
+ int i=0,j=0;
+
+ int count;
+ sds params;
+ sds out_result;
+ sds *tokens;
+
+ double arg_fs;
+ int arg_ftype;
+ double arg_fc1;
+ double arg_fc2;
+ double arg_beta;
+ int arg_wtype;
+ int arg_n;
+ double *lp_coef;
+ filter_t filt;
+
+ printf("FIR\n");
+
+ if (data == NULL)
+ {
+ ret = alloc_new_str("FIR [FTYPE] [FS] [FC1] [FC2] [WTYPE] [BETA] [N]\n");
+ return ret;
+ }
+
+ //prepare arguments
+ params = sdsnew(data);
+ out_result = sdsempty();
+ tokens = sdssplitargs(params, &count);
+
+ if (count == 1)
+ {
+ if (strncmp(tokens[0],"HELP",4)==0)
+ {
+ ret = alloc_new_str("FIR [Ftype=LP|HP|BP|BS] [Fs=1...1000000] [Fc1=1...1000000] [Fc2=1...1000000] [Wtype=RECT|HAMM|HANN|BLCK|KAIS] [beta=0.0...1.0] [n=1...63]\n");
+ } else if (strncmp(tokens[0],"FIR",3)==0)
+ {
+ ret = alloc_new_str("Finite response filter\n");
+ } else if (strncmp(tokens[0],"Ftype",5)==0)
+ {
+ ret = alloc_new_str("Filter type\n");
+ } else if (strncmp(tokens[0],"Fs",2)==0)
+ {
+ ret = alloc_new_str("Sampling frequency\n");
+ } else if (strncmp(tokens[0],"Fc1",3)==0)
+ {
+ ret = alloc_new_str("First cuttoff frequency\n");
+ } else if (strncmp(tokens[0],"Fc2",3)==0)
+ {
+ ret = alloc_new_str("Second cuttoff frequency\n");
+ } else if (strncmp(tokens[0],"Wtype",5)==0)
+ {
+ ret = alloc_new_str("Window type used to calculare\n");
+ } else if (strncmp(tokens[0],"beta",4)==0)
+ {
+ ret = alloc_new_str("coeficient used with Kaiser window\n");
+ } else if (strncmp(tokens[0],"n",1)==0)
+ {
+ ret = alloc_new_str("n-order filter\n");
+ }
+ sdsfree(params);
+ sdsfree(out_result);
+ sdsfreesplitres(tokens, count);
+ return ret;
+ }
+
+ if (count != 7)
+ {
+ PRINT("count = %d\n", count);
+ ret = alloc_new_str("Need 7 arguments\nFIR [FTYPE] [FS] [FC1] [FC2] [WTYPE] [BETA] [N]\n");
+ sdsfree(params);
+ sdsfree(out_result);
+ sdsfreesplitres(tokens, count);
+ return ret;
+ }
+
+ PNL();
+ arg_fs = atof(tokens[1]);
+ if (arg_fs>1000000)
+ {
+ arg_fs = 1000000; //1Mhz should be enought
+ } else if (arg_fs<2)
+ {
+ arg_fs = 2;
+ }
+
+ PNL();
+ arg_fc1 = atof(tokens[2]);
+ if (arg_fc1>1000000.0)
+ {
+ arg_fc1 = 1000000.0;
+ } else if (arg_fc1<1.0)
+ {
+ arg_fc1 = 1.0;
+ }
+
+ PNL();
+ arg_fc1 = atof(tokens[2]);
+ if (arg_fc1>1000000.0)
+ {
+ arg_fc1 = 1000000.0;
+ } else if (arg_fs<1.0)
+ {
+ arg_fc1 = 1.0;
+ }
+
+ PNL();
+ arg_n = atoi(tokens[6]);
+ if (arg_n>63)
+ {
+ arg_n = 63;
+ } else if (arg_n<1)
+ {
+ arg_n = 1;
+ }
+
+ PNL();
+ arg_beta = atof(tokens[5]);
+ if (arg_beta > 1.0)
+ {
+ arg_beta = 1.0;
+ } else if (arg_beta < 0.0)
+ {
+ arg_beta = 0.0;
+ }
+
+ PNL();
+
+ if (strncmp(tokens[4],"RECT",4)==0)
+ {
+ arg_wtype = FILT_WIN_RECT;
+ } else if (strncmp(tokens[4],"HAMM",4)==0)
+ {
+ arg_wtype = FILT_WIN_HAMM;
+ } else if (strncmp(tokens[4],"HANN",4)==0)
+ {
+ arg_wtype = FILT_WIN_HANN;
+ } else if (strncmp(tokens[4],"BLCK",4)==0)
+ {
+ arg_wtype = FILT_WIN_BLCK;
+ } else if (strncmp(tokens[4],"KAIS",4)==0)
+ {
+ arg_wtype = FILT_WIN_BLCK;
+ } else
+ {
+ arg_wtype = FILT_WIN_RECT;
+ }
+
+ PNL();
+ if (strncmp(tokens[0],"LP",2)==0)
+ {
+ arg_ftype = FILT_LP;
+ } else if (strncmp(tokens[0],"HP",2)==0)
+ {
+ arg_ftype = FILT_HP;
+ } else if (strncmp(tokens[0],"BP",2)==0)
+ {
+ arg_ftype = FILT_BP;
+ } else if (strncmp(tokens[0],"BS",2)==0)
+ {
+ arg_ftype = FILT_BS;
+ } else
+ {
+ arg_ftype = FILT_LP;
+ }
+
+ PNL();
+ //calculate
+ lp_coef = malloc(sizeof(double)*arg_n);
+ memset(lp_coef, 0, sizeof(double)*arg_n);
+
+ PNL();
+ filt.type = arg_ftype;
+ filt.fs = arg_fs;
+ filt.fc1 = arg_fc1;
+ filt.fc2 = arg_fc2;
+ filt.wtype = arg_wtype;
+ filt.n = arg_n;
+ filt.beta = arg_beta;
+
+ PNL();
+ filter_calc(&filt, lp_coef, arg_n);
+
+
+ //prepare output
+ for (i=0;i<arg_n;i++)
+ {
+ char str_double[16];
+ snprintf(str_double, 16, "%.3f ", lp_coef[i]);
+ out_result = sdscat(out_result, str_double);
+ }
+ out_result = sdscat(out_result, "\n");
+
+ ret = alloc_new_str(out_result);
+
+ PNL();
+
+ free(lp_coef);
+ sdsfree(params);
+ sdsfree(out_result);
+ sdsfreesplitres(tokens, count);
+
+ return ret;
+} \ No newline at end of file
generated by cgit v1.2.3 (git 2.39.1) at 2024-11-01 04:28:41 +0000