motor_ctl-4/User project/2.MotorDrive/Source/spdctrFSM.c

/************************************************************************
 Project:             Welling Motor Control Paltform
 Filename:            sysfsm.c
 Partner Filename:    sysfsm.h
 Description:         System finite state machine
 Complier:            IAR Embedded Workbench for ARM 7.80.4
 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 _STARTFSM_C_
#define _STARTFSM_C_
#endif
/************************************************************************
Include File
************************************************************************/
#include "syspar.h"
#include "user.h"
#include "FSM_1st.h"
#include "FSM_2nd.h"
#include "spdctrFSM.h"
#include "switchhall.h"
#include "spi_master.h"
#include "cmdgennew.h"
#include "power.h"
#include "sys_ctrl.h"
#include "can.h"
#include "canAppl.h"
#include "string.h"
/************************************************************************
 Constant Table:
************************************************************************/
UWORD T_rotate_count = 0;
UWORD T_rotateDelay_count = 0;
UWORD T_rotateCoef = 0;
UWORD T_rotateDelayCoef = 0;
UWORD uwAngle = 0;
UWORD uwAngle_sector = 0;
UWORD Angle[44] = {0};//0-SpiThetaOrigin;1-SpiThetaNow;2-44:Angle
UWORD AngleGet_count = 2;
/************************************************************************
 Exported Functions:
************************************************************************/
void InitPosDet_TbcupHook(void)
{
    scm_ulStatCt++;

    align_stIn.ulStatCt = scm_ulStatCt;
    align_voInitPos(&align_stIn, &align_stCoef, &align_stOut);
    scm_slIdRefPu = align_stOut.slIdRefPu;   // Q29
    scm_swIdRefPu = align_stOut.swIdRefPu;   // Q14
    scm_swIqRefPu = align_stOut.swIqRefPu;   // Q14
    scm_uwAngRefPu = align_stOut.uwAngRefPu; // Q15
}

void ParDet_TbcupHook(void)
{}
void StartUp_TbcupHook(void)
{
    flx_stCtrlOut.swIqLimPu = (SWORD)cof_uwCurMaxPu;
    scm_ulStatCt++;

    align_stIn.ulStatCt = scm_ulStatCt;
    align_stIn.swRotateDir = scm_swRotateDir;
    align_voStartUp(&align_stIn, &align_stCoef, &align_stOut);

    scm_slDragSpdRefPu = align_stOut.slDragSpdRefPu;
    scm_slDragSpdPu = align_stOut.slDragSpdPu;
    scm_swIdRefPu = align_stOut.swIdRefPu;
    scm_swIqRefPu = align_stOut.swIqRefPu;
    scm_slAngManuPu = align_stOut.slAngManuPu;
//    scm_uwAngRefPu = align_stOut.uwAngRefPu;
    scm_StartUpOvrFlg = align_stOut.blStartUpOvrFlg;
    
    if(AngleTest == TRUE)
    {   
        scm_swIdRefPu = align_stCoef.uwDragCurPu;
        scm_swIqRefPu = 0;
        scm_uwAngRefPu = StartUp_SpiSensorTest();
    }
    else
    {
        scm_uwAngRefPu = align_stOut.uwAngRefPu;
    }
}

void Open2Clz_TbcupHook(void)
{

    flx_stCtrlOut.swIqLimPu = (SWORD)cof_uwCurMaxPu;
    
    if(cp_stFlg.ThetaGetModelSelect == ANG_OBSERVER)
    {
        align_stIn.uwObsElecThetaPu = obs_stObsOutPu.uwElecThetaPu;
    }
    else if(cp_stFlg.ThetaGetModelSelect == ANG_RESOLVER)
    {
        align_stIn.uwObsElecThetaPu = spi_stResolverOut.uwSpiThetaPu;
    }
    else if(cp_stFlg.ThetaGetModelSelect == ANG_SWITCHHALL)
    {
        align_stIn.uwObsElecThetaPu = switchhall_stOut.uwLowThetaPu;
    }
    else
    {
        //do nothing
    }

    align_stIn.swCurRefrompu = swCurRefrompu; // swCurRefrompu;
    align_voOpen2Clz(&align_stIn, &align_stCoef, &align_stOut);

    scm_swIdRefPu = align_stOut.swIdRefPu;
    scm_swIqRefPu = align_stOut.swIqRefPu;
    scm_blCurSwitchOvrFlg = align_stOut.blCurSwitchOvrFlg;
    scm_blAngSwitchOvrFlg = align_stOut.blAngSwitchOvrFlg;
    scm_slAngManuPu = align_stOut.slAngManuPu;
    scm_uwAngManuPu = align_stOut.uwAngManuPu;
    scm_uwAngRefPu = align_stOut.uwAngRefPu;
}

void ClzLoop_TbcupHook(void)
{
    /*=======================================================================
                                  Flux weakening
      =======================================================================*/
    flx_stCtrlIn.swUalphaPu = crd_stVltIParkOut.swAlphaPu; // Q14
    flx_stCtrlIn.swUbetaPu = crd_stVltIParkOut.swBetaPu;   // Q14
    flx_stCtrlIn.uwVdcPu = adc_stUpOut.uwVdcLpfPu;         // Q14
    flx_stCtrlIn.swIqRefPu = swCurRefrompu;                // Q14
    flx_stCtrlIn.swUqRefPu = scm_swUqRefPu;                // Q14
    flx_voCtrl(&flx_stCtrlIn, &flx_stCtrlCoef, &flx_stCtrlOut);
    
    /*=======================================================================
                                     Power Limit
      =======================================================================*/
    mth_voLPFilter(adc_stUpOut.PCBTemp, &scm_stPCBTempLpf);
    pwr_stPwrLimIn.swMotorPwrPu = scm_stMotoPwrInLpf.slY.sw.hi; // Q15
    pwr_stPwrLimIn.swPCBTemp = scm_stPCBTempLpf.slY.sw.hi;  
    pwr_voPwrLimPI(&pwr_stPwrLimIn, &pwr_stPwrLimCof, &pwr_stPwrLimOut2); // Q14

    /*======================================================================= 
             Set Iq and Id reference for Constant Voltage Break
    =======================================================================*/
    cvb_stBrakeIn.uwVdcLpfPu = adc_stUpOut.uwVdcLpfPu;
    cvb_stBrakeIn.swIdRefPu = scm_swIdRefPu;
    cvb_stBrakeIn.swIqRefPu = swCurRefrompu;             
    cvb_stBrakeIn.swSpdPu = scm_stSpdFbkLpf.slY.sw.hi;
    cvb_stBrakeIn.uwAngelPu = scm_uwAngRefPu;
    cvb_stBrakeIn.uwSpdLpfAbsPu = scm_uwSpdFbkLpfAbsPu;
    cvb_voBrake(&cvb_stBrakeIn,&cvb_stBrakeCoef,&cvb_stBrakeOut);
 
    /** Idref可变范围-3A~0A且未弱磁，暂不做过多处理；Angle在down中更新，暂不改为改为恒压制动角度 **/
    if(cvb_stBrakeIn.uwVdcLpfPu >= cvb_stBrakeCoef.uwVdcStartCvbPu)
    {
        scm_swIdRefPu = cvb_stBrakeOut.swIdRefPu;
    }
    else
    {
        scm_swIdRefPu = flx_stCtrlOut.swIdRefPu;  //0;  //
    }  

    scm_swIqRefPu = cvb_stBrakeOut.swIqRefPu;
}

void Stop_TbcupHook(void)
{}

void Clz2Stop_TbcupHook(void)
{
    scm_ulStatCt++;

    /*=======================================================================
                                     Power Limit
      =======================================================================*/
    mth_voLPFilter(adc_stUpOut.PCBTemp, &scm_stPCBTempLpf);
    pwr_stPwrLimIn.swMotorPwrPu = scm_stMotoPwrInLpf.slY.sw.hi; // Q15
    pwr_stPwrLimIn.swPCBTemp = scm_stPCBTempLpf.slY.sw.hi;  
    pwr_voPwrLimPI(&pwr_stPwrLimIn, &pwr_stPwrLimCof, &pwr_stPwrLimOut2); // Q14

    /*======================================================================= 
             Set Iq and Id reference for Constant Voltage Break
    =======================================================================*/
    cvb_stBrakeIn.uwVdcLpfPu = adc_stUpOut.uwVdcLpfPu;
    cvb_stBrakeIn.swIdRefPu = scm_swIdRefPu;
    cvb_stBrakeIn.swIqRefPu = 0;//swCurRefrompu;            
    cvb_stBrakeIn.swSpdPu = scm_stSpdFbkLpf.slY.sw.hi;
    cvb_stBrakeIn.uwAngelPu = scm_uwAngRefPu;
    cvb_stBrakeIn.uwSpdLpfAbsPu = scm_uwSpdFbkLpfAbsPu;
    cvb_voBrake(&cvb_stBrakeIn,&cvb_stBrakeCoef,&cvb_stBrakeOut);
 
    /** Idref可变范围-3A~0A且未弱磁，暂不做过多处理；Angle在down中更新，暂不改为改为恒压制动角度 **/
    if(cvb_stBrakeIn.uwVdcLpfPu >= cvb_stBrakeCoef.uwVdcStartCvbPu)
    {
        scm_swIdRefPu = cvb_stBrakeOut.swIdRefPu;
    }
    else
    {
        scm_swIdRefPu = 0;  //
    }  

    scm_swIqRefPu = cvb_stBrakeOut.swIqRefPu;
}

void InitPosDet_TbcdownHook(void)
{
    /* Get angle for park transformation */
    scm_uwAngParkPu = scm_uwAngRefPu; // Q15
    scm_uwAngIParkPu = scm_uwAngParkPu;

    /*=======================================================================
                        Current decoupling
    =======================================================================*/
    acr_stUdqDcpOut.swUdPu = 0;
    acr_stUdqDcpOut.swUqPu = 0;
}
void ParDet_TbcdownHook(void)
{
    /* Get angle for park transformation */
    scm_uwAngParkPu = scm_uwAngRefPu; // Q15
    scm_uwAngIParkPu = scm_uwAngParkPu;

    /*=======================================================================
                        Current decoupling
    =======================================================================*/
    acr_stUdqDcpOut.swUdPu = 0;
    acr_stUdqDcpOut.swUqPu = 0;
}
void StartUp_TbcdownHook(void)
{
    /* Speed feedback LPF */
    if(cp_stFlg.ThetaGetModelSelect == ANG_OBSERVER)
    {
        mth_voLPFilter(obs_stObsOutPu.swElecFreqPu, &scm_stSpdFbkLpf);
        obs_stObsCalcIn.swUalphaPu = scm_swUalphaPu;              // Q14
        obs_stObsCalcIn.swUbetaPu = scm_swUbetaPu;                // Q14
        obs_stObsCalcIn.swIalphaPu = crd_stCurClarkOut.swAlphaPu; // Q14
        obs_stObsCalcIn.swIbetaPu = crd_stCurClarkOut.swBetaPu;   // Q14
        obs_stObsCalcIn.uwVdcPu = adc_stUpOut.uwVdcLpfPu;
        obs_voObsCalc(&obs_stObsCalcIn, &obs_stObsCoefPu, &obs_stObsOutPu);
    }
    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); // Q15

    /* Get angle for park transformation */
    scm_uwAngParkPu = scm_uwAngRefPu; // Q15
    scm_uwAngIParkPu = scm_uwAngParkPu;

    /*=======================================================================
                        Current decoupling
    =======================================================================*/
    acr_stUdqDcpOut.swUdPu = 0;
    acr_stUdqDcpOut.swUqPu = 0;
}
void Open2Clz_TbcdownHook(void)
{
    /* Speed feedback LPF */
    if(cp_stFlg.ThetaGetModelSelect == ANG_OBSERVER)
    {
        obs_stObsCalcIn.swUalphaPu = scm_swUalphaPu;              // Q14
        obs_stObsCalcIn.swUbetaPu = scm_swUbetaPu;                // Q14
        obs_stObsCalcIn.swIalphaPu = crd_stCurClarkOut.swAlphaPu; // Q14
        obs_stObsCalcIn.swIbetaPu = crd_stCurClarkOut.swBetaPu;   // Q14
        obs_stObsCalcIn.uwVdcPu = adc_stUpOut.uwVdcLpfPu;
        obs_voObsCalc(&obs_stObsCalcIn, &obs_stObsCoefPu, &obs_stObsOutPu);
        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); // Q15

    /* Get angle for park transformation */
    scm_uwAngParkPu = scm_uwAngRefPu; // Q15
    scm_uwAngIParkPu = scm_uwAngParkPu;

    /*=======================================================================
                        Current decoupling
    =======================================================================*/
    acr_stUdqDcpOut.swUdPu = 0;
    acr_stUdqDcpOut.swUqPu = 0;
}

void ClzLoop_TbcdownHook(void)
{
    ULONG ulTmp1;
    SWORD swAngCompPu = 0; // Q15

/* Speed feedback LPF */

    if(cp_stFlg.ThetaGetModelSelect == ANG_OBSERVER)
    {
        obs_stObsCalcIn.swUalphaPu = scm_swUalphaPu;              // Q14
        obs_stObsCalcIn.swUbetaPu = scm_swUbetaPu;                // Q14
        obs_stObsCalcIn.swIalphaPu = crd_stCurClarkOut.swAlphaPu; // Q14
        obs_stObsCalcIn.swIbetaPu = crd_stCurClarkOut.swBetaPu;   // Q14
        obs_stObsCalcIn.uwVdcPu = adc_stUpOut.uwVdcLpfPu;
        obs_voObsCalc(&obs_stObsCalcIn, &obs_stObsCoefPu, &obs_stObsOutPu);
        mth_voLPFilter(obs_stObsOutPu.swElecFreqPu, &scm_stSpdFbkLpf);
        scm_uwAngRefPu = obs_stObsOutPu.uwElecThetaPu;
    }
    else if(cp_stFlg.ThetaGetModelSelect == ANG_RESOLVER)
    {
        mth_voLPFilter(spi_stResolverOut.swSpdFbkPu, &scm_stSpdFbkLpf);
        scm_uwAngRefPu = spi_stResolverOut.uwSpiThetaPu;
    }
    else if(cp_stFlg.ThetaGetModelSelect == ANG_SWITCHHALL)
    {
        mth_voLPFilter(switchhall_stOut.swLowSpdPu, &scm_stSpdFbkLpf);
        scm_uwAngRefPu = switchhall_stOut.uwLowThetaPu;
    }
    else
    {
        //do nothing
    }

    /* Speed feedback Absolute */
    scm_uwSpdFbkLpfAbsPu = (UWORD)ABS(scm_stSpdFbkLpf.slY.sw.hi); // Q15

    /* Get angle for park transformation */
    scm_uwAngParkPu = scm_uwAngRefPu; // Q15

    if(cp_stFlg.ThetaGetModelSelect == ANG_OBSERVER)
    {
        swAngCompPu = (SWORD)(((SLONG)obs_stObsOutPu.swElecFreqPu * TBC_TM) >> 10); // Q15
    }
    else if(cp_stFlg.ThetaGetModelSelect == ANG_RESOLVER)
    {
        swAngCompPu = (SWORD)(((SLONG)spi_stResolverOut.swSpdFbkPu * TBC_TM) >> 10); // Q15
    }
    else if(cp_stFlg.ThetaGetModelSelect == ANG_SWITCHHALL)
    {
        swAngCompPu = (SWORD)(((SLONG)switchhall_stOut.swLowSpdPu * TBC_TM) >> 10); // Q15
    }
    else
    {
        //do nothing
    }

    ulTmp1 = scm_uwAngParkPu + ((swAngCompPu * 3) >> 1) + cof_sl720DegreePu; 
    scm_uwAngIParkPu = (UWORD)(ulTmp1 & 0x7FFF);
    /*=======================================================================
                        Current decoupling
    =======================================================================*/ 
    if(cp_stFlg.ThetaGetModelSelect == ANG_OBSERVER)
    {
        acr_stUdqDcpIn.swWsPu =  scm_stSpdFbkLpf.slY.sw.hi; // Q15
    }
    else if(cp_stFlg.ThetaGetModelSelect == ANG_RESOLVER)
    {
        acr_stUdqDcpIn.swWsPu = spi_stResolverOut.swSpdFbkPu; // Q15  swSpdFbkLpfPu spi_stResolverOut.swSpdFbkPu
    }
    else
    {
        //do nothing
    }
    acr_stUdqDcpIn.swIdRefPu = scm_swIdFdbLpfPu;//scm_swIdRefPu  scm_swIdFdbLpfPu          // Q14
    acr_stUdqDcpIn.swIqRefPu = scm_swIqFdbLpfPu; //scm_swIqRefPu  scm_swIqFdbLpfPu         // Q14  
    acr_stUdqDcpIn.swUdqLimPu = scm_swVsDcpLimPu;      // Q14
    acr_voUdqDcp(&acr_stUdqDcpIn, &acr_stUdqDcpCoef, &acr_stUdqDcpOut);
}

void Stop_TbcdownHook(void)
{
    scm_swIdRefPu = 0;
    scm_swIqRefPu = 0;

    sysctrl_voPwmInit();
    cmfsm_stFlg.blMotorStopFlg = TRUE;
}

void Clz2Stop_TbcdownHook(void)
{
    ULONG ulTmp1;
    SWORD swAngCompPu = 0; // Q15

/* Speed feedback LPF */

    if(cp_stFlg.ThetaGetModelSelect == ANG_OBSERVER)
    {
        obs_stObsCalcIn.swUalphaPu = scm_swUalphaPu;              // Q14
        obs_stObsCalcIn.swUbetaPu = scm_swUbetaPu;                // Q14
        obs_stObsCalcIn.swIalphaPu = crd_stCurClarkOut.swAlphaPu; // Q14
        obs_stObsCalcIn.swIbetaPu = crd_stCurClarkOut.swBetaPu;   // Q14
        obs_stObsCalcIn.uwVdcPu = adc_stUpOut.uwVdcLpfPu;
        obs_voObsCalc(&obs_stObsCalcIn, &obs_stObsCoefPu, &obs_stObsOutPu);
        mth_voLPFilter(obs_stObsOutPu.swElecFreqPu, &scm_stSpdFbkLpf);
        scm_uwAngRefPu = obs_stObsOutPu.uwElecThetaPu;
    }
    else if(cp_stFlg.ThetaGetModelSelect == ANG_RESOLVER)
    {
        mth_voLPFilter(spi_stResolverOut.swSpdFbkPu, &scm_stSpdFbkLpf);
        scm_uwAngRefPu = spi_stResolverOut.uwSpiThetaPu;
    }
    else if(cp_stFlg.ThetaGetModelSelect == ANG_SWITCHHALL)
    {
        mth_voLPFilter(switchhall_stOut.swLowSpdPu, &scm_stSpdFbkLpf);
        scm_uwAngRefPu = switchhall_stOut.uwLowThetaPu;
    }
    else
    {
        //do nothing
    }

    /* Speed feedback Absolute */
    scm_uwSpdFbkLpfAbsPu = (UWORD)ABS(scm_stSpdFbkLpf.slY.sw.hi); // Q15

    /* Get angle for park transformation */
    scm_uwAngParkPu = scm_uwAngRefPu; // Q15

    if(cp_stFlg.ThetaGetModelSelect == ANG_OBSERVER)
    {
        swAngCompPu = (SWORD)(((SLONG)obs_stObsOutPu.swElecFreqPu * TBC_TM) >> 10); // Q15
    }
    else if(cp_stFlg.ThetaGetModelSelect == ANG_RESOLVER)
    {
        swAngCompPu = (SWORD)(((SLONG)spi_stResolverOut.swSpdFbkPu * TBC_TM) >> 10); // Q15
    }
    else if(cp_stFlg.ThetaGetModelSelect == ANG_SWITCHHALL)
    {
        swAngCompPu = (SWORD)(((SLONG)switchhall_stOut.swLowSpdPu * TBC_TM) >> 10); // Q15
    }
    else
    {
        //do nothing
    }

    ulTmp1 = scm_uwAngParkPu + ((swAngCompPu * 3) >> 1) + cof_sl720DegreePu; 
    scm_uwAngIParkPu = (UWORD)(ulTmp1 & 0x7FFF);
    /*=======================================================================
                        Current decoupling
    =======================================================================*/ 
    if(cp_stFlg.ThetaGetModelSelect == ANG_OBSERVER)
    {
        acr_stUdqDcpIn.swWsPu =  scm_stSpdFbkLpf.slY.sw.hi; // Q15
    }
    else if(cp_stFlg.ThetaGetModelSelect == ANG_RESOLVER)
    {
        acr_stUdqDcpIn.swWsPu = spi_stResolverOut.swSpdFbkPu; // Q15  swSpdFbkLpfPu spi_stResolverOut.swSpdFbkPu
    }
    else
    {
        //do nothing
    }
    acr_stUdqDcpIn.swIdRefPu = scm_swIdFdbLpfPu;//scm_swIdRefPu  scm_swIdFdbLpfPu          // Q14
    acr_stUdqDcpIn.swIqRefPu = scm_swIqFdbLpfPu; //scm_swIqRefPu  scm_swIqFdbLpfPu         // Q14  
    acr_stUdqDcpIn.swUdqLimPu = scm_swVsDcpLimPu;      // Q14
    acr_voUdqDcp(&acr_stUdqDcpIn, &acr_stUdqDcpCoef, &acr_stUdqDcpOut);
}

void InitPosDet_TbsHook(void)
{}
void ParDet_TbsHook(void)
{}
void StartUp_TbsHook(void)
{
    /*=======================================================================
                          Speed PI output limit in "OpenDrg"
      =======================================================================*/
    if ((curSpeed_state.state == StartUp) && (scm_uwStartMd == START_ALIGN))
    {
        if (scm_swRotateDir * asr_stSpdPIOut.slIqRefPu < 0)
        {
            asr_stSpdPIOut.slIqRefPu = 0; // Q30
            asr_stSpdPIOut.slIqSumPu = 0; // Q30
            asr_stSpdPIOut.slIqiPu = 0;
        }
        else
        {
            if (scm_swRotateDir * asr_stSpdPIOut.slIqRefPu > ((SLONG)mn_uwDragCurPu << 16)) // Q30
            {
                asr_stSpdPIOut.slIqRefPu = scm_swRotateDir * ((SLONG)mn_uwDragCurPu << 16); // Q30
                asr_stSpdPIOut.slIqiPu = scm_swRotateDir * ((SLONG)mn_uwDragCurPu << 16);   // Q14+Q16=Q30
            }
        }
    }
}
void Open2Clz_TbsHook(void)
{}
void ClzLoop_TbsHook(void)
{}

void Stop_TbsHook(void)
{}

void Clz2Stop_TbsHook(void)
{}

void scm_voSpdCtrMdFSM(void) /* parasoft-suppress METRICS-28 "本项目圈复杂度无法更改，后续避免" */
{
    switch (curSpeed_state.state)
    {
    case Charge:

        break;

    case InitPosDet:
        /* Command run disable */
        /* Motor run flag set */
        cmfsm_stFlg.blMotorStopFlg = FALSE;
        if (!switch_flg.SysRun_Flag || switch_flg.SysFault_Flag || power_stPowStateOut.powerstate == POWER_OFF)
        {
            Switch_speed_FSM(&Stop_state);
        }
        else if(switch_flg.SysWarnning_Flag == 1)
        {
            Switch_speed_FSM(&Clz2Stop_state);
        }
        else if(scm_ulStatCt < mn_ulAlignRampTbcCt)
        {}
        else if (scm_ulStatCt >= (mn_ulAlignRampTbcCt + mn_ulAlignHoldTbcCt + 10))
        {
            if(cp_stFlg.SpiOffsetFirstSetFlg == 0)
            {
                cp_stFlg.SpiOffsetFirstSetFinishFlg = 1;
            }
            else
            {
                if(cp_stFlg.ThetaGetModelSelect == ANG_RESOLVER)
                {
                    align_stCoef.uwSPIreadOnceCt = 0;
                }
                else
                {}

                if(cp_stFlg.RunModelSelect == InitPos)
                { 
                }
                else if(cp_stFlg.RunModelSelect == ClZLOOP)
                {
                  if(cp_stFlg.ThetaGetModelSelect == ANG_OBSERVER)
                  {
                    Switch_speed_FSM(&StartUp_state);
                  }
                  else
                  {
                    Switch_speed_FSM(&ClzLoop_state);
                  }
                }
                else
                {
                    Switch_speed_FSM(&StartUp_state);
                }
            }
        }
        else
        {
            //do nothing
        }

        break;
    case StartUp:
        if (!switch_flg.SysRun_Flag || switch_flg.SysFault_Flag || power_stPowStateOut.powerstate == POWER_OFF)
        {
            Switch_speed_FSM(&Stop_state);
        }
        else if(switch_flg.SysWarnning_Flag == 1)
        {
            Switch_speed_FSM(&Clz2Stop_state);
        }
        else if (scm_StartUpOvrFlg == TRUE)
        {
            if(cp_stFlg.ThetaGetModelSelect == ANG_OBSERVER)
            {
                if(cp_stFlg.RunModelSelect == ClZLOOP || cp_stFlg.RunModelSelect == CityBIKE || cp_stFlg.RunModelSelect == MountainBIKE)
                {
                    if (FSM2nd_Run_state.state == Boost)
                    {
                        cmd_stCmdOut.slIntRefPu = ((SLONG)scm_stSpdFbkLpf.slY.sw.hi * 5) << 12; // Q29  5/4
                    }
                    else if (FSM2nd_Run_state.state == Assistance)
                    {
                        //do nothing
                    }
                    else
                    {
                        //do nothing
                    }
                    Switch_speed_FSM(&Open2Clz_state);
                }
            }
            else
            {

            }
        }
        /* Command run disable */
        break;
    case Open2Clz:
        /* Command run disable */
        if (!switch_flg.SysRun_Flag || switch_flg.SysFault_Flag || power_stPowStateOut.powerstate == POWER_OFF)
        {
            Switch_speed_FSM(&Stop_state);
        }
        else if(switch_flg.SysWarnning_Flag == 1)
        {
            Switch_speed_FSM(&Clz2Stop_state);
        }
        else if (scm_blCurSwitchOvrFlg && scm_blAngSwitchOvrFlg)/* Switch over */
        {
            Switch_speed_FSM(&ClzLoop_state);
        }

        break;
    case ClzLoop:
        /* Go to stop */
        cmfsm_stFlg.blMotorStopFlg = FALSE;
        if (switch_flg.SysFault_Flag == 1 || power_stPowStateOut.powerstate == POWER_OFF)
        {
            Switch_speed_FSM(&Stop_state);
        }
        else if(switch_flg.SysWarnning_Flag == 1)
        {
            Switch_speed_FSM(&Clz2Stop_state);
        }
        else if (!switch_flg.SysRun_Flag)
        {
            /* Go to stop */
            if ((ABS(scm_swSpdRefPu) < mn_swStopSpdRefPu) && (scm_uwSpdFbkLpfAbsPu < mn_swStopSpdRefPu))
            {
                scm_swIdRefPu = 0;
                scm_swIqRefPu = 0;

                Switch_speed_FSM(&Clz2Stop_state);
            }
            
            if (uart_swTorqRefNm == 0)
            {
                scm_swIdRefPu = 0;
                scm_swIqRefPu = 0;

                Switch_speed_FSM(&Clz2Stop_state);
            }
        }
        else
        {}

        break;
    case Clz2Stop:
        if (switch_flg.SysFault_Flag == 1 || power_stPowStateOut.powerstate == POWER_OFF)
        {
            Switch_speed_FSM(&Stop_state);
        }
        else if((scm_ulStatCt > 24000) || (scm_uwSpdFbkLpfAbsPu < mn_swStopSpdRefPu))
        {
            Switch_speed_FSM(&Stop_state);
        }
        else
        {
            //do nothing
        }
        
        break;
    case Stop:
        if (switch_flg.SysRun_Flag == TRUE && switch_flg.SysFault_Flag == FALSE && switch_flg.SysWarnning_Flag == FALSE) //&& power_stPowStateOut.powerstate == POWER_ON_END
        {
            scm_voSpdCtrMdInit();
            if(cp_stFlg.SpiOffsetFirstSetFlg == 0)
            {
                Switch_speed_FSM(&InitPosDet_state);
            }
            else
            {
                if(cp_stFlg.RunModelSelect == CityBIKE || cp_stFlg.RunModelSelect == MountainBIKE)
                {
                    if(cp_stFlg.ThetaGetModelSelect == ANG_OBSERVER)
                    {
                        Switch_speed_FSM(&InitPosDet_state);
                    }
                    else
                    {   
                        Switch_speed_FSM(&ClzLoop_state);
                    }
                }
                else  
                {
#ifdef RUN_ARCH_SIM
                    Switch_speed_FSM(&ClzLoop_state);                   
#else                    
                    Switch_speed_FSM(&InitPosDet_state);
#endif
                }
            }
        }
        break;
    default:
        break;
    }
}

void Switch_speed_FSM(const SPD_STATE_HOOK *in)
{
    scm_ulStatCt = 0;
    curSpeed_state = *in;
}

void Switch_speed_FSMInit(void)
{
    scm_ulStatCt = 0;
    curSpeed_state = Stop_state;
}

UWORD StartUp_SpiSensorTest(void)
{
    T_rotateCoef = 8*1000/cp_stControlPara.swDragSpdHz/6;
    T_rotateDelayCoef = T_rotateCoef*50;
    if(T_rotate_count > 0)
    {
        T_rotateDelay_count++;
        if(T_rotateDelay_count == T_rotateDelayCoef)
        {
            Angle[AngleGet_count] = spi_stResolverOut.uwSpiThetaPu;
            AngleGet_count++;
            if(AngleGet_count >= 44)
            {
              AngleTest = FALSE;
              Switch_speed_FSM(&Stop_state);
              AngleGet_count = 2;
              MC_MotorSPD_rpm_Percent = 0;
              cp_stFlg.RunModelSelect = MC_UpcInfo.stTestParaInfo.RunModelSelect;
              signal_state.Assist = FALSE;

              memcpy((UBYTE*)(Angle), (UBYTE*)(&spi_stResolverOut.swSpiThetaOffsetOrignPu), (uint32_t)2);
              memcpy((UBYTE*)(Angle+1), (UBYTE*)(&spi_stResolverOut.swSpiThetaOffsetPu), (uint32_t)2);
              SendData(ID_MC_TO_CDL, MODE_REPORT, 0xBF58, (UBYTE *)&Angle[0]);
            }
          
          T_rotate_count = 0;
          T_rotateDelay_count = 0;
          uwAngle_sector++;
          if(uwAngle_sector==6)
          {
            uwAngle_sector = 0;
          }
        }
    }
    else
    {
        if(uwAngle < ((uwAngle_sector+1)*cof_sl60DegreePu - (scm_slDragSpdPu * TBC_TM >> 10)))
        {
          uwAngle += ((SQWORD)scm_slDragSpdPu * TBC_TM >> 10);
        }
        else
        {
          T_rotate_count++;
          uwAngle = (uwAngle_sector+1)*cof_sl60DegreePu;
          if (uwAngle >= cof_sl360DegreePu)
          {
              uwAngle -= cof_sl360DegreePu;
          }
        }
    }
    
    return uwAngle;
}
/************************************************************************
 Function: void RUN_FSM_Main(void)
 Description:
 Call by:
 Input Variables:
 Output/Return Variables:
 Subroutine Call:
 Reference:
************************************************************************/

/************************************************************************
 Local Functions: N/A
************************************************************************/

/************************************************************************
 Copyright (c) 2018 Welling Motor Technology(Shanghai) Co. Ltd.
 All rights reserved.
************************************************************************/
#ifdef _STARTFSM_C_
#undef _STARTFSM_C_ /* parasoft-suppress MISRA2004-19_6 "本项目中无法更改，后续避免使用" */
#endif
/************************************************************************
 End of this File (EOF)!
 Do not put anything after this part!
************************************************************************/