jens
dd0f54d3bc
git-svn-id: http://moon:8086/svn/software/trunk/libsrc/iir@1 b431acfa-c32f-4a4a-93f1-934dc6c82436
330 lines
7.3 KiB
C++
Executable File
330 lines
7.3 KiB
C++
Executable File
/******************************************************************************/
|
|
#include <math.h>
|
|
#include <float.h>
|
|
#include <stdio.h>
|
|
#include <stdlib.h>
|
|
#include <string.h>
|
|
#include "iir.h"
|
|
|
|
/******************************************************************************/
|
|
const char *filterTypeString[] =
|
|
{
|
|
"Unknown filter type",
|
|
"Butterworth-Lowpass",
|
|
"Butterworth-Highpass",
|
|
"Butterworth-Bandpass",
|
|
"Butterworth-Bandstop"
|
|
};
|
|
|
|
/******************************************************************************/
|
|
int IIRCalcPartFilterCoeff1(CIIRCoeff *pCoeff, double fg, double fa, double Qi, unsigned filterType)
|
|
{
|
|
double K, A0;
|
|
unsigned error;
|
|
|
|
error = 0;
|
|
switch(filterType)
|
|
{
|
|
case IIR_FILTERTYPE_LOWPASS:
|
|
|
|
K = IIRBilTrans(fg, fa);
|
|
A0 = 1.0 /(K/Qi + 1);
|
|
|
|
pCoeff->m_pak[0] = 1.0;
|
|
pCoeff->m_pak[1] = (1 - K/Qi) * A0;
|
|
|
|
pCoeff->m_pbk[0] = 1.0;// * A0;
|
|
pCoeff->m_pbk[1] = 1.0;// * A0;
|
|
pCoeff->m_aScale = A0;
|
|
break;
|
|
|
|
case IIR_FILTERTYPE_HIGHPASS:
|
|
|
|
K = IIRBilTrans(fg, fa);
|
|
A0 = 1.0 /(K/Qi + 1);
|
|
|
|
pCoeff->m_pak[0] = 1.0;
|
|
pCoeff->m_pak[1] = (1 - K/Qi) * A0;
|
|
|
|
pCoeff->m_pbk[0] = 1.0*K;// * A0;
|
|
pCoeff->m_pbk[1] = -1.0*K;// * A0;
|
|
pCoeff->m_aScale = A0;
|
|
break;
|
|
|
|
default:
|
|
error = -1;
|
|
break;
|
|
}
|
|
return error;
|
|
}
|
|
|
|
int IIRCalcPartFilterCoeff2(CIIRCoeff *pCoeff, double fg, double fa, double Qi, unsigned filterType)
|
|
{
|
|
double K, KK, A0,B0;
|
|
double alpha, omega, sn, cs;
|
|
unsigned error;
|
|
|
|
K = IIRBilTrans(fg, fa);
|
|
KK = K*K;
|
|
A0 = 1.0 /(1 + KK + K/Qi);
|
|
|
|
omega = 2*pi*fg/fa;
|
|
sn = sin(omega);
|
|
cs = cos(omega);
|
|
alpha = 0.5*sn /Qi;
|
|
|
|
error = 0;
|
|
switch(filterType)
|
|
{
|
|
case IIR_FILTERTYPE_LOWPASS:
|
|
|
|
pCoeff->m_pak[0] = 1.0 ;
|
|
pCoeff->m_pak[1] = 2 *(1 - KK) * A0;
|
|
pCoeff->m_pak[2] = (1 + KK - K/Qi) * A0;
|
|
|
|
pCoeff->m_pbk[0] = 1.0*A0;
|
|
pCoeff->m_pbk[1] = 2.0*A0;
|
|
pCoeff->m_pbk[2] = 1.0*A0;
|
|
|
|
pCoeff->m_aScale = A0;
|
|
|
|
break;
|
|
|
|
case IIR_FILTERTYPE_HIGHPASS:
|
|
|
|
pCoeff->m_pak[0] = 1.0;
|
|
pCoeff->m_pak[1] = 2 *(1 - KK) * A0;
|
|
pCoeff->m_pak[2] = (KK - K/Qi + 1) * A0;
|
|
|
|
pCoeff->m_pbk[0] = 1.0*KK * A0;
|
|
pCoeff->m_pbk[1] = -2.0*KK * A0;
|
|
pCoeff->m_pbk[2] = 1.0*KK * A0;
|
|
pCoeff->m_aScale = A0;
|
|
break;
|
|
|
|
default:
|
|
error = -1;
|
|
break;
|
|
}
|
|
return error;
|
|
}
|
|
|
|
double IIRBilTrans(double fg, double fa)
|
|
{
|
|
return 1.0/(tan(pi*fg/fa));
|
|
}
|
|
|
|
void IIR(double *xn, double *yn, CIIRCoeff *pCoeff, unsigned numPoints)
|
|
{
|
|
unsigned n, k;
|
|
double y1, y2;
|
|
|
|
for (n=0; n < numPoints; n++)
|
|
{
|
|
y1 = 0;
|
|
y2 = 0;
|
|
for (k=0; k <= pCoeff->m_Nb; k++)
|
|
{
|
|
y1 = y1 + (pCoeff->m_pbk[k] * xn[pCoeff->m_Nb-k+n]);
|
|
if (!_finite(y1))
|
|
printf("\nException: MATH ERROR!\n");
|
|
}
|
|
// y1 = y1 *pCoeff->m_aScale;
|
|
|
|
for (k=1; k <= pCoeff->m_Na; k++)
|
|
{
|
|
y2 = y2 - (pCoeff->m_pak[k] * yn[pCoeff->m_Na-k+n]);
|
|
if (!_finite(y2))
|
|
printf("\nException: MATH ERROR!\n");
|
|
}
|
|
// y2 = y2 *pCoeff->m_bScale;
|
|
|
|
yn[pCoeff->m_Nb+n] = (y1 + y2);
|
|
if (!_finite(yn[pCoeff->m_Nb+n]))
|
|
printf("\nException: MATH ERROR!\n");
|
|
|
|
}
|
|
}
|
|
|
|
int IIRCalcFilterCoeff(double fg, double fa, double Qi, unsigned N, CIIRCoeff *pCoeff, unsigned filterType)
|
|
{
|
|
unsigned p, order_ap, order_bp;
|
|
div_t result;
|
|
unsigned numEvenFilterParts, filterCnt;
|
|
|
|
CIIRCoeff Temp(N, N);
|
|
CIIRCoeff Coeff1(1,1);
|
|
CIIRCoeff Coeff2(2,2);
|
|
|
|
double Qp;
|
|
FILE *pFile;
|
|
|
|
pFile = fopen("filter.out","w");
|
|
fprintf(pFile,"IIR-Filter Version 1.0\n");
|
|
fprintf(pFile,"Filter Coefficients for %u-Order-%s, Qi = %4.2f\n",N, filterTypeString[filterType],Qi);
|
|
fprintf(pFile,"fg = %9.2f Hz\nfa = %9.2f Hz\n",fg, fa);
|
|
|
|
p = 1;
|
|
filterCnt = 1;
|
|
|
|
switch (N)
|
|
{
|
|
case 0:
|
|
break;
|
|
|
|
default:
|
|
order_ap = 2;
|
|
order_bp = 2;
|
|
result = div(N,2);
|
|
numEvenFilterParts = result.quot;
|
|
|
|
if (result.rem != 0)
|
|
{
|
|
IIRCalcPartFilterCoeff1(&Coeff1, fg, fa, 1.0*Qi, filterType);
|
|
fprintf(pFile,"\n1.Partfilter Np = 1, Qp = 1.00\n");
|
|
IIRPrintCoeff(pFile,&Coeff1, 1);
|
|
filterCnt++;
|
|
}
|
|
|
|
while (p <= numEvenFilterParts)
|
|
{
|
|
Qp = IIRCalcQp(p, N);
|
|
if (p == 1)
|
|
{
|
|
IIRCalcPartFilterCoeff2(&Temp, fg, fa, Qp*Qi, filterType);
|
|
fprintf(pFile,"\n%u.Partfilter Np = 2, Qp = %4.2f\n", filterCnt, Qp);
|
|
IIRPrintCoeff(pFile, &Temp, 2);
|
|
filterCnt++;
|
|
p++;
|
|
if (numEvenFilterParts > 1)
|
|
continue;
|
|
memcpy(pCoeff->m_pak, Temp.m_pak, (order_ap+1)*sizeof(double));
|
|
memcpy(pCoeff->m_pbk, Temp.m_pbk, (order_bp+1)*sizeof(double));
|
|
continue;
|
|
}
|
|
|
|
IIRCalcPartFilterCoeff2(&Coeff2, fg, fa, Qp*Qi, filterType);
|
|
fprintf(pFile,"\n%u.Partfilter Np = 2, Qp = %4.2f\n", filterCnt, Qp);
|
|
IIRPrintCoeff(pFile,&Coeff2, 2);
|
|
order_ap = IIRMulPolynom(Coeff2.m_pak, 2, Temp.m_pak, order_ap, pCoeff->m_pak);
|
|
order_bp = IIRMulPolynom(Coeff2.m_pbk, 2, Temp.m_pbk, order_bp, pCoeff->m_pbk);
|
|
memcpy(Temp.m_pak, pCoeff->m_pak, (order_ap+1)*sizeof(double));
|
|
memcpy(Temp.m_pbk, pCoeff->m_pbk, (order_bp+1)*sizeof(double));
|
|
filterCnt++;
|
|
p++;
|
|
}
|
|
|
|
if (result.rem != 0)
|
|
{
|
|
if (result.quot == 0)
|
|
{
|
|
memcpy(pCoeff->m_pak, Coeff1.m_pak, 2*sizeof(double));
|
|
memcpy(pCoeff->m_pbk, Coeff1.m_pbk, 2*sizeof(double));
|
|
}
|
|
else
|
|
{
|
|
order_ap = IIRMulPolynom(Coeff1.m_pak, 1, Temp.m_pak, order_ap, pCoeff->m_pak);
|
|
order_bp = IIRMulPolynom(Coeff1.m_pbk, 1, Temp.m_pbk, order_bp, pCoeff->m_pbk);
|
|
}
|
|
}
|
|
|
|
fprintf(pFile,"\n\nResulted Filter N = %u\n",N);
|
|
IIRPrintCoeff(pFile,pCoeff, N);
|
|
// ScaleCoeff(pCoeff);
|
|
fprintf(pFile,"\n\nNormalized Filterkernel N = %u\n",N);
|
|
IIRPrintCoeff(pFile,pCoeff, N);
|
|
break;
|
|
}
|
|
|
|
fclose(pFile);
|
|
return 0;
|
|
}
|
|
|
|
|
|
unsigned IIRMulPolynom(double *pA, unsigned orderA, double *pB, unsigned orderB, double *pProduct)
|
|
{
|
|
unsigned cntA, cntB, newOrder;
|
|
|
|
newOrder = orderA+orderB;
|
|
memset(pProduct, 0, (newOrder+1)*sizeof(double));
|
|
|
|
for (cntA=0; cntA <= orderA; cntA++)
|
|
{
|
|
for (cntB=0; cntB <= orderB; cntB++)
|
|
pProduct[cntA+cntB] += pA[cntA] * pB[cntB];
|
|
}
|
|
|
|
return newOrder;
|
|
}
|
|
|
|
double IIRCalcQp(unsigned p, unsigned N)
|
|
{
|
|
return 1.0/(2*sin(pi*(2*p-1)/(2*N)));
|
|
}
|
|
|
|
void IIRPrintCoeff(FILE *pFile, CIIRCoeff *pCoeff, unsigned N)
|
|
{
|
|
unsigned k;
|
|
|
|
|
|
for (k=0; k <= N; k++)
|
|
{
|
|
fprintf(pFile,"a[%2u] = %9.6g, b[%2u] = %9.6g\n",k,pCoeff->m_pak[k],k,pCoeff->m_pbk[k]);
|
|
}
|
|
fprintf(pFile,"aScale = %9.6g, bScale = %9.6g\n\n",pCoeff->m_aScale, pCoeff->m_bScale);
|
|
|
|
fprintf(pFile,";DSP Coefficients\ncoef\n");
|
|
for (k=N; k > 0; k--)
|
|
{
|
|
fprintf(pFile,"\tdc\t%9.7g\t; a%u\n",pCoeff->m_pak[k]/2.0,k);
|
|
}
|
|
for (k=N; k > 0; k--)
|
|
{
|
|
fprintf(pFile,"\tdc\t%9.7g\t; b%u\n",pCoeff->m_pbk[k]/2.0,k);
|
|
}
|
|
|
|
}
|
|
|
|
|
|
void ScaleCoeff(CIIRCoeff *pCoeff)
|
|
{
|
|
unsigned i;
|
|
double val;
|
|
|
|
val=0;
|
|
for(i=0; i <= pCoeff->m_Na; i++)
|
|
val=MaxMag(val, pCoeff->m_pak[i]);
|
|
|
|
for(i=0; i <= pCoeff->m_Na; i++)
|
|
pCoeff->m_pak[i] /= val;
|
|
|
|
pCoeff->m_aScale = val;
|
|
|
|
val=0;
|
|
for(i=0; i <= pCoeff->m_Nb; i++)
|
|
val=MaxMag(val, pCoeff->m_pbk[i]);
|
|
|
|
for(i=0; i <= pCoeff->m_Nb; i++)
|
|
pCoeff->m_pbk[i] /= val;
|
|
|
|
pCoeff->m_bScale = val;
|
|
|
|
}
|
|
|
|
|
|
double MinMag(double val1, double val2)
|
|
{
|
|
if(fabs(val1) < fabs(val2))
|
|
return fabs(val1);
|
|
|
|
return fabs(val2);
|
|
}
|
|
|
|
double MaxMag(double val1, double val2)
|
|
{
|
|
if(fabs(val1) > fabs(val2))
|
|
return fabs(val1);
|
|
|
|
return fabs(val2);
|
|
}
|