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