/************************************************************************
 Project:             Welling Motor Control Paltform
 Filename:            torqobs.c
 Partner Filename:    torqobs.h
 Description:         Automatic current regulator
 Complier:            IAR Embedded Workbench for ARM 8.40, IAR Systems.
 CPU TYPE :           STM32F30x
*************************************************************************
 Copyright (c) 2022 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 _TORQOBS_C_
#define _TORQOBS_C_
#endif

/************************************************************************
 Included File:
*************************************************************************/
#include "torqobs.h"
/************************************************************************
 Constant Table:
*************************************************************************/

/*************************************************************************
 extern Parameter
**************************************************************************/
TORQOBS_COEFIN  torqobs_stCoefIn;
TORQOBS_COEFOUT torqobs_stCoef;
TORQOBS_IN      torqobs_stCalIn;
TORQOBS_OUT     torqobs_stCalOut;
/************************************************************************
 Exported Functions:
*************************************************************************/
/***************************************************************
 Function: ;
 Description:
 Call by: functions
 Input Variables:
 Output/Return Variables:
 Subroutine Call: N/A;
 Reference: N/A
****************************************************************/
void torqobs_voInit(void)
{
    torqobs_stCalOut.slSpdObsPu = 0;
    torqobs_stCalOut.slIqLoadPu = 0;
    torqobs_stCalOut.swErrZ1Pu = 0;
    torqobs_stCalOut.swIqLoadPu = 0;
    torqobs_stCalOut.swSpdObsPu = 0;
    torqobs_stCalOut.swSpdObsPu = 0;
    torqobs_stCalOut.swTorqObsPu = 0;
}
/***************************************************************
 Function: ;
 Description: 
 Call by: functions 
 Input Variables: 
 Output/Return Variables: 
 Subroutine Call: N/A;
 Reference: N/A
****************************************************************/
void torqobs_voCoef(TORQOBS_COEFIN *in, TORQOBS_COEFOUT*out)
{
    UWORD uwWebRadps;
    UWORD uwPbWt;
    UWORD uwWmb;
    UWORD uwTbUs;
    UWORD uwTbNm;
    UWORD uwJb;
    UWORD uwJmPu;
    UWORD uwFluxbWb;
    UWORD uwFluxPu;
    UWORD uwWtcPu;
    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->uwWtcHz > 50)
    {
        in->uwWtcHz = 50;
    }


    uwWebRadps = (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 = uwWebRadps / in->uwPairs;                        /* unit: 0.1rad/s, Mechanical radian frequency base */
    uwTbUs = (ULONG)100000000 / uwWebRadps;                  /* 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) / uwWebRadps;  /* unit: 0.001mWb, 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 = uwJmPu;
    ulJmPu = (ULONG)(in->uwRatioJm + 1) * uwJmPu; // Q0
    
    /* Kp = Wtc * Jm /FluxF  */
    uwWtcPu = (((ULONG)in->uwWtcHz << 15) / in->uwFbHz); // Q15  BW_HZ2PU(x)
    out->uwKpPu = (ulJmPu * uwWtcPu << 3) / uwFluxPu;                           // Q6
     /* Kit = Kp * Wtc * T_spd_ctrl */
    out->uwKitPu = (((ULONG)uwWtcPu * uwTCtrPu) >> 18) * out->uwKpPu * 4; // Q15
    
    if (out->uwKitPu < 1)
    {
        out->uwKitPu = 1;
    }
    out->ulJmPu_Inv = ((ULONG) 1 << 12) / ulJmPu; // Q12
    out->uwTCtrPu = uwTCtrPu;      // Q12
    out->uwCur2TorqKPu = uwFluxPu; // Q12
}

/***************************************************************
 Function: ;
 Description:
 Call by: functions
 Input Variables:
 Output/Return Variables:
 Subroutine Call: N/A;
 Reference: N/A
****************************************************************/
void torqobs_voCal(TORQOBS_IN *in, TORQOBS_COEFOUT *cof, TORQOBS_OUT *out)
{
    SLONG  slIqMaxPu;  // Q30
    SLONG  slIqMinPu;  // Q30
    SLONG  slSpdErrPu; // Q15
    SLONG  slDeltaErrPu;
    SQWORD sqIqLoadPu;
    SQWORD sqIqpPu;
    SQWORD sqIqiPu;
    SLONG  slDeltaTorqPu;
    SLONG slSpdObsPu;

    /* Coefficient get */
    slIqMaxPu = (SLONG)in->swIqMaxPu << 16; // Q14+Q16=Q30
    slIqMinPu = (SLONG)in->swIqMinPu << 16; // Q14+Q16=Q30
 
    /* Calculate the error */
    slSpdErrPu = (SLONG)in->swSpdPu - out->swSpdObsPu; // Q15

    if (slSpdErrPu > 32767)
    {
        slSpdErrPu = 32767;
    }
    else if (slSpdErrPu < -32767)
    {
        slSpdErrPu = -32767;
    }
    else
    {
        /* Nothing */
    }

    /* Calculate the delta error */
    slDeltaErrPu = slSpdErrPu - out->swErrZ1Pu;  // Q15

    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

//    if (sqIqiPu > 32768)
//    {
//        sqIqiPu = 32768;
//    }
//    else if (sqIqiPu < -32768)
//    {
//        sqIqiPu = -32768;
//    }
//    else
//    {}

    /* Calculate the Controller output */
    sqIqLoadPu = sqIqpPu + sqIqiPu + (SQWORD)out->slIqLoadPu; // Q30

    /* Iq output limit */
    if (sqIqLoadPu > slIqMaxPu)
    {
        out->slIqLoadPu = slIqMaxPu;
    }
    else if (sqIqLoadPu < slIqMinPu)
    {
        out->slIqLoadPu = slIqMinPu;
    }
    else
    {
        out->slIqLoadPu = (SLONG)sqIqLoadPu;
    }
    out->swIqLoadPu = out->slIqLoadPu >> 16; // Q30-Q16=Q14
    out->swErrZ1Pu = (SWORD)slSpdErrPu;

    out->swTorqObsPu = (SWORD)-(SWORD)((SLONG)out->swIqLoadPu * cof->uwCur2TorqKPu >> 12); // Q14+Q12-Q12=Q14

    slDeltaTorqPu = (SLONG)cof->uwCur2TorqKPu * (in->swIqfbkPu + out->swIqLoadPu) >> 12;           // Q14
    slSpdObsPu = out->slSpdObsPu + ((SLONG)slDeltaTorqPu * cof->uwTCtrPu >> 9) * cof->ulJmPu_Inv; // Q29
    if(slSpdObsPu >= 531502202L)
    {
        out->slSpdObsPu = 531502202L;
    }
    else if(slSpdObsPu <= -531502202L)
    {
        out->slSpdObsPu = -531502202L;
    }
    else
    {
        out->slSpdObsPu = slSpdObsPu;
    }
    
    out->swSpdObsPu = out->slSpdObsPu >> 14; // Q15
    out->slDeltaTorqPu = slDeltaTorqPu;
}
/*************************************************************************
 Local Functions (N/A)
*************************************************************************/

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