varnotch.c

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

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

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

	if (N->OK)
	{

		if (aciStatus == 0)
		{
			N->Work[0] = acrX;
			N->Work[1] = acrX;
			N->Work[2] = acrX;
			N->Work[3] = acrX;
			N->W0 = acrW0;
		}

		if (aciStatus == 0 || N->W0 > 0)
		{
			//Prewarp
			if (N->Prewarp > 0)
				rT = (float)(2.0 * tan(N->Prewarp * N->DT / 2.0) / N->Prewarp);
			else
				rT = N->DT;

			//Variable frequency
			if (N->W0 > 0) rT *= acrFreq / N->W0;

			//Numerator
			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;

			//Denominator
			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);
			}
		}

		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);

}