Moved commands to seperate directory, moved external sources to seperate directory, updated Makefile with auto include and auto compile features

1 files changed, 621 insertions, 0 deletions

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