path: root/cmd/cmd_fir.c

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