motor_ctl-4/User project/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 "Cadence.h"
#include "CodePara.h"
#include "hwsetup.h"

#ifdef RUN_ARCH_SIM
#include "test_user.h"
#endif
/******************************
 *
 *  Parameter
 *
 ******************************/
CADENCE_COF cadence_stFreGetCof = CADENCE_COF_DEFAULT;
CADENCE_OUT cadence_stFreGetOut = CADENCE_OUT_DEFAULT;

static ULONG cad_pvt_ulCadFreqPu = 0;
/***************************************************************
 Function: cadence_voCadenceCof;
 Description: cadence function coef cal
 Call by: 
 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;
    cadence_stFreGetCof.uwSartIntervalTimeCnt = CADENCE_START_INTERVALTIME / CADENCE_TIM_TIMERUNIT;
    cadence_stFreGetCof.uwNumbersValidPulse2Start = CADENCE_NUMBERS_VALIDPULSE2START;
    cadence_stFreGetCof.uwLfRecordTimeCnt = CADENCE_LF_RECORDTIME / CADENCE_TIM_TIMERUNIT;
    cadence_stFreGetCof.uwLfMinFrePu = ((((ULONG)CADENCE_LF_MINFRE << 10) / 100) << 10) / FBASE; // Q20
    cadence_stFreGetCof.uwHfMaxTimeCnt = CADENCE_HF_MAXTIME / CADENCE_TIM_TIMERUNIT;
    cadence_stFreGetCof.uwErrorResetCnt = CADENCE_ERROR_RESETTIME / CADENCE_TIM_TIMERUNIT;
    cadence_stFreGetCof.uwTimerUnit = CADENCE_TIM_TIMERUNIT;
    cadence_stFreGetCof.uwCadenceLPFgain = CADENCE_LPF_GAIN;
    cadence_stFreGetCof.uwMaxCadenceFre = ((ULONG)CADENCE_MAX_FREQUENCY << 20) / FBASE; // Q20
}

/***************************************************************
 Function: cadence_voCadenceIdle;
 Description: 
 Call by: 
 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) 
        {
            cadence_stFreGetOut.uwCaputureOverflowCnt = 0;
            cadence_stFreGetOut.uwCaputureNumCnt = 0;
        }
    }
    else
    {
        if (cadence_stFreGetOut.cadence_dir == CADENCE_DIR_BACKWARD) 
        {
            cadence_stFreGetOut.uwCaputureOverflowCnt = 0;
            cadence_stFreGetOut.uwCaputureNumCnt = 0;
            cadence_stFreGetOut.cadence_fsm = CADENCE_BACKWOR;
        }

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

        if (cadence_stFreGetOut.uwCaputureNumCnt >= cadence_stFreGetCof.uwNumbersValidPulse2Start)
        {
            cadence_stFreGetOut.blCadenceCalStartState = TRUE;
            cadence_stFreGetOut.uwCaputureOverflowCnt = 0;
            cadence_stFreGetOut.uwCaputureNumCnt = 1;
            cadence_stFreGetOut.cadence_fsm = CADENCE_HFreWork;
            cadence_stFreGetOut.uwForwardCnt = 0;
            cadence_stFreGetOut.uwCaputure1Cnt = (UWORD)TIMER1_CAP_CANDANCE;
            cad_pvt_ulCadFreqPu = 1 * CADENCESPEED_KMPERH2FREQPU;
            cadence_stFreGetOut.uwFrequencyPu =  (UWORD)cad_pvt_ulCadFreqPu;
            cadence_stFreGetOut.uwLPFFrequencyPu = (cadence_stFreGetOut.uwLPFFrequencyPu * cadence_stFreGetCof.uwCadenceLPFgain +
                                                        cadence_stFreGetOut.uwFrequencyPu * (100 - cadence_stFreGetCof.uwCadenceLPFgain)) /
                                                       100;
        }
    }
}
/***************************************************************
 Function: cadence_voCadenceHighFrequencyWork
 Description: 
 Call by: functions in main loop
 Input Variables: N/A
 Output/Return Variables: N/A
 Subroutine Call: N/A
 Reference: N/A
****************************************************************/
static ULONG ulCaputureCntErr = 0,ulCaputureCntErrLast = 0;
static void cadence_voCadenceHighFrequencyWork(UWORD source)
{
    if (source == 1 && cadence_stFreGetOut.uwCaputureNumCnt == 1)
    {
        cadence_stFreGetOut.uwCaputureOverflowCnt++;
        if (timer_interrupt_flag_get(TIMER1, TIMER_INT_FLAG_CH2) != 0)
        {
            cadence_stFreGetOut.uwCaputure2Cnt =(UWORD)TIMER1_CAP_CANDANCE;
            if(cadence_stFreGetOut.uwCaputure2Cnt > 9000) // TIMER1 half Period 
            {
                cadence_stFreGetOut.uwOverflowfirst= 1;
            }
            else
            {
                cadence_stFreGetOut.uwOverflowfirst= 2;
            }   
            ulCaputureCntErrLast = ulCaputureCntErr;
            if(cadence_stFreGetOut.uwOverflowfirst == 1)
            {
                cadence_stFreGetOut.uwCaputureOverflowCnt -=1;
                ulCaputureCntErr = ((ULONG)cadence_stFreGetOut.uwCaputureOverflowCnt * TIM1CLK_KHZ * cadence_stFreGetCof.uwTimerUnit) + cadence_stFreGetOut.uwCaputure2Cnt 
                                    -cadence_stFreGetOut.uwCaputure1Cnt;
                cadence_stFreGetOut.uwCaputureOverflowCnt = 1; 
                cadence_stFreGetOut.uwOverflowfirst = 0;
                
            }
            else
            {
                ulCaputureCntErr = ((ULONG)cadence_stFreGetOut.uwCaputureOverflowCnt * TIM1CLK_KHZ * cadence_stFreGetCof.uwTimerUnit) + cadence_stFreGetOut.uwCaputure2Cnt 
                                    -cadence_stFreGetOut.uwCaputure1Cnt;
                cadence_stFreGetOut.uwCaputureOverflowCnt = 0;
                cadence_stFreGetOut.uwOverflowfirst = 0;
            }

            /* Cadence Freq Cal */
            cad_pvt_ulCadFreqPu = (ULONG)(((UQWORD)TIM1CLK_KHZ * 1000 << 20) / ((((UQWORD)ulCaputureCntErr + (UQWORD)ulCaputureCntErrLast)>>1)* cadence_stFreGetCof.uwNumbersPulses * FBASE));

            cadence_stFreGetOut.uwCaputureNumCnt = 1;
            cadence_stFreGetOut.uwCaputure1Cnt = cadence_stFreGetOut.uwCaputure2Cnt; 
            timer_interrupt_flag_clear(TIMER1, TIMER_INT_FLAG_CH2);
        }
        else
        {
            cadence_stFreGetOut.uwOverflowfirst= 0;
        }
    }
    else if (source == 2)
    {

        if (cadence_stFreGetOut.uwCaputureNumCnt == 0)
        {
            cadence_stFreGetOut.uwCaputureNumCnt = 1;
            cadence_stFreGetOut.uwCaputure1Cnt = (UWORD)TIMER1_CAP_CANDANCE;  
        }
        else if (cadence_stFreGetOut.uwCaputureNumCnt == 1)
        {
            cadence_stFreGetOut.uwForwardCnt++;
            cadence_stFreGetOut.uwCaputureNumCnt = 2;

            cadence_stFreGetOut.uwCaputure2Cnt = (UWORD)TIMER1_CAP_CANDANCE;

            ulCaputureCntErrLast = ulCaputureCntErr;

            ulCaputureCntErr = ((ULONG)cadence_stFreGetOut.uwCaputureOverflowCnt * 720 * cadence_stFreGetCof.uwTimerUnit) -
                            cadence_stFreGetOut.uwCaputure1Cnt + cadence_stFreGetOut.uwCaputure2Cnt;
            cadence_stFreGetOut.uwCaputureOverflowCnt = 0;

            /* Cadence Freq Cal */
            cad_pvt_ulCadFreqPu = (ULONG)(((UQWORD)TIM1CLK_KHZ * 1000 << 20) / ((((UQWORD)ulCaputureCntErr + (UQWORD)ulCaputureCntErrLast)>>1)* cadence_stFreGetCof.uwNumbersPulses * FBASE));

            cadence_stFreGetOut.uwCaputureNumCnt = 1;
            cadence_stFreGetOut.uwCaputure1Cnt = cadence_stFreGetOut.uwCaputure2Cnt; 
            
        }
        else
        {
        	//do noting
        }
    }
    else
    {
    	//do noting
    }

    if (cadence_stFreGetOut.cadence_dir == CADENCE_DIR_BACKWARD) // 100ms
    {
        cadence_stFreGetOut.cadence_fsm = CADENCE_BACKWOR;
    }

    if (cad_pvt_ulCadFreqPu > cadence_stFreGetCof.uwMaxCadenceFre)
    {
        cadence_stFreGetOut.uwFrequencyPu = 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;
        cp_stHistoryPara.uwCadSensorAlamTimes++;
    }
    else if (cadence_stFreGetOut.uwCaputureOverflowCnt > cadence_stFreGetCof.uwHfMaxTimeCnt)
    {
        cadence_stFreGetOut.uwFrequencyPu = 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;
    }
    else 
    {
        cadence_stFreGetOut.uwFrequencyPu = cad_pvt_ulCadFreqPu;
        
        cadence_stFreGetOut.uwLPFFrequencyPu = (cadence_stFreGetOut.uwLPFFrequencyPu * cadence_stFreGetCof.uwCadenceLPFgain +
                                                cadence_stFreGetOut.uwFrequencyPu * (100 - cadence_stFreGetCof.uwCadenceLPFgain)) / 100;
        cadence_stFreGetOut.uwFreqPercent = (UWORD)(((ULONG)cadence_stFreGetOut.uwLPFFrequencyPu << 14) / cadence_stFreGetCof.uwMaxCadenceFre); // Q14
    }
}
/***************************************************************
 Function: cadence_voCadenceBackword
 Description: 
 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)
{
    cadence_stFreGetOut.uwFrequencyPu = 0;
    cadence_stFreGetOut.uwLPFFrequencyPu = 0;
    cadence_stFreGetOut.uwFreqPercent = 0;
    if (cadence_stFreGetOut.cadence_dir == CADENCE_DIR_FORWARD)
    {
        cadence_stFreGetOut.uwCaputureOverflowCnt = 0;
        cadence_stFreGetOut.uwCaputureNumCnt = 1;
        cadence_stFreGetOut.uwFreqPercent = 0;
        cadence_stFreGetOut.cadence_fsm = CADENCE_HFreWork;
        cadence_stFreGetOut.uwCaputure1Cnt = (UWORD)TIMER1_CAP_CANDANCE;
        cad_pvt_ulCadFreqPu = 1 * CADENCESPEED_KMPERH2FREQPU;
        cadence_stFreGetOut.uwFrequencyPu = (UWORD)cad_pvt_ulCadFreqPu;  
        cadence_stFreGetOut.uwLPFFrequencyPu = (cadence_stFreGetOut.uwLPFFrequencyPu * cadence_stFreGetCof.uwCadenceLPFgain +
                                                    cadence_stFreGetOut.uwFrequencyPu * (100 - cadence_stFreGetCof.uwCadenceLPFgain)) /
                                                   100;
    }
}
/***************************************************************
 Function: cadence_voCadenceDir
 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 (IO_CANDANCEDIR_STATE == 0)
    {
        cadence_stFreGetOut.cadence_dir = CADENCE_DIR_FORWARD;
    }
    else 
    {
        cadence_stFreGetOut.cadence_dir = CADENCE_DIR_BACKWARD;
    }
}
/***************************************************************
 Function: cadence_voCadenceError
 Description: 
 Call by: 
 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: 
Input Variables: N/A
Output/Return Variables: N/A
Subroutine Call: N/A
Reference: N/A
****************************************************************/
void cadence_voCadenceInit(void)
{
    cadence_stFreGetOut.uwFrequencyPu = 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.blCadenceSensorErrorFlg = FALSE;
    cadence_stFreGetOut.blCadenceCalStartState = FALSE;
    cadence_stFreGetOut.cadence_fsm = CADENCE_IDLE;
    cadence_stFreGetOut.cadence_dir = CADENCE_DIR_IDLE;
}
/***************************************************************
 Function: cadence_voCadenceCal
 Description: cadence frequency get
 Call by: 
 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_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!
*************************************************************************/