Initial import

git-svn-id: http://moon:8086/svn/software/trunk/libsrc/iir@1 b431acfa-c32f-4a4a-93f1-934dc6c82436
2014-07-19 07:44:42 +00:00
commit dd0f54d3bc
21 changed files with 1912 additions and 0 deletions
@@ -0,0 +1,225 @@
+/*************************************************************************/
+/* iir.c
+/*************************************************************************/
+#include "stdio.h"
+#include "math.h"
+#include "iir.h"
+
+/*************************************************************************/
+/* Global Variables
+/*************************************************************************/
+const char *filterTypeString[] =
+{
+	"Unknown filter type",
+	"Butterworth-Lowpass",
+	"Butterworth-Highpass",
+    "Butterworth-Bandpass",
+	"Butterworth-Bandstop",
+	"Peaking-EQ",
+	"Low Shelving-EQ",
+	"High Shelving-EQ"
+};
+
+/******************************************************************************/
+void IIRCalcFilterCoeff(struct _sIIRCoeff *pCoeff, double fa, double fg, double q, unsigned order, unsigned filterType)
+{
+	unsigned p;
+	double qp;
+	
+	IIRInit(pCoeff, order);
+
+	for(p=0; p < order/2;p++)
+	{
+		qp = q * IIRCalcQp(p+1, order);
+		IIRCalcPartFilterCoeff2(&pCoeff[p], 1.0, fa, fg, qp, filterType);
+	}
+}
+
+int IIRCalcPartFilterCoeff1(struct _sIIRCoeff *pCoeff, double fa, double fg, double Qi, unsigned filterType)
+{
+	double K, a0;
+	double alpha, omega, ks, kc;
+	unsigned error;
+
+	omega = 2*pi*fg/fa;
+	ks    = sin(omega);
+	kc    = cos(omega);
+	alpha = 0.5*ks /Qi;
+
+	K  = IIRBilTrans(fg, fa);
+	a0 = K/Qi + 1;
+
+	switch(filterType)
+	{
+	case IIR_FILTERTYPE_LOWPASS:
+	
+		pCoeff->ak0 = 1.0;
+		pCoeff->ak1 = (1 - K/Qi)/a0;
+		pCoeff->ak2 = 0.0;
+
+		pCoeff->bk0 = 1.0/a0;
+		pCoeff->bk1 = 1.0/a0;
+		pCoeff->bk2 = 0.0;
+
+		break;
+	
+	case IIR_FILTERTYPE_HIGHPASS:
+	
+		pCoeff->ak0 = 1.0;
+		pCoeff->ak1 = (1 - K/Qi) /a0;
+		pCoeff->ak2 = 0.0;
+
+		pCoeff->bk0 =  1.0*K /a0;
+		pCoeff->bk1 = -1.0*K /a0;
+		pCoeff->bk2 =  0.0;
+		
+		break;
+
+	default:
+		error = -1;
+		break;
+	}
+	return error;
+}
+
+int IIRCalcPartFilterCoeff2(struct _sIIRCoeff *pCoeff, double A, double fa, double fg, double qp, unsigned filterType)
+{
+	double a0;
+	double alpha, omega, ks, kc;
+	unsigned error;
+
+	omega = 2*pi*fg/fa;
+	ks    = sin(omega);
+	kc    = cos(omega);
+	alpha = 0.5*ks /qp;
+
+	error = 0;
+	switch(filterType)
+	{
+	case IIR_FILTERTYPE_LOWPASS:
+
+		a0 = 1 + alpha;
+		pCoeff->ak0 =  1.0;
+		pCoeff->ak1 = -2.0*kc /a0;
+		pCoeff->ak2 = (1 - alpha) /a0;
+
+		pCoeff->bk0 = 0.5*(1 - kc) /a0;
+		pCoeff->bk1 = (1 - kc) /a0;
+		pCoeff->bk2 = 0.5*(1 - kc) /a0;
+
+		break;
+	
+	case IIR_FILTERTYPE_HIGHPASS:
+
+		a0 = 1 + alpha;
+		pCoeff->ak0 =  1.0;
+		pCoeff->ak1 = -2.0*kc /a0;
+		pCoeff->ak2 = (1 - alpha) /a0;
+
+		pCoeff->bk0 = 0.5*(1 + kc) /a0;
+		pCoeff->bk1 = -(1 + kc) /a0;
+		pCoeff->bk2 = 0.5*(1 + kc) /a0;
+
+		break;
+
+	case IIR_FILTERTYPE_BANDPASS:
+
+		a0 = 1 + alpha;
+		pCoeff->ak0 =  1.0;
+		pCoeff->ak1 = -2.0*kc /a0;
+		pCoeff->ak2 = (1 - alpha) /a0;
+
+		pCoeff->bk0 = alpha /a0;
+		pCoeff->bk1 = 0;
+		pCoeff->bk2 = -alpha /a0;
+
+		break;
+
+	case IIR_FILTERTYPE_BANDSTOP:
+
+		a0 = 1 + alpha;
+		pCoeff->ak0 =  1.0;
+		pCoeff->ak1 = -2.0*kc /a0;
+		pCoeff->ak2 = (1 - alpha) /a0;
+
+		pCoeff->bk0 = 1.0 /a0;
+		pCoeff->bk1 = -2.0*kc /a0;
+		pCoeff->bk2 = 1.0 /a0;
+
+		break;
+
+	case IIR_FILTERTYPE_PEAKING:
+
+		a0 = 1 + (alpha/A);
+		pCoeff->ak0 =  1.0;
+		pCoeff->ak1 = -2.0*kc /a0;
+		pCoeff->ak2 = (1 - (alpha/A)) /a0;
+
+		pCoeff->bk0 =  (1 + (alpha*A)) /a0;
+		pCoeff->bk1 = -2.0*kc /a0;
+		pCoeff->bk2 =  (1 - (alpha*A)) /a0;
+
+		break;
+
+	default:
+		error = -1;
+		break;
+	}
+	return error;
+}
+
+void IIR(struct _sIIRCoeff *pCoeff, double *xn, double *yn, unsigned order, unsigned numPoints)
+{
+	double xp, yp;
+	unsigned i, p;
+	unsigned numSec = order/2;
+
+	for (i=0; i<numPoints; i++)
+	{
+		xp = xn[i];
+		for (p=0; p < numSec; p++)
+		{
+			yp = pCoeff[p].bk0*xp 
+			   + pCoeff[p].bk1*pCoeff[p].xn1
+			   + pCoeff[p].bk2*pCoeff[p].xn2
+			   - pCoeff[p].ak1*pCoeff[p].yn1
+			   - pCoeff[p].ak2*pCoeff[p].yn2;
+			
+			pCoeff[p].yn2 = pCoeff[p].yn1;
+			pCoeff[p].yn1 = yp;
+			pCoeff[p].xn2 = pCoeff[p].xn1;
+			pCoeff[p].xn1 = xp;
+			xp	  = yp;
+		}
+		yn[i] = yp;	
+	}
+}
+
+void IIRInit(struct _sIIRCoeff *pCoeff, unsigned order)
+{
+	unsigned n;
+	for(n=0; n < order/2; n++)
+	{
+		pCoeff[n].ak0 = 0;
+		pCoeff[n].ak1 = 0;
+		pCoeff[n].ak2 = 0;
+		pCoeff[n].bk0 = 0;
+		pCoeff[n].bk1 = 0;
+		pCoeff[n].bk2 = 0;
+		pCoeff[n].xn1 = 0;
+		pCoeff[n].xn2 = 0;
+		pCoeff[n].yn1 = 0;
+		pCoeff[n].yn2 = 0;
+	}
+}
+
+double IIRBilTrans(double fg, double fa)
+{
+	return 1.0/(tan(pi*fg/fa));
+}
+
+double IIRCalcQp(unsigned p, unsigned N)
+{
+	return 1.0/(2*sin(pi*(2*p-1)/(2*N)));
+}
+