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;

MOTORSPEED_OUT bikeSpdPIOut;
BikeSpdPI_IN bikespdPIin;
BikeSpdCoef  bikespeedcof;
/***************************************************************
 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.uwWheelDiameter = 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.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
****************************************************************/
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.uwWheelDiameter * 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 bikemotorspdCoef(void)
{
     bikespeedcof.uwKpPu = 20000 ;   //Q15
     bikespeedcof.uwKiPu = 8000 ;  //Q15
}


void bikemotorspdref(void)
{
    SWORD BikeSpeedKmHREF = 0;  //km/h
    SLONG slBikeSpdRefPu = 0; //Q20
    
    if(MC_ControlCode.GearSt == 0x01)
    {
         BikeSpeedKmHREF=10;
    }
    else if(MC_ControlCode.GearSt == 0x02)
    {
      BikeSpeedKmHREF=15;
    }
    else if(MC_ControlCode.GearSt == 0x03)
    {
     BikeSpeedKmHREF=20;
    }
    else if(MC_ControlCode.GearSt == 0x04)
    {
     BikeSpeedKmHREF=25;
    }
    else if(MC_ControlCode.GearSt == 0x33 || MC_ControlCode.GearSt == 0x05)
    {
      BikeSpeedKmHREF=30;

    }
    else
    {
      
    }
     slBikeSpdRefPu = BikeSpeedKmHREF * 1000 * 29127 / ass_ParaCong.uwWheelDiameter / FBASE ; //f(Q20)

       
     bikespdPIin.slSpdRefPu = slBikeSpdRefPu; // Q20
     bikespdPIin.slSpdFdkPu = bikespeed_stFreGetOut.uwLPFFrequencyPu ;  //Q20
     
     bikespdPIin.swbikespdMax= USER_MOTOR_5500RPM2PU  ;  //    Q15
     bikespdPIin.swbikespdMin= - USER_MOTOR_5500RPM2PU ;

     BikeSpdPI(&bikespdPIin, &bikeSpdPIOut);
}

void BikeSpdPI(BikeSpdPI_IN *in, MOTORSPEED_OUT *out)
{
      SLONG  slbikespeederr; 
      SLONG  slDeltaErr;
      SQWORD  motorspeedrefp;
      SQWORD  motorspeedrefi;
      SLONG  slmotorspdMax;
      SLONG  slmotorspdMin;
      SQWORD  motorspeedref;
      
      slmotorspdMax = in-> swbikespdMax << 15; //Q30
      slmotorspdMin = in-> swbikespdMin << 15; //Q30
    
    /* Calculate the error */
      slbikespeederr =  in->slSpdRefPu - in->slSpdFdkPu ;  //Q20
      if (slbikespeederr > 1048576L)
      {
        slbikespeederr = 1048576L;
      }
      else if (slbikespeederr < -1048576L)
      {
        slbikespeederr = -1048576L;
      }
      else
      {
        /* Nothing */
      }
      
    /* Calculate the delta error */
      slDeltaErr = slbikespeederr - out->slErrorZ1;  //Q20
      if (slDeltaErr > 1048576L)
      {
        slDeltaErr = 1048576L;
      }
      else if (slDeltaErr < -1048576L)
      {
        slDeltaErr = -1048576L;
      }
      else
      {
        /* Nothing */
      }
  
    //kp output
    motorspeedrefp = (SQWORD)slDeltaErr * bikespeedcof.uwKpPu >> 5 ; //Q30
   
    /* Calculate the integral output */
    motorspeedrefi = (SQWORD)slbikespeederr * bikespeedcof.uwKiPu >> 5 ; //Q30
    
    /* Calculate the Controller output */
    motorspeedref = motorspeedrefp + motorspeedrefi + out->slmotorspeedref ; //Q30
    /* motorspeed output limit */
    if (motorspeedref > slmotorspdMax)
    {
        out->slmotorspeedref = slmotorspdMax;
    }
    else if (motorspeedref < slmotorspdMin)
    {
        out->slmotorspeedref = slmotorspdMin;
    }
    else
    {
        out->slmotorspeedref = motorspeedref;
    }
    
    if( (in->slSpdRefPu == 0) && ( in->slSpdFdkPu == 0))
    {
      out->slmotorspeedref = 0 ;
    }
  
    out->swMotorSpeedRef = out->slmotorspeedref >> 15 ; // motorspeedrmp,Q15
       
    out->slErrorZ1 = slbikespeederr ;
    
    out->swSpdRefRpm = ((SLONG)out->swMotorSpeedRef * cof_uwVbRpm) >> 15 ;
    
    
   
}

void bikeSpdPIOutInit(void)
{
    bikeSpdPIOut.slErrorZ1 = 0;
    bikeSpdPIOut.slmotorspeedref = 0;
    bikeSpdPIOut.swMotorSpeedRef = 0;
    bikeSpdPIOut.swSpdRefRpm = 0 ;
    
}

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

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