motor_ctl-1/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 "typedefine.h"
#include "mathtool.h"
#include "bikespeed.h"
#include "CodePara.h"
#include "asr.h"
#include "user.h"
#include "usart.h"
#include "canAppl.h"
//#include "api.h"
#include "board_config.h"
#include "UserGpio_Config.h"
/******************************
 *
 *  Parameter
 *
 ******************************/
BIKESPEED_COF bikespeed_stFreGetCof = BIKESPEED_COF_DEFAULT;
BIKESPEED_OUT bikespeed_stFreGetOut = BIKESPEED_OUT_DEFAULT;
/***************************************************************
 Function: bikespeed_voBikeSpeedCof;
 Description: Bike speed cof calculation
 Call by: functions in main loop;
 Input Variables: N/A
 Output/Return Variables: N/A
 Subroutine Call: N/A;
 Reference: N/A
****************************************************************/
void bikespeed_voBikeSpeedCof(void)
{
    // bikespeed_stFreGetCof.uwNumbersPulses = BIKESPEED_NUMBERS_PULSES;
    bikespeed_stFreGetCof.uwSartIntervalTimeCnt = (UWORD)(((ULONG)BIKESPEED_START_INTERVALTIME_MS*1000) / BIKESPEED_TIM_TIMERUNIT_US);
    bikespeed_stFreGetCof.uwNumbersValidPulse2Start = BIKESPEED_NUMBERS_VALIDPULSE2START;
    bikespeed_stFreGetCof.uwHfMinTimeCnt = ((ULONG)BIKESPEED_HF_MINTIME_MS*1000) / BIKESPEED_TIM_TIMERUNIT_US;
    bikespeed_stFreGetCof.uwErrorResetCnt = ((ULONG)BIKESPEED_ERROR_RESETTIME_MS*1000) / BIKESPEED_TIM_TIMERUNIT_US;
    bikespeed_stFreGetCof.uwTimerUnit = BIKESPEED_TIM_TIMERUNIT_US;
    bikespeed_stFreGetCof.uwBikeSpeedLPFGain = BIKESPEED_LPF_GAIN;
    bikespeed_stFreGetCof.uwMaxBikeSpeedFre = ((ULONG)BIKESPEED_MAX_FREQUENCY << 20) / FBASE;
    bikespeed_stFreGetOut.uwFreTimesCnt = 0;

    bikespeed_stFreGetCof.uwBikespeedPwrErrorCnt = ((ULONG)BIKESPEED_POWER_ERROR_DETECT_MS*1000) / BIKESPEED_TIM_TIMERUNIT_US;
    bikespeed_stFreGetCof.uwBikespeedPwrRecoverCnt = ((ULONG)BIKESPEED_POWER_ERROR_RECOVER_MS*1000) / BIKESPEED_TIM_TIMERUNIT_US;
    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: functions in main loop;
 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 > ((ULONG)3000 * 1000 / bikespeed_stFreGetCof.uwTimerUnit)) //  3s
        {
            bikespeed_stFreGetOut.uwCaputureOverflowCnt = 0;
            bikespeed_stFreGetOut.uwCaputureOverflowCntLast = 0;
        }
    }
    else
    {
        if (bikespeed_stFreGetOut.uwCaputureNumCnt != 0 && bikespeed_stFreGetOut.uwCaputureOverflowCnt > bikespeed_stFreGetCof.uwSartIntervalTimeCnt) //10s
        {
            bikespeed_stFreGetOut.uwCaputureOverflowCnt = 0;
            bikespeed_stFreGetOut.uwCaputureOverflowCntLast = 0;
            bikespeed_stFreGetOut.uwCaputureNumCnt = 0;
        }
        bikespeed_stFreGetOut.uwCaputureNumCnt++;
        bikespeed_stFreGetOut.uwCaputureOverflowCnt = 0;
        bikespeed_stFreGetOut.uwCaputureOverflowCntLast = 0;
        if (bikespeed_stFreGetOut.uwCaputureNumCnt >= bikespeed_stFreGetCof.uwNumbersValidPulse2Start)
        {
            bikespeed_stFreGetOut.blBikeSpeedCalStartState = TRUE;
            bikespeed_stFreGetOut.uwCaputureOverflowCnt = 0;
            bikespeed_stFreGetOut.uwCaputureOverflowCntLast = 0;
            bikespeed_stFreGetOut.uwCaputureNumCnt = 0;
            bikespeed_stFreGetOut.bikespeed_fsm = BIKESPEED_WORK;
        }
    }
}

/***************************************************************
 Function: bikespeed_voBikeSpeedWork;
 Description: bike speed function in work state
 Call by: functions in main loop;
 Input Variables: N/A
 Output/Return Variables: N/A
 Subroutine Call: N/A;
 Reference: N/A
****************************************************************/
UWORD tstCapValue;
UWORD tstOverCnt;
UWORD tmpCnt = 0;
UWORD tmpCnt1 = 0;
static void bikespeed_voBikeSpeedWork(UWORD source)
{
    ULONG ulCaputureCntErr = 65536;
  //  ULONG ulCapAvgValue = 65536;
    ULONG tmp_uwFrequencyPu = 0;
    SQWORD tmp_slFrequencyErr = 0;
    UWORD tmp_OverflowCnt = 0;
      
    if (source == 1 && bikespeed_stFreGetOut.uwCaputureNumCnt == 1)
    {
       // bikespeed_stFreGetOut.uwCaputureOverflowCnt++;
        if(bikespeed_stFreGetOut.uwCaputureOverflowCnt >= ((ULONG)2500 * 1000 / bikespeed_stFreGetCof.uwTimerUnit))
        {
            ulCaputureCntErr = bikespeed_stFreGetOut.uwCaputureOverflowCnt * (72 * bikespeed_stFreGetCof.uwTimerUnit);
            tmp_uwFrequencyPu = (ULONG)(((SQWORD)72000000 << 20) / (((SQWORD)ulCaputureCntErr) * bikespeed_stFreGetCof.uwNumbersPulses * FBASE));
            if(tmp_uwFrequencyPu < bikespeed_stFreGetOut.uwFrequencyPu)
            {
                bikespeed_stFreGetOut.uwFrequencyPu = (UWORD)tmp_uwFrequencyPu;
            }
        }
        tstOverCnt = 500;
    }
    else if (source == 3)
    {
        tmpCnt ++;
        #if ((MOTOR_ID_SEL == MOTOR_LUNGU_WELLING_HAL_POLE8_1000W)||(MOTOR_ID_SEL ==MOTOR_LUNGU_WELLING_HAL_POLE10_750W_500Ratio)||(MOTOR_ID_SEL == MOTOR_LUNGU_WELLING_HAL_POLE8_750W)||(MOTOR_ID_SEL == MOTOR_LUNGU_WELLING_HAL_POLE8_500W) ||(MOTOR_ID_SEL ==MOTOR_LUNGU_WELLING_HAL_POLE8_500W_958Ratio))
          tmp_OverflowCnt = bikespeed_stFreGetOut.uwCaputureOverflowCnt + (bikespeed_stFreGetOut.uwLPFFrequencyPu>>6);
        #else
          tmp_OverflowCnt = bikespeed_stFreGetOut.uwCaputureOverflowCnt + (bikespeed_stFreGetOut.uwLPFFrequencyPu>>7);
        #endif
        tmpCnt1 = tmp_OverflowCnt;
        if (bikespeed_stFreGetOut.uwCaputureNumCnt == 0)
        {
            tmpCnt --;
            bikespeed_stFreGetOut.uwCaputureNumCnt = 1;
            //bikespeed_stFreGetOut.uwCaputure1Cnt =(UWORD)CapValutBike_CCRx();// (UWORD)iCap_GetCaptureValue(HW_BIKESPD_CAD_CAP, CAP_CH(3));
            bikespeed_stFreGetOut.uwCaputureOverflowCntLast = ((ULONG)3000 * 1000 / bikespeed_stFreGetCof.uwTimerUnit);
        }
        else if (bikespeed_stFreGetOut.uwCaputureNumCnt == 1)
        {
            tmpCnt --;
            bikespeed_stFreGetOut.uwCaputureNumCnt = 2;
           // bikespeed_stFreGetOut.uwCaputure2Cnt = (UWORD)CapValutBike_CCRx();// (UWORD)iCap_GetCaptureValue(HW_BIKESPD_CAD_CAP, CAP_CH(3));

            ulCaputureCntErr = (((SQWORD)bikespeed_stFreGetOut.uwCaputureOverflowCnt + bikespeed_stFreGetOut.uwCaputureOverflowCntLast) * (72 * bikespeed_stFreGetCof.uwTimerUnit)) ;
        
            /* Bike Speed Calculate */    
            tmp_uwFrequencyPu = (ULONG)(((SQWORD)72000000 << 20) / (((SQWORD)ulCaputureCntErr + bikespeed_stFreGetOut.ulCaputureCntErr) * bikespeed_stFreGetCof.uwNumbersPulses * FBASE));
            
            bikespeed_stFreGetOut.ulCaputureCntErr =0;// ulCaputureCntErr;

            bikespeed_stFreGetOut.uwFrequencyPu = (UWORD)tmp_uwFrequencyPu;
            
            //24609375 = 72M*Fbase*Q(8)/Q(20)
            tmp_slFrequencyErr = ((SQWORD)24609375)*((SQWORD)bikespeed_stFreGetOut.uwFrequencyPu - (SQWORD)bikespeed_stFreGetOut.uwFrequencyPuLast);
            bikespeed_stFreGetOut.swBikeSpeedFreAcc = (SWORD)((tmp_slFrequencyErr*2)/((SQWORD)ulCaputureCntErr + bikespeed_stFreGetOut.ulCaputureCntErr));
            bikespeed_stFreGetOut.uwFrequencyPuLast = bikespeed_stFreGetOut.uwFrequencyPu;
            
            //bikespeed_stFreGetOut.uwCaputure1Cnt = bikespeed_stFreGetOut.uwCaputure2Cnt;
            bikespeed_stFreGetOut.uwBikeForwardCnt++;
            bikespeed_stFreGetOut.uwCaputureOverflowCnt = 0;
            bikespeed_stFreGetOut.uwCaputureOverflowCntLast = 0;
            tstOverCnt = 0;
            bikespeed_stFreGetOut.uwCaputureNumCnt = 1;
        }
        else
        {       
            bikespeed_stFreGetOut.uwCaputureOverflowCntLast += bikespeed_stFreGetOut.uwCaputureOverflowCnt;
            bikespeed_stFreGetOut.uwCaputureOverflowCnt = 0;
        }
    }


    if (bikespeed_stFreGetOut.uwCaputureOverflowCnt >= bikespeed_stFreGetCof.uwHfMinTimeCnt) // 5s
    {
        bikespeed_stFreGetOut.uwFrequencyPu = 0;
        bikespeed_stFreGetOut.swBikeSpeedFreAcc = 0;
        bikespeed_stFreGetOut.ulCaputureCntErr = (65536L<<10);
        bikespeed_stFreGetOut.uwFrequencyPuLast = 0;
        bikespeed_stFreGetOut.blBikeSpeedCalStartState = FALSE;
        bikespeed_stFreGetOut.uwCaputureOverflowCnt = 0;
        bikespeed_stFreGetOut.uwCaputureOverflowCntLast = 0;
        bikespeed_stFreGetOut.uwCaputureNumCnt = 0;
        bikespeed_stFreGetOut.uwCaputure2Cnt = 0;
        bikespeed_stFreGetOut.uwCaputure1Cnt = 0;
        bikespeed_stFreGetOut.bikespeed_fsm = BIKESPEED_IDLE;
    }

    if (bikespeed_stFreGetOut.uwLPFFrequencyPu >= bikespeed_stFreGetCof.uwMaxBikeSpeedFre) // 100ms
    {
        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++;
    }
}

/***************************************************************
 Function: bikespeed_voBikeSpeedError;
 Description: bike speed error judge
 Call by: functions in main loop;
 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: functions in main loop;
 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_ParaCong.uwWheelPerimeter + ass_ParaCong.swDeltPerimeter)* bikespeed_stFreGetOut.uwBikeForwardCnt /(200 * bikespeed_stFreGetCof.uwNumbersPulses); //219cm
    if (Temptrip > bikespeed_stFreGetCof.uwMinTriptoUpdate)
    {
        bikespeed_stFreGetOut.uwBikeForwardCnt = 0;
        bikespeed_stFreGetOut.blUpdateTripCntFlg = TRUE;
    }
} 

/***************************************************************
  Function: bikespeed_voBikeSpeedInit;
  Description: Bike speed initialization
  Call by: functions in main loop;
  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.swBikeSpeedFreAcc = 0;
    bikespeed_stFreGetOut.ulCaputureCntErr = (65536L<<10);
    bikespeed_stFreGetOut.uwFrequencyPuLast = 0;
    bikespeed_stFreGetOut.uwCaputure1Cnt = 0;
    bikespeed_stFreGetOut.uwCaputure2Cnt = 0;
    bikespeed_stFreGetOut.uwCaputureNumCnt = 0;
    bikespeed_stFreGetOut.uwCaputureOverflowCnt = 0;
    bikespeed_stFreGetOut.uwCaputureOverflowCntLast = 0;
    bikespeed_stFreGetOut.uwCaputureErrorCnt = 0;
    bikespeed_stFreGetOut.blBikeSpeedSensorErrorFlg = FALSE;
    bikespeed_stFreGetOut.blBikeSpeedCalStartState = FALSE;
    bikespeed_stFreGetOut.bikespeed_fsm = BIKESPEED_IDLE;
    bikespeed_stFreGetOut.uwBikespeedPwrErrorCnt = FALSE;
    bikespeed_stFreGetOut.uwBikespeedPwrErrorCnt = 0;
    bikespeed_stFreGetOut.uwBikespeedPwrRecoverCnt = 0;
}

/***************************************************************
 Function: bikespeed_voBikeSpeedCal;
 Description: bike speed FSM
 Call by: functions in main loop;
 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;
    }
}


/*************************************************************************
 Local Functions (N/A)
*************************************************************************/

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