From 126fa84b8591c3b285b7f598089451aa22447f10 Mon Sep 17 00:00:00 2001 From: FreeArtMan Date: Sat, 20 May 2017 13:43:49 +0100 Subject: Moved commands to seperate directory, moved external sources to seperate directory, updated Makefile with auto include and auto compile features --- cmd/cmd_fir.c | 621 ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 621 insertions(+) create mode 100644 cmd/cmd_fir.c (limited to 'cmd/cmd_fir.c') diff --git a/cmd/cmd_fir.c b/cmd/cmd_fir.c new file mode 100644 index 0000000..95e0b53 --- /dev/null +++ b/cmd/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;jn1;++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; jn1;++j) + { + h_bp[j] = sinc_h[j]; + } + + fc = filt->fc1; + sincx1_(fc, sinc_h, filt->n1, filt->nflag); + for (j=0;jn1;++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; jn1; ++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; jwtype) + { + case FILT_WIN_RECT: + for (j=0; jn1; ++j) + { + win[j]=1.0; + } + break; + case FILT_WIN_HAMM: + for (j=0; jn1; ++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; jn1; ++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; jn1; ++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; jn1; ++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;jn1;++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; i1000000) + { + 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