motor_ctl-1/3.BasicFunction/Source/Cadence.c

/**
 * @file Cadence.c
 * @author Wang, Zhiyu(wangzy49@midea.com)
 * @brief Cadence of ebike
 * @version 0.1
 * @date 2021-09-29
 *
 * @copyright Copyright (c) 2021
 *
 */

/************************************************************************
 Beginning of File, do not put anything above here except notes
 Compiler Directives:
*************************************************************************/
#include "syspar.h"
#include "typedefine.h"
#include "mathtool.h"
//#include "cadence.h"
#include "CodePara.h"
#include "AssistCurve.h" 
#include "CadAssist.h"
#include "bikeinformation.h"
//#include "api.h"
#include "board_config.h"
#include "UserGpio_Config.h"
/******************************
 *
 *  Parameter
 *
 ******************************/
CADENCE_COF cadence_stFreGetCof = CADENCE_COF_DEFAULT;
CADENCE_OUT cadence_stFreGetOut = CADENCE_OUT_DEFAULT;

extern BIKESPEED_OUT    bikespeed_stFreGetOut;
CADDIR_STRUCT Cad_Dir={0,0,2};
/***************************************************************
 Function: cadence_voCadenceCof;
 Description: cadence function coef cal
 Call by: functions in main loop;
 Input Variables: N/A
 Output/Return Variables: N/A
 Subroutine Call: N/A;
 Reference: N/A
****************************************************************/
void cadence_voCadenceCof(void)
{
    // cadence_stFreGetCof.uwNumbersPulses = CADENCE_NUMBERS_PULSES;
    if(cp_stFlg.RunModelSelect==TorqAssist)
    {
       cadence_stFreGetCof.uwSartIntervalTimeCnt = ((ULONG)TORQUE_START_INTERVALTIME_MS * 1000) / TORQUE_TIM_TIMERUNIT_US;
       cadence_stFreGetCof.uwNumbersValidPulse2Start = TORQUE_NUMBERS_VALIDPULSE2START;
       cadence_stFreGetCof.uwLfRecordTimeCnt = ((ULONG)TORQUE_LF_RECORDTIME_MS * 1000) / TORQUE_TIM_TIMERUNIT_US;
       cadence_stFreGetCof.uwLfMinFrePu = ((((ULONG)TORQUE_LF_MINFRE << 10) / 100) << 10) / FBASE; // Q20
       if(cadence_stFreGetCof.uwTorque_NumbersPulses <= 36)
       {
           cadence_stFreGetCof.uwHfMaxTimeCnt = ((ULONG)((800L*12)/cadence_stFreGetCof.uwTorque_NumbersPulses)*1000) / TORQUE_TIM_TIMERUNIT_US;
       }
       else
       {
           cadence_stFreGetCof.uwHfMaxTimeCnt = ((ULONG)((800L*12)/36)*1000) / TORQUE_TIM_TIMERUNIT_US;
       }

       cadence_stFreGetCof.uwErrorResetCnt = ((ULONG)TORQUE_ERROR_RESETTIME_MS*1000) / TORQUE_TIM_TIMERUNIT_US;
       cadence_stFreGetCof.uwTimerUnit = TORQUE_TIM_TIMERUNIT_US;
       cadence_stFreGetCof.uwCadenceLPFgain = TORQUE_LPF_GAIN;
       cadence_stFreGetCof.uwMaxCadenceFre = ((ULONG)TORQUE_MAX_FREQUENCY << 20) / FBASE; // Q20
    }
    else
    {
    cadence_stFreGetCof.uwSartIntervalTimeCnt = ((ULONG)CADENCE_START_INTERVALTIME_MS * 1000) / CADENCE_TIM_TIMERUNIT_US;
    cadence_stFreGetCof.uwNumbersValidPulse2Start = CADENCE_NUMBERS_VALIDPULSE2START;
    cadence_stFreGetCof.uwLfRecordTimeCnt = ((ULONG)CADENCE_LF_RECORDTIME_MS * 1000) / CADENCE_TIM_TIMERUNIT_US;
    cadence_stFreGetCof.uwLfMinFrePu = ((((ULONG)CADENCE_LF_MINFRE << 10) / 100) << 10) / FBASE; // Q20
        if(cadence_stFreGetCof.uwCad_NumbersPulses <= 36)
    {
            cadence_stFreGetCof.uwHfMaxTimeCnt = ((ULONG)((800L*12)/cadence_stFreGetCof.uwCad_NumbersPulses)*1000) / CADENCE_TIM_TIMERUNIT_US;
    }
    else
    {
        cadence_stFreGetCof.uwHfMaxTimeCnt = ((ULONG)((800L*12)/36)*1000) / CADENCE_TIM_TIMERUNIT_US;
    }
    
    cadence_stFreGetCof.uwErrorResetCnt = ((ULONG)CADENCE_ERROR_RESETTIME_MS*1000) / CADENCE_TIM_TIMERUNIT_US;
    cadence_stFreGetCof.uwTimerUnit = CADENCE_TIM_TIMERUNIT_US;
    cadence_stFreGetCof.uwCadenceLPFgain = CADENCE_LPF_GAIN;
    cadence_stFreGetCof.uwMaxCadenceFre = ((ULONG)CADENCE_MAX_FREQUENCY << 20) / FBASE; // Q20
}
}

/***************************************************************
 Function: cadence_voFreGet;
 Description: cadence frequency get
 Call by: functions in main loop;
 Input Variables: N/A
 Output/Return Variables: N/A
 Subroutine Call: N/A;
 Reference: N/A
****************************************************************/
static void cadence_voCadenceIdle(UWORD source)
{
    if (source == 1)
    {
       // cadence_stFreGetOut.uwCaputureOverflowCnt++;
        if (cadence_stFreGetOut.uwCaputureOverflowCnt > cadence_stFreGetCof.uwSartIntervalTimeCnt)  //torque-1s;Cad-5S
        {
            cadence_stFreGetOut.uwCaputureOverflowCnt = 0;
            cadence_stFreGetOut.uwCaputureNumCnt = 0;
        }
    }
    else
    {
        if (cadence_stFreGetOut.cadence_dir == CADENCE_DIR_BACKWARD) // 0.25s
        {
            cadence_stFreGetOut.uwCaputureOverflowCnt = 0;
            cadence_stFreGetOut.uwCaputureNumCnt = 0;
        }

        if (cadence_stFreGetOut.cadence_dir == CADENCE_DIR_FORWARD)
        {
            cadence_stFreGetOut.uwCaputureNumCnt++;
            cadence_stFreGetOut.uwCaputureOverflowCnt = 0;
        }

        if (cadence_stFreGetOut.uwCaputureNumCnt >= cadence_stFreGetCof.uwNumbersValidPulse2Start) //1
        {
            cadence_stFreGetOut.blCadenceCalStartState = TRUE;
            cadence_stFreGetOut.uwCaputureOverflowCnt = 0;
            cadence_stFreGetOut.uwCaputureNumCnt = 0;
            cadence_stFreGetOut.cadence_fsm = CADENCE_HFreWork;
            cadence_stFreGetOut.uwForwardCnt = 0;
        }
    }
}

/***************************************************************
 Function: cadence_voFreGet;
 Description: cadence frequency get
 Call by: functions in main loop;
 Input Variables: N/A
 Output/Return Variables: N/A
 Subroutine Call: N/A;
 Reference: N/A
****************************************************************/
static void cadence_voCadenceLowFrequencyWork(UWORD source)
{
    if (source == 1)
    {
       // cadence_stFreGetOut.uwCaputureOverflowCnt++;
        if (cadence_stFreGetOut.uwCaputureOverflowCnt == cadence_stFreGetCof.uwLfRecordTimeCnt)
        {
            cadence_stFreGetOut.uwFrequencyPu = ((SQWORD)cadence_stFreGetOut.uwCaputureNumCnt << 20) * 1000 /
                                                (cadence_stFreGetCof.uwLfRecordTimeCnt * cadence_stFreGetCof.uwTimerUnit) /
                                                cadence_stFreGetCof.uwNumbersPulses /1000 /FBASE;
            cadence_stFreGetOut.uwCaputureOverflowCnt = 0;
            cadence_stFreGetOut.uwCaputureNumCnt = 0;

            if (cadence_stFreGetOut.uwFrequencyPu < (cadence_stFreGetCof.uwLfMinFrePu))
            {
                cadence_stFreGetOut.uwFrequencyPu = 0;
                cadence_stFreGetOut.uwLPFFrequencyPu = 0;
                cadence_stFreGetOut.blCadenceCalStartState = FALSE;
                cadence_stFreGetOut.uwCaputureOverflowCnt = 0;
                cadence_stFreGetOut.uwCaputureNumCnt = 0;
                cadence_stFreGetOut.cadence_fsm = CADENCE_IDLE;
            }
        }
    }
    else
    {
        cadence_stFreGetOut.uwCaputureNumCnt++;
    }

    if (cadence_stFreGetOut.uwFrequencyPu > cadence_stFreGetCof.uwMaxCadenceFre)
    {
        cadence_stFreGetOut.uwFrequencyPu = 0;
        cadence_stFreGetOut.blCadenceCalStartState = FALSE;
        cadence_stFreGetOut.blCadenceSensorErrorFlg = TRUE;
        cadence_stFreGetOut.uwCaputureOverflowCnt = 0;
        cadence_stFreGetOut.uwCaputureNumCnt = 0;
        cadence_stFreGetOut.uwCaputure2Cnt = 0;
        cadence_stFreGetOut.uwCaputure1Cnt = 0;
        cadence_stFreGetOut.cadence_fsm = CADENCE_ERROR;
    }
}
/***************************************************************
 Function: cadence_voFreGet;
 Description: cadence frequency get
 Call by: functions in main loop;
 Input Variables: N/A
 Output/Return Variables: N/A
 Subroutine Call: N/A;
 Reference: N/A
****************************************************************/
ULONG ulCaputureCntErr = 0;
UWORD test_stoptime1 = 0;
UWORD  Cad_startdeccnt=6;
static void cadence_voCadenceHighFrequencyWork(UWORD source)
{
    UWORD tmp_uwFrequencyPu = 0;
    ULONG tmp_ulStopTime = 0;
    ULONG tmp_ulBikeSpd = 0;
    if (source == 1)
    {
       // cadence_stFreGetOut.uwCaputureOverflowCnt++;
//        if(cadence_stFreGetOut.uwCaputureOverflowCnt > (cadence_stFreGetCof.uwHfMaxTimeCnt + 1))
//        {
//            cadence_stFreGetOut.uwCaputureOverflowCnt = (cadence_stFreGetCof.uwHfMaxTimeCnt + 1);
//        }
        
        if(cadence_stFreGetOut.uwCaputureNumCnt == 1)
        {
            if(cadence_stFreGetOut.uwCaputureOverflowCnt > (cadence_stFreGetOut.uwCntErrLast + 1))//1
            {
                ulCaputureCntErr = ((ULONG)cadence_stFreGetOut.uwCaputureOverflowCnt * (72L * cadence_stFreGetCof.uwTimerUnit))>>8;
                cadence_stFreGetOut.ulCaputureCntErr = ulCaputureCntErr;
                tmp_uwFrequencyPu = (UWORD)(((SQWORD)72000000 << 12) / (((SQWORD)cadence_stFreGetOut.ulCaputureCntErr) * cadence_stFreGetCof.uwNumbersPulses * FBASE));
                cadence_stFreGetOut.uwFrequencyPu = tmp_uwFrequencyPu;
            }
        }
    }
    else if (source == 2)
    {
        cadence_stFreGetOut.uwForwardCnt++;
        if(cadence_stFreGetOut.uwForwardCnt >= 24000)
        {
            cadence_stFreGetOut.uwForwardCnt = 24000;
        }
            
        if (cadence_stFreGetOut.uwCaputureNumCnt == 0)
        {
            cadence_stFreGetOut.uwCaputureNumCnt = 1;
           // cadence_stFreGetOut.uwCaputure1Cnt = (UWORD)CapValutCadence_CCRx();//(UWORD)iCap_GetCaptureValue(HW_BIKESPD_CAD_CAP, CAP_CH(2));
            ulCaputureCntErr = ((ULONG)cadence_stFreGetOut.uwCaputureOverflowCnt * (72L * cadence_stFreGetCof.uwTimerUnit))>>8;
            cadence_stFreGetOut.ulCaputureCntErr = ulCaputureCntErr;
            tmp_uwFrequencyPu = (UWORD)(((SQWORD)72000000 << 12) / (((SQWORD)cadence_stFreGetOut.ulCaputureCntErr) * cadence_stFreGetCof.uwNumbersPulses * FBASE));
            cadence_stFreGetOut.uwFrequencyPu = tmp_uwFrequencyPu;
            cadence_stFreGetOut.uwFrequencyLastPu = tmp_uwFrequencyPu;
            
            cadence_stFreGetOut.uwCntErrLast = cadence_stFreGetOut.uwCaputureOverflowCnt;
            cadence_stFreGetOut.uwCaputureOverflowCnt = 0;
        }
        else if (cadence_stFreGetOut.uwCaputureNumCnt == 1)
        {
            cadence_stFreGetOut.uwCaputureNumCnt = 2;
           // cadence_stFreGetOut.uwCaputure2Cnt = (UWORD)CapValutCadence_CCRx();//(UWORD)iCap_GetCaptureValue(HW_BIKESPD_CAD_CAP, CAP_CH(2));

            ulCaputureCntErr = (((ULONG)cadence_stFreGetOut.uwCaputureOverflowCnt * (72 * cadence_stFreGetCof.uwTimerUnit))  )>>8;
            cadence_stFreGetOut.ulCaputureCntErr = ulCaputureCntErr;
            if (cadence_stFreGetOut.ulCaputureCntErr > 2000)//1000
            {
                tmp_uwFrequencyPu = (UWORD)(((SQWORD)72000000 << 12) / (((SQWORD)cadence_stFreGetOut.ulCaputureCntErr + cadence_stFreGetOut.ulTimer2CntErr) * cadence_stFreGetCof.uwNumbersPulses * FBASE));
                cadence_stFreGetOut.uwCntErrLast = cadence_stFreGetOut.uwCaputureOverflowCnt;
                cadence_stFreGetOut.uwFrequencyLastPu = cadence_stFreGetOut.uwFrequencyPu;
                cadence_stFreGetOut.uwFrequencyPu = tmp_uwFrequencyPu;
                
                cadence_stFreGetOut.ulTimer2CntErr = 0;
            }
            else
            {
                cadence_stFreGetOut.ulTimer2CntErr += ulCaputureCntErr;
            }
           // cadence_stFreGetOut.uwCaputure1Cnt = cadence_stFreGetOut.uwCaputure2Cnt;
            cadence_stFreGetOut.uwCaputureOverflowCnt = 0;
            cadence_stFreGetOut.uwCaputureNumCnt = 1;
        }
    }


    if (cadence_stFreGetOut.cadence_dir == CADENCE_DIR_BACKWARD) // 100ms
    {
        cadence_stFreGetOut.cadence_fsm = CADENCE_BACKWOR;
    }
    
    if(cp_stFlg.RunModelSelect == CadAssist)
    {
    	 if(cadence_stFreGetCof.uwTorque_NumbersPulses <= 12)
       {
       	Cad_startdeccnt=6;
       }
       else
       {
       	Cad_startdeccnt=10;
       	}
        if(cadence_stFreGetOut.uwForwardCnt <= Cad_startdeccnt)
        {
          tmp_ulStopTime = ((ULONG)(Cad_startdeccnt+2 - cadence_stFreGetOut.uwForwardCnt)*cadence_stFreGetCof.uwHfMaxTimeCnt>>3)+(cadence_stFreGetCof.uwHfMaxTimeCnt>>3);
        }
        else
        {
          tmp_ulStopTime = (cadence_stFreGetCof.uwHfMaxTimeCnt>>2)+(cadence_stFreGetCof.uwHfMaxTimeCnt>>3);
        }
    }
    else
    {
        if(cadence_stFreGetOut.uwForwardCnt <= 16)
        {
          tmp_ulStopTime = ((ULONG)(32L - cadence_stFreGetOut.uwForwardCnt)*cadence_stFreGetCof.uwHfMaxTimeCnt>>4);
          if(tmp_ulStopTime>cadence_stFreGetCof.uwHfMaxTimeCnt)
              tmp_ulStopTime=cadence_stFreGetCof.uwHfMaxTimeCnt;
        }
        else
        {
          tmp_ulStopTime = cadence_stFreGetCof.uwHfMaxTimeCnt;//>>1;
        }
    }
    
    tmp_ulBikeSpd = (bikespeed_stFreGetOut.uwLPFFrequencyPu>>6);
    if((tmp_ulBikeSpd) > 12)   // 1hz
    {
        if(tmp_ulBikeSpd > 42)   //3hz
        {
            tmp_ulBikeSpd = 30L;
        }
        else
        {
            tmp_ulBikeSpd = (tmp_ulBikeSpd - 12L);
        }
    }
    else
    {
        tmp_ulBikeSpd = 0;
    }
    
    tmp_ulBikeSpd = tmp_ulBikeSpd * ((1L<<19)/(30L));  //Q20
     #if(INTELLIGENCADGEAR_EN==0)  
    tmp_ulStopTime = ((UQWORD)tmp_ulStopTime * ((1L<<20) - (tmp_ulBikeSpd)))>>20;
    #endif
    test_stoptime1 = tmp_ulStopTime;
    
    if (cadence_stFreGetOut.uwCaputureOverflowCnt >= tmp_ulStopTime) // 150ms--600ms
    {
        cadence_stFreGetOut.ulTimer2CntErr = 0;
        cadence_stFreGetOut.ulCaputureCntErr = 100000;
        cadence_stFreGetOut.uwFrequencyPu = 0;
        cadence_stFreGetOut.uwCntErrLast = 0;
        cadence_stFreGetOut.uwFrequencyLastPu = 0;
        cadence_stFreGetOut.uwLPFFrequencyPu = 0;
        cadence_stFreGetOut.blCadenceCalStartState = FALSE;
        cadence_stFreGetOut.blCadenceSensorErrorFlg = FALSE;
        cadence_stFreGetOut.uwCaputureOverflowCnt = 0;
        cadence_stFreGetOut.uwCaputureNumCnt = 0;
        cadence_stFreGetOut.uwCaputure2Cnt = 0;
        cadence_stFreGetOut.uwCaputure1Cnt = 0;
        cadence_stFreGetOut.cadence_dir = CADENCE_DIR_IDLE;
        cadence_stFreGetOut.cadence_fsm = CADENCE_IDLE;
        cadence_stFreGetOut.uwFreqPercent = 0;
        cadence_stFreGetOut.uwForwardCnt = 0;
    }

    if (cadence_stFreGetOut.uwLPFFrequencyPu > cadence_stFreGetCof.uwMaxCadenceFre)
    {
        cadence_stFreGetOut.ulTimer2CntErr = 0;
        cadence_stFreGetOut.uwFrequencyPu = 0;
        cadence_stFreGetOut.ulCaputureCntErr = 100000;
        cadence_stFreGetOut.uwCntErrLast = 0;
        cadence_stFreGetOut.uwFrequencyLastPu = 0;
        cadence_stFreGetOut.uwLPFFrequencyPu = 0;
        cadence_stFreGetOut.blCadenceCalStartState = FALSE;
        cadence_stFreGetOut.blCadenceSensorErrorFlg = TRUE;
        cadence_stFreGetOut.uwCaputureOverflowCnt = 0;
        cadence_stFreGetOut.uwCaputureNumCnt = 0;
        cadence_stFreGetOut.uwCaputure2Cnt = 0;
        cadence_stFreGetOut.uwCaputure1Cnt = 0;
        cadence_stFreGetOut.cadence_dir = CADENCE_DIR_ERROR;
        cadence_stFreGetOut.cadence_fsm = CADENCE_ERROR;
        cadence_stFreGetOut.uwFreqPercent = 0;
        cadence_stFreGetOut.uwForwardCnt = 0;
        cp_stHistoryPara.uwCadSensorAlamTimes++;
    }
}

/***************************************************************
 Function: cadence_voFreGet;
 Description: cadence frequency get
 Call by: functions in main loop;
 Input Variables: N/A
 Output/Return Variables: N/A
 Subroutine Call: N/A;
 Reference: N/A
****************************************************************/
static void cadence_voCadenceBackword(UWORD source)
{

//    if (source == 1)
//    {
//        cadence_stFreGetOut.uwCaputureOverflowCnt++;
//    }
    cadence_stFreGetOut.ulTimer2CntErr = 0;
    cadence_stFreGetOut.uwFrequencyPu = 0;
    cadence_stFreGetOut.ulCaputureCntErr = 100000;
    cadence_stFreGetOut.uwCntErrLast = 0;
    cadence_stFreGetOut.uwFrequencyLastPu = 0;
    cadence_stFreGetOut.uwLPFFrequencyPu = 0;
    cadence_stFreGetOut.uwFreqPercent = 0;
    if (cadence_stFreGetOut.cadence_dir == CADENCE_DIR_FORWARD)
    {
        cadence_stFreGetOut.uwCaputureOverflowCnt = 0;
        cadence_stFreGetOut.uwCaputureNumCnt = 0;
        cadence_stFreGetOut.uwCaputure2Cnt = 0;
        cadence_stFreGetOut.uwCaputure1Cnt = 0;
        cadence_stFreGetOut.uwFreqPercent = 0;
        cadence_stFreGetOut.cadence_fsm = CADENCE_HFreWork;
    }

    //    if (cadence_stFreGetOut.uwCaputureOverflowCnt >= cadence_stFreGetCof.uwHfMaxTimeCnt) // 100ms
    //    {
    //        cadence_stFreGetOut.uwFrequencyPu = 0;
    //        cadence_stFreGetOut.uwCntErrLast = 0;
    //        cadence_stFreGetOut.uwFrequencyLastPu = 0;
    //        cadence_stFreGetOut.uwLPFFrequencyPu = 0;
    //        cadence_stFreGetOut.blCadenceCalStartState = FALSE;
    //        cadence_stFreGetOut.blCadenceSensorErrorFlg = FALSE;
    //        cadence_stFreGetOut.uwCaputureOverflowCnt = 0;
    //        cadence_stFreGetOut.uwCaputureNumCnt = 0;
    //        cadence_stFreGetOut.uwCaputure2Cnt = 0;
    //        cadence_stFreGetOut.uwCaputure1Cnt = 0;
    //        cadence_stFreGetOut.cadence_dir = CADENCE_DIR_IDLE;
    //        cadence_stFreGetOut.cadence_fsm = CADENCE_IDLE;
    //        cadence_stFreGetOut.uwFreqPercent = 0;

    //    }
}
/***************************************************************
 Function: cadence_voFreGet;
 Description: cadence frequency get
 Call by: functions in main loop;
 Input Variables: N/A
 Output/Return Variables: N/A
 Subroutine Call: N/A;
 Reference: N/A
****************************************************************/
static void cadence_voCadenceDir(void)
{
    /*  踏频方向端口 */

//  if(Get_CAD_PORT() == RESET)
//    {
//       if(Cad_Dir.uwPortSta!=0)
//        {
//            Cad_Dir.uwPortSta=0;
//            if(Cad_Dir.uwPortLowcnt>( Cad_Dir.uwPortHightcnt+Cad_Dir.uwPortHightcnt>>2))
//             {
//                 cadence_stFreGetOut.cadence_dir=  CADENCE_DIR_BACKWARD;
//                 cadence_voCadenceInit();
//                 cadence_stFreGetOut.cadence_fsm = CADENCE_IDLE;
//             }
//        }
//        if(Cad_Dir.uwPortLowcnt<60000)
//         Cad_Dir.uwPortLowcnt++;
//     }
//     else
//     {
//      if(Cad_Dir.uwPortSta==0)
//       {
//             Cad_Dir.uwPortSta=1;
//             if(Cad_Dir.uwPortHightcnt>(Cad_Dir.uwPortLowcnt+4))
//             {
//                 cadence_stFreGetOut.cadence_dir = CADENCE_DIR_FORWARD;
//             }
//             else if(Cad_Dir.uwPortLowcnt>( Cad_Dir.uwPortHightcnt+Cad_Dir.uwPortHightcnt>>2))
//             {
//                 cadence_stFreGetOut.cadence_dir=  CADENCE_DIR_BACKWARD;
//                 cadence_voCadenceInit();
//                 cadence_stFreGetOut.cadence_fsm = CADENCE_IDLE;
//             }
//             Cad_Dir.uwPortLowcnt=0;
//             Cad_Dir.uwPortHightcnt=0;
//        }
//        if(Cad_Dir.uwPortHightcnt<60000)
//             Cad_Dir.uwPortHightcnt++;
//     }


    cadence_stFreGetOut.cadence_dir = CADENCE_DIR_FORWARD;
}
/***************************************************************
 Function: cadence_voFreGet;
 Description: cadence frequency get
 Call by: functions in main loop;
 Input Variables: N/A
 Output/Return Variables: N/A
 Subroutine Call: N/A;
 Reference: N/A
****************************************************************/
static void cadence_voCadenceError(UWORD source)
{
    if (source == 1)
    {
        cadence_stFreGetOut.uwCaputureErrorCnt++;
    }

    if (cadence_stFreGetOut.uwCaputureErrorCnt == cadence_stFreGetCof.uwErrorResetCnt)
    {
        cadence_voCadenceInit();
        cadence_stFreGetOut.cadence_fsm = CADENCE_IDLE;
    }
}

/***************************************************************
Function: cadence_voFreGetInit;
Description: cadence frequency get initialization
Call by: functions in main loop;
Input Variables: N/A
Output/Return Variables: N/A
Subroutine Call: N/A;
Reference: N/A
****************************************************************/
void cadence_voCadenceInit(void)
{
    cadence_stFreGetOut.ulTimer2CntErr = 0;
    cadence_stFreGetOut.uwFrequencyPu = 0;
    cadence_stFreGetOut.ulCaputureCntErr = 100000;
    cadence_stFreGetOut.uwCntErrLast = 0;
    cadence_stFreGetOut.uwFrequencyLastPu = 0;
    cadence_stFreGetOut.uwLPFFrequencyPu = 0;
    cadence_stFreGetOut.uwCaputure1Cnt = 0;
    cadence_stFreGetOut.uwCaputure2Cnt = 0;
    cadence_stFreGetOut.uwCaputureNumCnt = 0;
    cadence_stFreGetOut.uwCaputureErrorCnt = 0;
    cadence_stFreGetOut.uwCaputureOverflowCnt = 0;
    cadence_stFreGetOut.uwForwardCnt = 0;
    cadence_stFreGetOut.blCadenceSensorErrorFlg = FALSE;
    cadence_stFreGetOut.blCadenceCalStartState = FALSE;
    cadence_stFreGetOut.cadence_fsm = CADENCE_IDLE;
    cadence_stFreGetOut.cadence_dir = CADENCE_DIR_IDLE;
}

/***************************************************************
 Function: cadence_voFreGet;
 Description: cadence frequency get
 Call by: functions in main loop;
 Input Variables: N/A
 Output/Return Variables: N/A
 Subroutine Call: N/A;
 Reference: N/A
****************************************************************/
void cadence_voCadenceCal(UWORD source)
{
    cadence_voCadenceDir();

    switch (cadence_stFreGetOut.cadence_fsm)
    {
    case CADENCE_IDLE:
        cadence_voCadenceIdle(source);
        break;
    case CADENCE_LFreWork:
        cadence_voCadenceLowFrequencyWork(source);
        break;
    case CADENCE_HFreWork:
        cadence_voCadenceHighFrequencyWork(source);
        break;
    case CADENCE_BACKWOR:
        cadence_voCadenceBackword(source);
        break;
    case CADENCE_ERROR:
        cadence_voCadenceError(source);
        break;
    default:
    	break;
    }
}

/*************************************************************************
 End of this File (EOF)!
 Do not put anything after this part!
*************************************************************************/