motor_ctl-1/1.FrameLayer/Source/FSM_1st.c

/**
 * @file sysfsm1.c
 * @author Zhang, Kai(zhangkai71@midea.com)
 * @brief System FSM
 * @version 0.1
 * @date 2021-09-27
 *
 * @copyright Copyright (c) 2021
 *
 */

#include "syspar.h"
#include "user.h"
#include "FSM_1st.h"
#include "FSM_2nd.h"
#include "FuncLayerAPI.h"
#include "power.h"
#include "canAppl.h"
#include "MosResCalib.h"
/******************************
 *
 *  Parameter
 *
 ******************************/
FSM_SYS_HOOK   FSM1st_Sys_state;
FSM_SYS_HOOK   SysInit_state = {SysInit, SysInit_hook, SysInit_TbcUphook, SysInit_TbcDownhook, SysInit_Tbshook};
FSM_SYS_HOOK   SysRdy_state = {SysRdy, SysRdy_hook, SysRdy_TbcUphook, SysRdy_TbcDownhook, SysRdy_Tbshook};
FSM_SYS_HOOK   SysFault_state = {SysFault, SysFault_hook, SysFault_TbcUphook, SysFault_TbcDownhook, SysFault_Tbshook};
SYS_FSM_STATUS FaultFROM = SysInit;
SYS_FMS_FLAG   switch_flg = SYS_FSM_FLG_DEFAULT;
SYS_RDY_FLG    sysfsm_stFlg = SYS_RDY_FLG_DEFAULT;
SIGNAL_STATE   signal_state;
ULONG          swShutCnt = 0;
ULONG          HMI_SEND = 0;
ULONG          swSwitchStateCnts = 0;
ULONG          sysfsm_ulInitOutCt = 0;
void (*switchfuction[3][3])(void) = {ReadytoRun, ReadytoRun, InittoFault,\
                                    ReadytoRun, ReadytoRun, RdytoFault,\
                                    ReadytoRun, ReadytoRun, ReadytoRun};
ULONG gtemp = 0;
void FSM_voInit(void)
{
    FSM1st_Sys_state = SysInit_state;
    switch_flg.HMI_Flag = FALSE;
    switch_flg.SysRdy_Flag = FALSE;
    switch_flg.SysRun_Flag = FALSE;
    switch_flg.SysFault_Flag = FALSE;
    switch_flg.SysCoef_Flag = FALSE;
    switch_flg.Standby_flg = 0;

    signal_state.Sensor = FALSE;
    signal_state.Assist = FALSE;
    signal_state.Switched = FALSE;
    signal_state.Light = FALSE;
    signal_state.Display = FALSE;
    
    sysfsm_ulInitOutCt = 0;
    sysfsm_stFlg.CommuRdy_Flag = TRUE;
}

void FSM_1st_Main(void)
{
    switch (FSM1st_Sys_state.state)
    {
    case SysInit:
        /*code*/
        if (switch_flg.SysFault_Flag == 1)
        {
            Switch_sys_FSM(&SysFault_state);
            FaultFROM = SysInit;
        }

        if (switch_flg.SysRdy_Flag == 1)
        {
            /* System FSM_2nd init */
            RUN_FSM_voInit();
            sysfsm_ulInitOutCt = cp_ulSystickCnt;
            Switch_sys_FSM(&SysRdy_state);
        }

        break;

    case SysRdy:

        swSwitchStateCnts++;
        /* Fault detection*/
        if (switch_flg.SysFault_Flag == 1)
        {
            Switch_sys_FSM(&SysFault_state);
            FaultFROM = SysRdy;
        }
        /* Shut down detection*/
        if (signal_state.Switched == 1)
        {
            swShutCnt++;
            if (swShutCnt >= 1000)
            {
                swShutCnt = 0;
            }
        }
        else
        {
            swShutCnt = 0;
        }
        /* Code here*/
        if (switch_flg.SysRun_Flag == 0)
        {
            if (swSwitchStateCnts <= DISPLAY_DISABLE_CNT)
            {
                switch_flg.Standby_flg = 1; /* send display disable message*/
            }
            else if (swSwitchStateCnts <= SHUT_DOWN_CNT)
            {
                switch_flg.Standby_flg = 2; //* EEPROM write and send BMS shut down message */
            }
            else
            {
                switch_flg.Standby_flg = 3; //* EEPROM write and send BMS shut down message */
            }
        }
        else
        {
            /* system run*/
        }

        break;

    case SysFault:
        if (!switch_flg.SysFault_Flag)
        {
            alm_voHandleRst();
            Switch_sys_FSM(&SysInit_state);
            FSM_voInit();
        }
        break;

    default:
        break;
    }
}

void SysInit_hook(void)
{
    /* code */

    /* CommunicationInit */
    CommuInit();
    if (adc_stDownOut.blADCCalibFlg && adc_stUpOut.blADCCalibFlg && sysfsm_stFlg.CommuRdy_Flag)
    {
        /* BikeLightInit */
        bikelight_voBikeLightInit(&BikeLedCheck_F);
        bikelight_voBikeLightInit(&BikeLedCheck_DC_1);
        bikelight_voBikeLightInit(&BikeLedCheck_DC_2);
        /* BikeThrottleInit */
        bikethrottle_voBikeThrottleInit();
        /* TorqueSensorInit */
        torsensor_voTorSensorInit();

        bikebrake_voBikeBrakeInit();

        BikeRatioCal_Init(ass_ParaCong.uwNmFrontChainring, ass_ParaCong.uwNmBackChainring);
        switch_flg.SysRdy_Flag = TRUE;
        
        AssitCoefInit();
    }
}

void SysInit_TbcUphook(void)
{
    adc_voCalibration(&adc_stCof, &adc_stDownOut, &adc_stUpOut);

}
void SysInit_TbcDownhook(void)
{}
void SysInit_Tbshook(void)
{}

void SysRdy_hook(void)
{
  
    if (switch_flg.SysCoef_Flag == FALSE && (cp_ulSystickCnt - sysfsm_ulInitOutCt)>=500) 
    {
        AssitCoefInit();
        bikelight_voBikeLightCoef(ass_ParaCong.uwLightConfig);
        ass_voAssistCoefCal();
        bikespeed_voBikeSpeedCof();
        bikethrottle_voBikeThrottleCof();
        cadence_voCadenceCof(); 
        torsensor_voTorSensorCof();
        scm_voSpdCtrMdCoef();
        switch_flg.SysCoef_Flag = TRUE;
    }

    //  Coef Update
    if (cp_stFlg.ParaAssistUpdateFinishFlg == TRUE && cp_stFlg.ParaUpdateFlg == FALSE)
    {
        if (FSM2nd_Run_state.state == Exit)
        {
            AssitCoefInit();
            bikelight_voBikeLightCoef(ass_ParaCong.uwLightConfig);
            ass_voAssistCoefCal();
            bikespeed_voBikeSpeedCof();
            bikethrottle_voBikeThrottleCof();
            cadence_voCadenceCof();
            torsensor_voTorSensorCof();
            cp_stFlg.ParaAssistUpdateFinishFlg = FALSE;
        }
        else
        {}
    }
    else
    {}

    if (cp_stFlg.ParaMotorDriveUpdateFinishFlg == TRUE && cp_stFlg.ParaUpdateFlg == FALSE)
    {
        if (FSM2nd_Run_state.state == Exit)
        {
            scm_voSpdCtrMdCoef();
            alm_voCoef();
            cp_stFlg.ParaMotorDriveUpdateFinishFlg = FALSE;
        }
        else
        {}
    }
    else
    {}

    //   2ND FSM Control
    FSM_2nd_Main();
    FSM2nd_Run_state.Event_hook();

    //   Run Flg Judge
    if ((signal_state.Assist == 0 && signal_state.Sensor == 0) || (switch_flg.SysFault_Flag) || (power_stPowStateOut.powerstate == POWER_OFF_END) ||
        (switch_flg.SysCoef_Flag == FALSE) || (((cp_stFlg.ParaMotorDriveUpdateFinishFlg == TRUE) || ( cp_stFlg.ParaAssistUpdateFinishFlg == TRUE))&&FSM2nd_Run_state.state == Exit) )
    {
        switch_flg.SysRun_Flag = FALSE;
    }
    else
    {
        switch_flg.SysRun_Flag = TRUE;
    }


    if (switch_flg.SysRun_Flag == 0)
    {

        if (switch_flg.Standby_flg == 1)
        {
            /* send display disable message*/
        }
        else if (switch_flg.Standby_flg == 2)
        {
            /* EEPROM write and send BMS shut down message */
        }
    }
    else
    {}
}

void SysRdy_TbcUphook(void)
{
   gtemp ++;
}
void SysRdy_TbcDownhook(void)
{
    FSM2nd_Run_state.Tbcup_hook();
    FSM2nd_Run_state.Tbcdown_hook();
}
void SysRdy_Tbshook(void)
{
    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);

    FSM2nd_Run_state.Tbs_hook();
}

void SysFault_TbcUphook(void)
{
    /* Add your code below */
    alm_voHandleTBC(&alm_stIn);
}
void SysFault_hook(void)
{
    /* Bike Sensor Fault Handle */
    alm_voHandle1MS(&alm_stBikeIn);
}
void SysFault_TbcDownhook(void)
{}
void SysFault_Tbshook(void)
{}

void Switch_sys_FSM(FSM_SYS_HOOK *in)
{
    swSwitchStateCnts = 0;
    switch_flg.Standby_flg = 0;
    void (*function)(void) = switchfuction[FSM1st_Sys_state.state][in->state];
    function();
    FSM1st_Sys_state = *in;
}

void ReadytoRun(void)
{}

void InittoFault(void)
{
//  event_voAlarmParaSaveCtrl();
}

void RdytoFault(void)
{
//  event_voAlarmParaSaveCtrl();
}

void CommuInit(void)
{
    if (sysfsm_stFlg.CommuRdy_Flag == FALSE)
    {
        static ULONG sysfsm_ulCommuCt = 0;
        sysfsm_ulCommuCt++;
        if (sysfsm_ulCommuCt <= 100)
        {
            switch_flg.Standby_flg = 1;
            if (switch_flg.HMI_Flag == 1)
            {}
        }
        else if (sysfsm_ulCommuCt <= 800)
        {
            switch_flg.Standby_flg = 2;
            /*  code */
        }
        else
        {

            switch_flg.Standby_flg = 3;
            /* alarm */
        }
    }
    else
    {}
}