notch.c

包括常规PID、NOTCH、LAG2 等函数的实现方法

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

//Notch filter
float Notch (int aciStatus, struct tNOTCH *N, float acrX)
{
	float rWT1, rWT2;
	float y, rT;
	int i;

	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;
			rWT1 = N->W1 * rT;
			N->Num[0] = 4.0F + 4.0F*N->D1*rWT1 + rWT1*rWT1;
			N->Num[1] =-8.0F + 2.0F*rWT1*rWT1;
			N->Num[2] = 4.0F - 4.0F*N->D1*rWT1 + rWT1*rWT1;
			rWT2 = N->W2 * rT;
			N->Den[0] = 4.0F + 4.0F*N->D2*rWT2 + rWT2*rWT2;
			N->Den[1] =-8.0F + 2.0F*rWT2*rWT2;
			N->Den[2] = 4.0F - 4.0F*N->D2*rWT2 + rWT2*rWT2;
			//For unity DC gain:
			for (i=0; i<=2; i++)
			{
				N->Num[i] *= (N->W2 * N->W2);
				N->Den[i] *= (N->W1 * N->W1);
			}
			N->Work[0] = acrX;
			N->Work[1] = acrX;
			N->Work[2] = acrX;
			N->Work[3] = acrX;
		}

		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->Den[0];

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

		return(y);
	}

	else return(acrX);

}