motor_ctl-4/User project/3.BasicFunction/Source/bikethrottle.c

/**
 * @file Bikethrottle.c
 * @author Wang, Zhiyu(wangzy49@midea.com)
 * @brief throttle 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 "user.h"
#include "typedefine.h"
#include "mathtool.h"
#include "bikethrottle.h"
#include "api.h"
#include "board_config.h"
#include "bikespeed.h"
#include "bikebrake.h"
/******************************
 *
 * static Parameter
 *
 ******************************/
static BIKETHROTTLE_COF bikethrottle_stBikeThrottleCof = BIKETHROTTLE_COF_DEFAULT;
static LPF_OUT          scm_stBikeThrottleLpf;
BIKETHROTTLE_OUT        bikethrottle_stBikeThrottleOut = BIKETHROTTLE_OUT_DEFAULT;
BIKECRUISE_COF          bikeCruise = BIKECRUISE_OUT_DEFAULT;
/******************************
 *
 * extern Parameter
 *
 ******************************/

/***************************************************************
 Function: bikethrottle_voBikeThrottleCof;
 Description: bikethrottle 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 bikethrottle_voBikeThrottleCof(void)
{
    ULONG ulLpfTm = 0;

    bikethrottle_stBikeThrottleCof.uwMaxThrottleVolOutputPu = ((ULONG)BIKETHROTTLE_VOLTAGE_MAX_RANGE << 14) / (VBASE * 100); // Q15
    bikethrottle_stBikeThrottleCof.uwMinThrottleVolOutputPu = ((ULONG)BIKETHROTTLE_VOLTAGE_MIN_RANGE << 14) / (VBASE * 100); // Q15
    bikethrottle_stBikeThrottleCof.uwThrottleVoFaultlPu     = ((ULONG)BIKETHROTTLE_VOLTAGE_FAULT_RANGE << 14) / (VBASE * 100);
    bikethrottle_stBikeThrottleCof.uwThrottleVolSen2McuGain = BIKETHROTTLE_VOLTAGE_SEN2MCUGAIN;
    bikethrottle_stBikeThrottleCof.uwThrottleVolLPFFrq = BIKETHROTTLE_LPF_FRQ;
    bikethrottle_stBikeThrottleCof.uwThrottleVolLPFDisFrq = BIKETHROTTLE_LPF_DISCRETEHZ;
    bikethrottle_stBikeThrottleCof.uwThrottleVolReg2Pu = (UWORD)((((UQWORD)33 << 24) / 10) / (1 << ADC_RESOLUTION_BIT) /
                                                         bikethrottle_stBikeThrottleCof.uwThrottleVolSen2McuGain * 100 * 10 /
                                                         VBASE); // Real Voltage to Pu Q24
    // 3.3/4096/harwaregain/VBase;
    bikethrottle_stBikeThrottleCof.uwThrottleOffsetPu = bikethrottle_stBikeThrottleCof.uwMinThrottleVolOutputPu;

    /* Bike Throttle limit coef */
    ulLpfTm = 1000000 / bikethrottle_stBikeThrottleCof.uwThrottleVolLPFFrq;
    mth_voLPFilterCoef(ulLpfTm, bikethrottle_stBikeThrottleCof.uwThrottleVolLPFDisFrq, &scm_stBikeThrottleLpf.uwKx);

    bikethrottle_stBikeThrottleCof.uwThrottleErrorCnt = BIKETHROTTLE_ERROR_TIME / BIKETHROTTLE_ERROE_TIMEUNIT;
    bikethrottle_stBikeThrottleCof.uwThrottleRecoverCnt = BIKETHROTTLE_RECOVER_TIME / BIKETHROTTLE_ERROE_TIMEUNIT;
}

/***************************************************************
 Function: bikethrottle_voBikeThrottleInit;
 Description: Bike throttle signal initialization
 Call by: functions in main loop;
 Input Variables: N/A
 Output/Return Variables: N/A
 Subroutine Call: N/A;
 Reference: N/A
****************************************************************/
void bikethrottle_voBikeThrottleInit(void)
{
    bikethrottle_stBikeThrottleOut.uwThrottleVolReg = 0;
    bikethrottle_stBikeThrottleOut.uwThrottleVolPu = 0;
    bikethrottle_stBikeThrottleOut.uwThrottleVolLPFPu = 0;
    bikethrottle_stBikeThrottleOut.uwThrottlePercent = 0;
    bikethrottle_stBikeThrottleOut.uwThrottleErrorCnt = 0;
    bikethrottle_stBikeThrottleOut.uwThrottleRecoverCnt = 0;
    bikethrottle_stBikeThrottleOut.blThrottleErrorFlg = FALSE;
}

/***************************************************************
 Function: bikethrottle_voBikeThrottleADC;
 Description: bikethrottle get
 Call by: functions in main loop;
 Input Variables: N/A
 Output/Return Variables: N/A
 Subroutine Call: N/A;
 Reference: N/A
****************************************************************/
void bikethrottle_voBikeThrottleADC(void) // need to match ADC_StartConversion(ADC1);
{
    if(bikespeed_stSpdLimCoef.blThrottleExist)
    {
        if (bikethrottle_stBikeThrottleOut.blThrottleErrorFlg == TRUE)
        {
            bikethrottle_stBikeThrottleOut.uwThrottleVolPu = 0;
            bikethrottle_stBikeThrottleOut.uwThrottleVolLPFPu = 0;
            bikethrottle_stBikeThrottleOut.uwThrottlePercent = 0;
            bikethrottle_stBikeThrottleOut.uwThrottleRecoverCnt++;
            if (bikethrottle_stBikeThrottleOut.uwThrottleRecoverCnt == bikethrottle_stBikeThrottleCof.uwThrottleRecoverCnt)
            {
                bikethrottle_stBikeThrottleOut.blThrottleErrorFlg = FALSE;
                bikethrottle_stBikeThrottleOut.uwThrottleRecoverCnt = 0;
            }
        }
        else
        {
            bikethrottle_stBikeThrottleOut.uwThrottleVolReg = iAdc_GetResultPointer(0)[HW_ADC_THRO_CH]; //hw_uwADC2[6]; 
            bikethrottle_stBikeThrottleOut.uwThrottleVolPu =
                             (UWORD)(((ULONG)bikethrottle_stBikeThrottleOut.uwThrottleVolReg * bikethrottle_stBikeThrottleCof.uwThrottleVolReg2Pu) >> 10); // Q14
            //mth_voLPFilter((SWORD)bikethrottle_stBikeThrottleOut.uwThrottleVolPu, &scm_stBikeThrottleLpf);
            //bikethrottle_stBikeThrottleOut.uwThrottleVolLPFPu = scm_stBikeThrottleLpf.slY.sw.hi;
            bikethrottle_stBikeThrottleOut.uwThrottleVolLPFPu = bikethrottle_stBikeThrottleOut.uwThrottleVolPu;

            if (bikethrottle_stBikeThrottleOut.uwThrottleVolPu < bikethrottle_stBikeThrottleCof.uwMinThrottleVolOutputPu)
            {
                bikethrottle_stBikeThrottleOut.uwThrottleVolLPFPu = bikethrottle_stBikeThrottleCof.uwMinThrottleVolOutputPu;
                bikethrottle_stBikeThrottleOut.uwThrottleErrorCnt++;
                if (bikethrottle_stBikeThrottleOut.uwThrottleErrorCnt == bikethrottle_stBikeThrottleCof.uwThrottleErrorCnt)
                {
                    bikethrottle_stBikeThrottleOut.blThrottleErrorFlg = TRUE;
                    bikethrottle_stBikeThrottleOut.uwThrottleErrorCnt = 0;
                }
            }
            else if (bikethrottle_stBikeThrottleOut.uwThrottleVolPu > bikethrottle_stBikeThrottleCof.uwMaxThrottleVolOutputPu)
            {
                bikethrottle_stBikeThrottleOut.uwThrottleVolLPFPu = bikethrottle_stBikeThrottleCof.uwMaxThrottleVolOutputPu;
                bikethrottle_stBikeThrottleOut.uwThrottleErrorCnt++;
                if (bikethrottle_stBikeThrottleOut.uwThrottleErrorCnt == bikethrottle_stBikeThrottleCof.uwThrottleErrorCnt)
                {
                    bikethrottle_stBikeThrottleOut.blThrottleErrorFlg = TRUE;
                    bikethrottle_stBikeThrottleOut.uwThrottleErrorCnt = 0;
                }
            }
            else
            {
                bikethrottle_stBikeThrottleOut.uwThrottleErrorCnt = 0;
                bikethrottle_stBikeThrottleOut.uwThrottlePercent =
                    (bikethrottle_stBikeThrottleOut.uwThrottleVolLPFPu - bikethrottle_stBikeThrottleCof.uwThrottleOffsetPu) * 1000 /
                    (bikethrottle_stBikeThrottleCof.uwMaxThrottleVolOutputPu - bikethrottle_stBikeThrottleCof.uwMinThrottleVolOutputPu);
            }
            bikethrottle_voThrottlecruise();
            bikethrottle_voStartAllow();
        }
    }
    else
    {
        bikethrottle_stBikeThrottleOut.uwThrottlePercent = 0;
    }
}
/***************************************************************
 Function: Throttlecruise;
 Description:  
 Call by: functions in 100ms TimeTask;
 Input Variables: N/A
 Output/Return Variables: N/A
 Subroutine Call: N/A;
 Reference: N/A
****************************************************************/
void bikethrottle_voThrottlecruise(void)
{
    if((ass_stParaCong.uwControlFunEN & 0xFF) == 0xAA)
    {
        if((bikethrottle_stBikeThrottleOut.uwThrottleVolPu < (bikeCruise.ThrottlekeepVault+180)) && (bikethrottle_stBikeThrottleOut.uwThrottleVolPu > (bikeCruise.ThrottlekeepVault-180)))
        {
           if((BikeBrake_blGetstate() == FALSE)
              &&(alm_blAlmOccrFlg == FALSE)
              &&(cp_stBikeRunInfoPara.uwBikeGear > 0)
              &&(cp_stBikeRunInfoPara.uwBikeGear <= 5)
              &&(bikethrottle_stBikeThrottleOut.uwThrottlePercent > 200)
              &&(bikeCruise.CruiseMode == 0))
           {
               if((cp_ulSystickCnt - bikeCruise.KeepInTime) >= CRUISESETINGTIME) //8秒指拨不动进入巡航
               {
                   bikeCruise.CruiseMode = 1;
                   bikeCruise.CruisePercent = bikethrottle_stBikeThrottleOut.uwThrottlePercent;
                   bikeCruise.ThrottleResetCruiseFlag = 1;
               }
           }
           if(bikethrottle_stBikeThrottleOut.uwThrottlePercent < 200)
           {
              bikeCruise.KeepInTime = cp_ulSystickCnt;
           }
        }
        else
        {
           bikeCruise.ThrottlekeepVault = bikethrottle_stBikeThrottleOut.uwThrottleVolPu;
           bikeCruise.KeepInTime = cp_ulSystickCnt;
          // bikeCruise.ThrottleResetCruiseFlag = 0;
        }

       //有故障退出，刹车退出，0档退出
       if((alm_blAlmOccrFlg == TRUE)
           ||(BikeBrake_blGetstate() == TRUE)
           ||(cp_stBikeRunInfoPara.uwBikeGear == 0)
           ||(bikeCruise.ThrottleResetCruiseFlag == 3))
       {
           bikeCruise.CruiseMode = 0;
           bikeCruise.KeepInTime = cp_ulSystickCnt;
           bikeCruise.ThrottleResetCruiseFlag = 0;
       }
         //指拨重新调速退出
        if(bikethrottle_stBikeThrottleOut.uwThrottlePercent > 350)
        {
            if(bikeCruise.ThrottleResetCruiseFlag == 2)
            {
                bikeCruise.ThrottleResetCruiseFlag = 3;
            }
        }
        else
        {
            if((bikeCruise.CruiseMode) && (bikeCruise.ThrottleResetCruiseFlag == 1) && (bikethrottle_stBikeThrottleOut.uwThrottlePercent < 200))
            {
                bikeCruise.ThrottleResetCruiseFlag = 2;
            }
        }
        
        if(bikeCruise.CruiseMode != 0)
        {
           bikethrottle_stBikeThrottleOut.uwThrottlePercent = bikeCruise.CruisePercent;
        }
    }
}
/***************************************************************
 Function: Throttlecruise;
 Description:  
 Call by: functions in 100ms TimeTask;
 Input Variables: N/A
 Output/Return Variables: N/A
 Subroutine Call: N/A;
 Reference: N/A
****************************************************************/
void bikethrottle_voStartAllow(void)
{
#if(THOTTLE_STARTRUNATSPEED == 0xAA)
    static UBYTE SetZeroRunMode = 0;
    
    if((bikespeed_stFreGetOut.uwLPFFrequencyPu < bikespeed_stFreGetCof.uwBikespeedKmh2Pu) && (bikethrottle_stBikeThrottleOut.uwThrottlePercent <= 200))
    {
        SetZeroRunMode = 0;
    }
    else if(cp_stBikeRunInfoPara.BikeSpeedKmH >= BIKETHROTTLE_STARTRUNSPEED)
    {
        SetZeroRunMode = 1;
    }
    if(SetZeroRunMode == 0)
    {
        bikethrottle_stBikeThrottleOut.uwThrottlePercent = 0;
    }
#endif
}
/*************************************************************************
 End of this File (EOF)!
 Do not put anything after this part!
*************************************************************************/