/************************************************************************
 Project:             Welling Motor Control Paltform
 Filename:            acr.c
 Partner Filename:    acr.h
 Description:         Automatic current regulator
 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):
WLBDM_M4_SR_20180831-new FSM1.1, by mz, create this file;
************************************************************************/

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

/************************************************************************
 Included File:
*************************************************************************/
#include "acr.h"

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

/************************************************************************
 Exported Functions:
*************************************************************************/
/***************************************************************
 Function: acr_voCurPICoef;
 Description: Coefficient calculation for "acr_voCurPI"
 Call by: functions in main loop;
 Input Variables: ACR_CURPI_COFIN
 Output/Return Variables: ACR_CURPI_COF
 Subroutine Call: N/A;
 Reference: N/A
****************************************************************/
void acr_voCurPICoef(ACR_CURPI_COFIN *in, ACR_CURPI_COF *out)
{
    // in->uwLPu(Q10);				out->uwKpPu(Q12)
    // in->uwRsPu(Q15);				out->uwKitPu(Q15)
    // in->uwTCurCtrPu(Q15);                     out->uwRaPu(Q15)
    // in->uwWicPu(Q10)
    // in->uwRaCoefPu(Q0)

    UWORD uwTiPu;
    ULONG ulWebRadps;
    UWORD uwRbOm;
    UWORD uwLbHm;
    UWORD uwLPu;
    UWORD uwRsPu;
    UWORD uwTCurCtrPu;
    UWORD uwWicPu;

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

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

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

    if (in->uwRaCoef > 10)
    {
        in->uwRaCoef = 10;
    }
    else if (in->uwRaCoef <= 0)
    {
        in->uwRaCoef = 0;
    }

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

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

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

    ulWebRadps = (ULONG)2 * 31416 * in->uwFbHz / 1000;           /* unit: 0.1rad/s, Electrical radian frequency base */
    uwRbOm = (ULONG)in->uwUbVt * 100000 / in->uwIbAp;            /* unit: 0.1mOhm, Resistance base */
    uwLbHm = (ULONG)uwRbOm * 100000 / ulWebRadps;                /* unit: 0.01uH, Inductance base */
    uwLPu = ((ULONG)in->uwLHm << 10) / uwLbHm;                   /* Q10, D/Q axis inductance */
    uwRsPu = ((ULONG)in->uwMtRsOh << 15) / uwRbOm;               /* Q15, Phase resistance */
    uwTCurCtrPu = (ULONG)2 * 102944 * in->uwFbHz / in->uwFTbcHz; /* Q15, Current control period Pu, pi(Q15)=102944 */
    uwWicPu = ((ULONG)in->uwWicHz << 10) /
              in->uwFbHz; // BW_HZ2PU // Q10, Current loop frequency bandwidth             //BW_HZ2PU // Q10, Current loop frequency bandwidth

    // Ti = L / R
    // Kp = Wic * L
    // Kit = (Kra+1) * Kp * T_cnt_ctrl / Ti
    out->uwRaPu = in->uwRaCoef * uwRsPu;       // Q0 + Q15 = Q15
    out->uwKpPu = (ULONG)uwWicPu * uwLPu >> 8; // Q10 + Q10 - Q8 = Q12
    uwTiPu = ((ULONG)uwLPu << 11) /
             uwRsPu; // Q10 + Q11 - Q15 = Q6
    out->uwKitPu = ((ULONG)(in->uwRaCoef + 1) * out->uwKpPu * uwTCurCtrPu >> 6) / uwTiPu;   //Q12 + Q15 - Q6 - Q6 = Q15
   //out->uwKitPu = (out->uwKpPu * uwTCurCtrPu >> 6)/ uwTiPu; // Q12 + Q15 - Q6 - Q6 = Q15
}

/***************************************************************
 Function: acr_voCurPI;
 Description: current PI regulator;
 Call by: functions in TBC;
 Input Variables: ACR_CURPI_IN,ACR_CURPI_COF
 Output/Return Variables: ACR_CURPI_OUT
 Subroutine Call: N/A;
 Reference: N/A
****************************************************************/
void acr_voCurPI(ACR_CURPI_IN *in, ACR_CURPI_COF *coef, ACR_CURPI_OUT *out)
{
    // in->swCurRefPu(Q14);    cof->uwKpPu(Q12);   	out->slURefPu(Q26)
    // in->swCurFdbPu(Q14);    cof->uwKitPu(Q15);  	out->swURefPu(Q14)
    // in->swULimPu(Q12);                          	out->swErrZ1Pu(Q14)
    SLONG slErrPu;
    SLONG slDeltaErrPu; // Q14
    // SLONG slUiPreLimPu;
    SLONG  slUpPu, slUiPu;     // Q29
    SLONG  slUmaxPu, slUminPu; // Q29
    SQWORD sqURefPu;
    SQWORD sqUpPu;

    slUmaxPu = (SLONG)in->swUmaxPu << 15; // Q14+Q15=Q29
    slUminPu = (SLONG)in->swUminPu << 15; // Q14+Q15=Q29

    /*U(k) = U(k-1) + Kp * (Err(k) - Err(k-1)) + Kit * Err(k)*/
    slErrPu = in->swCurRefPu - in->swCurFdbPu; // Q14
    if (slErrPu > 32767)
    {
        slErrPu = 32767;
    }
    else if (slErrPu < -32768)
    {
        slErrPu = -32768;
    }
    else
    {
        /* Nothing */
    }
    slDeltaErrPu = slErrPu - out->swErrZ1Pu; // Q14
    if (slDeltaErrPu > 32767)
    {
        slDeltaErrPu = 32767;
    }
    else if (slDeltaErrPu < -32768)
    {
        slDeltaErrPu = -32768;
    }
    else
    {
        /* Nothing */
    }

    slUpPu = slDeltaErrPu * coef->uwKpPu; // Q14+Q12=Q26
    sqUpPu = (SQWORD)slUpPu << 3;

    slUiPu = slErrPu * coef->uwKitPu; // Q14+Q15=Q29

    sqURefPu = sqUpPu + (SQWORD)slUiPu + (SQWORD)out->slURefPu; // Q29
    if (sqURefPu >= slUmaxPu)
    {
        out->slURefPu = slUmaxPu;
        out->slURefRemPu = 1;
    }
    else if (sqURefPu <= slUminPu)
    {
        out->slURefPu = slUminPu;
        out->slURefRemPu = 1;
    }
    else
    {
        out->slURefPu = sqURefPu;
    }
    out->swURefPu = out->slURefPu >> 15; // Q29-Q15=Q14
    out->swErrZ1Pu = (SWORD)slErrPu;
}
/***************************************************************
 Function: acr_voCurPIInit;
 Description: Current PI initialization
 Call by: functions in main loop;
 Input Variables: N/A
 Output/Return Variables: N/A
 Subroutine Call: N/A;
 Reference: N/A
****************************************************************/
void acr_voCurPIInit(void)
{
    acr_stCurIdPIOut.slURefPu = 0;
    acr_stCurIdPIOut.swURefPu = 0;
    acr_stCurIdPIOut.swErrZ1Pu = 0;
    acr_stCurIdPIOut.slURefRemPu = 0;

    acr_stCurIqPIOut.slURefPu = 0;
    acr_stCurIqPIOut.swURefPu = 0;
    acr_stCurIqPIOut.swErrZ1Pu = 0;
    acr_stCurIqPIOut.slURefRemPu = 0;
}

/***************************************************************
 Function: acr_voUdqDcp;
 Description: Feedforward decoupled, Calculate Output Voltage
 Call by: functions in TBC;
 Input Variables: ACR_UDQDCP_IN
 Output/Return Variables: ACR_UDQDCP_OUT
 Subroutine Call: N/A;
 Reference: N/A
****************************************************************/
void acr_voUdqDcp(ACR_UDQDCP_IN *in, ACR_UDQDCP_COF *coef, ACR_UDQDCP_OUT *out)
{
    // in->swIdRefPu(Q14);    	cof->uwLdPu(Q10);   	out->swUdPu(Q14)
    // in->swIqRefPu(Q14);   	cof->uwLqPu(Q10);  		out->swUqPu(Q14)
    // in->swWsPu(Q15);          cof->uwFluxFPu(Q12);

    SLONG slUdDcp; //(Q12)
    SLONG slUqDcp; //(Q12)

    // UFFd = - ws * Lq * Iq_ref , max 6;min -6,related with Lq
    slUdDcp = -((SLONG)in->swWsPu * coef->uwLqPu) >> 15; // Q15+Q10-Q15=Q10
    slUdDcp = ((SLONG)slUdDcp * in->swIqRefPu) >> 10;    // Q10+Q14-Q10=Q14;
    if (slUdDcp > 32767)
    {
        out->swUdPu = 32767;
    }
    else if (slUdDcp < -32767)
    {
        out->swUdPu = -32767;
    }
    else
    {
        out->swUdPu = slUdDcp;
    }

    // UFFq = ws * Ld * Id_ref + ws * FluxF,max 2 ;min -2,related with Ld
    slUqDcp = (((SLONG)coef->uwLdPu * in->swIdRefPu) >> 12) + coef->uwFluxFPu; // Q10+Q14-Q12=Q12
    slUqDcp = ((SLONG)in->swWsPu * slUqDcp) >> 13;                             // Q15+Q12-Q13=Q14
    if (slUqDcp > 32767)
    {
        out->swUqPu = 32767;
    }
    else if (slUqDcp < -32767)
    {
        out->swUqPu = -32767;
    }
    else
    {
        out->swUqPu = slUqDcp;
    }

    if (out->swUqPu > in->swUdqLimPu) // Q14
    {
        out->swUqPu = in->swUdqLimPu;
    }
    else if (out->swUqPu < (-in->swUdqLimPu))
    {
        out->swUqPu = -in->swUdqLimPu;
    }
    else
    {}

    if (out->swUdPu > in->swUdqLimPu)
    {
        out->swUdPu = in->swUdqLimPu;
    }
    else if (out->swUdPu < (-in->swUdqLimPu))
    {
        out->swUdPu = -in->swUdqLimPu;
    }
    else
    {}
}

/***************************************************************
 Function: acr_voUdqDcpCoef;
 Description: Coefficient calculation for "acr_voUdqDcp"
 Call by: functions in main loop;
 Input Variables: ACR_UDQDCP_COFIN
 Output/Return Variables: ACR_UDQDCP_COF
 Subroutine Call: N/A;
 Reference: N/A
****************************************************************/
void acr_voUdqDcpCoef(ACR_UDQDCP_COFIN *in, ACR_UDQDCP_COF *out)
{
    ULONG ulWebRadps;
    UWORD uwFluxbWb;
    UWORD uwRbOm;
    UWORD uwLbHm;

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

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

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

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

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

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

    ulWebRadps = (ULONG)2 * 31416 * in->uwFbHz / 1000;         /* unit: 0.1rad/s, Electrical radian frequency base */
    uwFluxbWb = (ULONG)in->uwUbVt * 1000000 / ulWebRadps;      /* unit: 0.001mWb, Flux linkage base */
    out->uwFluxFPu = ((ULONG)in->uwMtFlxWb << 12) / uwFluxbWb; /* unit: 0.001mWb, Q12, Flux linkage */
    uwRbOm = (ULONG)in->uwUbVt * 100000 / in->uwIbAp;          /* unit: 0.1mOhm, Resistance base */
    uwLbHm = (ULONG)uwRbOm * 100000 / ulWebRadps;              /* unit: 0.01uH, Inductance base */
    out->uwLdPu = ((ULONG)in->uwLdHm << 10) / uwLbHm;          /* Q10, D axis inductance */
    out->uwLqPu = ((ULONG)in->uwLqHm << 10) / uwLbHm;          /* Q10, Q axis inductance */

}

/***************************************************************
 Function: acr_voUdqDcpInit;
 Description: Feedforward decoupled initialization
 Call by: functions in main loop;
 Input Variables: N/A
 Output/Return Variables: N/A
 Subroutine Call: N/A;
 Reference: N/A
****************************************************************/
void acr_voUdqDcpInit(void)
{
    acr_stUdqDcpOut.swUdPu = 0;
    acr_stUdqDcpOut.swUqPu = 0;
}

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

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