#include "pid.h"
static INT32S L_sub(register INT32S src_dst, register INT32S src2)
{
return src2-src_dst;
}
static INT32S L_deposit_l(register INT16S ssrc)
{
return (INT32S)(ssrc);
}
static INT16S extract_l(register INT32S lsrc)
{
return (INT16S)lsrc;
}
static INT32S L_mult(register INT16S sinp1, register INT16S sinp2)
{
register INT32S laccum;
laccum=sinp1;
laccum*=sinp2;
return laccum;
}
static INT32S L_add(register INT32S src_dst, register INT32S src2)
{
return src_dst+src2;
}
INT16S CaiXinBoPIDController(INT16S DesiredValue,INT16S MeasuredValue,sCaiXinBoPID *pParams)
{
INT32S ProportionalPortion, IntegralPortion, PIDoutput;
INT16S InputError;
/*-------------------------------------------------------------------------------------------------*/
/* Saturation mode must be set */
/* InputError = sub(DesiredValue, MeasuredValue); */ /* input error */
/*-------------------------------------------------------------------------------------------------*/
/* input error calculation - 16bit range, with and without saturation mode */
PIDoutput = L_sub(L_deposit_l(DesiredValue),L_deposit_l(MeasuredValue)); /* input error - 32bit range */
if(PIDoutput > MAX_16) /* inpur error is greater than 0x00007fff = 32767 - 32bit range */
InputError = MAX_16; /* input error = max. positive 16 bit signed value */
else
if(PIDoutput < MIN_16) /* input error is less than 0xffff7fff = -32768 - 32bit range */
InputError = MIN_16; /* input error = min. negative 16 bit signed value */
else
InputError = extract_l(PIDoutput); /* input error - 16bit range */
/*-------------------------------------------------------------------------------------------------*/
/* proportional portion calculation */
ProportionalPortion = L_mult((pParams -> ProportionalGain), InputError) >> (pParams -> ProportionalGainScale + 1);
/*-------------------------------------------------------------------------------------------------*/
/* integral portion calculation */
IntegralPortion = L_mult((pParams -> IntegralGain), InputError) >> (pParams -> IntegralGainScale + 1);
/* integral portion in step k + integral portion in step k-1 */
IntegralPortion = L_add(IntegralPortion, L_deposit_l(pParams -> IntegralPortionK_1));
/* integral portion limitation */
if(IntegralPortion > pParams -> PositivePIDLimit)
pParams -> IntegralPortionK_1 = pParams -> PositivePIDLimit;
else
if(IntegralPortion < pParams -> NegativePIDLimit)
pParams -> IntegralPortionK_1 = pParams -> NegativePIDLimit;
else
pParams -> IntegralPortionK_1 = extract_l(IntegralPortion);
/*-------------------------------------------------------------------------------------------------*/
/* derivative portion calculation */
PIDoutput = L_sub(L_deposit_l(InputError),L_deposit_l(pParams -> InputErrorK_1)); /* [e(k) - e(k-1)] - 32bit range */
pParams -> InputErrorK_1 = InputError; /* e(k-1) = e(k) */
if(PIDoutput > MAX_16) /* [e(k) - e(k-1)] is greater than 0x00007fff = 32767 - 32bit range */
InputError = MAX_16; /* [e(k) - e(k-1)] = max. positive 16 bit signed value - 16 bit range */
else
if(PIDoutput < MIN_16) /* [e(k) - e(k-1)] is less than 0xffff7fff = -32768 - 32bit range */
InputError = MIN_16; /* [e(k) - e(k-1)] = min. negative 16 bit signed value - 16 bit range */
else
InputError = extract_l(PIDoutput); /* [e(k) - e(k-1)] - 16bit range */
/* drivative portion in step k - integer */
PIDoutput = L_mult((pParams -> DerivativeGain), InputError) >> (pParams -> DerivativeGainScale + 1);
/*-------------------------------------------------------------------------------------------------*/
/* controller output calculation */
PIDoutput = L_add(PIDoutput, ProportionalPortion); /* derivative portion + proportional portion */
PIDoutput = L_add(PIDoutput, L_deposit_l(pParams -> IntegralPortionK_1)); /* + integral portion = controller output */
/* controller output limitation */
if(PIDoutput > pParams -> PositivePIDLimit)
PIDoutput = pParams -> PositivePIDLimit;
else
if(PIDoutput < pParams -> NegativePIDLimit)
PIDoutput = pParams -> NegativePIDLimit;
/*-------------------------------------------------------------------------------------------------*/
return (extract_l(PIDoutput)); /* controller output with limitation */
}
评论0