/************************************************************************
 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 "user.h"

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

/************************************************************************
 Private Variables
*************************************************************************/

/************************************************************************
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_stPwrLimOut.swIqLimPu      = (SWORD)cof_uwCurMaxPu;
    pwr_stPwrLimOut.swPwrErrZ1     = 0;
    pwr_stPwrLimOut.slIqLimDetaSum = 0;
    pwr_stPwrLimOut.swIqLimDetaSum = 0;
    pwr_stPwrLimOut.swIqLimPu      = 0;

    pwr_stPwrLimOut2.slIqLimDetaSum   = 0;
    pwr_stPwrLimOut2.swIqLimDetaSum   = 0;
    pwr_stPwrLimOut2.swPwrErrZ1       = 0;
    pwr_stPwrLimOut2.swPwrLimActualPu = (SWORD)cof_uwPwrMaxPu;
    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
    {
        // do nothing
    }

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

    if (in->swPwrLimW > 30000)
    {
        in->swPwrLimW = 30000;
    }
    else if (in->swPwrLimW < 100)
    {
        in->swPwrLimW = 100;
    }
    else
    {
        // do noting
    }
    if (in->uwPwrErrW < 20)
    {
        in->uwPwrErrW = 20;
    }
    else if (in->uwPwrErrW >= in->swPwrLimW)
    {
        in->uwPwrErrW = in->swPwrLimW - 10;
    }
    else
    {
        // do noting
    }

    if (in->swIqMaxAp > in->uwIBaseAp)
    {
        in->swIqMaxAp = (SWORD)in->uwIBaseAp;
    }
    else if (in->swIqMaxAp < 1)
    {
        in->swIqMaxAp = 1;
    }
    else
    {
        // do nothing
    }

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

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

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

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

    out->swPwrLimStartTemp = (SWORD)in->swPwrLimStartTempCe;
    out->swPwrLimEndTemp   = (SWORD)((in->swAlarmTempCe + in->swPwrLimStartTempCe) >> 1);
    out->swPwrLimTempCof   = (out->swPwrLimPu >> 1) / (out->swPwrLimEndTemp - out->swPwrLimStartTemp); 
    out->swIqLimTempCof =
        ((SLONG)out->swIqMaxPu * 83 >> 7) / (out->swPwrLimEndTemp - out->swPwrLimStartTemp);   ///< 0.65=83>>7

    out->uwPwrLimStartBatCap = 20;
    out->uwPwrLimEndBatCap   = 5;
    out->uwPwrlimBatCapCof =
        ((SLONG)out->swPwrLimPu * 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:
************************************************************************/
void pwr_voPwrLim(const PWRLIM_IN *in, const PWRLIM_COF *cof, PWRLIM_OUT *out)
{
    SWORD swEmPwLimIqPu; // Q14

    if (in->swMotorPwrPu > cof->swPwrLimPu)
    {
        if (in->swSpdPu >= 0)
        {
            swEmPwLimIqPu =
                (SWORD)(((SLONG)cof->swPwrLimPu - (SLONG)in->swMotorPwrPu) * (SLONG)cof->uwPwrLimKp >> 6) + cof->swIqMaxPu; // Q15+Q5-Q6=Q14
            if (swEmPwLimIqPu > cof->swIqMaxPu)
            {
                swEmPwLimIqPu = cof->swIqMaxPu;
            }
            else if (swEmPwLimIqPu < 0)
            {
                swEmPwLimIqPu = 0;
            }
            else
            {
                // do nothing
            }
            out->swIqLimPu = swEmPwLimIqPu;
        }
        else
        {
            swEmPwLimIqPu = (SWORD)((((SLONG)in->swMotorPwrPu - cof->swPwrLimPu) * (SWORD)cof->uwPwrLimKp >> 6) - cof->swIqMaxPu); // Q14
            if (swEmPwLimIqPu < -cof->swIqMaxPu)
            {
                swEmPwLimIqPu = -cof->swIqMaxPu;
            }
            else if (swEmPwLimIqPu > 0)
            {
                swEmPwLimIqPu = 0;
            }
            else
            {
                // do nothing
            }
            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(const PWRLIM_IN *in, const PWRLIM_COF *cof, PWRLIM_OUT *out)
{
    SWORD swError, swErrorDeta;                                                     ///< Q14
    SLONG slError;                                                                  ///< Q15
    SLONG slIqLimDetaP, slIqLimDetaI, slIqLimSum;                                   ///< Q30
    SWORD swPwrLimActualPu1 = cof->swPwrLimPu, swPwrLimActualPu2 = cof->swPwrLimPu; ///< Q15
    SWORD swIqlimPu1 = cof->swIqMaxPu, swIqlimPu2 = cof->swIqMaxPu;                 ///< Q14

    /* 根据温度限制功率，电机测试模式开放 */
    if (cp_stFlg.RunModelSelect == ClZLOOP)
    {
        if (in->swPCBTemp < cof->swPwrLimStartTemp)
        {
            swPwrLimActualPu1 = cof->swPwrLimPu;
        }
        else if (in->swPCBTemp < cof->swPwrLimEndTemp)
        {
            swPwrLimActualPu1 = cof->swPwrLimPu - ((in->swPCBTemp - cof->swPwrLimStartTemp) * cof->swPwrLimTempCof);
        }
        else
        {
            swPwrLimActualPu1 = cof->swPwrLimPu >> 1; ///< EndTemp时限制为1/2的功率
        }
    }

    /* 根据电池电量限制功率 */
    if (in->uwBatCap > cof->uwPwrLimStartBatCap)
    {
        swPwrLimActualPu2 = cof->swPwrLimPu;
    }
    else if (in->uwBatCap > cof->uwPwrLimEndBatCap)
    {
        swPwrLimActualPu2 = cof->swPwrLimPu - ((cof->uwPwrLimStartBatCap - in->uwBatCap) * cof->uwPwrlimBatCapCof);
    }
    else
    {
        swPwrLimActualPu2 = cof->swPwrLimPu >> 2; ///< EndTemp时限制为1/4的功率
    }

    out->swPwrLimActualPu = (swPwrLimActualPu1 < swPwrLimActualPu2) ? swPwrLimActualPu1 : swPwrLimActualPu2;

    /* 根据PCB温度限制电流 */
    if (cp_stFlg.RunModelSelect == CityBIKE || cp_stFlg.RunModelSelect == MountainBIKE)
    {
        if (in->swPCBTemp < cof->swPwrLimStartTemp)
        {
            swIqlimPu1 = cof->swIqMaxPu;
        }
        else if (in->swPCBTemp < cof->swPwrLimEndTemp)
        {
            swIqlimPu1 = cof->swIqMaxPu - ((SLONG)(in->swPCBTemp - cof->swPwrLimStartTemp) * cof->swIqLimTempCof);
        }
        else
        {
            swIqlimPu1 = (SWORD)((SLONG)cof->swIqMaxPu * 45 >> 7); ///< EndTemp时限制为额定电流=0.35*最大电流
        }
    }

    /* 功率限制PI调节:根据功率限制电流 */
    slError = out->swPwrLimActualPu - in->swMotorPwrPu;
    if (slError >= 32768)
    {
        slError = 32768;
    }
    else if (slError < -32768)
    {
        slError = -32768;
    }
    else
    {
        // do nothing
    }

    swError         = (SWORD)slError;
    swErrorDeta     = swError - out->swPwrErrZ1;
    out->swPwrErrZ1 = swError;
    slIqLimDetaP    = (SLONG)cof->uwPwrLimPIKp * swErrorDeta; ///< Q15+Q15=Q30
    slIqLimDetaI    = (SLONG)cof->uwPwrLimPIKi * swError;     ///< Q15+Q15=Q30
    slIqLimSum      = out->slIqLimDetaSum + slIqLimDetaP + slIqLimDetaI;

    if (slIqLimSum > 0)
    {
        out->slIqLimDetaSum = 0;
    }
    else if (slIqLimSum < -((SLONG)cof->swIqMaxPu << 16))
    {
        out->slIqLimDetaSum = -((SLONG)cof->swIqMaxPu << 16);
    }
    else
    {
        out->slIqLimDetaSum = slIqLimSum; ///< Q30
    }
    out->swIqLimDetaSum = (SWORD)(out->slIqLimDetaSum >> 16); ///< Q30-Q16=Q14

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

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

/************************************************************************
Local Functions (N/A)
************************************************************************/

/************************************************************************
Copyright (c) 2018 Welling Motor Technology(Shanghai) Co. Ltd.
All rights reserved.
************************************************************************/
#ifdef _PWRLIM_C_
#undef _PWRLIM_C_ /* parasoft-suppress MISRA2004-19_6 "本项目中无法更改，后续避免使用" */
#endif
/************************************************************************
End of this File (EOF)!
Do not put anything after this part!
************************************************************************/