#include "IIR.h"
double IIR_NUM[Nmbr_of_S][3]={{1,2,1},{1,2,1},{1,2,1},{1,1,0}};//Nmbr_of_S=4
double IIR_DEM[Nmbr_of_S][3]=
{
{1, -1.314474906723, 0.7447131399677},
{1, -1.068673995798, 0.4184596094403},
{1, -0.946227509319, 0.2559354008692},
{1, -0.454527052386, 0}
};
double IIR_G[4]={ 0.1075595583111,0.0874464034107,0.07742697288753,0.272736473807};
int IIR(int x_ad)
{
int i,y_da;
double x[Nmbr_of_S+1];
double w2[Nmbr_of_S],w1[Nmbr_of_S],w0[Nmbr_of_S],m2[Nmbr_of_S],m1[Nmbr_of_S],m0[Nmbr_of_S];
x[0] = x_ad/32768.0;
for(i=0;i<Nmbr_of_S;i++)
{
m0[i]=x[i];
m1[i]=m0[i];
m2[i]=m1[i];
w2[i]=IIR_NUM[i][2]*m0[i]+IIR_NUM[i][1]*m1[i]+IIR_NUM[i][0]*m2[i]-IIR_DEM[i][1]*w1[i]-IIR_DEM[i][2]*w0[i];
if(w2[0]>32767) w2[i]=32767;
if(w2[0]<-32767) w2[i]=-32767;
w0[i]=w1[i];
w1[i]=w2[i];
x[i+1]=w2[i]*IIR_G[i];
}
y_da=(int)(x[Nmbr_of_S]*32768.0);
return y_da;
}
//===========================================================================
// No more.
//===========================================================================
|