bandpass.c

低通滤波器的实现函数

#include <math.h>
#include "utils.h"

//Notch filter
float BandPass (int aciStatus, struct tBandPass *N, float acrX, float y0)
{
	float rWT, rDWT4, rTT;
	float y, rT;

	if (N->OK)
	{

		if (aciStatus == 0)
		{
			if (N->Prewarp > 0)
				rT = (float)(2.0 * tan(N->Prewarp * N->DT / 2.0) / N->Prewarp);
			else
				rT = N->DT;
			rWT = N->Freq * rT;
			rDWT4 = 4 * N->Damp * rWT;
			rTT = N->Tlag / rT;
			N->Den[0] = 4.0F + rDWT4 + rWT*rWT;
			N->Den[1] = 2.0F * (rWT*rWT - 4.0F) / N->Den[0];
			N->Den[2] =(4.0F - rDWT4 + rWT*rWT) / N->Den[0];
			N->Num[0] = rDWT4 * (2.0F * rTT + 1.0F) * N->Gain / N->Den[0];
			N->Num[1] =-4.0F * rDWT4 * rTT * N->Gain / N->Den[0];
			N->Num[2] = rDWT4 * (2.0F * rTT - 1.0F) * N->Gain / N->Den[0];
			N->Work[0] = acrX;
			N->Work[1] = acrX;
			N->Work[2] = y0;
			N->Work[3] = y0;
		}

		y = N->Num[0]*acrX + N->Num[1]*N->Work[0] + N->Num[2]*N->Work[1]
			 - N->Den[1]*N->Work[2] - N->Den[2]*N->Work[3];

		N->Work[1] = N->Work[0];
		N->Work[0] = acrX;
		N->Work[3] = N->Work[2];
		N->Work[2] = y;

		return(y);
	}

	else return(y0);

}