PID函数转录自变频器

/****************************************************************
函数说明：PID函数(暂时不考虑D增益的作用)
输入偏差为int型变量
输出结果为long型变量。右移16位得到需要的结果
比例增益在pid内部倍左移 4位
****************************************************************/
void PID(PID_STRUCT * pid)
{
    long m_Max,m_Min,m_Out,m_OutKp,m_OutKi;
    long vMax = 0x7FFFFFFF;
    
    SETOVM;
     m_Max = ((long)pid->Max)<<16;                        // 最大值
    m_Min = ((long)pid->Min)<<16;                        // 最小值
    
    m_OutKp = (long)pid->KP * (long)pid->Deta;            // 比例
    m_OutKp = __IQsat(m_OutKp, (vMax>> (4+ pid->QP)), -(vMax>> (4+ pid->QP))); //保证使用算术右移
    m_OutKp = m_OutKp << (4+ pid->QP);
    
     m_OutKi = (long)pid->KI * (long)pid->Deta;// 积分
    m_OutKi = __IQsat(m_OutKi, (vMax >> pid->QI), -(vMax >> pid->QI));
    m_OutKi = m_OutKi << pid->QI;
    
    // 饱和情况下的去饱和处理
    if((m_OutKp > m_Max) && (pid->Total > 0))
    {
        pid->Total -= (pid->Total>>8) + 1;  //加1去掉滤波静差
        m_OutKi = 0;
    }
    else if((m_OutKp < m_Min) && (pid->Total < 0))
    {
        pid->Total -= (pid->Total>>8) - 1; 
        m_OutKi = 0;
    }
    pid->Total += m_OutKi;     
    m_Out       = pid->Total + m_OutKp;
     pid->Out    = __IQsat(m_Out,m_Max,m_Min);
    pid->Total  = __IQsat(pid->Total,m_Max,m_Min);

    CLROVM;
}
/****************************************************************
函数说明：
PID函数(暂时不考虑D增益的作用)
输入偏差为long型变量;上下限需要与偏差的Q值相同

输出为llong型变量，右移16位得到需要的结果
*考虑增加饱和情况下的去饱和处理*

比例增益在pid内部倍左移 4位
****************************************************************/
void PIDLongRegulate(PID_STRUCT_LONG * pid)
{
    llong  m_Max,m_Min,m_Out,m_OutKp,m_OutKi;
     m_Max = ((llong)pid->Max)<<16;                        //最大值
    m_Min = ((llong)pid->Min)<<16;                        //最小值
    m_OutKp = (llong)pid->KP * (llong)pid->Deta << 4;        //比例
    
    m_OutKi = (llong)pid->KI * (llong)pid->Deta;

// 饱和情况下的去饱和处理
    if(((m_OutKp > m_Max) && (pid->Total > 0)) ||
       ((m_OutKp < m_Min) && (pid->Total < 0))) 
    {
        pid->Total -= (pid->Total>>8);
        m_OutKi = 0;
    }

    pid->Total += m_OutKi;        
    m_Out      = pid->Total + m_OutKp;
    if(m_Out < m_Min)               pid->Out = m_Min;
    else if(pid->Out > m_Max)       pid->Out = m_Max;
    else                            pid->Out = m_Out;

    if(pid->Total < m_Min)          pid->Total = m_Min;
    else if(pid->Total > m_Max)     pid->Total = m_Max;      
}

void PID32(PID32_STRUCT * pid)
{
    long  m_OutKp,m_OutKi;
    long  mTotalMax,mTotalMin;

    //if(pid->Deta == 0)        return;

    //计算比例作用，并调整积分作用
    m_OutKp = ((llong)pid->KP * (llong)pid->Deta) >> (16-4);    
    //调整积分的上(下)限
    if(pid->Deta > 0)        
    {
        mTotalMax = pid->Max - m_OutKp;
        if(mTotalMax < 0)    mTotalMax  = 0;

        mTotalMin = pid->Min;
    }
    else
    {
        mTotalMin = pid->Min - m_OutKp;
        if(mTotalMin > 0)    mTotalMin  = 0;

        mTotalMax = pid->Max;
    }

    //计算积分作用
    m_OutKi = ((llong)pid->KI * (llong)pid->Deta)>>16;
    pid->Total = pid->Total + m_OutKi;
    pid->Total = __IQsat(pid->Total, mTotalMax, mTotalMin);

    //计算PID的输出
    pid->Out = pid->Total + m_OutKp;
    pid->Out = __IQsat(pid->Out, pid->Max, pid->Min);
}