motor_ctl-4/User project/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 "AssistCurve.h"
#include "cmdgennew.h"
#include "CodePara.h"
#include "FSM_2nd.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 */
    //    sysctrl_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();
    /* Flux weakening init */
    flx_voInit();
    //  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_stIqLoadLpf.slY.sl = 0; 
    scm_stIdFbkLpf.slY.sl = 0; // Id feedback LPF
    scm_stIqFbkLpf.slY.sl = 0; // Iq feedback 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_stSpdFbkLpf.slY.sl = 0;    // Speed feedback LPF
    scm_uwSpdFbkLpfAbsPu = 0;      // Q15, Speed feedback LPF absolute
    scm_swMotorPwrInPu = 0;        // Q15, Input power of motor
    scm_stMotoPwrInLpf.slY.sl = 0; // Input power of motor after LPF
    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
    SLONG slLqPu = 0;
    ULONG ulAccel100rpmpsPu = USER_MOTOR_100RPMPS2PU_Q29;

    if (ABS(scm_swIqRefPu) < mn_swIqTurn1Pu)
    {
        scm_uwLqPu = cof_uwLqPu;
    }
    else
    {
        slLqPu = mn_slLqTurn1Pu + (((SLONG)ABS(scm_swIqRefPu) - mn_swIqTurn1Pu) * mn_swKLqSat >> 10); // Q10
        if (slLqPu < cof_uwLqMinPu)
        {
            scm_uwLqPu = cof_uwLqMinPu;
        }
        else if (slLqPu > cof_uwLqPu)
        {
            scm_uwLqPu = cof_uwLqPu;
        }
        else
        {
            scm_uwLqPu = (UWORD)slLqPu;
        }
    }

    spi_stResolverCoefIn.uwFbHz = cof_uwFbHz;
    spi_stResolverCoefIn.uwFreqTbcHz = FTBC_HZ;
    spi_stResolverCoefIn.uwSpdPllWvcHz = 30;//cp_stControlPara.swObsSpdPLLBandWidthHz; // Real Value, Unit:Hz
    spi_stResolverCoefIn.uwSpdPllMcoef = 2;//cp_stControlPara.swObsSpdPLLM;
    spi_voResolverCoef(&spi_stResolverCoefIn, &spi_stResolverCoef);
    /* 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 = (UWORD)(((ULONG)scm_uwLqPu * cof_uwLbHm) >> 10);   // Real Value, unit: 0.01mH, q Inductance
    obs_stObsCoefIn.uwLdHm = cp_stMotorPara.swLdmH;                             // 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);
    /* 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;
    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 / 100, FTBS_HZ, &scm_stIqLoadLpf.uwKx); //50Hz
    /* Id PI coefficient calculate */
    acr_stCurIdPICoefIn.uwFbHz = FBASE;
    acr_stCurIdPICoefIn.uwUbVt = VBASE;
    acr_stCurIdPICoefIn.uwIbAp = IBASE;
    acr_stCurIdPICoefIn.uwLHm = cp_stMotorPara.swLdmH;
    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.swLqmH;
    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.swLdmH;
    acr_stUdqDcpCoefIn.uwLqHm = cp_stMotorPara.swLqmH;
    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);

    /* 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 */
        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)
        /* 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);

        pwr_stPwrLimCofIn.swPwrLimW = cp_stControlPara.swPwrLimitValWt; // 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_voPwrLimCof(&pwr_stPwrLimCofIn, &pwr_stPwrLimCof);

        /*Accelaration&Decelaration limit*/
        if (ABS(scm_swSpdRefPu) < USER_MOTOR_300RPM2PU)
        {
            cmd_stCmdCoefIn.ulAccelPu =  ulAccel100rpmpsPu*2; // Q29
        }
        else
        {
            cmd_stCmdCoefIn.ulAccelPu = ulAccel100rpmpsPu*2; // Q29
        }
        cmd_stCmdCoefIn.ulDecelPu =  ulAccel100rpmpsPu*10; // 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_INIT_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
****************************************************************/

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
    {
        //do nothing
    }

    /* Speed feedback Absolute */
    scm_uwSpdFbkLpfAbsPu = (UWORD)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 = (SWORD)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(cp_stFlg.RotateDirectionSelect == ForwardRotate)
            {
                cmd_stCmdIn.swSpdCmdRpm = tempSpeed;
            }
            else
            {
                cmd_stCmdIn.swSpdCmdRpm = -tempSpeed;
            }
        }
        else
        {
            //do nothing
        }
        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 = scm_stSpdFbkLpf.slY.sw.hi; // Q15

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

    if (scm_swRotateDir > 0)
    {
        asr_stSpdPIIn.swIqMaxPu = swIqLowerPu;
        asr_stSpdPIIn.swIqMinPu = -swIqLowerPu;
    }
    else
    {
        asr_stSpdPIIn.swIqMaxPu = swIqLowerPu;
        asr_stSpdPIIn.swIqMinPu = -swIqLowerPu;
    }
    asr_voSpdPI(&asr_stSpdPIIn, &asr_stSpdPICoef, &asr_stSpdPIOut);
    
    /* 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 = spi_stResolverOut.swSpdFbkPu;
//    torqobs_voCal(&torqobs_stCalIn, &torqobs_stCoef, &torqobs_stCalOut);
//    mth_voLPFilter(torqobs_stCalOut.swIqLoadPu, &scm_stIqLoadLpf);
//    swCurRefrompu = asr_stSpdPIOut.swIqRefPu - scm_stIqLoadLpf.slY.sw.hi;      
//    if (swCurRefrompu > swIqLowerPu)
//    {
//        swCurRefrompu = swIqLowerPu;
//    }
//    else if (swCurRefrompu < -swIqLowerPu)
//    {
//        swCurRefrompu = -swIqLowerPu;
//    }
//    else
//    {}
    
    swCurRefrompu = asr_stSpdPIOut.swIqRefPu;
    curSpeed_state.Tbs_hook();
}

static SWORD deltC, switchCNT, switchflg;
static BOOL blTorqCompFlg;
static SWORD swTmpSpdRate = 0;
static LPF_OUT swTmpSpdRateLpf;
static SWORD swTmpSpdFbkPuZ1 = 0;
static SWORD swSpdRateAbsPu;
static 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
    {
        //do nothing
    }

    /* Speed feedback Absolute */
    scm_uwSpdFbkLpfAbsPu = (UWORD)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 = (SWORD)((flx_stCtrlOut.swIqLimPu < ABS(pwr_stPwrLimOut2.swIqLimPu)) ? flx_stCtrlOut.swIqLimPu : ABS(pwr_stPwrLimOut2.swIqLimPu));  
    /* 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 = spi_stResolverOut.swSpdFbkPu;
    torqobs_voCal(&torqobs_stCalIn, &torqobs_stCoef, &torqobs_stCalOut);
    mth_voLPFilter((torqobs_stCalOut.swIqLoadPu + scm_swIqFdbLpfPu), &scm_stIqLoadLpf);
    
    
//    /* Spd Fbk Compensation Calculate */
//    swTmpSpdRate = spi_stResolverOut.swSpdFbkPu - swTmpSpdFbkPuZ1; //Q15
//    mth_voLPFilterCoef(1000000 / 30, FTBS_HZ, &swTmpSpdRateLpf.uwKx); //30Hz,TBS
//    mth_voLPFilter(swTmpSpdRate, &swTmpSpdRateLpf);
//    swTmpSpdFbkPuZ1 = spi_stResolverOut.swSpdFbkPu;
//    scm_swSpdFbkCompPu = scm_stSpdFbkLpf.slY.sw.hi + (SLONG)swTmpSpdRateLpf.slY.sw.hi * FTBS_HZ / 30; //30Hz,TBS

    /* Iqref Compensation */
    if(((uart_swTorqRefNm < -200)||(uart_swTorqRefNm > 200)) && (Ass_FSM !=Spd2Torq) && (Ass_FSM !=SpeedAssit))
    {
       /* Open Loop */
       //swTestIqref  = uart_swTorqRefNm - (((SLONG)swTmpSpdRateLpf.slY.sw.hi * cof_uwJmPu * 2 << 11) / cof_uwFluxPu);  //Q15+Q0+Q11-Q12=Q14
       
       /* Observer */
       swTestIqref  = uart_swTorqRefNm - scm_stIqLoadLpf.slY.sw.hi;
    }
    else
    {
       swTestIqref = uart_swTorqRefNm;
    }

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

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

    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
****************************************************************/
static CRD_PARK_IN  Test_U_in;
static CRD_PARK_OUT Test_U_out;
void scm_voSpdCtrMdUpTbc(void)
{
    /*=======================================================================
                    Max voltage of current PI out
    =======================================================================*/
    scm_swVsLimPu = (SWORD)((SWORD)adc_stUpOut.uwVdcLpfPu * (SLONG)scm_uwAcrLimCof >> 15);     // Q14+Q15-Q15=Q14
    scm_swVsDcpLimPu = (SWORD)((SWORD)adc_stUpOut.uwVdcLpfPu * (SLONG)scm_uwUdcpLimCof >> 15); // Q14+Q15-Q15=Q14

    /*=======================================================================
                                 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
****************************************************************/
static UWORD DCPswitch = 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();
    /*=======================================================================
                            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;

    /*=======================================================================
                          Calculate input power of motor
    =======================================================================*/

    scm_swMotorPwrInPu = (SWORD)(((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 = (SWORD)(scm_stMotoPwrInLpf.slY.sw.hi *  (SLONG)cof_uwPbWt >> 15); // unit: 0.1w
    
    /*=======================================================================
                            Id current PI control
    =======================================================================*/
    //DCPswitch = 0;           //0 with forwardFeedBack    1 without forwardFeedBack
//      scm_swIdRefPu = 0;
    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)
    {
        acr_stCurIdPIIn.swUmaxPu = scm_swVsDcpLimPu - acr_stUdqDcpOut.swUdPu;  // Q14
        acr_stCurIdPIIn.swUminPu = -scm_swVsDcpLimPu - acr_stUdqDcpOut.swUdPu; // Q14
      
//        acr_stCurIdPIIn.swUmaxPu = scm_swVsLimPu - acr_stUdqDcpOut.swUdPu;  // Q14
//        acr_stCurIdPIIn.swUminPu = -scm_swVsLimPu - acr_stUdqDcpOut.swUdPu; // Q14       
    }
    else
    {
        //do nothing
    }
    acr_voCurPI(&acr_stCurIdPIIn, &acr_stCurIdPICoef, &acr_stCurIdPIOut);

    /*=======================================================================
                            Iq current PI control
    =======================================================================*/
    acr_stCurIqPIIn.swCurRefPu = scm_swIqRefPu; // Q14
    acr_stCurIqPIIn.swCurFdbPu = scm_swIqFdbLpfPu;
    if (DCPswitch == 1)
    {
        acr_stCurIqPIIn.swUmaxPu = scm_swVsDcpLimPu;  // Q14
        acr_stCurIqPIIn.swUminPu = -scm_swVsDcpLimPu; // Q14
    }
    else if (DCPswitch == 0)
    {        
        if(FSM2nd_Run_state.state == Assistance)
        {
          acr_stCurIqPIIn.swUmaxPu = ass_stCalOut.swVoltLimitPu - acr_stUdqDcpOut.swUqPu;  // Q14
          acr_stCurIqPIIn.swUminPu = -ass_stCalOut.swVoltLimitPu - acr_stUdqDcpOut.swUqPu; // Q14
        }
        else
        {
           acr_stCurIqPIIn.swUmaxPu = scm_swVsDcpLimPu - acr_stUdqDcpOut.swUqPu;  // Q14
           acr_stCurIqPIIn.swUminPu = -scm_swVsDcpLimPu - acr_stUdqDcpOut.swUqPu; // Q14
        } 
    }
    else
    {
        //do nothing
    }
    acr_voCurPI(&acr_stCurIqPIIn, &acr_stCurIqPICoef, &acr_stCurIqPIOut);

    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
    {
        //do nothing
    }
    
    /*=======================================================================
                        IPark transformation for current
    =======================================================================*/
    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 = (SWORD)(((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
       {
           //do nothing
       }
       
       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);
    ULONG samplingTick[2];
    samplingTick[0]=pwm_stGenOut.uwSigRTrig;
    samplingTick[1]=pwm_stGenOut.uwRdsonTrig;
    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);
}
/*************************************************************************
 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!
*************************************************************************/