/************************************************************************
 Project:             Welling Motor Control Paltform
 Filename:            pwrlim.c
 Partner Filename:    pwrlim.h
 Description:         Motor input power limit
 Complier:            IAR Embedded Workbench for ARM 7.80, IAR Systems.
 CPU TYPE :           GD32F3x0
*************************************************************************
 Copyright (c) 2018 Welling Motor Technology(Shanghai) Co. Ltd.
 All rights reserved.
*************************************************************************
*************************************************************************
Revising History (ECL of this file):
Fjy create this file;
************************************************************************/

/************************************************************************
 Beginning of File, do not put anything above here except notes
 Compiler Directives:
*************************************************************************/
#ifndef _PWRLIM_C_
#define _PWRLIM_C_
#endif

/************************************************************************
Include File
************************************************************************/
#include "syspar.h"
#include "user.h"

/************************************************************************
Constant Table:
************************************************************************/

/************************************************************************
Exported Functions:
************************************************************************/
/************************************************************************
 Function: pwr_voPwrLimInit;
 Description: Initialize Variables for "pwr_voPwrLim"
 Call by:
 Input Variables:
 Output/Return Variables:
 Subroutine Call:
 Reference:
************************************************************************/
void pwr_voPwrLimInit(void)
{
    pwr_stPwrLimOut2.swIqLimPu = cof_uwCurMaxPu;
    pwr_stPwrLimOut2.swErrorZ1 = 0; // Q14
    pwr_stPwrLimOut2.slIqLimDetaSum = 0;
    pwr_stPwrLimOut2.swIqLimDetaSum = 0;
    pwr_stPwrLimOut2.swIqLimPu = 0;
}

/************************************************************************
Function: pwr_voPwrLimCof;
Description: Coefficient calculation for "pwr_voPwrLim"
Call by:
Input Variables:
Output/Return Variables:
Subroutine Call:
Reference:
************************************************************************/
void pwr_voPwrLimCof(PWRLIM_COFIN *in, PWRLIM_COF *out)
{
    UWORD uwPwrLim;
    ULONG ulPbWt;

    if (in->uwIBaseAp > 32767)
    {
        in->uwIBaseAp = 32767;
    }
    else if (in->uwIBaseAp < 1)
    {
        in->uwIBaseAp = 1;
    }
    else
    {}

    if (in->uwUbVt < 100)
    {
        in->uwUbVt = 100;
    }
    else
    {}

    if (in->swPwrLimW > 30000)
    {
        in->swPwrLimW = 30000;
    }
    else if (in->swPwrLimW < 100)
    {
        in->swPwrLimW = 100;
    }
    else
    {}

    if (in->uwPwrErrW < 20)
    {
        in->uwPwrErrW = 20;
    }
    else if (in->uwPwrErrW >= in->swPwrLimW)
    {
        in->uwPwrErrW = in->swPwrLimW - 10;
    }
    else
    {}

    if (in->swIqMaxAp > 32766)
    {
        in->swIqMaxAp = 32766;
    }
    else if (in->swIqMaxAp < 1)
    {
        in->swIqMaxAp = 1;
    }
    else
    {}

    out->swIqMaxPu = (SWORD)(((SLONG)in->swIqMaxAp << 14) / in->uwIBaseAp); // Q14, Max phase current (peak value)

    ulPbWt = ((ULONG)3 * in->uwUbVt * in->uwIBaseAp / 100) >> 1; // Q0, unit: 0.1w, Power base

    uwPwrLim = ((ULONG)in->uwPwrErrW << 15) / ulPbWt;            // Q15
    out->uwPwrLimKp = (((SLONG)out->swIqMaxPu << 6) / uwPwrLim); // 14+6-15=Q5

    out->swPwrLimStartThresholdTemp = in->uwPwrLimSTARTCe; // PWRLIM_START_THRESHOLD_TEMP; // Ce Q0
    out->swPwrLimENDThresholdTemp = (in->uwPwrLimSTARTCe + in->uwPwrLimENDCe) >> 1;

    out->swPwrLimMotorStartThresholdTemp = in->uwPwrLimMotTempSTARTCe;                                 // PWRLIM_START_THRESHOLD_TEMP; // Ce Q0
    out->swPwrLimMotorENDThresholdTemp = (in->uwPwrLimMotTempSTARTCe + in->uwPwrLimMotTempENDCe) >> 1; // in->uwPwrLimMotTempENDCe;

    out->uwPwrLimStartBatCap = in->uwPwrLimStartBatCap;
    out->uwPwrLimEndBatCap = in->uwPwrLimEndBatCap;

    if (in->swPwrLimW > ((SWORD)in->uwPwrErrW))
    {
        out->swMotorPwrLimitPu = (SWORD)((((ULONG)in->swPwrLimW - (ULONG)in->uwPwrErrW) << 15) / ulPbWt); // Q15
    }
    else
    {
        out->swMotorPwrLimitPu = 0;
    }

    /* 根据PCB温度线性限制功率系数，PuQ15 */
    out->swPwrLimTempGain = (out->swMotorPwrLimitPu >> 1) / (out->swPwrLimENDThresholdTemp - out->swPwrLimStartThresholdTemp);
    /* 根据PCB温度线性限制电流系数，PuQ14 */
    out->swCurLimTempGain = (out->swIqMaxPu * 5 >> 3) / (out->swPwrLimENDThresholdTemp - out->swPwrLimStartThresholdTemp);
    /* 根据MOTOR温度线性限制电流系数，PuQ14 */
    out->swCurLimMotorTempGain = (out->swIqMaxPu * 5 >> 3) / (out->swPwrLimMotorENDThresholdTemp - out->swPwrLimMotorStartThresholdTemp);
    /* 根据电量线性限制功率系数，PuQ15 */
    out->uwPwrlimBatCapCof = ((SLONG)out->swMotorPwrLimitPu * 3 >> 2) / (out->uwPwrLimStartBatCap - out->uwPwrLimEndBatCap);

    out->uwPwrLimPIKp = in->uwPwrLimPIKp; // q15
    out->uwPwrLimPIKi = in->uwPwrLimPIKi; // q15
}

/************************************************************************
Function: pwr_voPwrLim;
Description: Power Limit Using Kp;
Call by:
Input Variables:
Output/Return Variables:
Subroutine Call:
Reference:
************************************************************************/
SWORD swEmPwLimIqPu; // Q14
void  pwr_voPwrLim(PWRLIM_IN *in, PWRLIM_COF *cof, PWRLIM_OUT *out)
{
    if (in->swMotorPwrPu > cof->swMotorPwrLimitPu)
    {
        if (in->swSpdPu >= 0)
        {
            swEmPwLimIqPu = (SWORD)((SLONG)(cof->swMotorPwrLimitPu - in->swMotorPwrPu) * cof->uwPwrLimKp >> 6) + cof->swIqMaxPu; // Q15+Q5-Q6=Q14
            if (swEmPwLimIqPu > cof->swIqMaxPu)
            {
                swEmPwLimIqPu = cof->swIqMaxPu;
            }
            else if (swEmPwLimIqPu < 0)
            {
                swEmPwLimIqPu = 0;
            }
            else
            {}
            out->swIqLimPu = swEmPwLimIqPu;
        }
        else
        {
            swEmPwLimIqPu = ((SLONG)(in->swMotorPwrPu - cof->swMotorPwrLimitPu) * cof->uwPwrLimKp >> 6) - cof->swIqMaxPu; // Q14
            if (swEmPwLimIqPu < -cof->swIqMaxPu)
            {
                swEmPwLimIqPu = -cof->swIqMaxPu;
            }
            else if (swEmPwLimIqPu > 0)
            {
                swEmPwLimIqPu = 0;
            }
            else
            {}
            out->swIqLimPu = swEmPwLimIqPu;
        }
    }
    else
    {
        if (in->swSpdPu >= 0)
        {
            out->swIqLimPu = cof->swIqMaxPu;
        }
        else
        {
            out->swIqLimPu = -cof->swIqMaxPu;
        }
    }
}

/************************************************************************
Function: pwr_voPwrLim;
Description: Power Limit Using Kp and Ki;
Call by:
Input Variables:
Output/Return Variables:
Subroutine Call:
Reference:
************************************************************************/
void pwr_voPwrLimPI(PWRLIM_IN *in, PWRLIM_COF *cof, PWRLIM_OUT *out)
{
    SWORD        swERROR, swErrorDeta;
    SLONG        slERROR, slIqLimDetaP, slIqLimDetaI;
    SLONG        slIqLimSum;
    SLONG        slIqMaxLimSum ;
    static SWORD swPCBTempCurLimitPu, swMotorTempCurLimitPu;
    SWORD        swMotorPwriqfdb;
    SWORD        swPwrLimActualPu1 = cof->swMotorPwrLimitPu, swPwrLimActualPu2 = cof->swMotorPwrLimitPu; ///< Q15
    SWORD        swIqlimPu1 = cof->swIqMaxPu, swIqlimPu2 = cof->swIqMaxPu;                               ///< Q14
    
    slIqMaxLimSum = (SLONG) cof->swIqMaxPu<<16;
    /* 根据温度限制功率，电机测试模式开放 */                                                         
    if (cp_stFlg.RunModelSelect == ClZLOOP)
    {
        if (in->swPCBTemp < cof->swPwrLimStartThresholdTemp)
        {
            swPwrLimActualPu1 = cof->swMotorPwrLimitPu;
        }
        else if (in->swPCBTemp < cof->swPwrLimENDThresholdTemp)
        {
            swPwrLimActualPu1 = cof->swMotorPwrLimitPu - ((in->swPCBTemp - cof->swPwrLimStartThresholdTemp) * cof->swPwrLimTempGain);
        }
        else
        {
            swPwrLimActualPu1 = cof->swMotorPwrLimitPu >> 1; ///< EndTemp时限制为1/2的功率
        }
    }
    /* 根据电池电量限制功率 */
    if (in->uwBatCap > cof->uwPwrLimStartBatCap)
    {
        swPwrLimActualPu2 = cof->swMotorPwrLimitPu;
    }
    else if (in->uwBatCap > cof->uwPwrLimEndBatCap)
    {
        swPwrLimActualPu2 = cof->swMotorPwrLimitPu - ((cof->uwPwrLimStartBatCap - in->uwBatCap) * cof->uwPwrlimBatCapCof);
    }
    else
    {
        swPwrLimActualPu2 = cof->swMotorPwrLimitPu >> 2; ///< EndTemp时限制为1/4的功率
    }
    out->swMotorPwrLimitActualPu = (swPwrLimActualPu1 < swPwrLimActualPu2) ? swPwrLimActualPu1 : swPwrLimActualPu2;
    
    if (cp_stFlg.RunModelSelect == CadAssist || cp_stFlg.RunModelSelect == TorqAssist)
    {
        /* 根据PCB温度限制电流 */
        if(in->swPCBTemp < cof->swPwrLimStartThresholdTemp)
        {
            swPCBTempCurLimitPu = cof->swIqMaxPu; // Q0, unit: 0.1w, Power limit value
        }
        else if(in->swPCBTemp < cof->swPwrLimENDThresholdTemp)
        {
            swPCBTempCurLimitPu = cof->swIqMaxPu - ((in->swPCBTemp - cof->swPwrLimStartThresholdTemp) * cof->swCurLimTempGain);
        }
        else
        {
            swPCBTempCurLimitPu = cof->swIqMaxPu - ((cof->swPwrLimENDThresholdTemp - cof->swPwrLimStartThresholdTemp) * cof->swCurLimTempGain);
        }
        /* 根据MOTOR温度限制电流 */
        if(in->swMotorTemp < cof->swPwrLimMotorStartThresholdTemp)
        {
            swMotorTempCurLimitPu = cof->swIqMaxPu; // Q0, unit: 0.1w, Power limit value
        }
        else if(in->swMotorTemp < cof->swPwrLimMotorENDThresholdTemp)
        {
            swMotorTempCurLimitPu = cof->swIqMaxPu - ((in->swMotorTemp - cof->swPwrLimMotorStartThresholdTemp) * cof->swCurLimMotorTempGain);
        }
        else
        {
            swMotorTempCurLimitPu =
                cof->swIqMaxPu - ((cof->swPwrLimMotorENDThresholdTemp - cof->swPwrLimMotorStartThresholdTemp) * cof->swCurLimMotorTempGain);
        }
        swIqlimPu1 = (swMotorTempCurLimitPu < swPCBTempCurLimitPu) ? swMotorTempCurLimitPu : swPCBTempCurLimitPu;
    }
    /* 功率限制PI调节:根据功率限制电流 */
    slERROR = ((out->swMotorPwrLimitActualPu * in->uwThrottleLimit_K) >> 14) - in->swMotorPwrPu;
//    slERROR = cof->swMotorPwrLimitPu - in->swMotorPwrPu;
    if (slERROR >= 32768)
    {
        slERROR = 32768;
    }
    else if (slERROR < -32768)
    {
        slERROR = -32768;
    }
    else
    {}

    swERROR = slERROR;
    swErrorDeta = swERROR - out->swErrorZ1;
    out->swErrorZ1 = swERROR;
    slIqLimDetaP = (SLONG)cof->uwPwrLimPIKp * swErrorDeta; // Q15+Q15 = Q30;
    slIqLimDetaI = (SLONG)cof->uwPwrLimPIKi * swERROR;     // Q15+Q15 = Q30;
    slIqLimSum = out->slIqLimDetaSum + slIqLimDetaP + slIqLimDetaI;
    out->slIqLimDetaSum = slIqLimSum; // Q30

    //--------------------
    if(in->swMotoriqfdb<0)
    {
        swMotorPwriqfdb=-in->swMotoriqfdb;
    }
    else
    {
        swMotorPwriqfdb=in->swMotoriqfdb;
    }
    if(pwr_stPwrLimOut2.swErrorZ1 <0)
       {
         if(pwr_stPwrLimOut2.uwfisrttime==0)
         {
           //  pwr_stPwrLimOut2.Mortor_time++;
         //  if(pwr_stPwrLimOut2.Mortor_time>800)  //
           {
               pwr_stPwrLimOut2.Mortor_time=8000;
               pwr_stPwrLimOut2.uwfisrttime=1000;
               out->slIqLimDetaSum= ( swMotorPwriqfdb-cof->swIqMaxPu)<<16;
           }
         }

       }
       else
       {
           if(pwr_stPwrLimOut2.Mortor_time>0)
           {
               pwr_stPwrLimOut2.Mortor_time--;
           }
           else
           {
               pwr_stPwrLimOut2.uwfisrttime=0;
           }
       }
    //---------------------

    if (out->slIqLimDetaSum > 0)
    {
        out->slIqLimDetaSum = 0;
    }
    else if (out->slIqLimDetaSum < -slIqMaxLimSum)
    {
        out->slIqLimDetaSum = -slIqMaxLimSum;
    }
    else
    {}
    out->swIqLimDetaSum = out->slIqLimDetaSum >> 16; // Q30-Q16 =Q14;

    swIqlimPu2 = cof->swIqMaxPu + out->swIqLimDetaSum;

    /* 电流限制值输出 */
    out->swIqLimPu = (swIqlimPu1 < swIqlimPu2) ? swIqlimPu1 : swIqlimPu2;
    
//    out->swIqLimPu = swIqlimPu2;

    if (out->swIqLimPu <= 0)
    {
        out->swIqLimPu = 0;
    }
    else
    {}
}
/************************************************************************
Local Functions (N/A)
************************************************************************/

/************************************************************************
Copyright (c) 2018 Welling Motor Technology(Shanghai) Co. Ltd.
All rights reserved.
************************************************************************/
#ifdef _PWRLIM_C_
#undef _PWRLIM_C_
#endif
/************************************************************************
End of this File (EOF)!
Do not put anything after this part!
************************************************************************/