/************************************************************************
 Project:             Welling Motor Control Paltform
 Filename:            obs.c
 Partner Filename:    obs.h
 Description:         The position and speed observer for PMSM
 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 _OBS_C_
#define _OBS_C_
#endif

/************************************************************************
 Included File:
*************************************************************************/
#include "syspar.h"
#include "user.h"

/************************************************************************
 Private Variables
*************************************************************************/
SWORD obs_pvt_swEMFAlphaCompPu = 0; // Q14, Output stationary alfa-axis EMF value
SWORD obs_pvt_swEMFBetaCompPu = 0;  // Q14, Output stationary beta-axis EMF value
SLONG obs_pvt_slDeltaErrPu = 0;     // Q15, Last delta value for PLL PI control

/************************************************************************
 Constant Table (N/A)
*************************************************************************/

/************************************************************************
 Exported Functions:
*************************************************************************/
/***************************************************************
 Function: obs_voObsInit;
 Description: observer output initialization
 Call by:
 Input Variables: N/A
 Output/Return Variables: N/A
 Subroutine Call: N/A;
 Reference: N/A
****************************************************************/
void obs_voObsInit(void)
{
    obs_stObsOutPu.slFluxAlphaPu = 0;  // Q26
    obs_stObsOutPu.slFluxBetaPu = 0;   // Q26
    obs_stObsOutPu.swElecFreqPu = 0;   // Q15
    obs_stObsOutPu.slElecFreqPu = 0;   // Q29
    obs_stObsOutPu.uwElecThetaPu = 0;  // Q15
    obs_stObsOutPu.slElecThetaPu = 0;  // Q29
    obs_stObsOutPu.swActiveFluxPu = 0; // Q10
    obs_stObsOutPu.swFluxAlphaPu = 0;  // Q10
    obs_stObsOutPu.swFluxBetaPu = 0;   // Q10

    obs_stFluxAlphaPIOut.slURefPu = 0;  // Q28
    obs_stFluxAlphaPIOut.swURefPu = 0;  // Q14
    obs_stFluxAlphaPIOut.swErrZ1Pu = 0; // Q10

    obs_stFluxBetaPIOut.slURefPu = 0;  // Q28
    obs_stFluxBetaPIOut.swURefPu = 0;  // Q14
    obs_stFluxBetaPIOut.swErrZ1Pu = 0; // Q10

    obs_pvt_swEMFAlphaCompPu = 0;
    obs_pvt_swEMFBetaCompPu = 0;
    obs_pvt_slDeltaErrPu = 0;
}

/***************************************************************
 Function: obs_voObsCoef;
 Description: Coefficient calculation for "asr_voSpdPI"
 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 obs_voObsCoef(OBS_COF_IN *in, OBS_COF *out)
{
    UWORD uwWebRadps;
    UWORD uwMvcPu;
    ULONG ulDamper;
    UWORD uwDamper;
    ULONG ulKp;
    ULONG ulKi;
    UWORD uwDampingRatio;
    UWORD uwCrossFreq;

    if (in->uwRbOm < 10)
    {
        in->uwRbOm = 10;
    }

    if (in->uwLbHm < 10)
    {
        in->uwLbHm = 10;
    }

    if (in->uwFluxbWb < 10)
    {
        in->uwFluxbWb = 10;
    }

    if (in->uwFbHz < 10)
    {
        in->uwFbHz = 10;
    }
    else if (in->uwFbHz > 10000)
    {
        in->uwFbHz = 10000;
    }

    if (in->uwFreqTbcHz < 10)
    {
        in->uwFreqTbcHz = 10;
    }

    if (in->uwSpdPllMcoef > 100)
    {
        in->uwSpdPllMcoef = 100;
    }

    if ((in->uwFluxWb >> 5) > in->uwFluxbWb)
    {
        in->uwFluxWb = (in->uwFluxbWb << 5);
    }

    if (in->uwRsOm > in->uwRbOm)
    {
        in->uwRsOm = in->uwRbOm;
    }

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

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

    if (in->uwFbHz > ((ULONG)in->uwFreqTbcHz * 0x2AAA >> 15))
    {
        in->uwFbHz = (ULONG)in->uwFreqTbcHz * 0x2AAA >> 15;
    }

    uwWebRadps = (ULONG)2 * 31416 * in->uwFbHz / 10000; // unit: rad/s, Electrical radian frequency base

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

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

    out->uwFluxPPu = ((ULONG)in->uwFluxWb << 10) / in->uwFluxbWb;    // Q10
    out->uwRsPu = ((ULONG)in->uwRsOm << 15) / in->uwRbOm;            // Q15
    out->uwLqPu = ((ULONG)in->uwLqHm << 10) / in->uwLbHm;            // Q10
    out->uwLdPu = ((ULONG)in->uwLdHm << 10) / in->uwLbHm;            // Q10
    out->uwCurTsPu = ((ULONG)205887 * in->uwFbHz) / in->uwFreqTbcHz; // Q15, Q15(2pi)-->205887
    out->uwCurTs = ((ULONG)in->uwFbHz << 10) / in->uwFreqTbcHz;      // Q10, TBC time

    /************************Speed PLL Coefficient*****************************/
    uwMvcPu = ((ULONG)in->uwSpdPllWvcHz << 10) / in->uwFbHz; // Q10

    /* PLL Kp=M*w/sqrt(1+M^2) */
    ulDamper = (1 + in->uwSpdPllMcoef * in->uwSpdPllMcoef) << 8;              // Q8
    uwDamper = mth_slSqrt(ulDamper);                                          // Q4
    out->uwSpdPllKpPu = ((ULONG)in->uwSpdPllMcoef * uwMvcPu / uwDamper) << 8; // Q10-Q4+Q8=Q14

    /* PLL Ki=w^2*T_cnt_ctrl/sqrt(1+M^2) */
    out->uwSpdPllKiPu = ((((ULONG)uwMvcPu * out->uwCurTsPu) / uwDamper) * uwMvcPu) >> 17; // Q10+Q15-Q4+Q10-Q17=Q14

    /**************************Flux PI Coefficient*****************************/
    /* Flux PI regulator Kp=2*k*w */
    uwDampingRatio = ((ULONG)in->uwFluxDampingRatio << 12) / 10;           // Q12
    uwCrossFreq = ((ULONG)2 * 31416 * in->uwFluxCrossFreqHz / 10000) << 8; // Q8

    ulKp = (ULONG)uwDampingRatio * uwCrossFreq * 2; // Q12+Q8=Q20
    out->uwFluxKpPu = ulKp / uwWebRadps >> 2;       // Q20-Q2=Q18

    /* Flux PI regulator Ki=w^2 */
    ulKi = (ULONG)uwCrossFreq * uwCrossFreq;                                   // Q8+Q8=Q16
    out->uwFluxKitPu = (ulKi / uwWebRadps * out->uwCurTsPu / uwWebRadps) >> 3; // Q16+Q15-Q3=Q28
}

/***************************************************************
 Function: obs_voObsCalc
 Description: speed and position caculate
 Call by: tbc_it
 Input Variables:
 Output/Return Variables:
 Subroutine Call:
 Reference: N/A
****************************************************************/
void obs_voObsCalc(OBS_IN *in, OBS_COF *coef, OBS_OUT *out)
{
    SWORD  swEMFAlphaPu, swEMFBetaPu;
    SWORD  swRtFluxAlphaPu, swRtFluxBetaPu;
    SINCOS sincosTmp; //, sincosTmp1;
    SLONG  slDeltaTmpPu, slDeltaErrTmpPu;
    SLONG  slKpTmpPu, slKitTmpPu;
    SLONG  slSpdRefPu;
    SLONG  slPreRtPosPu, slActRtPosPu;
    UWORD  uwPreRtPosPu;
    SWORD  swFluxDPu, swFluxQPu;
    SWORD  swMaxVsPu;

    /*****************************Current Model*****************************/
    /* Predict the rotor Angle according to last rotor angle and speed */
    slPreRtPosPu = out->slElecThetaPu + ((out->slElecFreqPu >> 10) * coef->uwCurTs); // Q29-Q10+Q10=Q29
    if (slPreRtPosPu >= 0x20000000)                                                  // Q29
    {
        slPreRtPosPu -= 0x20000000;
    }
    else if (slPreRtPosPu < 0)
    {
        slPreRtPosPu += 0x20000000;
    }
    uwPreRtPosPu = (UWORD)(slPreRtPosPu >> 14); // Q29-Q14=Q15
    mth_voSinCos(uwPreRtPosPu, &sincosTmp);

    /* Current Park (I_alpha I_beta -> Id Iq) */
    obs_stParkInPu.swAlphaPu = in->swIalphaPu; // Q14
    obs_stParkInPu.swBetaPu = in->swIbetaPu;   // Q14
    obs_stParkInPu.uwThetaPu = uwPreRtPosPu;   // Q15
    crd_voPark(&obs_stParkInPu, &obs_stFluxParkPu);

    /* Flux_D = Ld*id+Flux_PM; Flux_Q = Lq*iq */
    swFluxDPu = coef->uwFluxPPu + (((SLONG)obs_stFluxParkPu.swDPu * coef->uwLdPu) >> 14); // Q14+Q10-Q14=Q10
    swFluxQPu = ((SLONG)obs_stFluxParkPu.swQPu * coef->uwLqPu) >> 14;                     // Q14+Q10-Q14=Q10

    /* Flux IPark (Fluxd Fluxq -> Fluxalpha Fluxbeta) */
    obs_stIParkInPu.swDPu = swFluxDPu;        // Q10
    obs_stIParkInPu.swQPu = swFluxQPu;        // Q10
    obs_stIParkInPu.uwThetaPu = uwPreRtPosPu; // Q15
    crd_voIPark(&obs_stIParkInPu, &obs_stFluxIParkPu);

    /*****************************Voltage Model*****************************/
    swEMFAlphaPu = in->swUalphaPu - (in->swIalphaPu * coef->uwRsPu >> 15); // Q14+Q15-Q15=Q14
    swEMFBetaPu = in->swUbetaPu - (in->swIbetaPu * coef->uwRsPu >> 15);    // Q14+Q15-Q15=Q14

    obs_pvt_swEMFAlphaCompPu = swEMFAlphaPu + obs_stFluxAlphaPIOut.swURefPu; // Q14
    obs_pvt_swEMFBetaCompPu = swEMFBetaPu + obs_stFluxBetaPIOut.swURefPu;    // Q14

    out->slFluxAlphaPu = out->slFluxAlphaPu + ((SLONG)obs_pvt_swEMFAlphaCompPu * coef->uwCurTsPu >> 3); // Q14+Q15-Q3=Q26
    out->slFluxBetaPu = out->slFluxBetaPu + ((SLONG)obs_pvt_swEMFBetaCompPu * coef->uwCurTsPu >> 3);    // Q14+Q15-Q3=Q26
    out->swFluxAlphaPu = out->slFluxAlphaPu >> 16;                                                      // Q26-Q16=Q10
    out->swFluxBetaPu = out->slFluxBetaPu >> 16;                                                        // Q26-Q16=Q10

    swMaxVsPu = ((SLONG)in->uwVdcPu * 0x5555) >> 15; // Q14+Q15-Q15=Q14

    /* Flux alpha PI regulator */
    obs_stFluxAlphaPIIn.swRefPu = obs_stFluxIParkPu.swAlphaPu; // Q10
    obs_stFluxAlphaPIIn.swFdbPu = out->swFluxAlphaPu;          // Q10
    obs_stFluxAlphaPIIn.swUmaxPu = swMaxVsPu;                  // Q14
    obs_stFluxAlphaPIIn.swUminPu = -swMaxVsPu;                 // Q14
    obs_stFluxAlphaPICoef.uwFluxKpPu = coef->uwFluxKpPu;       // Q18
    obs_stFluxAlphaPICoef.uwFluxKitPu = coef->uwFluxKitPu;     // Q28
    obs_voFluxPI(&obs_stFluxAlphaPIIn, &obs_stFluxAlphaPICoef, &obs_stFluxAlphaPIOut);

    /* Flux beta PI regulator */
    obs_stFluxBetaPIIn.swRefPu = obs_stFluxIParkPu.swBetaPu; // Q10
    obs_stFluxBetaPIIn.swFdbPu = out->swFluxBetaPu;          // Q10
    obs_stFluxBetaPIIn.swUmaxPu = swMaxVsPu;                 // Q14
    obs_stFluxBetaPIIn.swUminPu = -swMaxVsPu;                // Q14
    obs_stFluxBetaPICoef.uwFluxKpPu = coef->uwFluxKpPu;      // Q18
    obs_stFluxBetaPICoef.uwFluxKitPu = coef->uwFluxKitPu;    // Q28
    obs_voFluxPI(&obs_stFluxBetaPIIn, &obs_stFluxBetaPICoef, &obs_stFluxBetaPIOut);

    /* Stator Flux -> Rotor Flux */
    swRtFluxAlphaPu = out->swFluxAlphaPu - (((SLONG)in->swIalphaPu * coef->uwLqPu) >> 14); // Q14+Q10-Q14=Q10
    swRtFluxBetaPu = out->swFluxBetaPu - (((SLONG)in->swIbetaPu * coef->uwLqPu) >> 14);    // Q14+Q10-Q14=Q10

    /* PLL to trace the latest speed */
    slDeltaTmpPu = (((SLONG)swRtFluxBetaPu * sincosTmp.swCosPu) - ((SLONG)swRtFluxAlphaPu * sincosTmp.swSinPu)) >> 10; // Q10+Q15-Q10=Q15
    slDeltaErrTmpPu = slDeltaTmpPu - obs_pvt_slDeltaErrPu;                                                             // Q15
    obs_pvt_slDeltaErrPu = slDeltaTmpPu;                                                                               // Q15
    slKpTmpPu = slDeltaErrTmpPu * coef->uwSpdPllKpPu;                                                                  // Q15+Q14=Q29
    slKitTmpPu = slDeltaTmpPu * coef->uwSpdPllKiPu;                                                                    // Q15+Q14=Q29
    slSpdRefPu = slKpTmpPu + slKitTmpPu + out->slElecFreqPu;                                                           // Q29

    /* Limit the speed value to avoid overflow */
    if (slSpdRefPu >= 0x20000000)
    {
        slSpdRefPu = 0x20000000 - 1; // Q29
    }
    else if (slSpdRefPu <= -0x20000000)
    {
        slSpdRefPu = -0x20000000; // Q29
    }
    out->slElecFreqPu = slSpdRefPu;              // Q29
    out->swElecFreqPu = out->slElecFreqPu >> 14; // Q29-Q14=Q15
    /* Electrical speed to Electrical position */
    // slActRtPosPu = out->slElecThetaPu + ((((slSpdRefPu + out->slElecFreqPu) >> 1) >> 10) * coef->uwCurTs); 			//Q29+Q10-Q10=Q29
    slActRtPosPu = out->slElecThetaPu + (((SLONG)out->swElecFreqPu * coef->uwCurTs) << 4);
    if (slActRtPosPu >= 0x20000000)
    {
        slActRtPosPu -= 0x20000000; // Q15+Q14=Q29
    }
    else if (slActRtPosPu < 0)
    {
        slActRtPosPu += 0x20000000; // Q15+Q14=Q29
    }
    out->slElecThetaPu = slActRtPosPu;             // Q29
    out->uwElecThetaPu = out->slElecThetaPu >> 14; // Q29-Q14=Q15

    // mth_voSinCos(out->uwElecThetaPu, &sincosTmp1);
    // slActiveFluxPu = (SLONG)swRtFluxAlphaPu * sincosTmp1.swCosPu + (SLONG)swRtFluxBetaPu * sincosTmp1.swSinPu; 		//Q10+Q15=Q25
    // out->swActiveFluxPu = slActiveFluxPu >> 15; 																	    //Q25-Q15=Q10
    // //Q29-Q14=Q15
}
/***************************************************************
 Function: obs_voFluxPI;
 Description: Flux 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 obs_voFluxPI(OBS_FLUXPI_IN *in, OBS_FLUXPI_COF *coef, OBS_FLUXPI_OUT *out)
{
    SWORD tmp_swErrPu, tmp_swDeltaErrPu;
    SLONG tmp_slUpPu, tmp_slUiPu, tmp_slURefPu;
    SLONG tmp_slUmaxPu, tmp_slUminPu;

    /* U(k) = U(k-1) + Kp * (Err(k) - Err(k-1)) + Kit * Err(k) */
    tmp_swErrPu = in->swRefPu - in->swFdbPu;                     // Q10
    tmp_swDeltaErrPu = tmp_swErrPu - out->swErrZ1Pu;             // Q10
    tmp_slUpPu = (SLONG)tmp_swDeltaErrPu * coef->uwFluxKpPu;     // Q10+Q18=Q28
    tmp_slUiPu = ((SLONG)tmp_swErrPu * coef->uwFluxKitPu) >> 10; // Q10+Q28-Q10=Q28
    tmp_slURefPu = out->slURefPu + tmp_slUpPu + tmp_slUiPu;      // Q28
    tmp_slUmaxPu = (SLONG)in->swUmaxPu << 14;                    // Q14+Q14=Q28
    tmp_slUminPu = (SLONG)in->swUminPu << 14;                    // Q14+Q14=Q28
    if (tmp_slURefPu > tmp_slUmaxPu)
    {
        tmp_slURefPu = tmp_slUmaxPu;
    }
    else if (tmp_slURefPu < tmp_slUminPu)
    {
        tmp_slURefPu = tmp_slUminPu;
    }
    else
    {}
    out->swURefPu = tmp_slURefPu >> 14; // Q28-Q14=Q14
    out->slURefPu = tmp_slURefPu;       // Q28
    out->swErrZ1Pu = tmp_swErrPu;       // Q10
}

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