motor_ctl-1/2.MotorDrive/Source/spdctrmode.c

/************************************************************************
 Project:             Welling Motor Control Paltform
 Filename:            spdctrmode.c
 Partner Filename:    spdctrmode.h
 Description:         Speed control mode
 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 _SPDCTRMODE_C_
#define _SPDCTRMODE_C_
#endif

/************************************************************************
 Included File
*************************************************************************/
#include "syspar.h"
#include "user.h"
#include "bikeinformation.h"
#include "FuncLayerAPI.h"
#include "AssistCurve.h"
#include "cmdgennew.h"
#include "CodePara.h"
#include "CadAssist.h"
//#include "api.h"
#include "TimeTask_Event.h"
#include "emcdeal.h"
#include "LoadObsTheta.h"
#include "obs.h"
#include "FSM_1st.h"
#include "cmdgennew.h"
/************************************************************************
 Constant Table (N/A)
*************************************************************************/

/*************************************************************************
 Exported Functions (N/A)
*************************************************************************/
/***************************************************************
 Function: scm_voSpdCtrMdInit;
 Description: Speed control mode initializing function
 Call by: rmd_voModeSchd();
 Input Variables: N/A
 Output/Return Variables: N/A
 Subroutine Call: ...;
 Reference: N/A
****************************************************************/
void scm_voSpdCtrMdInit(void)
{
    //    /* PWM init */
    //    hw_voPWMInit();
    /*cmd handle Initial */
    cmd_voCmdInit();
    /* Current PI init */
    acr_voCurPIInit();
    /* Current decoupling init */
    acr_voUdqDcpInit();
    /* Sensorless observer init */
//    obs_voObsInit();
    /* SPI position sensor init */
    //spi_voResolverInit();
    /* Speed PI init */
    asr_voSpdPIInit();
//    asrnew_voSpdPIInit();
    torqobs_voInit();
//    LoadObsTheta_voInit();
    /* Flux weakening init */
    #if(FLUX_MODE == 0)
        spdflx_voInit();
    #elif(FLUX_MODE == 1)
        flx_voInit();
    #else
        //Config Error
    #endif
    //  fw_voInit();
    /* Power Limit init */
    pwr_voPwrLimInit();
    /* SVPWM init */
    pwm_voInit();
    /* Dead time init */
    dbc_voDBCompInit();
    /* Contant voltage brake init */
    cvb_voBrakeInit();
    /* switchHall init */
    switchhall_voInit();
    /* Align pos startup open2clz clzloop init */
    align_voInit();

    scm_stSpdFbkLpf.slY.sw.hi = 0;
    scm_swSpdRefPu = 0;
    scm_swUalphaPu = 0;             // Q14
    scm_swUbetaPu = 0;              // Q14
    scm_stIdFbkLpf.slY.sl = 0;      // Id feedback LPF
    scm_stIqFbkLpf.slY.sl = 0;      // Iq feedback LPF
    scm_stIqRefforDesat.slY.sl = 0; // Iq Ref for desaturation Lpf
    scm_stIqFbkforDesat.slY.sl = 0; // Iq Fbk for desaturation Lpf
    scm_swIdRefPu = 0;              // Q14
    scm_swIqRefPu = 0;              // Q14
    scm_uwAngRefPu = 0;             // Q15
    scm_uwAngParkPu = 0;            // Q15
    scm_uwAngIParkPu = 0;           // Q15
    scm_swRotateDir = -1;           // Direction of motor rotate
    scm_ulStatCt = 0;               // Status hold time count
    scm_uwAngManuPu = 0;            // Q15, Angle given manually
    scm_slAngManuPu = 0;
    scm_slDragSpdPu = 0;           // Q15, Drag speed
    scm_slDragSpdRefPu = 0;        // Q29, intermediate Drag speed
    scm_blCurSwitchOvrFlg = FALSE; // Current switch over flag
    scm_blAngSwitchOvrFlg = FALSE; // Angle switch over flag
    scm_uwAngSwitchK = 0;          // Angle switch weight value
    scm_swMotorPwrInWt = 0;        // unit: w, Input power of motor
    scm_blCoefUpdateFlg = FALSE;   // Coefficient update flag
    scm_stIqLoadLpf.slY.sl = 0;
    scm_stSpdFbkLpf.slY.sl = 0;    // Speed feedback LPF
    scm_uwSpdFbkLpfAbsPu = 0;      // Q15, Speed feedback LPF absolute
    scm_swMotorPwrInPu = 0;        // Q15, Input power of motor
    scm_swSystemPwrInPu = 0;       // Q15, Input power of System
    scm_stMotoPwrInLpf.slY.sl = 0; // Input power of motor after LPF
    scm_stPCBTempLpf.slY.sl = 0;
    scm_stMotorTempLpf.slY.sl = 0;
    scm_swMotorPwrInLpfWt = 0; // unit: 0.1w, Input power of motor after LPF
    scm_uwMotorPwrInAvgPu = 0; // Q15, Input power of motor after average filter

    scm_swIdFdbLpfPu = 0;
    scm_swIqFdbLpfPu = 0;

    scm_swUdRefPu = 0;
    scm_swUqRefPu = 0;
    scm_swUalphaFbkPu = 0;
    scm_swUbetaFbkPu = 0;
    scm_swUalphaRefPu = 0;
    scm_swUbetaRefPu = 0;
    scm_swUalphaCompPu = 0;
    scm_swUbetaCompPu = 0;

    scm_uwHfiAngZ1Pu = 0;
    scm_slAngSumPu = 0;
    scm_slAngErrPu = 0;
    scm_blAngSumOvrFlg = FALSE;
    scm_uwRunMdSw = 1;
    scm_ulRunMdSwCt = 0;
    scm_ulCloseCt = 0;
    scm_uwStartMd = cp_stControlPara.swStartMode;
    scm_uwStartMdSw = scm_uwStartMd;
    scm_uwInitPosMd = cp_stControlPara.swInitPosMode;
    scm_uwInitPosMdSw = scm_uwInitPosMd;
    scm_uwHfiOvrCnt = 0;
    scm_slIdRefPu = 0;
}
/***************************************************************
 Function: scm_voSpdCtrMdCoef;
 Description: Speed control mode TBS scheduler
 Call by: tbs_voIsr();
 Input Variables: N/A
 Output/Return Variables: N/A
 Subroutine Call: ...;
 Reference: N/A
****************************************************************/
void scm_voSpdCtrMdCoef(void)
{
    ULONG ulLpfTm; // unit: us
    UWORD uwLqPu = 0;
    ULONG ulAccel100rpmpsPu = USER_MOTOR_240RPMPS2PU_Q29;

    if (abs(scm_swIqRefPu) < mn_swIqTurn1Pu)
    {
        scm_uwLqPu = cof_uwLqPu;
    }
    else
    {
        uwLqPu = mn_slLqTurn1Pu + ((SLONG)(abs(scm_swIqRefPu) - mn_swIqTurn1Pu) * mn_swKLqSat >> 10); // Q10
        if (uwLqPu < cof_uwLqMinPu)
        {
            scm_uwLqPu = cof_uwLqMinPu;
        }
        else if (uwLqPu > cof_uwLqPu)
        {
            scm_uwLqPu = cof_uwLqPu;
        }
        else
        {
            scm_uwLqPu = uwLqPu;
        }
    }
    /* Sensorless observer coefficient calculate */
//    obs_stObsCoefIn.uwRbOm = cof_uwRbOm;                                        // Real Value, unit: 0.01Ohm, Resistance base
//    obs_stObsCoefIn.uwLbHm = cof_uwLbHm;                                        // Real Value, unit: 0.01mH, Inductance base
//    obs_stObsCoefIn.uwFluxbWb = cof_uwFluxbWb;                                  // Real Value, unit: 0.01mWb, Flux linkage base
//    obs_stObsCoefIn.uwFbHz = cof_uwFbHz;                                        // Real Value, Unit:Hz  frequency base
//    obs_stObsCoefIn.uwRsOm = cp_stMotorPara.swRsOhm;                            // Real Value, unit: 0.01Ohm, Resistance base
//    obs_stObsCoefIn.uwLqHm = ((ULONG)scm_uwLqPu * cof_uwLbHm) >> 10;            // Real Value, unit: 0.01mH, q Inductance
//    obs_stObsCoefIn.uwLdHm = cp_stMotorPara.uwLdmH;                             // Real Value, unit: 0.01mH, d Inductance
//    obs_stObsCoefIn.uwFluxWb = cp_stMotorPara.swFluxWb;                         // Real Value, unit: 0.01mWb, Flux linkage
//    obs_stObsCoefIn.uwFreqTbcHz = FTBC_HZ;                                      // Real Value, Unit:Hz  Tbc
//    obs_stObsCoefIn.uwFluxDampingRatio = cp_stControlPara.swObsFluxPIDampratio; // Real Value, unit:0.1
//    obs_stObsCoefIn.uwFluxCrossFreqHz = cp_stControlPara.swObsFluxPICrossfreHz; // Real Value, unit:Hz
//    obs_stObsCoefIn.uwSpdPllWvcHz = cp_stControlPara.swObsSpdPLLBandWidthHz;    // Real Value, Unit:Hz
//    obs_stObsCoefIn.uwSpdPllMcoef = cp_stControlPara.swObsSpdPLLM;
//    obs_voObsCoef(&obs_stObsCoefIn, &obs_stObsCoefPu);
    /* Speed PI coefficient calculate */
    asr_stSpdPICoefIn.uwUbVt = VBASE;
    asr_stSpdPICoefIn.uwIbAp = IBASE;
    asr_stSpdPICoefIn.uwFbHz = FBASE;
    asr_stSpdPICoefIn.uwFTbsHz = FTBS_HZ;
    asr_stSpdPICoefIn.uwPairs = cp_stMotorPara.swMotrPolePairs;
    asr_stSpdPICoefIn.uwMtJm = cp_stMotorPara.swJD;
    asr_stSpdPICoefIn.uwMtFlxWb = cp_stMotorPara.swFluxWb;
    asr_stSpdPICoefIn.uwMcoef = cp_stControlPara.swAsrPIM;
    asr_stSpdPICoefIn.uwWvcHz = cp_stControlPara.swAsrPIBandwidth;
    asr_stSpdPICoefIn.uwRatioJm = cp_stControlPara.swAsrSpdInerRate;
    asr_voSpdPICoef(&asr_stSpdPICoefIn, &asr_stSpdPICoef);

    /* 拖拽参数初始化 */
    align_voCoef();

    /* Reduced Order Torque Observe coefficient calculate */
    torqobs_stCoefIn.uwUbVt = VBASE;
    torqobs_stCoefIn.uwIbAp = IBASE;
    torqobs_stCoefIn.uwFbHz = FBASE;
    torqobs_stCoefIn.uwFTbsHz = FTBS_HZ;
    torqobs_stCoefIn.uwPairs = cp_stMotorPara.swMotrPolePairs;
    torqobs_stCoefIn.uwMtJm = cp_stMotorPara.swJD << 2; // cp_stMotorPara.swJD;
    torqobs_stCoefIn.uwMtFlxWb = cp_stMotorPara.swFluxWb;
    torqobs_stCoefIn.uwWtcHz = 50; // cp_stControlPara.swAsrPIBandwidth;
    torqobs_stCoefIn.uwRatioJm = cp_stControlPara.swAsrSpdInerRate;
    torqobs_voCoef(&torqobs_stCoefIn, &torqobs_stCoef);
    mth_voLPFilterCoef(1000000 / 30, FTBS_HZ, &scm_stIqLoadLpf.uwKx); // 50Hz

    /* Full Order Torque Observer coefficient calculate */
//    LoadObsTheta_stCoefIn.uwFbHz = FBASE;
//    LoadObsTheta_stCoefIn.uwFluxbWb = cof_uwFluxbWb;
//    LoadObsTheta_stCoefIn.uwFluxWb = cp_stMotorPara.swFluxWb;
//    LoadObsTheta_stCoefIn.uwFTbcHz = FTBC_HZ;
//    LoadObsTheta_stCoefIn.uwJb = cof_uwJb;
//    LoadObsTheta_stCoefIn.uwMtJm = cp_stMotorPara.swJD << 2;
//    LoadObsTheta_stCoefIn.uwWtcHz = 200;
//    LoadObsTheta_stCoefIn.uwMCoef = 100;
//    LoadObsTheta_voCoef();

    /* Id PI coefficient calculate */
    acr_stCurIdPICoefIn.uwFbHz = FBASE;
    acr_stCurIdPICoefIn.uwUbVt = VBASE;
    acr_stCurIdPICoefIn.uwIbAp = IBASE;
    acr_stCurIdPICoefIn.uwLHm = cp_stMotorPara.uwLdmH;
    acr_stCurIdPICoefIn.uwMtRsOh = cp_stMotorPara.swRsOhm;
    acr_stCurIdPICoefIn.uwFTbcHz = FTBC_HZ;
    acr_stCurIdPICoefIn.uwRaCoef = cp_stControlPara.swAcrRaCoef;     // Coefficient of Active Resistance
    acr_stCurIdPICoefIn.uwWicHz = cp_stControlPara.swAcrPIBandwidth; // Current loop frequency bandwidth
    acr_voCurPICoef(&acr_stCurIdPICoefIn, &acr_stCurIdPICoef);
    /* Iq PI coefficient calculate */
    acr_stCurIqPICoefIn.uwFbHz = FBASE;
    acr_stCurIqPICoefIn.uwUbVt = VBASE;
    acr_stCurIqPICoefIn.uwIbAp = IBASE;
    acr_stCurIqPICoefIn.uwLHm = cp_stMotorPara.uwLqmH;
    acr_stCurIqPICoefIn.uwMtRsOh = cp_stMotorPara.swRsOhm;
    acr_stCurIqPICoefIn.uwFTbcHz = FTBC_HZ;
    acr_stCurIqPICoefIn.uwRaCoef = cp_stControlPara.swAcrRaCoef;
    acr_stCurIqPICoefIn.uwWicHz = cp_stControlPara.swAcrPIBandwidth;
    acr_voCurPICoef(&acr_stCurIqPICoefIn, &acr_stCurIqPICoef);
    /* Current decoupling coefficient calculate */
    acr_stUdqDcpCoefIn.uwLdHm = cp_stMotorPara.uwLdmH;
    acr_stUdqDcpCoefIn.uwLqHm = cp_stMotorPara.uwLqmH;
    acr_stUdqDcpCoefIn.uwMtFlxWb = cp_stMotorPara.swFluxWb;
    acr_stUdqDcpCoefIn.uwUbVt = VBASE;
    acr_stUdqDcpCoefIn.uwFbHz = FBASE;
    acr_stUdqDcpCoefIn.uwIbAp = IBASE;
    acr_voUdqDcpCoef(&acr_stUdqDcpCoefIn, &acr_stUdqDcpCoef);

    /* Id feedback low pass filter coef */
    ulLpfTm = 1000000 / cp_stControlPara.swAcrCurFbLpfFre;
    mth_voLPFilterCoef(ulLpfTm, FTBC_HZ, &scm_stIdFbkLpf.uwKx);
    /* Iq feedback low pass filter coef */
    ulLpfTm = 1000000 / cp_stControlPara.swAcrCurFbLpfFre;
    mth_voLPFilterCoef(ulLpfTm, FTBC_HZ, &scm_stIqFbkLpf.uwKx);
    
    ulLpfTm = 1000000 / 100;   //1000hz
    mth_voLPFilterCoef(ulLpfTm, FTBC_HZ, &scm_stIqRefforDesat.uwKx);
    
    ulLpfTm = 1000000 / 100;   //1000hz
    mth_voLPFilterCoef(ulLpfTm, FTBC_HZ, &scm_stIqFbkforDesat.uwKx);

    /* Coefficient update only once */
    if (!scm_blCoefUpdateFlg)
    {
        /* Deadband compensation coefficient calculate */
        dbc_stDbCompCoefIn.uwDeadBandTimeNs = cp_stControlPara.swIPMDeadTimeNs; // unit: ns, Dead band time
        dbc_stDbCompCoefIn.uwPosSwOnTimeNs = cp_stControlPara.swIPMTurnOnNs;    // unit: ns, IPM switch-on time at positive current
        dbc_stDbCompCoefIn.uwPosSwOffTimeNs = cp_stControlPara.swIPMTurnOnNs;   // unit: ns, IPM switch-off time at positive current
        dbc_stDbCompCoefIn.uwNegSwOnTimeNs = cp_stControlPara.swIPMTurnOnNs;    // unit: ns, IPM switch-on time at negative current
        dbc_stDbCompCoefIn.uwNegSwOffTimeNs = cp_stControlPara.swIPMTurnOnNs;   // unit: ns, IPM switch-off time at negative current
        dbc_stDbCompCoefIn.ulPWMPerUs = PWM_PERIOD_US;                          // unit: 0.1us, PWM period
        dbc_stDbCompCoefIn.uwKcoefVtPerAp = cp_stControlPara.swDbcK;            // Q6, Deadband compensation slope coefficient
        dbc_stDbCompCoefIn.uwVBaseVt = VBASE;                                   // Q0, Vbase
        dbc_stDbCompCoefIn.uwIBaseAp = IBASE;                                   // Q0, Ibase
        dbc_voDBCompCoef(&dbc_stDbCompCoefIn, &dbc_stDbCompCoef);

        /* Flux weakening coefficient calculate */
        #if(FLUX_MODE == 0)
            //spdflx_stCtrlCoefIn.swRSpeedPu = (((SLONG)cp_stMotorPara.swRSpeedRpm * (SLONG)cp_stMotorPara.swMotrPolePairs / 60)/ FBASE)*2^15; 化简为如下，避免溢出
	        spdflx_stCtrlCoefIn.swRSpeedPu = (SWORD)((8192L)*(SLONG)cp_stMotorPara.swRSpeedRpm * (SLONG)cp_stMotorPara.swMotrPolePairs / (15 * FBASE));
	        spdflx_stCtrlCoefIn.swIdMinPu = (SWORD)((16384L)*(SLONG)cp_stMotorPara.swIdMinA / IBASE);
            spdflx_stCtrlCoefIn.swIpeakMaxPu = (SWORD)((16384L)*(SLONG)cp_stMotorPara.swIpeakMaxA / IBASE);
            spdflx_voCoef(&spdflx_stCtrlCoefIn,  &spdflx_stCtrlCoef);
        #elif(FLUX_MODE == 1)
            flx_stCtrlCoefIn.swIdMaxAp = (SWORD)cp_stMotorPara.swIdMaxA;                  // Q0,unit: 0.01A
            flx_stCtrlCoefIn.swIdMinAp = (SWORD)cp_stMotorPara.swIdMinA;                  // Q0,unit: 0.01A
            flx_stCtrlCoefIn.uwRsOhm = cp_stMotorPara.swRsOhm;                            // Q0,unit: 0.1mOhm
            flx_stCtrlCoefIn.swIdPIOutMinAp = (SWORD)cp_stControlPara.swFwIdPIOutMin;     // Q0,unit: 0.01A
            flx_stCtrlCoefIn.uwCharCurCrossFreqHz = cp_stControlPara.swFwCharCurCrossFre; // Q0,unit: SQRT(1/2piR)
            flx_stCtrlCoefIn.uwCharCurDampRatio = cp_stControlPara.swFwCharCurDampRatio;  // Q0,unit: SQRT(pi/2R)
            flx_stCtrlCoefIn.uwIdRegKpPu = cp_stControlPara.swFwIdKpPu;                   // Q16,unit: A/V2
            flx_stCtrlCoefIn.uwIdRegKiPu = cp_stControlPara.swFwIdKiPu;                   // Q16,unit: A/V2
            flx_stCtrlCoefIn.uwPWMDutyMax = cp_stControlPara.swFwPWMMaxDuty;              // Q0,%
            flx_stCtrlCoefIn.uwVdcLpfFreqHz = cp_stControlPara.swFwVdcLPFFre;             // Q0,unit: Hz
            flx_stCtrlCoefIn.uwVdcMinCalcTmMs = cp_stControlPara.swFwVdcMinCalTMms;       // Q0,unit: ms
            flx_stCtrlCoefIn.uwFwCurLimAp = cp_stMotorPara.swIpeakMaxA;                   // Q0,unit: 0.01A
            flx_stCtrlCoefIn.uwIdMinLimRatio = cp_stControlPara.swFwIdMinLimRatio;        // Q0,0.01
            flx_stCtrlCoefIn.uwUbVt = VBASE;                                              // Q0,unit: 0.1V, Voltage base
            flx_stCtrlCoefIn.uwFreqTbcHz = FTBC_HZ;                                       // Q0
            flx_stCtrlCoefIn.uwIBaseAp = IBASE;                                           // Q0,unit: 0.01A, Base Current
            flx_stCtrlCoefIn.uwFBaseHz = FBASE;                                           // Q0,unit: Hz, Base Frequency
            flx_voCoef(&flx_stCtrlCoefIn, &flx_stCtrlCoef);

            //        fw_stFluxWeakeningCoefInPu
            //        fw_voFluxWeakeningCoef(fw_stFluxWeakeningCoefInPu,flx_stCtrlCoef)
        #else
            //Config Error
        #endif

        /* Constant vlotage brake coefficient calculate */
        cvb_stBrakeCoefIn.uwVdcCvbVt = cp_stControlPara.swCvbConstantVolBrakeV;
        cvb_stBrakeCoefIn.uwLowSpdRpm = cp_stControlPara.swCvbConstantSpdLowRpm;
        cvb_stBrakeCoefIn.swIqRefMaxAp = cp_stMotorPara.swIpeakMaxA;
        cvb_stBrakeCoefIn.swIdRefMaxAp = cp_stMotorPara.swIdMaxA;
        cvb_stBrakeCoefIn.swIdRefMinAp = cp_stMotorPara.swIdMinA;
        cvb_stBrakeCoefIn.uwVBaseVt = VBASE;
        cvb_stBrakeCoefIn.uwIBaseAp = IBASE;
        cvb_stBrakeCoefIn.uwFBaseHz = FBASE;
        cvb_stBrakeCoefIn.uwMotorPairs = cp_stMotorPara.swMotrPolePairs;
        cvb_voBrakeCoef(&cvb_stBrakeCoefIn, &cvb_stBrakeCoef);

        /* Speed feedback low pass filter coef */
        ulLpfTm = 1000000 / cp_stControlPara.swAsrSpdFbLPFFre;
        mth_voLPFilterCoef(ulLpfTm, FTBC_HZ, &scm_stSpdFbkLpf.uwKx);

        /* Power limit coef */
        ulLpfTm = 1000000 / cp_stControlPara.swPwrLimitLPFFre;
        mth_voLPFilterCoef(ulLpfTm, FTBC_HZ, &scm_stMotoPwrInLpf.uwKx);

        ulLpfTm = 1000000 / 10;
        mth_voLPFilterCoef(ulLpfTm, FTBC_HZ, &scm_stPCBTempLpf.uwKx);

        ulLpfTm = 1000000 / 10;
        mth_voLPFilterCoef(ulLpfTm, FTBC_HZ, &scm_stMotorTempLpf.uwKx);

        // /* Torque Sensor limit coef */
        // ulLpfTm = 1000000 / torsensor_stTorSensorCof.uwTorSensorLPFFrq;
        // mth_voLPFilterCoef(ulLpfTm, FTBC_HZ, &scm_stTorSensorLpf.uwKx);

        // /* Bike Throttle limit coef */
        // ulLpfTm = 1000000 / bikethrottle_stBikeThrottleCof.uwThrottleVolLPFFrq;
        // mth_voLPFilterCoef(ulLpfTm, FTBC_HZ, &scm_stBikeThrottleLpf.uwKx);

        pwr_stPwrLimCofIn.swPwrLimW = cp_stControlPara.swPwrLimitValWtCALC; // Q0, unit: 0.1w, Power limit value
        pwr_stPwrLimCofIn.uwPwrErrW = cp_stControlPara.swPwrLimitErrWt; // Q0, unit: 0.1w, Start power limit when "VAL - ERR"
        pwr_stPwrLimCofIn.swIqMaxAp = cp_stMotorPara.swIpeakMaxA;       // Q0, unit: 0.01A, Max phase current (peak value)
        pwr_stPwrLimCofIn.uwIBaseAp = IBASE;                            // Q0,unit: 0.01A, Base Current
        pwr_stPwrLimCofIn.uwUbVt = VBASE;                               // Q0,unit: 0.1V, Voltage base
        pwr_stPwrLimCofIn.uwPwrLimPIKp = cp_stControlPara.swPwrLimitKpPu;
        pwr_stPwrLimCofIn.uwPwrLimPIKi = cp_stControlPara.swPwrLimitKiPu;
        pwr_stPwrLimCofIn.uwPwrLimSTARTCe = cp_stControlPara.swAlmPwrLimitStartTempVal;
        pwr_stPwrLimCofIn.uwPwrLimENDCe = cp_stControlPara.swAlmOverHeatCeVal;
        pwr_stPwrLimCofIn.uwPwrLimMotTempSTARTCe = cp_stControlPara.swAlmPwrLimitMotorStartTempVal;
        pwr_stPwrLimCofIn.uwPwrLimMotTempENDCe = cp_stControlPara.swAlmMotorOverHeatCeVal;
        pwr_stPwrLimCofIn.uwPwrLimStartBatCap = PWRLIM_START_BATCAP;
        pwr_stPwrLimCofIn.uwPwrLimEndBatCap = PWRLIM_END_BATCAP;
        pwr_voPwrLimCof(&pwr_stPwrLimCofIn, &pwr_stPwrLimCof);

        /*Accelaration&Decelaration limit*/
        if (abs(scm_swSpdRefPu) < USER_MOTOR_300RPM2PU)
        {
            cmd_stCmdCoefIn.ulAccelPu = ulAccel100rpmpsPu; // Q29
        }
        else
        {
            cmd_stCmdCoefIn.ulAccelPu = ulAccel100rpmpsPu; // Q29
        }
        cmd_stCmdCoefIn.ulDecelPu = USER_MOTOR_3000RPMPS2PU_Q29; // Q29
        cmd_stCmdCoefIn.swBrakeSpdDeltaPu = USER_MOTOR_100RPM2PU;
        cmd_voCmdCoef(&cmd_stCmdCoefIn, &cmd_stCmdCoef);

        pwm_stGenCoefIn.uwPWMDutyMax = cp_stControlPara.swPWMMaxDuty;
        pwm_stGenCoefIn.uwPWM7To5Duty = cp_stControlPara.swPWM7to5Duty;
        pwm_stGenCoefIn.uwPWMMinSample1Pu = cp_stControlPara.swPWMMinSampleDuty1;
        pwm_stGenCoefIn.uwPWMMinSample2Pu = cp_stControlPara.swPWMMinSampleDuty2;
        pwm_stGenCoefIn.uwPWMMinSample3Pu = cp_stControlPara.swPWMMinSampleDuty3;
        pwm_stGenCoefIn.uwSampleSteadyPu = cp_stControlPara.swPWMSampleToSteady;
        pwm_stGenCoefIn.uwSingelResisSamplePu = cp_stControlPara.swPWMSampleSigR;
        pwm_stGenCoefIn.uwOvmNo = cp_stControlPara.swPWMOverMdlMode;
        pwm_stGenCoefIn.uwPWMPd = HW_PWM_PERIOD;
        pwm_voGenCoef(&pwm_stGenCoefIn, &pwm_stGenCoef);

        scm_uwAcrLimCof = (UWORD)((ULONG)cp_stControlPara.swPWMMaxDuty * cp_stControlPara.uwAcrCurOutLim / 1000);   // Q15
        scm_uwUdcpLimCof = (UWORD)((ULONG)cp_stControlPara.swPWMMaxDuty * cp_stControlPara.uwAcrUdcpOutLim / 1000); // Q15
    }
}
/***************************************************************
 Function: scm_voSpdCtrMdTbs;
 Description: Speed control mode TBS scheduler
 Call by: tbs_voIsr();
 Input Variables: N/A
 Output/Return Variables: N/A
 Subroutine Call: ...;
 Reference: N/A
****************************************************************/
SWORD testIqref;
void  scm_voSpdCtrMdTbs(void)
{
    SWORD swIqLowerPu;
    /* Speed feedback LPF */

    if (cp_stFlg.ThetaGetModelSelect == ANG_OBSERVER)
    {
//        mth_voLPFilter(obs_stObsOutPu.swElecFreqPu, &scm_stSpdFbkLpf);
    }
    else if (cp_stFlg.ThetaGetModelSelect == ANG_RESOLVER)
    {
      //  mth_voLPFilter(spi_stResolverOut.swSpdFbkPu, &scm_stSpdFbkLpf);
    }
    else if (cp_stFlg.ThetaGetModelSelect == ANG_SWITCHHALL)
    {
        scm_stSpdFbkLpf.slY.sw.hi = switchhall_stOut.swLowSpdLpfPu;
    }
    else
    {}

    /* Speed feedback Absolute */
    scm_uwSpdFbkLpfAbsPu = abs(scm_stSpdFbkLpf.slY.sw.hi);

    /*============================================================
                Speed command generator to generate speed ramp
      =============================================================*/
    if (curSpeed_state.state == ClzLoop || curSpeed_state.state == Open2Clz)
    {
        cmd_stCmdIn.swSpdCmdRpm = uart_slSpdRefRpm;

        cmd_stCmdIn.swSpdNowPu = scm_stSpdFbkLpf.slY.sw.hi;
        cmd_voCmdOut(&cmd_stCmdIn, &cmd_stCmdCoef, &cmd_stCmdOut);
        scm_swRotateDir = cmd_stCmdOut.swNewCmdDir;
        scm_swSpdRefPu = cmd_stCmdOut.swIntRefPu; // cmd_stCmdGenOut.Out.swSpdRefPu;
    }
    else if (curSpeed_state.state == StartUp)
    {
        SWORD tempSpeed = 0;
        tempSpeed = (cp_stControlPara.swDragSpdHz * 60 / cp_stMotorPara.swMotrPolePairs);
        if (cp_stFlg.RunModelSelect == ClZLOOP)
        {
            if (uart_slSpdRefRpm > 0)
            {
                cmd_stCmdIn.swSpdCmdRpm = tempSpeed;
            }
            else
            {
                cmd_stCmdIn.swSpdCmdRpm = -tempSpeed;
            }
        }
        else if (cp_stFlg.RunModelSelect == VFContorl || cp_stFlg.RunModelSelect == IFContorl)
        {

            if (uart_slSpdRefRpm > 0)
            {
                cmd_stCmdIn.swSpdCmdRpm = tempSpeed;
            }
            else
            {
                cmd_stCmdIn.swSpdCmdRpm = -tempSpeed;
            }

            //            if(cp_stFlg.RotateDirectionSelect == ForwardRotate)
            //            {
            //                cmd_stCmdIn.swSpdCmdRpm = tempSpeed;
            //            }
            //            else
            //            {
            //                cmd_stCmdIn.swSpdCmdRpm = -tempSpeed;
            //            }
        }
        else
        {}
        cmd_stCmdIn.swSpdNowPu = scm_stSpdFbkLpf.slY.sw.hi;
        cmd_voCmdOut(&cmd_stCmdIn, &cmd_stCmdCoef, &cmd_stCmdOut);
        scm_swRotateDir = cmd_stCmdOut.swNewCmdDir;
        scm_swSpdRefPu = cmd_stCmdOut.swIntRefPu; // cmd_stCmdGenOut.Out.swSpdRefPu;
    }
    else
    {
        cmd_stCmdIn.swSpdCmdRpm = 0;
        cmd_stCmdIn.swSpdNowPu = scm_stSpdFbkLpf.slY.sw.hi;
        cmd_voCmdOut(&cmd_stCmdIn, &cmd_stCmdCoef, &cmd_stCmdOut);
        scm_swRotateDir = cmd_stCmdOut.swNewCmdDir;
        scm_swSpdRefPu = cmd_stCmdOut.swIntRefPu; // cmd_stCmdGenOut.Out.swSpdRefPu;
    }
    /*=======================================================================
                                 Speed PI Controller
        =======================================================================*/
    asr_stSpdPIIn.swSpdRefPu = scm_swSpdRefPu; // Q15
    // asr_stSpdPIIn.swSpdFdbPu = switchhall_stOut.swLowSpdPu;
    asr_stSpdPIIn.swSpdFdbPu = scm_stSpdFbkLpf.slY.sw.hi; // Q15

    if (curSpeed_state.state != ClzLoop)
    {
        swIqLowerPu = flx_stCtrlOut.swIqLimPu;
    }
    else
    {
        swIqLowerPu = (flx_stCtrlOut.swIqLimPu < abs(pwr_stPwrLimOut2.swIqLimPu)) ? flx_stCtrlOut.swIqLimPu : abs(pwr_stPwrLimOut2.swIqLimPu);
        swIqLowerPu = (swIqLowerPu < abs(cvb_stBrakeOut.swIqLimPu)) ? swIqLowerPu : abs(cvb_stBrakeOut.swIqLimPu);
    }

    if (scm_swRotateDir > 0)
    {
        asr_stSpdPIIn.swIqMaxPu = swIqLowerPu;
        //        asr_stSpdPIIn.swIqMinPu = -swIqLowerPu;
        asr_stSpdPIIn.swIqMinPu = 0;
    }
    else
    {
        asr_stSpdPIIn.swIqMaxPu = 0;
        //        asr_stSpdPIIn.swIqMaxPu = swIqLowerPu;
        asr_stSpdPIIn.swIqMinPu = -swIqLowerPu;
    }
    asr_voSpdPI(&asr_stSpdPIIn, &asr_stSpdPICoef, &asr_stSpdPIOut);

    //    swCurRefrompu = (abs(asr_stSpdPIOut.swIqRefPu) < abs(uart_swTorqRefNm)) ? abs(asr_stSpdPIOut.swIqRefPu) : abs(uart_swTorqRefNm);
    //    swCurRefrompu = -swCurRefrompu;
    swCurRefrompu = asr_stSpdPIOut.swIqRefPu;

    /* New ASR */
    //    asrnew_stSpdPIIn.swSpdRefPu = scm_swSpdRefPu;            // Q15
    //    asrnew_stSpdPIIn.swSpdFdbPu = scm_stSpdFbkLpf.slY.sw.hi; // Q15
    //    if (scm_swRotateDir > 0)
    //    {
    //        asrnew_stSpdPIIn.swIqMaxPu = swIqLowerPu;
    //        asrnew_stSpdPIIn.swIqMinPu = 0;//-swIqLowerPu;
    //    }
    //    else
    //    {
    //        asrnew_stSpdPIIn.swIqMaxPu = swIqLowerPu;
    //        asrnew_stSpdPIIn.swIqMinPu = 0;//-swIqLowerPu;
    //    }
    //    asrnew_voSpdPI(&asrnew_stSpdPIIn, &asrnew_stSpdPICoef, &asrnew_stSpdPIOut);
    //    swCurRefrompu = asrnew_stSpdPIOut.swIqRefPu;

    // swCurRefrompu = testIqref;
    curSpeed_state.Tbs_hook();
}

SWORD   deltC, switchCNT, switchflg;
SWORD   swTmpSpdRate = 0;
LPF_OUT swTmpSpdRateLpf;
SWORD   swTmpSpdFbkPuZ1 = 0;
SLONG   slTmpAcc;
SWORD   swTestIqref;
void    scm_voTorqCtrMdTbs(void)
{
    SWORD swIqLowerPu;
    /* Speed feedback LPF */

    if (cp_stFlg.ThetaGetModelSelect == ANG_OBSERVER)
    {
//        mth_voLPFilter(obs_stObsOutPu.swElecFreqPu, &scm_stSpdFbkLpf);
    }
    else if (cp_stFlg.ThetaGetModelSelect == ANG_RESOLVER)
    {
       // mth_voLPFilter(spi_stResolverOut.swSpdFbkPu, &scm_stSpdFbkLpf);
    }
    else if (cp_stFlg.ThetaGetModelSelect == ANG_SWITCHHALL)
    {
        scm_stSpdFbkLpf.slY.sw.hi = switchhall_stOut.swLowSpdLpfPu;
    }
    else
    {}

    /* Speed feedback Absolute */
    scm_uwSpdFbkLpfAbsPu = abs(scm_stSpdFbkLpf.slY.sw.hi);

    //    /*============================================================
    //              Speed command generator to generate speed ramp
    //    =============================================================*/
    //    cmd_stCmdIn.swSpdCmdRpm = -(((SLONG)cadence_stFreGetOut.uwLPFFrequencyPu * 8000) >> 10) * 6000 >> 15;
    //    cmd_stCmdIn.swSpdNowPu = scm_stSpdFbkLpf.slY.sw.hi;
    //    cmd_voCmdOut(&cmd_stCmdIn, &cmd_stCmdCoef, &cmd_stCmdOut);
    //    /*=======================================================================
    //                                Get speed command
    //      =======================================================================*/
    //    scm_swRotateDir = cmd_stCmdOut.swNewCmdDir;
    //    scm_swSpdRefPu = cmd_stCmdOut.swIntRefPu; //cmd_stCmdGenOut.Out.swSpdRefPu;

    /*=======================================================================
                                 Speed PI Controller
        =======================================================================*/

    swIqLowerPu = (flx_stCtrlOut.swIqLimPu < abs(pwr_stPwrLimOut2.swIqLimPu)) ? flx_stCtrlOut.swIqLimPu : abs(pwr_stPwrLimOut2.swIqLimPu);
    swIqLowerPu = (swIqLowerPu < abs(cvb_stBrakeOut.swIqLimPu)) ? swIqLowerPu : abs(cvb_stBrakeOut.swIqLimPu);

    //    if (uart_swTorqRefNm > swIqLowerPu)
    //    {
    //        uart_swTorqRefNm = swIqLowerPu;
    //    }
    //    else if (uart_swTorqRefNm < -swIqLowerPu)
    //    {
    //        uart_swTorqRefNm = -swIqLowerPu;
    //    }
    //    else
    //    {}
    //    swCurRefrompu = uart_swTorqRefNm;

    /* Torque observe */
    if (scm_swRotateDir > 0)
    {
        torqobs_stCalIn.swIqMaxPu = swIqLowerPu;
        torqobs_stCalIn.swIqMinPu = -swIqLowerPu;
    }
    else
    {
        torqobs_stCalIn.swIqMaxPu = swIqLowerPu;
        torqobs_stCalIn.swIqMinPu = -swIqLowerPu;
    }
    torqobs_stCalIn.swIqfbkPu = scm_swIqFdbLpfPu;
    torqobs_stCalIn.swSpdPu = switchhall_stOut.swLowSpdPu;
    torqobs_voCal(&torqobs_stCalIn, &torqobs_stCoef, &torqobs_stCalOut);
    mth_voLPFilter((torqobs_stCalOut.swIqLoadPu + scm_swIqFdbLpfPu), &scm_stIqLoadLpf);

    //    mth_voLPFilter(LoadObsTheta_Y.swIqCompPu, &scm_stIqLoadLpf);

    ///////test////////
    //    mth_voLPFilterCoef(1000000 / 15, FTBS_HZ, &swTmpSpdRateLpf.uwKx); //30Hz,TBS
    //    mth_voLPFilter(LoadObsTheta_Y.swIqCompPu, &swTmpSpdRateLpf);

    /* Spd Fbk Compensation Calculate */
    //    swTmpSpdRate = switchhall_stOut.swLowSpdPu - swTmpSpdFbkPuZ1; //Q15
    //    mth_voLPFilterCoef(1000000 / 30, FTBS_HZ, &swTmpSpdRateLpf.uwKx); //30Hz,TBS
    //    mth_voLPFilter(swTmpSpdRate, &swTmpSpdRateLpf);
    //    swTmpSpdFbkPuZ1 = switchhall_stOut.swLowSpdPu;
    //    slTmpAcc = (SLONG)swTmpSpdRateLpf.slY.sw.hi * FTBS_HZ / FBASE;  //Q15

    /* Iqref Compensation */
    if (((uart_swTorqRefNm < -200) || (uart_swTorqRefNm > 200)))
    {
        /* Without Comp */
        swTestIqref = uart_swTorqRefNm;

        /* Open Loop */
        // swTestIqref  = uart_swTorqRefNm - (((SLONG)slTmpAcc* cof_uwJmPu  << 11) / cof_uwFluxPu);  //Q15+Q0+Q11-Q12=Q14

        /* Observer */
        //       swTestIqref  = uart_swTorqRefNm - scm_stIqLoadLpf.slY.sw.hi;
        // swTestIqref  = uart_swTorqRefNm - swTmpSpdRateLpf.slY.sw.hi;
    }
    else
    {
        swTestIqref = uart_swTorqRefNm;
    }

    if (swTestIqref > swIqLowerPu)
    {
        swTestIqref = swIqLowerPu;
    }
    else if (swTestIqref < -swIqLowerPu)
    {
        swTestIqref = -swIqLowerPu;
    }
    else
    {}
    swCurRefrompu = swTestIqref;

    curSpeed_state.Tbs_hook();
}

/***************************************************************
 Function: scm_voSpdCtrMdUpTbc;
 Description: Speed control mode TBC scheduler
 Call by: tbc_voIsr();
 Input Variables: N/A
 Output/Return Variables: N/A
 Subroutine Call: ...;
 Reference: N/A
****************************************************************/
CRD_PARK_IN  Test_U_in;
CRD_PARK_OUT Test_U_out;

void scm_voSpdCtrMdUpTbc(void)
{
    /*=======================================================================
                    Max voltage of current PI out
    =======================================================================*/
    scm_swVsLimPu = (SWORD)((ULONG)adc_stUpOut.uwVdcLpfPu * scm_uwAcrLimCof >> 15);            // Q14+Q15-Q15=Q14
    scm_swVsDcpLimPu_Assist = (SWORD)((ULONG)adc_stUpOut.uwVdcLpfPu * scm_uwUdcpLimCof >> 15); // Q14+Q15-Q15=Q14
    scm_swVsDcpLimPu = scm_swVsDcpLimPu_Assist;

    /*=======================================================================
                                 Voltage get
    =======================================================================*/
    /* Get Ualpha & Ubeta from command voltage */
    scm_swUalphaPu = pwm_stGenOut.swUalphaPu - scm_swUalphaCompPu; // Q14
    scm_swUbetaPu = pwm_stGenOut.swUbetaPu - scm_swUbetaCompPu;    // Q14
    /*=======================================================================
                            Startup control FSM
    =======================================================================*/
    scm_voSpdCtrMdFSM();
    curSpeed_state.Tbcup_hook();
}

/***************************************************************
 Function: scm_voSpdCtrMdTbc;
 Description: Speed control mode TBC scheduler
 Call by: tbc_voIsr();
 Input Variables: N/A
 Output/Return Variables: N/A
 Subroutine Call: ...;
 Reference: N/A
****************************************************************/
UWORD DCPswitch = 0;
UWORD testtheta = 0;
SWORD IQqqqqq = 0, SwitchFlggg = 1;
SLONG IQCNT = 0;
UWORD uwIqStopCnt;
SLONG swUqmax,swUqmin;
SLONG swUdmax,swUdmin;
UWORD UqDecCount = 0, UdDecCount = 0;
void  scm_voSpdCtrMdDownTbc(void)
{
    /*=======================================================================
                    Clark transformation for phase current
    =======================================================================*/
    crd_stClarkIn.swAPu = adc_stDownOut.swIaPu; // Q14
    crd_stClarkIn.swBPu = adc_stDownOut.swIbPu; // Q14
    crd_stClarkIn.swCPu = adc_stDownOut.swIcPu; // Q14
    crd_voClark(&crd_stClarkIn, &crd_stCurClarkOut);
    /*=======================================================================
                              Code Of spdFSM
    =======================================================================*/
    curSpeed_state.Tbcdown_hook();
    //    if(SwitchFlggg == 0)
    //   {
    //      IQCNT++;
    //      scm_swIqRefPu = -4000;
    //      if(IQCNT > 2000)
    //      {
    //        IQCNT=0;
    //        SwitchFlggg =1;
    //      }
    //   }
    //   else
    //   {
    //      scm_swIqRefPu = IQqqqqq;
    //   }
    //
    /*=======================================================================
                            Current loop control
    =======================================================================*/
    /* Get Id & Iq for current PI control */
    /*=======================================================================
                        Park transformation for current
    =======================================================================*/
    crd_stParkIn.swAlphaPu = crd_stCurClarkOut.swAlphaPu; // Q14
    crd_stParkIn.swBetaPu = crd_stCurClarkOut.swBetaPu;   // Q14
    crd_stParkIn.uwThetaPu = scm_uwAngParkPu;             // Q15
    crd_voPark(&crd_stParkIn, &crd_stCurParkOut);
    /*=======================================================================
                            Current feedback LPF
    =======================================================================*/
    mth_voLPFilter(crd_stCurParkOut.swDPu, &scm_stIdFbkLpf);
    mth_voLPFilter(crd_stCurParkOut.swQPu, &scm_stIqFbkLpf);
    scm_swIdFdbLpfPu = scm_stIdFbkLpf.slY.sw.hi;
    scm_swIqFdbLpfPu = scm_stIqFbkLpf.slY.sw.hi;
    //    scm_swIdFdbLpfPu = crd_stCurParkOut.swDPu;
    //    scm_swIqFdbLpfPu = crd_stCurParkOut.swQPu;

    /*=======================================================================
                          Calculate input power of motor
    =======================================================================*/
    scm_swSystemPwrInPu =
        ((((SLONG)adc_stUpOut.uwVdcLpfPu * (SLONG)adc_stUpOut.uwIbusAvgLpfPu) >> 13) * (SLONG)683) >> 10; // power = udc*idc; Q14+Q14-Q13=Q15
    scm_swMotorPwrInPu = ((SLONG)Test_U_out.swDPu * scm_swIdFdbLpfPu + (SLONG)Test_U_out.swQPu * scm_swIqFdbLpfPu) >> 13; // Q14+Q14-Q13=Q15
    mth_voLPFilter(scm_swMotorPwrInPu, &scm_stMotoPwrInLpf);
    scm_swMotorPwrInLpfWt = scm_stMotoPwrInLpf.slY.sw.hi * cof_uwPbWt >> 15; // unit: 0.1w

    /*=======================================================================
                            Id current PI control
    =======================================================================*/
    DCPswitch = 0;                              // 0 with forwardFeedBack    1 without forwardFeedBack
    acr_stCurIdPIIn.swCurRefPu = scm_swIdRefPu; // Q14
    acr_stCurIdPIIn.swCurFdbPu = scm_swIdFdbLpfPu;
    if (DCPswitch == 1)
    {
        acr_stCurIdPIIn.swUmaxPu = scm_swVsDcpLimPu;  // Q14
        acr_stCurIdPIIn.swUminPu = -scm_swVsDcpLimPu; // Q14
    }
    else if (DCPswitch == 0)
    {
        if(switch_flg.SysRun_Flag == FALSE)
        {
            if(UdDecCount>=100)          // each 100/8000s decrease to 1000/1024*U
            {
                UdDecCount = 0;
                acr_stCurIdPIIn.swUmaxPu = (SWORD)(((SLONG)swUdmax*1020)>>10);
                acr_stCurIdPIIn.swUminPu = (SWORD)(((SLONG)swUdmin*1020)>>10);
            }
            else
            {
                UdDecCount++;
                acr_stCurIdPIIn.swUmaxPu = swUdmax;  // Q14
                acr_stCurIdPIIn.swUminPu = swUdmin; // Q14
            }
        }
        else
        {
            acr_stCurIdPIIn.swUmaxPu = scm_swVsDcpLimPu - acr_stUdqDcpOut.swUdPu;  // Q14
            acr_stCurIdPIIn.swUminPu = -scm_swVsDcpLimPu - acr_stUdqDcpOut.swUdPu; // Q14
        }
    }
    else
    {}

    swUdmin = acr_stCurIqPIIn.swUminPu;
    swUdmax = acr_stCurIqPIIn.swUmaxPu;

    acr_voCurPI(&acr_stCurIdPIIn, &acr_stCurIdPICoef, &acr_stCurIdPIOut);

    /*=======================================================================
                            Iq current PI control
    =======================================================================*/


    mth_voLPFilter(scm_swIqRefPu, &scm_stIqRefforDesat);
    mth_voLPFilter(scm_swIqFdbLpfPu, &scm_stIqFbkforDesat);
//-------------limit min vault
    if (cp_stFlg.RotateDirectionSelect == ForwardRotate)
        {
         if(acr_stUdqDcpOut.swUqPu<=0)
             acr_stUdqDcpOut.swUqPu=0;
        }
     else if (cp_stFlg.RotateDirectionSelect == BackwardRotate)
    {
         if(acr_stUdqDcpOut.swUqPu>=0)
             acr_stUdqDcpOut.swUqPu=0;
    }
//------------------
    if (DCPswitch == 1)
    {
        acr_stCurIqPIIn.swUmaxPu = scm_swVsDcpLimPu;  // Q14
        acr_stCurIqPIIn.swUminPu = -scm_swVsDcpLimPu; // Q14
    }
    else if (DCPswitch == 0)
    {
        if(event_blCurrentAssFlag == TRUE)
        {
            if (cp_stFlg.RotateDirectionSelect == ForwardRotate)
            {
                if (((SLONG)ass_stCadAssCalOut.swVoltLimitPu - (SLONG)acr_stUdqDcpOut.swUqPu) > 0)
                {
                    acr_stCurIqPIIn.swUmaxPu = ass_stCadAssCalOut.swVoltLimitPu - acr_stUdqDcpOut.swUqPu;  // Q14
                }
                else
                {
                    acr_stCurIqPIIn.swUmaxPu = 0;
                }

                if(ass_stCadAssCalOut.blPreStopFlag == TRUE)
                {
                    acr_stCurIqPIIn.swUminPu = 0;
                }
                else
                {
                    acr_stCurIqPIIn.swUminPu = -scm_swVsDcpLimPu - acr_stUdqDcpOut.swUqPu;
                }
            }
            else if (cp_stFlg.RotateDirectionSelect == BackwardRotate)
            {
                if (((SLONG)ass_stCadAssCalOut.swVoltLimitPu - (SLONG)acr_stUdqDcpOut.swUqPu) < 0)
                {
                    acr_stCurIqPIIn.swUminPu = ass_stCadAssCalOut.swVoltLimitPu - acr_stUdqDcpOut.swUqPu;  // Q14
                }
                else
                {
                    acr_stCurIqPIIn.swUminPu = 0;
                }

                if(ass_stCadAssCalOut.blPreStopFlag == TRUE )
                {
                    acr_stCurIqPIIn.swUmaxPu = 0;
                }
                else
                {
                    acr_stCurIqPIIn.swUmaxPu = scm_swVsDcpLimPu - acr_stUdqDcpOut.swUqPu;
                }
            }
            else
            {
                /* 方向错误 */
            }
        }
        else
        {
            if(switch_flg.SysRun_Flag == FALSE)
            {
                if(UqDecCount>=100)          // each 100/8000s decrease to 1000/1024*U
                {
                    UqDecCount = 0;
                    acr_stCurIqPIIn.swUmaxPu = (SWORD)(((SLONG)swUqmax*1010)>>10);
                    acr_stCurIqPIIn.swUminPu = (SWORD)(((SLONG)swUqmin*1010)>>10);
                }
                else
                {
                    UqDecCount++;
                    acr_stCurIqPIIn.swUmaxPu = swUqmax;  // Q14
                    acr_stCurIqPIIn.swUminPu = swUqmin; // Q14
                }
            }
            else
            {
                acr_stCurIqPIIn.swUmaxPu = scm_swVsDcpLimPu - acr_stUdqDcpOut.swUqPu;  // Q14
                acr_stCurIqPIIn.swUminPu = -scm_swVsDcpLimPu - acr_stUdqDcpOut.swUqPu; // Q14
            }
        }
    }
    else
    {}
    swUqmin = acr_stCurIqPIIn.swUminPu;
    swUqmax = acr_stCurIqPIIn.swUmaxPu;
  
#if 0
    if(0 == scm_swIqRefPu)
    {
        uwIqStopCnt++;
        if(uwIqStopCnt >= 500)
        {
            uwIqStopCnt = 500;
        }
    }
    else
    {
        uwIqStopCnt = 0;
    }
    if((500 == uwIqStopCnt) && (scm_uwSpdFbkLpfAbsPu < 1500))
    {
        if((cp_stFlg.RotateDirectionSelect == BackwardRotate)
                && (scm_swIqFdbLpfPu > 0))
        {
            acr_stCurIqPIIn.swCurRefPu = 0; // Q14
            acr_stCurIqPIIn.swCurFdbPu = 0;
            acr_stCurIqPIOut.swURefPu=0;
            acr_stCurIqPIOut.slURefPu=0;
            scm_swUqRefPu=0;
        }
        else if((cp_stFlg.RotateDirectionSelect == ForwardRotate)
                && (scm_swIqFdbLpfPu < 0))
        {
            acr_stCurIqPIIn.swCurRefPu = 0; // Q14
            acr_stCurIqPIIn.swCurFdbPu = 0;
            acr_stCurIqPIOut.swURefPu=0;
            acr_stCurIqPIOut.slURefPu=0;
            scm_swUqRefPu=0;
        }
        else
        {
            acr_stCurIqPIIn.swCurRefPu = scm_swIqRefPu; // Q14
            acr_stCurIqPIIn.swCurFdbPu = scm_swIqFdbLpfPu;
        }
    }
    else
    {
        acr_stCurIqPIIn.swCurRefPu = scm_swIqRefPu; // Q14
        acr_stCurIqPIIn.swCurFdbPu = scm_swIqFdbLpfPu;
    }
#else
    acr_stCurIqPIIn.swCurRefPu = scm_swIqRefPu; // Q14
    acr_stCurIqPIIn.swCurFdbPu = scm_swIqFdbLpfPu;
#endif
    acr_voCurPI(&acr_stCurIqPIIn, &acr_stCurIqPICoef, &acr_stCurIqPIOut);
    //    if ((DCPswitch == 1) && (scm_uwSpdFbkLpfAbsPu > 30922)) // Q15 2000rpm
    //    {
    //        acr_stCurIdPIOut.slURefPu = acr_stCurIdPIOut.slURefPu - ((SLONG)acr_stUdqDcpOut.swUdPu << 15);
    //        acr_stCurIqPIOut.slURefPu = acr_stCurIqPIOut.slURefPu - ((SLONG)acr_stUdqDcpOut.swUqPu << 15);
    //        acr_stCurIdPIOut.swURefPu = acr_stCurIdPIOut.swURefPu - acr_stUdqDcpOut.swUdPu;
    //        acr_stCurIqPIOut.swURefPu = acr_stCurIqPIOut.swURefPu - acr_stUdqDcpOut.swUqPu;
    //        DCPswitch = 0;
    //    }
    //    if ((DCPswitch == 0) && (scm_uwSpdFbkLpfAbsPu < 8192))
    //    {
    //        acr_stCurIdPIOut.slURefPu = acr_stCurIdPIOut.slURefPu + ((SLONG)acr_stUdqDcpOut.swUdPu << 15);
    //        acr_stCurIqPIOut.slURefPu = acr_stCurIqPIOut.slURefPu + ((SLONG)acr_stUdqDcpOut.swUqPu << 15);
    //        acr_stCurIdPIOut.swURefPu = acr_stCurIdPIOut.swURefPu + acr_stUdqDcpOut.swUdPu;
    //        acr_stCurIqPIOut.swURefPu = acr_stCurIqPIOut.swURefPu + acr_stUdqDcpOut.swUqPu;
    //        DCPswitch = 1;
    //    }

    if (DCPswitch == 1)
    {
        scm_swUqRefPu = acr_stCurIqPIOut.swURefPu; // Q14
        scm_swUdRefPu = acr_stCurIdPIOut.swURefPu; // Q14
    }
    else if (DCPswitch == 0)
    {
        scm_swUqRefPu = acr_stCurIqPIOut.swURefPu + acr_stUdqDcpOut.swUqPu; // Q14
        scm_swUdRefPu = acr_stCurIdPIOut.swURefPu + acr_stUdqDcpOut.swUdPu; // Q14
    }
    else
    {}
    
    if(event_blCurrentAssFlag == TRUE)
    {
        if (cp_stFlg.RotateDirectionSelect == ForwardRotate)
        {
            if (scm_swUqRefPu > ass_stCadAssCalOut.swVoltLimitPu)
            {
                scm_swUqRefPu = ass_stCadAssCalOut.swVoltLimitPu; // Q14=Q14
            }
        }
        else if (cp_stFlg.RotateDirectionSelect == BackwardRotate)
        {
            if (scm_swUqRefPu < ass_stCadAssCalOut.swVoltLimitPu)
            {
                scm_swUqRefPu = ass_stCadAssCalOut.swVoltLimitPu; // Q14=Q14
            }
        }
        else
        {
            /* 方向错误 */
        }
    }
    else
    {}

    /*=======================================================================
                        IPark transformation for current
    =======================================================================*/
#if(EMCDEAL_EN!=0)
    if(EcmDeal.EmcModeFlag==FALSE)
       {
           EcmDeal.swUdRefPu=scm_swUdRefPu;
            EcmDeal.swUqRefPu=scm_swUqRefPu;
       }
       else
       {
           scm_swUdRefPu=EcmDeal.swUdRefPu;
           scm_swUqRefPu=EcmDeal.swUqRefPu;
       }

#endif
    //-----------------2024
//    scm_swUdRefPu=0;
//    if (cp_stFlg.RotateDirectionSelect == ForwardRotate)
//    {
//        if(bikethrottle_stBikeThrottleOut.uwThrottlePercent>250)
//               {
//                 scm_swUqRefPu=bikethrottle_stBikeThrottleOut.uwThrottlePercent*10-2000;
//               }
//               else
//               {
//                   if(scm_swUqRefPu>0)
//                   scm_swUqRefPu--;
//                   else if(scm_swUqRefPu<0)
//                       scm_swUqRefPu=0;
//
//
//               }
//
//    }
//    else if (cp_stFlg.RotateDirectionSelect == BackwardRotate)
//    {
//        if(bikethrottle_stBikeThrottleOut.uwThrottlePercent>250)
//        {
//          scm_swUqRefPu=-bikethrottle_stBikeThrottleOut.uwThrottlePercent*10+2000;
//        }
//        else
//        {
//            if(scm_swUqRefPu<0)
//            scm_swUqRefPu++;
//            else if(scm_swUqRefPu>0)
//                scm_swUqRefPu=0;
//
//
//        }
//    }
//----------------------

    crd_stIParkIn.swDPu = scm_swUdRefPu;
    crd_stIParkIn.swQPu = scm_swUqRefPu;
    crd_stIParkIn.uwThetaPu = scm_uwAngIParkPu;
    crd_voIPark(&crd_stIParkIn, &crd_stVltIParkOut);

/*=======================================================================
                            Deadband compensation
    =======================================================================*/
#if (0)
    dbc_stDbCompIn.swIaPu = adc_stDownOut.swIaPu;      // Q14
    dbc_stDbCompIn.swIbPu = adc_stDownOut.swIbPu;      // Q14
    dbc_stDbCompIn.swIcPu = adc_stDownOut.swIcPu;      // Q14
    dbc_stDbCompIn.uwVdcPu = adc_stUpOut.uwVdcLpfPu;   // Q14
    dbc_stDbCompIn.swWsPu = scm_stSpdFbkLpf.slY.sw.hi; // Q15
                                                       //    dbc_stDbCompCoef.uwNegWinVoltDuty = mn_uwNegWinVoltDuty;
                                                       //    dbc_stDbCompCoef.uwPosLostVoltDuty = mn_uwPosLostVoltDuty;
    dbc_voDBComp(&dbc_stDbCompIn, &dbc_stDbCompCoef, &dbc_stDbCompOut);
#endif

    scm_swUalphaRefPu = crd_stVltIParkOut.swAlphaPu + dbc_stDbCompOut.swUalphaCompPu; // Q14
    scm_swUbetaRefPu = crd_stVltIParkOut.swBetaPu + dbc_stDbCompOut.swUbetaCompPu;    // Q14
    scm_swUalphaCompPu = dbc_stDbCompOut.swUalphaCompPu;                              // Q14
    scm_swUbetaCompPu = dbc_stDbCompOut.swUbetaCompPu;                                // Q14
    /*=======================================================================
                                PWM generate
    =======================================================================*/
    if (cp_stFlg.RunModelSelect == VFContorl)
    {
        SWORD swVFVolAmp = 0;
        swVFVolAmp = ((SLONG)cp_stControlPara.swDragVolAp << 14) / VBASE;
        if (cp_stFlg.RotateDirectionSelect == ForwardRotate)
        {
            crd_stIParkIn.swDPu = 0;
            crd_stIParkIn.swQPu = swVFVolAmp;
        }
        else if (cp_stFlg.RotateDirectionSelect == BackwardRotate)
        {
            crd_stIParkIn.swDPu = 0;
            crd_stIParkIn.swQPu = swVFVolAmp;
        }
        else
        {}

        crd_stIParkIn.uwThetaPu = scm_uwAngIParkPu; // scm_uwAngIParkPu;
        crd_voIPark(&crd_stIParkIn, &crd_stVltIParkOut);
        scm_swUalphaRefPu = crd_stVltIParkOut.swAlphaPu;
        scm_swUbetaRefPu = crd_stVltIParkOut.swBetaPu;
    }



    pwm_stGenIn.swUalphaPu = scm_swUalphaRefPu;   // Q14
    pwm_stGenIn.swUbetaPu = scm_swUbetaRefPu;     // Q14
    pwm_stGenIn.uwVdcPu = adc_stUpOut.uwVdcLpfPu; // Q14
    pwm_voGen(&pwm_stGenIn, &pwm_stGenCoef, &pwm_stGenOut);

   // iPwm_SetCompareGroupValues16(0, pwm_stGenOut.uwNewTIM1COMPR);

//    if (cp_stFlg.CurrentSampleModelSelect == SINGLERESISITANCE)
//    {
//        ULONG samplingTick[2];
//        samplingTick[0] = pwm_stGenOut.uwFirstTrigCOMPR;
//        samplingTick[1] = pwm_stGenOut.uwSecondTrigCOMPR;
//        //iPwm_SyncMultiSamplingCountUp(0, &samplingTick[0], 2);
//    }

    Test_U_in.swAlphaPu = pwm_stGenOut.swUalphaPu - scm_swUalphaCompPu; // Q14
    Test_U_in.swBetaPu = pwm_stGenOut.swUbetaPu - scm_swUbetaCompPu;    // Q14
    Test_U_in.uwThetaPu = scm_uwAngIParkPu;                             // Q15
    crd_voPark(&Test_U_in, &Test_U_out);

    ////    pwm_stGenOut.uwNewTIM1COMPR[0] = 500;
    ////    pwm_stGenOut.uwNewTIM1COMPR[1] = 500;
    ////    pwm_stGenOut.uwNewTIM1COMPR[2] = 500;
    ////    pwm_stGenOut.uwNewTIM1COMPR[3] = 1700;
    ////    pwm_stGenOut.uwNewTIM1COMPR[4] = 1700;
    ////    pwm_stGenOut.uwNewTIM1COMPR[5] = 1700;
}
/*************************************************************************
 Local Functions (N/A)
*************************************************************************/

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