/************************************************************************
Project:             Welling Motor Control Paltform
Filename:            flxwkn.c
Partner Filename:    flxwkn.h
Description:         Flux weakening control for PMSM
Complier:            IAR Embedded Workbench for ARM 7.80.4
CPU TYPE :           GD32F3x0
*************************************************************************
Copyright (c) 2018 Welling Motor Technology(Shanghai) Co. Ltd.
All rights reserved.
*************************************************************************
*************************************************************************
Revising History (ECL of this file):

************************************************************************/
/************************************************************************
Beginning of File, do not put anything above here except notes
Compiler Directives:
************************************************************************/
#ifndef _FLXWKN_C_
#define _FLXWKN_C_
#endif
/************************************************************************
Include File
************************************************************************/
#include "flxwkn.h"
#include "mathtool.h"
#define abs(A)             (((A) < 0) ? - (A) : (A))
/************************************************************************
Private Variables:
************************************************************************/
UWORD   flx_pvt_uwVdcMinTmpPu = 0; // Q14, Temporary minimum value of Vdc in one cycle(50Hz)
UWORD   flx_pvt_uwVdcMinPu = 0;    // Q14, Minimum value of Vdc in one cycle(50Hz)
UWORD   flx_pvt_uwVdcMaxTmpPu = 0; // Q14, Temporary maximum value of Vdc in one cycle(50Hz)
UWORD   flx_pvt_uwVdcMaxPu = 0;    // Q14, Maxmium value of Vdc in one cycle(50Hz)
UWORD   flx_pvt_uwVdcAvgPu = 0;    // Q14, Average value of Vdc in one cycle(50Hz)
LPF_OUT flx_pvt_stVdcAvgLpf;       // Q14, Average value of Vdc after LPF
UWORD   flx_pvt_uwVdcPeriodCt = 0; // Count Vdc period
UWORD   flx_pvt_swErrZ1Pu = 0;     // Q14
SWORD   flx_pvt_swUqErrZ1 = 0;     // Q14

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

/************************************************************************
Exported Functions:
************************************************************************/
/************************************************************************
Function:
Description:
Call by:
Input Variables:
Output/Return Variables:
Subroutine Call:
Reference:
************************************************************************/
void flx_voInit(void)
{
    flx_pvt_uwVdcMinTmpPu = 0xFFFF; // Q14, Temporary minimum value of Vdc in one cycle(50Hz)
    flx_pvt_uwVdcMinPu = 0;         // Q14, Minimum value of Vdc in one cycle(50Hz)
    flx_pvt_uwVdcMaxTmpPu = 0;      // Q14, Temporary maximum value of Vdc in one cycle(50Hz)
    flx_pvt_uwVdcMaxPu = 0;         // Q14, Maximum value of Vdc in one cycle(50Hz)
    flx_pvt_uwVdcAvgPu = 0;         // Q14, Average value of Vdc in one cycle(50Hz)
    flx_pvt_stVdcAvgLpf.slY.sl = 0; // Q30, Average value of Vdc after LPF
    flx_pvt_uwVdcPeriodCt = 0;      // Q0,Count Vdc period
    flx_pvt_swErrZ1Pu = 0;          // Q14
    flx_stCtrlOut.slIdSumPu = 0;    // Q31
    flx_stCtrlOut.swIqLimPu = 0;    // Q14, Iq limit value used for speed loop out
    flx_stCtrlOut.swIdRefPu = 0;    // Q14, Id reference value used for current loop regulation
    flx_stCtrlOut.slIdMinEstPu = 0; // Q30, Id min Estimate
    flx_pvt_swUqErrZ1 = 0;          // Q14
}

/************************************************************************
Function:
Description:
Call by:
Input Variables:
Output/Return Variables:
Subroutine Call:
Reference:
************************************************************************/
void flx_voCoef(FW_CTRL_COEFIN *in, FW_CTRL_COEF *out)
{
    ULONG ulLpfTm; // unit: us
    SLONG slKp, slKi, slCurErr;
    UWORD uwCrossFreq;

    if (in->uwPWMDutyMax < 2)
    {
        in->uwPWMDutyMax = 2;
    }
    else if (in->uwPWMDutyMax > 1000)
    {
        in->uwPWMDutyMax = 1000;
    }
    else
    {}

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

    if (in->uwFwCurLimAp > ((SWORD)32767))
    {
        in->uwFwCurLimAp = (SWORD)32767;
    }
    else
    {}

    if (in->swIdMaxAp > ((SWORD)in->uwIBaseAp))
    {
        in->swIdMaxAp = (SWORD)in->uwIBaseAp;
    }
    else if (in->swIdMaxAp < (-(SWORD)in->uwIBaseAp))
    {
        in->swIdMaxAp = (-(SWORD)in->uwIBaseAp);
    }
    else
    {}

    if (in->swIdMinAp > ((SWORD)in->uwFwCurLimAp))
    {
        in->swIdMinAp = (SWORD)in->uwFwCurLimAp;
    }
    else if (in->swIdMinAp < (-(SWORD)in->uwFwCurLimAp))
    {
        in->swIdMinAp = (-(SWORD)in->uwFwCurLimAp);
    }
    else
    {}

    if (in->swIdPIOutMinAp <= abs(in->swIdMinAp))
    {
        in->swIdPIOutMinAp = abs(in->swIdMinAp) + 1;
    }
    else if (in->swIdPIOutMinAp > ((SWORD)in->uwIBaseAp))
    {
        in->swIdPIOutMinAp = (SWORD)in->uwIBaseAp;
    }
    else
    {}

    if (in->uwVdcMinCalcTmMs > 2000)
    {
        in->uwVdcMinCalcTmMs = 2000;
    }
    else
    {}

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

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

    if (in->uwCharCurDampRatio > 16)
    {
        in->uwCharCurDampRatio = 16;
    }
    else
    {}

    if (in->uwCharCurCrossFreqHz > 32)
    {
        in->uwCharCurCrossFreqHz = 32;
    }
    else
    {}

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

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

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

    out->uwVdcRatioPu =
        ((ULONG)in->uwPWMDutyMax * 5000 / 1000 * in->uwPWMDutyMax / 1000); // Q14,6640 = Q14((2/pi)*(2/pi))  5461   3640 =Q14((sqrt(2)/3)*(sqrt(2)/3))

    out->slIdMaxPu = (((SLONG)in->swIdMaxAp << 14) / in->uwIBaseAp) << 16;              // Q30=Q14+Q16
    out->slIdMinPu = (((SLONG)in->swIdMinAp << 14) / in->uwIBaseAp) << 16;              // Q30=Q14+Q16
//    out->slIdMinLimPu = out->slIdMinPu / 100 * in->uwIdMinLimRatio;                     // Q30
//    out->slIdPIOutMinPu = -((((SLONG)in->swIdPIOutMinAp << 14) / in->uwIBaseAp) << 16); // Q30=Q14+Q16
    out->slIdPIOutMinPu = out->slIdMinPu;
    out->uwFwCurLimPu = (UWORD)(((SLONG)in->uwFwCurLimAp << 14) / in->uwIBaseAp);       // Q14
    out->swIqLimMaxPu =
        mth_slSqrt(((SLONG)out->uwFwCurLimPu * out->uwFwCurLimPu) - (((SLONG)out->slIdMaxPu >> 16) * ((SLONG)out->slIdMaxPu >> 16))); // Q14=sqrt(Q28)

    out->uwIdRegKpPu = in->uwIdRegKpPu;                                                                                            // Q16
    out->uwIdRegKitPu = in->uwIdRegKiPu;                                                                                           // Q16
//    slCurErr = mth_slSqrt(((SLONG)out->uwFwCurLimPu * out->uwFwCurLimPu) - (out->slIdMinPu >> 16) * (out->slIdMinPu >> 16)) << 16; // Q30
//    out->uwIqRegKpPu = slCurErr / ((-out->slIdPIOutMinPu + out->slIdMinPu) >> 14);                                                 // Q14=Q30-Q16
//    if (out->uwIqRegKpPu >= 32767)
//    {
//        out->uwIqRegKpPu = 32767;
//    }

    out->uwVdcMinCalcTm = (UWORD)((ULONG)in->uwVdcMinCalcTmMs * in->uwFreqTbcHz / 1000); // Q0
    ulLpfTm = 1000000 / in->uwVdcLpfFreqHz;
    mth_voLPFilterCoef(ulLpfTm, in->uwFreqTbcHz, &out->uwVdcAvgLpfCoef);

//    out->uwRsPu = (UWORD)(((ULONG)in->uwRsOhm << 15) / ((ULONG)in->uwUbVt * 100000 / in->uwIBaseAp)); // Q15, Phase resistance
//    uwCrossFreq = (UWORD)(((ULONG)in->uwCharCurCrossFreqHz << 15) / in->uwFBaseHz);                   // Q15
//    slKp = (ULONG)(in->uwCharCurDampRatio << 12) * uwCrossFreq * 2;                                   // Q12+Q15=Q27
//    out->uwKpPu = slKp >> 11;                                                                         // Q27-Q11=Q16
//    slKi = (ULONG)uwCrossFreq * uwCrossFreq;                                                          // Q15+Q15=Q30
//    out->slKitPu = ((SLONG)(slKi >> 6) * (2 * 102944 * in->uwFBaseHz / in->uwFreqTbcHz >> 7) >> 7);   // Q30-Q6+(Q15-Q7)-Q7=Q25,pi(Q15)=102944
}
/************************************************************************
Function:
Description:
Call by:
Input Variables:
Output/Return Variables:
Subroutine Call:
Reference:
************************************************************************/
// Q14
void flx_voCtrl(FW_CTRL_IN *in, FW_CTRL_COEF *coef, FW_CTRL_OUT *out)
{                     // Q14
    SWORD swErrDltPu; // Q14
    SLONG slIdpPu;    // Q31
    SLONG slIdiPu;    // Q31
    SLONG slIdSumPu;  // Q31
    SWORD swUqErr, swUqDeltaErr;
    SLONG slIdMinPPu, slIdMinIPu, slErrPu;
    SWORD swErrPu;
    SWORD swVdc2Pu;    // Q14
    SWORD swUalpha2Pu; // Q14
    SWORD swUbeta2Pu;

    /* Get minimum value of Vdc in one cycle(50Hz) */
    flx_pvt_uwVdcPeriodCt++;
    if (flx_pvt_uwVdcPeriodCt <= 1)
    {
        flx_pvt_uwVdcAvgPu = in->uwVdcPu;
        flx_pvt_stVdcAvgLpf.slY.sl = in->uwVdcPu << 16;
        out->slIdMinEstPu = coef->slIdMinPu; // Q30, Id min Estimate
    }
    else if (flx_pvt_uwVdcPeriodCt < (coef->uwVdcMinCalcTm + 1))
    {
        if (flx_pvt_uwVdcMinTmpPu > in->uwVdcPu)
        {
            flx_pvt_uwVdcMinTmpPu = in->uwVdcPu; // Q14
        }
        if (flx_pvt_uwVdcMaxTmpPu < in->uwVdcPu)
        {
            flx_pvt_uwVdcMaxTmpPu = in->uwVdcPu; // Q14
        }
    }
    else
    {
        flx_pvt_uwVdcPeriodCt = 1;
        flx_pvt_uwVdcMinPu = flx_pvt_uwVdcMinTmpPu; // Q14
        flx_pvt_uwVdcMinTmpPu = in->uwVdcPu;        // Q14
        flx_pvt_uwVdcMaxPu = flx_pvt_uwVdcMaxTmpPu; // Q14
        flx_pvt_uwVdcMaxTmpPu = in->uwVdcPu;        // Q14
        flx_pvt_uwVdcAvgPu = (flx_pvt_uwVdcMinPu + flx_pvt_uwVdcMaxPu) >> 1;
    }

    /* Vdc LPF */
    flx_pvt_stVdcAvgLpf.uwKx = coef->uwVdcAvgLpfCoef; // Q15
    mth_voLPFilter(flx_pvt_uwVdcAvgPu, &flx_pvt_stVdcAvgLpf);

    /* Output voltage PI contorl */
    swVdc2Pu =
        (SWORD)(((SLONG)flx_pvt_stVdcAvgLpf.slY.sw.hi * (SLONG)flx_pvt_stVdcAvgLpf.slY.sw.hi >> 14) * (SLONG)coef->uwVdcRatioPu >> 14); // Q14=Q14+Q14-Q14+Q14-Q14
    swUalpha2Pu = (SWORD)((SLONG)in->swUalphaPu * (SLONG)in->swUalphaPu >> 14);                                                                  // Q14
    swUbeta2Pu = (SWORD)((SLONG)in->swUbetaPu * (SLONG)in->swUbetaPu >> 14);                                                                     // Q14
    slErrPu = (SLONG)swVdc2Pu - (SLONG)swUalpha2Pu - (SLONG)swUbeta2Pu;                                                                        // Q14
    if (slErrPu > 32767)
    {
        slErrPu = 32767;
    }
    else if (slErrPu < -32768)
    {
        slErrPu = -32768;
    }
    else
    {}
    swErrPu = slErrPu;
    swErrDltPu = (SWORD)((SLONG)swErrPu - (SLONG)flx_pvt_swErrZ1Pu);       // Q14
    flx_pvt_swErrZ1Pu = swErrPu;                    // Q14
    slIdpPu = (SLONG)swErrDltPu * (SLONG)coef->uwIdRegKpPu;       // Q30=Q14+Q16
    slIdiPu = (SLONG)swErrPu * (SLONG)coef->uwIdRegKitPu;         // Q30=Q14+Q16
    slIdSumPu = out->slIdSumPu + slIdpPu + slIdiPu; // Q30
    /* Limit Id PI output */
    if (slIdSumPu > coef->slIdMaxPu)
    {
        slIdSumPu = coef->slIdMaxPu; // Q30
    }
    else if (slIdSumPu < coef->slIdPIOutMinPu)
    {
        slIdSumPu = coef->slIdPIOutMinPu; // Q30
    }
    else
    {}
    out->slIdSumPu = slIdSumPu; // Q30

    /* Id limit*/
    if (out->slIdSumPu < out->slIdMinEstPu)
    {
        out->swIdRefPu = out->slIdMinEstPu >> 16; // Q30, Id min Estimate
    }
    else
    {
        out->swIdRefPu = out->slIdSumPu >> 16; // Q14=Q30-Q16
    }

    /* Id PI output less than minimum value */
    if (out->slIdSumPu < out->slIdMinEstPu)
    {
        out->swIqLimPu = mth_slSqrt(((SLONG)coef->uwFwCurLimPu * coef->uwFwCurLimPu) - ((SLONG)out->swIdRefPu * out->swIdRefPu)); // Q14=sqrt(Q28)
//        out->swIqLimPu = (SWORD)mth_slSqrt(((SLONG)coef->uwFwCurLimPu * coef->uwFwCurLimPu) - (out->swIdRefPu * out->swIdRefPu)) +
//                         ((((out->slIdSumPu - out->slIdMinEstPu) >> 16) * coef->uwIqRegKpPu) >> 14); // Q14
//        swUqErr = -in->swUqRefPu + (in->swIqRefPu * coef->uwRsPu >> 15);                             // Q14
//        swUqDeltaErr = swUqErr - flx_pvt_swUqErrZ1;
//        flx_pvt_swUqErrZ1 = swUqErr;                                     // Q14
//        slIdMinPPu = swUqDeltaErr * coef->uwKpPu;                        // Q14+16=30
//        slIdMinIPu = swUqErr * coef->slKitPu >> 9;                       // Q14+25-9=30
//        out->slIdMinEstPu = out->slIdMinEstPu + slIdMinPPu + slIdMinIPu; // Q30
//
//        if (out->slIdMinEstPu < coef->slIdMinPu)
//        {
//            out->slIdMinEstPu = coef->slIdMinPu;
//        }
//        else if (out->slIdMinEstPu > coef->slIdMinLimPu)
//        {
//            out->slIdMinEstPu = coef->slIdMinLimPu;
//        }
//        else
//        {}
    }
    else if (out->slIdSumPu >= coef->slIdMaxPu)
    {
        out->swIqLimPu = coef->swIqLimMaxPu;
    }
    else
    {
        out->swIqLimPu = mth_slSqrt(((SLONG)coef->uwFwCurLimPu * coef->uwFwCurLimPu) - ((SLONG)out->swIdRefPu * out->swIdRefPu)); // Q14=sqrt(Q28)
    }

    /* Iq limit */
    if (out->swIqLimPu < 0)
    {
        out->swIqLimPu = 0; // Q14
    }
}

/************************************************************************
Local Functions: N/A
************************************************************************/

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