motor_ctl-4/User project/3.BasicFunction/Source/bikespeed.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 "bikespeed.h"
#include "CodePara.h"
#include "api.h"
#include "board_config.h"
#include "AssistCurve.h"
/******************************
 *
 *  Parameter
 *
 ******************************/
BIKESPEED_COF bikespeed_stFreGetCof = BIKESPEED_COF_DEFAULT;
BIKESPEED_OUT bikespeed_stFreGetOut = BIKESPEED_OUT_DEFAULT;

BIKESPDPI_IN bikespeed_stPIIn;
static BIKESPDPI_COF  bikespeed_stPICof;
BIKESPDPI_OUT bikespeed_stPIOut;

static ULONG bikespeed_pvt_FreqPu = 0;
/***************************************************************
 Function: bikespeed_voBikeSpeedCof
 Description: Bike speed cof calculation
 Call by: 
 Input Variables: N/A
 Output/Return Variables: N/A
 Subroutine Call: N/A;
 Reference: N/A
****************************************************************/
void bikespeed_voBikeSpeedCof(void)
{
#ifdef RUN_ARCH_SIM 
    bikespeed_stFreGetCof.uwNumbersPulses = BIKESPEED_NUMBERS_PULSES;
#endif
    bikespeed_stFreGetCof.uwSartIntervalTimeCnt = BIKESPEED_START_INTERVALTIME / BIKESPEED_TIM_TIMERUNIT;
    bikespeed_stFreGetCof.uwNumbersValidPulse2Start = BIKESPEED_NUMBERS_VALIDPULSE2START;
    bikespeed_stFreGetCof.uwHfMinTimeCnt = BIKESPEED_HF_MINTIME / BIKESPEED_TIM_TIMERUNIT;
    bikespeed_stFreGetOut.uwCaputureOverflowMinCnt= bikespeed_stFreGetCof.uwHfMinTimeCnt;
    bikespeed_stFreGetCof.uwErrorResetCnt = BIKESPEED_ERROR_RESETTIME / BIKESPEED_TIM_TIMERUNIT;
    bikespeed_stFreGetCof.uwTimerUnit = BIKESPEED_TIM_TIMERUNIT;
    bikespeed_stFreGetCof.uwBikeSpeedLPFGain = BIKESPEED_LPF_GAIN;
    bikespeed_stFreGetCof.uwMaxBikeSpeedFre = ((ULONG)BIKESPEED_MAX_FREQUENCY << 20) / FBASE;

    bikespeed_stFreGetCof.uwBikespeedPwrErrorCnt = BIKESPEED_POWER_ERROR_DETECT / BIKESPEED_POWER_ERROR_TIMEUNIT;
    bikespeed_stFreGetCof.uwBikespeedPwrRecoverCnt = BIKESPEED_POWER_ERROR_RECOVER / BIKESPEED_POWER_ERROR_TIMEUNIT;
    bikespeed_stFreGetCof.uwBikespeedPwrErrorVoltagePuDown = ((ULONG)BIKESPEED_POWER_ERROR_VOLTAGE_DOWN << 14) / VBASE;
    bikespeed_stFreGetCof.uwBikespeedPwrErrorVoltagePuUp = ((ULONG)BIKESPEED_POWER_ERROR_VOLTAGE_UP << 14) / VBASE;
    bikespeed_stFreGetCof.uwWheelPerimeter = 600;
    bikespeed_stFreGetCof.uwMinTriptoUpdate = 100; // 100m
}

/***************************************************************
 Function: bikespeed_voBikeSpeedIdle;
 Description: bike speed function in idel state
 Call by: 
 Input Variables: N/A
 Output/Return Variables: N/A
 Subroutine Call: N/A
 Reference: N/A
****************************************************************/
static void bikespeed_voBikeSpeedIdle(UWORD source)
{
    if (source == 1)
    {
        bikespeed_stFreGetOut.uwCaputureOverflowCnt++;
        if (bikespeed_stFreGetOut.uwCaputureOverflowCnt > (30000 / bikespeed_stFreGetCof.uwTimerUnit)) //  30s
        {
            bikespeed_stFreGetOut.uwCaputureOverflowCnt = 0;
        }
    }
    else
    {
        if (bikespeed_stFreGetOut.uwCaputureNumCnt != 0 && bikespeed_stFreGetOut.uwCaputureOverflowCnt > bikespeed_stFreGetCof.uwSartIntervalTimeCnt)
        {
            bikespeed_stFreGetOut.uwCaputureOverflowCnt = 0;
            bikespeed_stFreGetOut.uwCaputureNumCnt = 0;
        }
        bikespeed_stFreGetOut.uwCaputureNumCnt++;
        bikespeed_stFreGetOut.uwCaputureOverflowCnt = 0;
        if (bikespeed_stFreGetOut.uwCaputureNumCnt >= bikespeed_stFreGetCof.uwNumbersValidPulse2Start)
        {
            bikespeed_stFreGetOut.blBikeSpeedCalStartState = TRUE;
            bikespeed_stFreGetOut.uwCaputureOverflowCnt = 0;
//            bikespeed_stFreGetOut.uwCaputureNumCnt = 0;
            bikespeed_stFreGetOut.uwCaputureNumCnt = 1;
            bikespeed_stFreGetOut.bikespeed_fsm = BIKESPEED_WORK;
            bikespeed_pvt_FreqPu = 1 * BIKESPEED_KMPERH2FREQPU ;
            bikespeed_stFreGetOut.uwFrequencyPu =  (UWORD)bikespeed_pvt_FreqPu;  
            bikespeed_stFreGetOut.uwCaputure1Cnt = (UWORD)iCap_GetCaptureValue(0, CAP_CH(3));
        }
    }
}

/***************************************************************
 Function: bikespeed_voBikeSpeedWork
 Description: bike speed function in work state
 Call by: 
 Input Variables: N/A
 Output/Return Variables: N/A
 Subroutine Call: N/A
 Reference: N/A
****************************************************************/
static void bikespeed_voBikeSpeedWork(UWORD source)
{
    ULONG ulCaputureCntErr = 0;
    if (source == 1 && bikespeed_stFreGetOut.uwCaputureNumCnt == 1)
    {
        bikespeed_stFreGetOut.uwCaputureOverflowCnt++;    
    }
    else if (source == 3)
    {
        if (bikespeed_stFreGetOut.uwCaputureNumCnt == 0)
        {
            bikespeed_stFreGetOut.uwCaputureNumCnt = 1;
            bikespeed_stFreGetOut.uwCaputure1Cnt = (UWORD)iCap_GetCaptureValue(0, CAP_CH(3));
        }
        else if (bikespeed_stFreGetOut.uwCaputureNumCnt == 1)
        {
            bikespeed_stFreGetOut.uwCaputureNumCnt = 2;
            bikespeed_stFreGetOut.uwCaputure2Cnt = (UWORD)iCap_GetCaptureValue(0, CAP_CH(3));

            ulCaputureCntErr = (ULONG)(((UQWORD)bikespeed_stFreGetOut.uwCaputureOverflowCnt * (720 * bikespeed_stFreGetCof.uwTimerUnit))+ bikespeed_stFreGetOut.uwCaputure2Cnt
                                - bikespeed_stFreGetOut.uwCaputure1Cnt);
            
            bikespeed_stFreGetOut.uwCaputureOverflowMinCnt = (UWORD)(bikespeed_stFreGetOut.uwCaputureOverflowCnt << 1);
            if(bikespeed_stFreGetOut.uwCaputureOverflowMinCnt > bikespeed_stFreGetCof.uwHfMinTimeCnt)
            {
                bikespeed_stFreGetOut.uwCaputureOverflowMinCnt = bikespeed_stFreGetCof.uwHfMinTimeCnt;
            }
            else if(bikespeed_stFreGetOut.uwCaputureOverflowMinCnt < 1)
            {
                bikespeed_stFreGetOut.uwCaputureOverflowMinCnt = 1;
            }
            else
            {
                //do nothing
            }
            bikespeed_stFreGetOut.uwCaputureOverflowMinCntTest = bikespeed_stFreGetOut.uwCaputureOverflowMinCnt;

            /* BikeSpeed Freq Cal */
            bikespeed_pvt_FreqPu = (ULONG)(((UQWORD)720000 << 20) / ((UQWORD)ulCaputureCntErr * bikespeed_stFreGetCof.uwNumbersPulses * FBASE));
            bikespeed_stFreGetOut.uwCaputure1Cnt = bikespeed_stFreGetOut.uwCaputure2Cnt;
            bikespeed_stFreGetOut.uwBikeForwardCnt++;
            bikespeed_stFreGetOut.uwCaputureOverflowCnt = 0;
            bikespeed_stFreGetOut.uwCaputureNumCnt = 1;
        }
        else
        {
        	//do nothing
        }
    }
    else
    {
    	//do nothing
    }
   
    if (bikespeed_pvt_FreqPu > bikespeed_stFreGetCof.uwMaxBikeSpeedFre) 
    {
        bikespeed_stFreGetOut.uwFrequencyPu = 0;
        bikespeed_stFreGetOut.blBikeSpeedCalStartState = FALSE;
        bikespeed_stFreGetOut.uwCaputureOverflowCnt = 0;
        bikespeed_stFreGetOut.uwCaputureNumCnt = 0;
        bikespeed_stFreGetOut.uwCaputure2Cnt = 0;
        bikespeed_stFreGetOut.uwCaputure1Cnt = 0;
        bikespeed_stFreGetOut.bikespeed_fsm = BIKESPEED_ERROR;
        bikespeed_stFreGetOut.blBikeSpeedSensorErrorFlg = TRUE;
        bikespeed_stFreGetOut.uwCaputureOverflowMinCnt = bikespeed_stFreGetCof.uwHfMinTimeCnt;
        cp_stHistoryPara.uwBikeSpdSensorAlamTimes++;
    }
    else if (bikespeed_stFreGetOut.uwCaputureOverflowCnt > bikespeed_stFreGetOut.uwCaputureOverflowMinCnt) 
    {
        bikespeed_stFreGetOut.uwFrequencyPu = 0;
        bikespeed_stFreGetOut.blBikeSpeedCalStartState = FALSE;
        bikespeed_stFreGetOut.uwCaputureOverflowCnt = 0;
        bikespeed_stFreGetOut.uwCaputureNumCnt = 0;
        bikespeed_stFreGetOut.uwCaputure2Cnt = 0;
        bikespeed_stFreGetOut.uwCaputure1Cnt = 0;
        bikespeed_stFreGetOut.bikespeed_fsm = BIKESPEED_IDLE;
        bikespeed_stFreGetOut.uwCaputureOverflowMinCnt = bikespeed_stFreGetCof.uwHfMinTimeCnt;
    }
    else 
    {
        bikespeed_stFreGetOut.uwFrequencyPu = (UWORD)bikespeed_pvt_FreqPu;
    }
}

/***************************************************************
 Function: bikespeed_voBikeSpeedError
 Description: bike speed error judge
 Call by: 
 Input Variables: N/A
 Output/Return Variables: N/A
 Subroutine Call: N/A
 Reference: N/A
****************************************************************/
static void bikespeed_voBikeSpeedError(UWORD source)
{
    if (source == 1)
    {
        bikespeed_stFreGetOut.uwCaputureErrorCnt++;
    }

    if (bikespeed_stFreGetOut.blBikeSpeedSensorPwrErrorFlg != TRUE &&
        bikespeed_stFreGetOut.uwCaputureErrorCnt >= bikespeed_stFreGetCof.uwErrorResetCnt)
    {
        bikespeed_voBikeSpeedInit();
    }
}

void bikespeed_voGetBikeSpeedPwrError(UWORD BikeSpeedPwrVolPu)
{
    if ((bikespeed_stFreGetOut.blBikeSpeedSensorPwrErrorFlg != TRUE) && (BikeSpeedPwrVolPu > bikespeed_stFreGetCof.uwBikespeedPwrErrorVoltagePuUp ||
                                                                         BikeSpeedPwrVolPu < bikespeed_stFreGetCof.uwBikespeedPwrErrorVoltagePuDown))
    {
        bikespeed_stFreGetOut.uwBikespeedPwrErrorCnt++;
        if (bikespeed_stFreGetOut.uwBikespeedPwrErrorCnt == bikespeed_stFreGetCof.uwBikespeedPwrErrorCnt)
        {
            bikespeed_stFreGetOut.bikespeed_fsm = BIKESPEED_ERROR;
            bikespeed_stFreGetOut.blBikeSpeedSensorPwrErrorFlg = TRUE;
            bikespeed_stFreGetOut.uwBikespeedPwrErrorCnt = 0;
            cp_stHistoryPara.uwBikeSpdSensorAlamTimes++;
        }
    }
    else
    {
        bikespeed_stFreGetOut.uwBikespeedPwrErrorCnt = 0;
    }

    if ((bikespeed_stFreGetOut.blBikeSpeedSensorPwrErrorFlg == TRUE) && (BikeSpeedPwrVolPu < bikespeed_stFreGetCof.uwBikespeedPwrErrorVoltagePuUp &&
                                                                         BikeSpeedPwrVolPu > bikespeed_stFreGetCof.uwBikespeedPwrErrorVoltagePuDown))
    {
        bikespeed_stFreGetOut.uwBikespeedPwrRecoverCnt++;
        if (bikespeed_stFreGetOut.uwBikespeedPwrRecoverCnt == bikespeed_stFreGetCof.uwBikespeedPwrRecoverCnt)
        {
            bikespeed_stFreGetOut.bikespeed_fsm = BIKESPEED_IDLE;
            bikespeed_stFreGetOut.blBikeSpeedSensorPwrErrorFlg = FALSE;
            bikespeed_stFreGetOut.uwBikespeedPwrRecoverCnt = 0;
            bikespeed_stFreGetOut.uwCaputureErrorCnt = 0;
        }
    }
    else
    {
        bikespeed_stFreGetOut.uwBikespeedPwrRecoverCnt = 0;
    }
}

/***************************************************************
 Function: bikespeed_voBikeSpeedInit
 Description: Bike speed initialization
 Call by: 
 Input Variables: N/A
 Output/Return Variables: N/A
 Subroutine Call: N/A
 Reference: N/A
****************************************************************/
void bikespeed_votempTripCal(void)
{
    UWORD Temptrip;
    Temptrip = (ULONG)(ass_stParaCong.uwWheelPerimeter + ass_stParaCong.swDeltPerimeter)* bikespeed_stFreGetOut.uwBikeForwardCnt /100; //219cm
    if (Temptrip > bikespeed_stFreGetCof.uwMinTriptoUpdate)
    {
        bikespeed_stFreGetOut.uwBikeForwardCnt = 0;
        bikespeed_stFreGetOut.blUpdateTripCntFlg = TRUE;
    }
} 

/***************************************************************
  Function: bikespeed_voBikeSpeedInit
  Description: Bike speed initialization
  Call by: 
  Input Variables: N/A
  Output/Return Variables: N/A
  Subroutine Call: N/A
  Reference: N/A
 ****************************************************************/
void bikespeed_voBikeSpeedInit(void)
{
    bikespeed_stFreGetOut.uwFrequencyPu = 0;
    bikespeed_stFreGetOut.uwLPFFrequencyPu = 0;
    bikespeed_stFreGetOut.uwCaputure1Cnt = 0;
    bikespeed_stFreGetOut.uwCaputure2Cnt = 0;
    bikespeed_stFreGetOut.uwCaputureNumCnt = 0;
    bikespeed_stFreGetOut.uwCaputureOverflowCnt = 0;
    bikespeed_stFreGetOut.uwCaputureErrorCnt = 0;
    bikespeed_stFreGetOut.blBikeSpeedSensorErrorFlg = FALSE;
    bikespeed_stFreGetOut.blBikeSpeedCalStartState = FALSE;
    bikespeed_stFreGetOut.bikespeed_fsm = BIKESPEED_IDLE;
    bikespeed_stFreGetOut.uwBikespeedPwrErrorCnt = 0;
    bikespeed_stFreGetOut.uwBikespeedPwrRecoverCnt = 0;
    bikespeed_stFreGetOut.uwCaputureOverflowMinCntTest = 0;
}

/***************************************************************
 Function: bikespeed_voBikeSpeedCal;
 Description: bike speed FSM
 Call by: 
 Input Variables: N/A
 Output/Return Variables: N/A
 Subroutine Call: N/A
 Reference: N/A
****************************************************************/
void bikespeed_voBikeSpeedCal(UWORD source)
{
    switch (bikespeed_stFreGetOut.bikespeed_fsm)
    {
    case BIKESPEED_IDLE:
        bikespeed_voBikeSpeedIdle(source);
        break;
    case BIKESPEED_WORK:
        bikespeed_voBikeSpeedWork(source);
        break;
    case BIKESPEED_ERROR:
        bikespeed_voBikeSpeedError(source);
        break;
    default:
        break;
    }
}

/***************************************************************
 Function: bikespeed_voPIInit
 Description:  
 Call by: 
 Input Variables: N/A
 Output/Return Variables: N/A
 Subroutine Call: N/A
 Reference: N/A
****************************************************************/
void bikespeed_voPIInit(void)
{
    bikespeed_stPIOut.slErrorZ1 = 0;
    bikespeed_stPIOut.slIqRefPu = 0;
    bikespeed_stPIOut.swIqRefPu = 0;
}
/***************************************************************
 Function: bikespeed_voPICoef
 Description:  
 Call by: 
 Input Variables: N/A
 Output/Return Variables: N/A
 Subroutine Call: N/A
 Reference: N/A
****************************************************************/
void bikespeed_voPICoef(void)
{
     bikespeed_stPICof.uwKpPu = 25000 ;   //Q15
     bikespeed_stPICof.uwKiPu = 500 ;    //Q15
}
/***************************************************************
 Function: bikespeed_voPI
 Description:  
 Call by: 
 Input Variables: N/A
 Output/Return Variables: N/A
 Subroutine Call: N/A
 Reference: N/A
****************************************************************/
void bikespeed_voPI(const BIKESPDPI_IN *in, BIKESPDPI_OUT *out)
{
    SLONG  slIqMaxPu, slIqMinPu;  // Q30
    SLONG  slSpdErrPu, slDeltaErrPu; //Q20
    SQWORD sqIqRefPu, sqIqpPu, sqIqiPu; //Q30
    
    slIqMaxPu = (SLONG)in->swIqMaxPu << 16; // Q14+Q16=Q30
    slIqMinPu = (SLONG)in->swIqMinPu << 16; // Q14+Q16=Q30

    slSpdErrPu = (SLONG)in->slSpdRefPu - in->slSpdFdkPu; // Q20
    if (slSpdErrPu > 1048576L)
    {
        slSpdErrPu = 1048576L;
    }
    else if (slSpdErrPu < -1048576L)
    {
        slSpdErrPu = -1048576L;
    }
    else
    {
    /* Nothing */
    }
    
    slDeltaErrPu = slSpdErrPu - out->slErrorZ1;
    if (slDeltaErrPu > 1048576L)
    {
    	slDeltaErrPu = 1048576L;
    }
    else if (slDeltaErrPu < -1048576L)
    {
    	slDeltaErrPu = -1048576L;
    }
    else
    {
    /* Nothing */
    }

    bikespeed_stPICof.uwKpPu = 25000; 
    if(((SWORD)ABS(out->slErrorZ1) - (SWORD)ABS(slSpdErrPu)) > 20)   //Fast Approach
    {
        bikespeed_stPICof.uwKiPu = 0; 
        
        if(slSpdErrPu < 0 && slSpdErrPu > (SWORD)-3 * BIKESPEED_KMPERH2FREQPU)
        {
            bikespeed_stPICof.uwKpPu = 6250;
            bikespeed_stPICof.uwKiPu = 200;
        }
        else if(slSpdErrPu <= (SWORD)-3 * BIKESPEED_KMPERH2FREQPU)
        {
            bikespeed_stPICof.uwKpPu = 6250;
            bikespeed_stPICof.uwKiPu = 400;
        }
        else
        {
        	//do nothing
        }
    }
    else if(((SWORD)ABS(out->slErrorZ1) - (SWORD)ABS(slSpdErrPu)) > 0) //Fast Approach
    {
        if(in->slSpdFdkPu<500)
        {
            bikespeed_stPICof.uwKiPu = 500;
        }
        else
        {
            bikespeed_stPICof.uwKiPu = 1000; 
        }
        
        if(slSpdErrPu < 0 && slSpdErrPu > (SWORD)-3 * BIKESPEED_KMPERH2FREQPU)
        {
            bikespeed_stPICof.uwKpPu = 6250;
            bikespeed_stPICof.uwKiPu = 200;
        }
        else if(slSpdErrPu <= (SWORD)-3 * BIKESPEED_KMPERH2FREQPU)
        {
            bikespeed_stPICof.uwKpPu = 6250;
            bikespeed_stPICof.uwKiPu = 400;
        }
        else
        {
        	//do nothing
        }
    }
    else  //Away
    {
        if(in->slSpdFdkPu<500)
        {
            bikespeed_stPICof.uwKiPu = 500;
        }
        else
        {
            bikespeed_stPICof.uwKiPu = 3000; 
        }
            
        if(slSpdErrPu > (0 * BIKESPEED_KMPERH2FREQPU) && slSpdErrPu <= (3 * BIKESPEED_KMPERH2FREQPU))
        {
            bikespeed_stPICof.uwKiPu  = 0;
        }
    }
    
    if(in->slSpdFdkPu >= (27 * BIKESPEED_KMPERH2FREQPU))
    {
        bikespeed_stPICof.uwKiPu  = 6000;
    }
    
    sqIqpPu = ((SQWORD)slDeltaErrPu * bikespeed_stPICof.uwKpPu) << 4; // Q30     
    sqIqiPu = (SQWORD)slSpdErrPu * bikespeed_stPICof.uwKiPu ; // Q30
    sqIqRefPu = sqIqpPu + sqIqiPu + (SQWORD)out->slIqRefPu; // Q30

    if (sqIqRefPu > slIqMaxPu)
    {
        out->slIqRefPu = slIqMaxPu;
    }
    else if (sqIqRefPu < slIqMinPu)
    {
        out->slIqRefPu = slIqMinPu;
    }
    else
    {
        out->slIqRefPu = (SLONG)sqIqRefPu;
    }

    out->swIqRefPu = (SWORD)(out->slIqRefPu >> 16); // Q30-Q16=Q14
    out->slErrorZ1 = slSpdErrPu;
}
/*************************************************************************
 Local Functions (N/A)
*************************************************************************/

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