transferfunction.c

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

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

//Transfer function
float TransferFunction (int aciStatus, struct tTransferFunction *T, float acrX, float y0)
{	int i;
	double y;

	if (T->OK)
	{	if (aciStatus == 0)
		{	for (i=0; i<T->iNumOrder; i++) T->UWork[i] = acrX;
			for (i=0; i<T->iDenOrder; i++) T->YWork[i] = y0;
		}

		y = (double) acrX * (double) T->Num[0];
		for (i=0; i<T->iNumOrder; i++) y += T->Num[i+1] * (double) T->UWork[i];
		for (i=0; i<T->iDenOrder; i++) y -= T->Den[i+1] * (double) T->YWork[i];

		for (i=T->iNumOrder; i>0; i--) T->UWork[i] = T->UWork[i-1];
		T->UWork[0] = acrX;
		for (i=T->iDenOrder; i>0; i--) T->YWork[i] = T->YWork[i-1];
		T->YWork[0] = (float) y;

		return((float) y);
	}
	else return(y0);
}