motor_ctl-1/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 "AssistCurve.h"
#include "bikeinformation.h"
#include "cmdgennew.h"
#include "FuncLayerAPI.h"
#include "AngleObserver_discrete.h"
#include "canAppl.h"
#include "obs.h"
/************************************************************************
 Constant Table:
************************************************************************/

/************************************************************************
 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;
    if(align_stOut.swIqRefPu > 0)
    {
        if((scm_swIqRefPu + 2) <= align_stOut.swIqRefPu)
        {
            scm_swIqRefPu += 2;
        }
        else
        {
            scm_swIqRefPu = align_stOut.swIqRefPu;
        }
    }
    else if(align_stOut.swIqRefPu < 0)
    {   
        if((scm_swIqRefPu - 2) >= align_stOut.swIqRefPu)
        {
            scm_swIqRefPu -= 2;
        }
        else
        {
            scm_swIqRefPu = align_stOut.swIqRefPu;
        }
    }
    else
    {
        scm_swIqRefPu = 0;
    }
    scm_slAngManuPu = align_stOut.slAngManuPu;
    scm_uwAngRefPu = align_stOut.uwAngRefPu;
    scm_StartUpOvrFlg = align_stOut.blStartUpOvrFlg;
}

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;
        //align_stIn.uwObsElecThetaPu = rtY.Angle_Filtered;
    }
    else
    {}

    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
      =======================================================================*/
    #if(FLUX_MODE == 0)
        spdflx_stCtrlIn.swSpdFbkLpfAbsPu = (SWORD)scm_uwSpdFbkLpfAbsPu;
        spdflx_voCtrl( &spdflx_stCtrlIn, &spdflx_stCtrlCoef,  &spdflx_stCtrlOut );
        flx_stCtrlOut.swIdRefPu = spdflx_stCtrlOut.swIdRefPu;
        flx_stCtrlOut.swIqLimPu = spdflx_stCtrlOut.swIqLimPu;

    #elif(FLUX_MODE == 1)
        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);
    #else
        //Config Error
    #endif

    /*=======================================================================
                                     Power Limit
      =======================================================================*/
    mth_voLPFilter(adc_stUpOut.PCBTemp, &scm_stPCBTempLpf);
    mth_voLPFilter(adc_stUpOut.MotorTemp, &scm_stMotorTempLpf);
    pwr_stPwrLimIn.swMotorPwrPu = scm_stMotoPwrInLpf.slY.sw.hi; // Q15
    pwr_stPwrLimIn.swPCBTemp = scm_stPCBTempLpf.slY.sw.hi;  
    pwr_stPwrLimIn.swMotorTemp = scm_stMotorTempLpf.slY.sw.hi;  
    pwr_stPwrLimIn.uwBatCap = MC_RunInfo.SOC;
    pwr_voPwrLimPI(&pwr_stPwrLimIn, &pwr_stPwrLimCof, &pwr_stPwrLimOut2); // Q14
}

void Stop_TbcupHook(void)
{}

void Clz2Stop_TbcupHook(void)
{
    scm_ulStatCt++;

    /*=======================================================================
                                         Power Limit
      =======================================================================*/
    mth_voLPFilter(adc_stUpOut.PCBTemp, &scm_stPCBTempLpf);
    mth_voLPFilter(adc_stUpOut.MotorTemp, &scm_stMotorTempLpf);
    pwr_stPwrLimIn.swMotorPwrPu = scm_stMotoPwrInLpf.slY.sw.hi; // Q15
    pwr_stPwrLimIn.swPCBTemp = scm_stPCBTempLpf.slY.sw.hi;
    pwr_stPwrLimIn.swMotorTemp = scm_stMotorTempLpf.slY.sw.hi;
    pwr_stPwrLimIn.uwBatCap = MC_RunInfo.SOC;
    pwr_voPwrLimPI(&pwr_stPwrLimIn, &pwr_stPwrLimCof, &pwr_stPwrLimOut2); // Q14
}

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
    {}

    /* Speed feedback Absolute */
    scm_uwSpdFbkLpfAbsPu = 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
    {}

    /* Speed feedback Absolute */
    scm_uwSpdFbkLpfAbsPu = 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;
}
SWORD thetaoffset=0;
SLONG temptheta=0;
SWORD SwitchFlg=0;
UWORD uwAngRefPu = 0;
UWORD uwAngSwitchK = 0;
_Bool blAngSwitchOvrFlg = FALSE;
UWORD cnt;

SWORD tstThetaDelta1;
SWORD tstThetaDelta2;
SWORD tstThetaCorrect;
UWORD  UdqDcpOutCount = 0;
void ClzLoop_TbcdownHook(void)
{
    ULONG ulTmp1;
    SWORD swAngCompPu; // Q15
    
/* Speed feedback LPF */
//    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);
    
    /* Angle Switch */
//    if(scm_stSpdFbkLpf.slY.sw.hi > 7801)  // 2000rpm
//    {
//        cp_stFlg.ThetaGetModelSelect = ANG_OBSERVER;
//    }
//    else
//    {
//        if(scm_stSpdFbkLpf.slY.sw.hi < 5851)  // 1500rpm
//        {
//            cp_stFlg.ThetaGetModelSelect = ANG_SWITCHHALL;
//        }
//    }
        
    
//    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);
    
//    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;
//        temptheta = (SWORD)obs_stObsOutPu.uwElecThetaPu + thetaoffset;
//
//        if (temptheta >= cof_sl360DegreePu)
//        {
//            temptheta -= cof_sl360DegreePu;
//        }
//        else if (temptheta < (-(cof_sl360DegreePu)))
//        {
//            temptheta += cof_sl360DegreePu;
//        }
//        scm_uwAngRefPu=temptheta;
//        tstThetaCorrect=scm_uwAngRefPu-rtY.Angle_Filtered;
//
//
//    }
//    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);
        //mth_voLPFilter(switchhall_stOut.swLowSpdLpfPu, &scm_stSpdFbkLpf);
        scm_stSpdFbkLpf.slY.sw.hi = switchhall_stOut.swLowSpdLpfPu;
        
         //  scm_uwAngRefPu = switchhall_stOut.slLowThetaPu;
             scm_uwAngRefPu = rtY.Angle_Filtered;
        //scm_uwAngRefPu = LoadObsTheta_Y.uwThetaObsPu;
    }
    else
    {}

    /*======================================================================= 
             Set Iq limit and Id reference for Constant Voltage Break
    =======================================================================*/
    cvb_stBrakeIn.uwVdcLpfPu = (SWORD)adc_stUpOut.uwVdcLpfPu;
    cvb_stBrakeIn.swIdRefPu = scm_swIdRefPu;
    cvb_stBrakeIn.swIqRefPu = swCurRefrompu; //scm_swIqRefPu;            
    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);
    
    scm_swIqRefPu = cvb_stBrakeOut.swIqRefPu;
    scm_uwAngRefPu = cvb_stBrakeOut.uwAngelPu;
    if(cvb_stBrakeIn.uwVdcLpfPu >= cvb_stBrakeCoef.uwVdcStartCvbPu)
    {
        scm_swIdRefPu = cvb_stBrakeOut.swIdRefPu;
    }
    else
    {
        scm_swIdRefPu = flx_stCtrlOut.swIdRefPu;
    }  
    
    /* Speed feedback Absolute */
    scm_uwSpdFbkLpfAbsPu = abs(scm_stSpdFbkLpf.slY.sw.hi); // Q15

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

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

    ulTmp1 = scm_uwAngParkPu + ((swAngCompPu * 3) >> 1) + cof_sl720DegreePu; // ˫���²���2.5��PWM���ڣ� �����²���1.5��PWM����
    scm_uwAngIParkPu = ulTmp1 & 0x7FFF;
    ulTmp1 = scm_uwAngParkPu + ((swAngCompPu * 2) >> 1) + cof_sl720DegreePu;
    scm_uwAngIParkPu1 = ulTmp1 & 0x7FFF;
    /*=======================================================================
                        Current decoupling
    =======================================================================*/
    if(switch_flg.SysRun_Flag == TRUE)
    {
        acr_stUdqDcpIn.swWsPu = scm_stSpdFbkLpf.slY.sw.hi; // switchhall_stOut.swLowSpdLpfPu;//scm_stSpdFbkLpf.slY.sw.hi; //Q15
        acr_stUdqDcpIn.swIdRefPu = 0;//scm_swIdFdbLpfPu; //scm_swIdFdbLpfPu;//scm_swIdRefPu;          // Q14
        acr_stUdqDcpIn.swIqRefPu = 0;//scm_swIqFdbLpfPu; //scm_swIqFdbLpfPu;//scm_swIqRefPu;          // Q14 scm_swIqFdbLpfPu 
        acr_stUdqDcpIn.swUdqLimPu = scm_swVsDcpLimPu;      // Q14
        acr_voUdqDcp(&acr_stUdqDcpIn, &acr_stUdqDcpCoef, &acr_stUdqDcpOut);
    }
    else
    {
       if(++UdqDcpOutCount>100)
        {
          UdqDcpOutCount=0;
        acr_stUdqDcpOut.swUdPu = ((SLONG)acr_stUdqDcpOut.swUdPu*1010)>>10;
        acr_stUdqDcpOut.swUqPu = ((SLONG)acr_stUdqDcpOut.swUqPu*1010)>>10;
        }
    }
}

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

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

    /* Angle Switch */
//    if(scm_stSpdFbkLpf.slY.sw.hi > 7801)  // 2000rpm
//    {
//        cp_stFlg.ThetaGetModelSelect = ANG_OBSERVER;
//    }
//    else
//    {
//        if(scm_stSpdFbkLpf.slY.sw.hi < 5851)  // 1500rpm
//        {
//            cp_stFlg.ThetaGetModelSelect = ANG_SWITCHHALL;
//        }
//    }


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

//    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;
//        temptheta = (SWORD)obs_stObsOutPu.uwElecThetaPu + thetaoffset;
//
//        if (temptheta >= cof_sl360DegreePu)
//        {
//            temptheta -= cof_sl360DegreePu;
//        }
//        else if (temptheta < (-(cof_sl360DegreePu)))
//        {
//            temptheta += cof_sl360DegreePu;
//        }
//        scm_uwAngRefPu=temptheta;
//        tstThetaCorrect=scm_uwAngRefPu-rtY.Angle_Filtered;
//
//
//    }
//    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);
        //mth_voLPFilter(switchhall_stOut.swLowSpdLpfPu, &scm_stSpdFbkLpf);
        scm_stSpdFbkLpf.slY.sw.hi = switchhall_stOut.swLowSpdLpfPu;

         //  scm_uwAngRefPu = switchhall_stOut.slLowThetaPu;
             scm_uwAngRefPu = rtY.Angle_Filtered;
        //scm_uwAngRefPu = LoadObsTheta_Y.uwThetaObsPu;
    }
    else
    {}

    /*=======================================================================
             Set Iq limit and Id reference for Constant Voltage Break
    =======================================================================*/
    cvb_stBrakeIn.uwVdcLpfPu = (SWORD)adc_stUpOut.uwVdcLpfPu;
    cvb_stBrakeIn.swIdRefPu = scm_swIdRefPu;
    cvb_stBrakeIn.swIqRefPu = 0;//swCurRefrompu; //scm_swIqRefPu;
    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);

    scm_swIqRefPu = cvb_stBrakeOut.swIqRefPu;
    scm_uwAngRefPu = cvb_stBrakeOut.uwAngelPu;
    if(cvb_stBrakeIn.uwVdcLpfPu >= cvb_stBrakeCoef.uwVdcStartCvbPu)
    {
        scm_swIdRefPu = cvb_stBrakeOut.swIdRefPu;
    }
    else
    {
        scm_swIdRefPu = 0;//flx_stCtrlOut.swIdRefPu;
    }

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

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

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

    ulTmp1 = scm_uwAngParkPu + ((swAngCompPu * 3) >> 1) + cof_sl720DegreePu; // ˫���²���2.5��PWM���ڣ� �����²���1.5��PWM����
    scm_uwAngIParkPu = ulTmp1 & 0x7FFF;
    ulTmp1 = scm_uwAngParkPu + ((swAngCompPu * 2) >> 1) + cof_sl720DegreePu;
    scm_uwAngIParkPu1 = ulTmp1 & 0x7FFF;
    /*=======================================================================
                        Current decoupling
    =======================================================================*/
    if(switch_flg.SysRun_Flag == TRUE)
    {
        acr_stUdqDcpIn.swWsPu = scm_stSpdFbkLpf.slY.sw.hi; // switchhall_stOut.swLowSpdLpfPu;//scm_stSpdFbkLpf.slY.sw.hi; //Q15
        acr_stUdqDcpIn.swIdRefPu = 0;//scm_swIdFdbLpfPu; //scm_swIdFdbLpfPu;//scm_swIdRefPu;          // Q14
        acr_stUdqDcpIn.swIqRefPu = 0;//scm_swIqFdbLpfPu; //scm_swIqFdbLpfPu;//scm_swIqRefPu;          // Q14 scm_swIqFdbLpfPu
        acr_stUdqDcpIn.swUdqLimPu = scm_swVsDcpLimPu;      // Q14
        acr_voUdqDcp(&acr_stUdqDcpIn, &acr_stUdqDcpCoef, &acr_stUdqDcpOut);
    }
    else
    {
       if(++UdqDcpOutCount>100)
        {
          UdqDcpOutCount=0;
        acr_stUdqDcpOut.swUdPu = ((SLONG)acr_stUdqDcpOut.swUdPu*1010)>>10;
        acr_stUdqDcpOut.swUqPu = ((SLONG)acr_stUdqDcpOut.swUqPu*1010)>>10;
        }
    }
}

void Stop_TbcdownHook(void)
{

    scm_swIdRefPu = 0;
    scm_swIqRefPu = 0;
    scm_swUdRefPu=0;
    scm_swUqRefPu=0;
    hw_voPWMInit();
    cmfsm_stFlg.blMotorStopFlg = TRUE;
}

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)
{
    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.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 if(cp_stFlg.ThetaGetModelSelect == ANG_SWITCHHALL)
////                  {
////                    Switch_speed_FSM(&StartUp_state);
////                  }
//                else
                {
                Switch_speed_FSM(&ClzLoop_state);
                }
            }
            else
            {
                Switch_speed_FSM(&StartUp_state);
            }
        }
        else
        {}

        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);
        }
//        if (scm_StartUpOvrFlg == TRUE)
//        {
//            if(cp_stFlg.ThetaGetModelSelect == ANG_OBSERVER)  //|| cp_stFlg.ThetaGetModelSelect ==ANG_SWITCHHALL
//            {
//                if(cp_stFlg.RunModelSelect == ClZLOOP || cp_stFlg.RunModelSelect == CadAssist || cp_stFlg.RunModelSelect == TorqAssist)
//                {
//                    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)
//                    {}
//                    else
//                    {}
//                    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_END)
        {
            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_uwStopSpdRefPu) && (scm_uwSpdFbkLpfAbsPu < mn_uwStopSpdRefPu))
                /*||(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_uwStopSpdRefPu))
        {
            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.RunModelSelect == ClZLOOP || cp_stFlg.RunModelSelect == CadAssist || cp_stFlg.RunModelSelect == TorqAssist)
            {
//                if(cp_stFlg.ThetaGetModelSelect == ANG_OBSERVER)
//                {
//                    Switch_speed_FSM(&InitPosDet_state);
//                }
//                else
                {   
                    Switch_speed_FSM(&ClzLoop_state);
                }
            }
            else  
            {
                Switch_speed_FSM(&InitPosDet_state);
            }
        }
        break;
    default:
        break;
    }
}

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

void Switch_speed_FSMInit(void)
{
    scm_ulStatCt = 0;
    //adc_stDownOut.swIaPu = 0;
     //adc_stDownOut.swIbPu = 0;
    // adc_stDownOut.swIcPu = 0;
    curSpeed_state = Stop_state;
}

/************************************************************************
 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_
#endif
/************************************************************************
 End of this File (EOF)!
 Do not put anything after this part!
************************************************************************/