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 "typedefine.h"
#include "mathtool.h"
#include "cadence.h"
#include "CodePara.h"
#include "hwsetup.h"
/******************************
 *
 *  Parameter
 *
 ******************************/
CADENCE_COF cadence_stFreGetCof = CADENCE_COF_DEFAULT;
CADENCE_OUT cadence_stFreGetOut = CADENCE_OUT_DEFAULT;

/***************************************************************
 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;
    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_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) 
        {
            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;
        }

        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 = 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 / 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
****************************************************************/
UWORD cadanceCnt;
ULONG ulCaputureCntErr = 0;
static void cadence_voCadenceHighFrequencyWork(UWORD source)
{ 
    if (source == 1 && cadence_stFreGetOut.uwCaputureNumCnt == 1)
    {
        cadence_stFreGetOut.uwCaputureOverflowCnt++;
    }
    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++;
            cadanceCnt++;
            cadence_stFreGetOut.uwCaputureNumCnt = 2;
            cadence_stFreGetOut.uwCaputure2Cnt = (UWORD)TIMER1_CAP_CANDANCE;

            ulCaputureCntErr = ((ULONG)cadence_stFreGetOut.uwCaputureOverflowCnt * 720 * cadence_stFreGetCof.uwTimerUnit) -
                               cadence_stFreGetOut.uwCaputure1Cnt + cadence_stFreGetOut.uwCaputure2Cnt;
            if (ulCaputureCntErr > 10)
            {
                cadence_stFreGetOut.uwFrequencyPu =
                    (UWORD)(((SQWORD)720000 << 20) / ((SQWORD)ulCaputureCntErr * cadence_stFreGetCof.uwNumbersPulses * FBASE));
                cadence_stFreGetOut.uwLPFFrequencyPu = (cadence_stFreGetOut.uwLPFFrequencyPu * cadence_stFreGetCof.uwCadenceLPFgain +
                                                        cadence_stFreGetOut.uwFrequencyPu * (100 - cadence_stFreGetCof.uwCadenceLPFgain)) /
                                                       100;
                cadence_stFreGetOut.uwFreqPercent = ((ULONG)cadence_stFreGetOut.uwLPFFrequencyPu << 14) / cadence_stFreGetCof.uwMaxCadenceFre; // Q14
            }
            cadence_stFreGetOut.uwCaputure1Cnt = cadence_stFreGetOut.uwCaputure2Cnt;
            cadence_stFreGetOut.uwCaputureOverflowCnt = 0;
            cadence_stFreGetOut.uwCaputureNumCnt = 1;
        }
    }


    if (cadence_stFreGetOut.cadence_dir == CADENCE_DIR_BACKWARD) 
    {
        cadence_stFreGetOut.cadence_fsm = CADENCE_BACKWOR;
    }
    
    /* Enter idle state when time out*/
    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;
    }
    
    /* Candence over frequency error */
    if (cadence_stFreGetOut.uwLPFFrequencyPu > 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++;
    }
}

/***************************************************************
 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.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 = 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.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 (IO_CANDANCEDIR_STATE != 0)
    {
        cadence_stFreGetOut.cadence_dir = CADENCE_DIR_BACKWARD;
    }
    if (IO_CANDANCEDIR_STATE == 0)
    {
        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.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_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!
*************************************************************************/