/************************************************************************
 Project:             Welling Motor Control Paltform
 Filename:            asr.c
 Partner Filename:    asr.h
 Description:         Automatic speed 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):

************************************************************************/

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

#include "asr.h"

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

/************************************************************************
 Exported Functions:
*************************************************************************/
/***************************************************************
 Function: asr_voSpdPIInit;
 Description: Speed 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 asr_voSpdPIInit(void)
{
    asr_stSpdPIOut.slIqiPu = 0;
    asr_stSpdPIOut.slIqSumPu = 0;
    asr_stSpdPIOut.slIqRefPu = 0;
    asr_stSpdPIOut.swIqRefPu = 0;
    asr_stSpdPIOut.swErrZ1Pu = 0;
}

/***************************************************************
 Function: asr_voSpdPICoef;
 Description: Coefficient calculation for "asr_voSpdPICoef"
 Call by: functions in main loop;
 Input Variables: ASR_SPDPI_COFIN
 Output/Return Variables: ASR_SPDPI_COF
 Subroutine Call: N/A;
 Reference: N/A
****************************************************************/
void asr_voSpdPICoef(ASR_SPDPI_COFIN *in, ASR_SPDPI_COF *out)
{
    ULONG ulWebRadps;
    UWORD uwPbWt;
    UWORD uwWmb;
    UWORD uwTbUs;
    UWORD uwTbNm;
    UWORD uwJb;
    UWORD uwJmPu;
    UWORD uwFluxbWb;
    UWORD uwFluxPu;
    UWORD uwWvcPu;
    UWORD uwTCtrPu;
    ULONG ulJmPu;

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

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

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

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

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

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

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

    if (in->uwRatioJm > 10)
    {
        in->uwRatioJm = 10;
    }

    if (in->uwWvcHz > 30)
    {
        in->uwWvcHz = 30;
    }

    if (in->uwMcoef > 20)
    {
        in->uwMcoef = 20;
    }

    ulWebRadps = (ULONG)2 * 31416 * in->uwFbHz / 1000;       /* unit: 0.1rad/s, Electrical radian frequency base */
    uwPbWt = (ULONG)3 * in->uwUbVt * in->uwIbAp / 100 >> 1;  /* unit: 0.1w, Power base */
    uwWmb = ulWebRadps / in->uwPairs;                        /* unit: 0.1rad/s, Mechanical radian frequency base */
    uwTbUs = (ULONG)100000000 / ulWebRadps;                  /* unit: 0.1us, Time base */
    uwTbNm = (ULONG)uwPbWt * 1000 / uwWmb;                   /* unit: mNm, Torque base */
    uwJb = ((ULONG)uwTbNm * uwTbUs * 10) / uwWmb;            /* unit: 10^-10*kg*m2, Rotational inertia base */
    uwJmPu = ((ULONG)in->uwMtJm * 1000) / uwJb;              /* Q0, Rotational inertia */
    uwFluxbWb = ((ULONG)in->uwUbVt * 1000000) / ulWebRadps;  /* unit: 0.01mWb, Flux linkage base */
    uwFluxPu = ((ULONG)in->uwMtFlxWb << 12) / uwFluxbWb;     /* Q12, Flux linkage */
    uwTCtrPu = (ULONG)2 * 12868 * in->uwFbHz / in->uwFTbsHz; /* Q12, Speed control period Pu, pi(Q12)=12868 */

    /* Jm */
    ulJmPu = (ULONG)(in->uwRatioJm + 1) * uwJmPu; // Q0

    /* Kp = Wvc*Jm/FluxF */
    uwWvcPu = (((ULONG)in->uwWvcHz << 15) / in->uwFbHz); // Q15  BW_HZ2PU(x)
    out->uwKpPu = (ulJmPu * uwWvcPu << 3) / uwFluxPu;    // Q0+Q15+Q3-Q12=Q6
    // out->uwKpPu = (ulJmPu * uwWvcPu ) / uwFluxPu;
    /* Kit = Kp*T_spd_ctrl*Wvc/M */
    out->uwKitPu = ((((ULONG)uwWvcPu * uwTCtrPu) >> 18) * out->uwKpPu) / in->uwMcoef; // Q15+Q12-Q18+Q6=Q15
    if (out->uwKitPu < 1)
    {
        out->uwKitPu = 1;
    }

    /* Kb = 1/Kp */
    out->uwKbPu = ((ULONG)1 << 15) / out->uwKpPu; // Q15-Q6=Q9
//	 out->uwKbPu = 5 * uwTCtrPu / uwJmPu;    
}

/***************************************************************
 Function: asr_voSpdPI;
 Description: speed PI regulator;
 Call by: functions in TBS;
 Input Variables: ASR_SPDPI_IN,SPD_SPDPI_COF
 Output/Return Variables: ASR_SPDPI_OUT
 Subroutine Call: N/A;
 Reference: N/A
****************************************************************/
void  asr_voSpdPI(ASR_SPDPI_IN *in, ASR_SPDPI_COF *cof, ASR_SPDPI_OUT *out)
{
    SLONG  slIqMaxPu;  // Q30
    SLONG  slIqMinPu;  // Q30
    SLONG  slSpdErrPu; // Q15
    SLONG  slDeltaErrPu;
    SQWORD sqIqRefPu;
    SQWORD sqIqpPu;
    SQWORD sqIqiPu;

    /* Coefficient get */
    slIqMaxPu = (SLONG)in->swIqMaxPu << 16; // Q14+Q16=Q30
    slIqMinPu = (SLONG)in->swIqMinPu << 16; // Q14+Q16=Q30

    /* Calculate the error */
    slSpdErrPu = (SLONG)in->swSpdRefPu - in->swSpdFdbPu; // Q15
    
    if (slSpdErrPu > 32767)
    {
        slSpdErrPu = 32767;
    }
    else if (slSpdErrPu < -32767)
    {
        slSpdErrPu = -32767;
    }
    else
    {
        /* Nothing */
    }

    /* Calculate the delta error */
    slDeltaErrPu = slSpdErrPu - out->swErrZ1Pu;
    if (slDeltaErrPu > 32767)
    {
        slDeltaErrPu = 32767;
    }
    else if (slDeltaErrPu < -32768)
    {
        slDeltaErrPu = -32768;
    }
    else
    {
        /* Nothing */
    }

    sqIqpPu = ((SQWORD)slDeltaErrPu * cof->uwKpPu) << 9; // Q30
    /* Calculate the integral output */
    sqIqiPu = (SQWORD)slSpdErrPu * cof->uwKitPu; // Q30
    /* Calculate the Controller output */
    sqIqRefPu = sqIqpPu + sqIqiPu + (SQWORD)out->slIqRefPu; // Q30

    /* Iq output limit */
    if (sqIqRefPu > slIqMaxPu)
    {
        out->slIqRefPu = slIqMaxPu;
    }
    else if (sqIqRefPu < slIqMinPu)
    {
        out->slIqRefPu = slIqMinPu;
    }
    else
    {
        out->slIqRefPu = (SLONG)sqIqRefPu;
    }

    out->swIqRefPu = out->slIqRefPu >> 16; // Q30-Q16=Q14
    out->swErrZ1Pu = (SWORD)slSpdErrPu;
}
/************************************************************************
 Copyright (c) 2018 Welling Motor Technology(Shanghai) Co. Ltd.
 All rights reserved.
*************************************************************************/
#ifdef _ASR_C_
#undef _ASR_C_
#endif

/*************************************************************************
 End of this File (EOF)��
 Do not put anything after this part!
*************************************************************************/