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 "typedefine.h"
#include "mathtool.h"
#include "bikespeed.h"
#include "CodePara.h"
#include "asr.h"
#include "user.h"
#include "can.h"
#include "canAppl.h"
#include "hwsetup.h"
/******************************
 *
 *  Parameter
 *
 ******************************/
BIKESPEED_COF bikespeed_stFreGetCof = BIKESPEED_COF_DEFAULT;
BIKESPEED_OUT bikespeed_stFreGetOut = BIKESPEED_OUT_DEFAULT;

BIKESPDPI_IN bikespeed_stPIIn;
BIKESPDPI_COF  bikespeed_stPICof;
BIKESPDPI_OUT bikespeed_stPIOut;
/***************************************************************
 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 = 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: 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 > (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_stFreGetOut.uwFrequencyPu = 1 * BIKESPEED_KMPERH2FREQPU;  
            bikespeed_stFreGetOut.uwCaputure1Cnt = (UWORD)TIMER1_CAP_BIKESPD;
        }
    }
}

/***************************************************************
 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
****************************************************************/
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)TIMER1_CAP_BIKESPD;
        }
        else if (bikespeed_stFreGetOut.uwCaputureNumCnt == 1)
        {
            bikespeed_stFreGetOut.uwCaputureNumCnt = 2;
            bikespeed_stFreGetOut.uwCaputure2Cnt = (UWORD)TIMER1_CAP_BIKESPD;

            ulCaputureCntErr = ((SQWORD)bikespeed_stFreGetOut.uwCaputureOverflowCnt * (720 * bikespeed_stFreGetCof.uwTimerUnit)) -
                               bikespeed_stFreGetOut.uwCaputure1Cnt + bikespeed_stFreGetOut.uwCaputure2Cnt;
            
            bikespeed_stFreGetOut.uwCaputureOverflowMinCnt = bikespeed_stFreGetOut.uwCaputureOverflowCnt << 1;
            if(bikespeed_stFreGetOut.uwCaputureOverflowMinCnt > bikespeed_stFreGetCof.uwHfMinTimeCnt)
            {
              bikespeed_stFreGetOut.uwCaputureOverflowMinCnt = bikespeed_stFreGetCof.uwHfMinTimeCnt;
            }
            else
            {
            }
            // ulCaputureCntErr =  ((SQWORD)bikespeed_stFreGetOut.uwCaputureOverflowCnt*(72000000/100/(1000/bikespeed_stFreGetCof.uwTimerUnit))) -
            // bikespeed_stFreGetOut.uwCaputure1Cnt + bikespeed_stFreGetOut.uwCaputure2Cnt;
            bikespeed_stFreGetOut.uwFrequencyPu =
                (UWORD)(((SQWORD)720000 << 20) / ((SQWORD)ulCaputureCntErr * bikespeed_stFreGetCof.uwNumbersPulses * FBASE));
            bikespeed_stFreGetOut.uwCaputure1Cnt = bikespeed_stFreGetOut.uwCaputure2Cnt;
            bikespeed_stFreGetOut.uwBikeForwardCnt++;
            bikespeed_stFreGetOut.uwCaputureOverflowCnt = 0;
            bikespeed_stFreGetOut.uwCaputureNumCnt = 1;
        }
    }
   
    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;
    }

    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)bikespeed_stFreGetCof.uwWheelPerimeter * 3216 * bikespeed_stFreGetOut.uwBikeForwardCnt / 1000) >> 10; // 3216 = Q10(3.1415926) m
    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.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 = 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;
    }
}

/***************************************************************
 Function: bikespeedinput, motorspeedoutput;
 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;
}

void bikespeed_voPICoef(void)
{
     bikespeed_stPICof.uwKpPu = 25000 ;   //Q15
     bikespeed_stPICof.uwKiPu = 500 ;    //Q15
}

void bikespeed_voPI(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 (slSpdErrPu > 1048576L)
    {
        slSpdErrPu = 1048576L;
    }
    else if (slSpdErrPu < -1048576L)
    {
        slSpdErrPu = -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 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  //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 = 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!
*************************************************************************/