#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