motor_ctl-1/1.FrameLayer/Source/TimeTask_Event.c

/**
 * @file TimeTask_Event.c
 * @author Zhang, Kai(zhangkai71@midea.com)
 * @brief Time task control
 * @version 0.1
 * @date 2021-09-27
 *
 * @copyright Copyright (c) 2021
 *
 */
#include "TimeTask_Event.h"
#include "AssistCurve.h"
#include "bikeinformation.h"
#include "CadAssist.h"
#include "FSM_1st.h"
#include "FSM_2nd.h"
#include "FuncLayerAPI.h"
#include "usart.h"
#include "canAppl.h"
#include "flash_master.h"
#include "queue.h"
#include "spdctrmode.h"
//#include "at32f421.h"//#include "stm32f10x.h"
#include "string.h"
#include "syspar.h"
#include "user.h"
#include "emcdeal.h"
#include "MosResCalib.h"
#include "bikespeed.h"
#include "UserGpio_Config.h"
#include "uart_TORG4BBSensor.h"
//#include "asr.h"

//#include "STLmain.h"

/******************************
 *
 *  Parameter
 *
 ******************************/
SLONG Signal_detect(void);

_op_ Op[proc_cnt] = {{Event_5ms, EVE1MSCNT_5ms, EVE1MSCNT_5ms},
                     {Event_10ms, EVE1MSCNT_10ms, EVE1MSCNT_10ms},
                     {Event_20ms, EVE1MSCNT_20ms, EVE1MSCNT_20ms},
                     {Event_100ms, EVE1MSCNT_100ms, EVE1MSCNT_100ms},
                     {Event_200ms, EVE1MSCNT_200ms, EVE1MSCNT_200ms}};

static SQWORD TimingTaskTimerTick = 0;
static SQWORD TimingTaskTimerTickTemp = 0;
static SQWORD TimingTaskTimerTickTempOld = 0;
static SQWORD TimingTaskTimerTickPassed = 0;
static UWORD  LoopServerExecutedFlag = 0;

ULONG Event_ulPowerSleepCnt = 0;
_Bool Event_blPowerSleepFlag = FALSE;

UWORD MovFiltCnt=0;
UWORD MovFiltVault=20;
ASS_MODE event_enMode;
_Bool event_blCurrentAssFlag = FALSE;
void BOKESPEED_Handler(void);
void TAPIN_Handler(void);
/******************************
 *
 *  Functions
 *
 ******************************/
void  event_voInit(void)
{
    event_enMode = NoneAss;
    event_blCurrentAssFlag = FALSE;
    MovFiltCnt=0;
    MovFiltVault=20;
}

void  Event_1ms(void)
{
    /* 一层状态机 */
    FSM_1st_Main();
    FSM1st_Sys_state.Event_hook();
    

    // user code end here
    /* 开关机控制 */
    if(cp_stFlg.RunModelSelect == CadAssist || cp_stFlg.RunModelSelect == TorqAssist)
    {
        event_voPowerDownCtrl();
    }
    event_voAutParaSaveCtrl();
    power_voPowerManagement(cp_stFlg.ParaSaveEEFlg,Event_blPowerSleepFlag);
    
    // Bike light control
    Can_Light_switch();
#if(EMCDEAL_EN!=0)
    cp_stBikeRunInfoPara.uwLightSwitch=1;
#endif
     bikelight_voBikeLightControl(cp_stBikeRunInfoPara.uwLightSwitch, BikeBrake_blGetstate());
    // cp_history info update
    Can_voMC_Run_1ms();

    // Brake detection 
    bikebrake_voBikeBrakeDetect();

    /* 电池电量限电流 */
    AssistCurrentLimitAccordingBMS(MC_RunInfo.SOC);

    /* 限制母线电流 */
    AssCurLimAccordIdc(adc_stUpOut.uwIbusAvgLpfPu);
#if(BIKE_OXFORD_EN==0)
    if(power_stPowStateOut.powerstate != POWER_ON_END)
    {
       cp_stBikeRunInfoPara.uwBikeGear = 0;
    }
#endif

    if(cp_stFlg.RunModelSelect == CadAssist)
    {
        /* 踏频中轴助力控制 */
        ass_voAssistFunc();
        uart_swTorqRefNm = ass_stCadAssCalOut.swAssCurrentPu;
        event_blCurrentAssFlag = ass_stCadAssCalOut.blAssistflag;
    }
    else if(cp_stFlg.RunModelSelect == TorqAssist)
    {
        /* 力矩AD值处理 */
//        torsensor_voTorADC();
        /* 力矩中轴助力输入 */
        ass_voTorqAssistInput();        
        /* 根据踏频脉冲对力矩信号进行滑动平均处理 */
        event_voTorqMovAve();
        /* 力矩中轴助力控制 */
        event_voAssCmd();
    }
    else
    {

    }
    // user code end here
    if ((event_blCurrentAssFlag == TRUE) && (cp_stFlg.RunPermitFlg == TRUE))
    {
        signal_state.Sensor = TRUE;
    }
    else
    {
        signal_state.Sensor = FALSE;
    }
      
    // spd assist model flg
    if((cp_stFlg.RunModelSelect == CadAssist) || (cp_stFlg.RunModelSelect == TorqAssist))
    {

    }
    else
    {
        if ((uart_slSpdRefRpm >= 10 || uart_slSpdRefRpm <= -10) && cp_stFlg.RunPermitFlg == TRUE)
        {
            signal_state.Assist = TRUE;
        }
        else
        {
            signal_state.Assist = FALSE;
        }
    }
     StartSelectAssistMode();
     if(power_stPowStateOut.powerstate == POWER_ON_END)
     {
     BOKESPEED_Handler();
     TAPIN_Handler();
     }
}




void Event_5ms(void)
{
   // DL_GPIO_togglePins(LED_PORT, LED_LED1_PIN);
    Can_voMC_Run_5ms();
#if(EMCDEAL_EN!=0)
    Emchandle();
#endif

}


void Event_10ms(void)
{
    // throttle vol ADC update
    bikethrottle_voBikeThrottleADC();

    // spd cmd set
    if(cp_stFlg.RunModelSelect != CadAssist && cp_stFlg.RunModelSelect != TorqAssist)
    {
        if(cp_stFlg.RotateDirectionSelect == ForwardRotate)
        {
            uart_slSpdRefRpm = ((SLONG)MC_MotorSPD_rpm_Percent * cp_stMotorPara.swRSpeedRpm) / 50;
//            uart_slSpdRefRpm = 5500;
//            uart_slSpdRefRpm = Signal_detect();
        }
        else if(cp_stFlg.RotateDirectionSelect == BackwardRotate)
        {
            uart_slSpdRefRpm =  -((SLONG)MC_MotorSPD_rpm_Percent * cp_stMotorPara.swRSpeedRpm) / 50;
//            uart_slSpdRefRpm = -5500;
//            uart_slSpdRefRpm = -Signal_detect();
        }
       //uart_slSpdRefRpm = - ((SLONG)bikethrottle_stBikeThrottleOut.uwThrottlePercent* 5750)/1000;  
        if(abs(uart_slSpdRefRpm) < 300)
        {
          uart_slSpdRefRpm = 0;
        }
    }

    if(bikespeed_stFreGetOut.uwBikeSpeedStopFlag!=0)
    {
        bikespeed_stFreGetOut.DECvnt=20;
    }
    else
    {
        bikespeed_stFreGetOut.DECvnt=10;
    }
    if(bikespeed_stFreGetOut.uwFrequencyPu > (bikespeed_stFreGetOut.uwLPFFrequencyPu + bikespeed_stFreGetOut.DECvnt))//20
    {
        bikespeed_stFreGetOut.uwLPFFrequencyPu = bikespeed_stFreGetOut.uwLPFFrequencyPu + bikespeed_stFreGetOut.DECvnt;
    }
    else if((bikespeed_stFreGetOut.uwFrequencyPu + bikespeed_stFreGetOut.DECvnt) < (bikespeed_stFreGetOut.uwLPFFrequencyPu))//20
    {
        bikespeed_stFreGetOut.uwLPFFrequencyPu = bikespeed_stFreGetOut.uwLPFFrequencyPu - bikespeed_stFreGetOut.DECvnt;
    }
    else
    {
        bikespeed_stFreGetOut.uwLPFFrequencyPu = bikespeed_stFreGetOut.uwFrequencyPu;
    }

    cadence_stFreGetOut.uwLPFFrequencyPuLast = cadence_stFreGetOut.uwLPFFrequencyPu;
    cadence_stFreGetOut.uwLPFFrequencyPu = (cadence_stFreGetOut.uwLPFFrequencyPu * cadence_stFreGetCof.uwCadenceLPFgain +
                                                        cadence_stFreGetOut.uwFrequencyPu * (100 - cadence_stFreGetCof.uwCadenceLPFgain)) /
                                                       100;
    
    cadence_stFreGetOut.uwFreqPercent = ((ULONG)cadence_stFreGetOut.uwLPFFrequencyPu << 14) / cadence_stFreGetCof.uwMaxCadenceFre; // Q14
    

    TORG4BB_Torque_Handler();

    /* Trip cal when open */
    bikespeed_votempTripCal();

#if(SampleModelSelect == COMBINATION)
   switch (pwm_stGenOut.uwSingelRSampleArea)
      {
        case SampleA:
            adc_LibSUMA.GainTemp   = (SLONG)((SLONG)adc_stUpOut.swCalibIaPu<< 10) / (SLONG)adc_stUpOut.swSampCapIaPu;
            adc_voSRCalibration(&adc_stCof , &adc_LibSUMA);
           if( adc_LibSUMA.uwCalibcomplete!=0)
           {
               adc_stCof.uwCalibcoefIA=adc_LibSUMA.uwCalibcoef;
            adc_LibSUMA.uwCalibcomplete=0;
           }
            break;
        case SampleB:
            adc_LibSUMB.GainTemp = (SLONG)((SLONG)adc_stUpOut.swCalibIbPu<< 10) / (SLONG)adc_stUpOut.swSampCapIbPu;
            adc_voSRCalibration(&adc_stCof , &adc_LibSUMB);
            if( adc_LibSUMB.uwCalibcomplete!=0)
            {
                adc_stCof.uwCalibcoefIB=adc_LibSUMB.uwCalibcoef;
                adc_LibSUMB.uwCalibcomplete=0;
            }
            break;

        default:
            break;
      }


       #endif
   CadSmartDisting();
}

void  Event_20ms(void)
{
  //wdt_counter_reload();//IWDG_ReloadCounter();   看門狗
  //  DL_WWDT_restart(WWDT0_INST);
 //   static  UWORD timeled=0;
 //      if(++timeled>10)
 //      {
 //          timeled=0;
//           DL_GPIO_togglePins(LED_PORT, LED_LED1_PIN);
//           DL_GPIO_togglePins(LED_PORT, LED_LED2_PIN);
 //      }

}

void Event_100ms(void)
{
    if((cp_stControlPara.uwControlFunEN&0xff)==0xaa)
    {
        Throttlecruise();
    }


}


UWORD tstCnt = 0;
void  Event_200ms(void)
{
    static UWORD  Cruisetime_1S=0;
    static UWORD  uwWwdttime=0;

    if(++uwWwdttime>=10)
    {
        uwWwdttime=0;
        DL_WWDT_restart(WWDT0_INST);
    }

#if(BIKE_OXFORD_EN!=0)
 static UWORD  time_1S=0;
 if(++time_1S>5)
 {
     time_1S=0;
     SendData(ID_PBU_TO_BMS, MODE_READ, 0x5000, (uint8_t *)NULL);
 }
#endif
 if((cp_stControlPara.uwControlFunEN&0xff)  ==0xaa)
 {
    if(++Cruisetime_1S>5)
    {
        Cruisetime_1S=0;
        OBC_ControlFUN.ubControlFunCruise  =(UBYTE)cp_stControlPara.uwControlFunEN &0xff;//巡航使能位
        OBC_ControlFUN.ubControlSta        =   bikeCruise.CruiseMode;
    //   SendData(ID_MC_BC, MODE_REPORT, 0x1902, (uint8_t *)&OBC_ControlFUN.ubControlFunCruise);
        DataProcess(&UART_TxBuff_Struct_OBC, ID_MC_BC, MODE_REPORT, 0x1902,  (uint8_t *)&OBC_ControlFUN.ubControlFunCruise);
    }
 }
 else
 {
     Cruisetime_1S=5;
 }

//    if (switch_flg.SysFault_Flag == TRUE)
//    {
//        SendData(ID_MC_BC, MODE_REPORT, 0x1104, (uint8_t *)&MC_ErrorCode.Code);
//    }
    /* 故障出现存储故障日志 */
//    if(switch_flg.SysFault_Flag)
//    {
//        if(cp_stFlg.ErrorSaveFlg == 0)
//        {
//           cp_stFlg.ErrorSaveFlg = 1;
//        }
//        cp_stFlg.ErrorSave2FlashFlg  = TRUE;
//    }
//    else
//    {
//        cp_stFlg.ErrorSaveFlg = 0;
//    }
//
//
//    if(cp_stFlg.ErrorSaveFlg == 1)
//    {
//        tstCnt ++;
//        stErrorLog.NotesInfo1 = 1;
//        stErrorLog.NotesInfo2 = 2;
//        stErrorLog.NotesInfo3 = 3;
//        que_ubPushIn(&stFlashErrorLog, &stErrorLog, 1);
//        cp_stFlg.ErrorSaveFlg = 2;
//    }
    CalcResVaule(&MosCal_A,adc_stUpOut.PCBTemp);
    CalcResVaule(&MosCal_B,adc_stUpOut.PCBTemp);
    CalcResVaule(&MosCal_C,adc_stUpOut.PCBTemp);
    /* Ebike车速、踏频、力矩、温度传感器故障判断 */
    if((cp_stFlg.RunModelSelect == CadAssist) || (cp_stFlg.RunModelSelect == TorqAssist))
    {
        alm_stBikeIn.uwTroqReg = torsensor_stTorSensorOut.uwTorqueReg;
        alm_stBikeIn.uwTroqPu = torsensor_stTorSensorOut.uwTorqueLPFPu;//torsensor_stTorSensorOut.uwTorquePu;
        alm_stBikeIn.blBikeSpdOvrFlg = bikespeed_stFreGetOut.blBikeSpeedSensorPwrErrorFlg;
        alm_stBikeIn.blCadenceFreqOvrFlg = cadence_stFreGetOut.blCadenceSensorErrorFlg;
        alm_stBikeIn.swMotorSpdAbsPu = scm_uwSpdFbkLpfAbsPu;
        alm_stBikeIn.uwBikeSpdPu = bikespeed_stFreGetOut.uwFrequencyPu;
        alm_stBikeIn.uwCadenceFreqPu = cadence_stFreGetOut.uwLPFFrequencyPu;
        alm_stBikeIn.uwMotorNTCReg = adc_stUpOut.MotorTempReg;
        alm_stBikeIn.uwPCBNTCReg = adc_stUpOut.PCBTempReg;
        alm_stBikeIn.uwThrottleReg = adc_stUpOut.uwThrottleReg;
        alm_stBikeIn.blThrottleExistFlg = TRUE;
        alm_stBikeIn.blMotorNTCExistFlg = FALSE;
        alm_voDetec200MS(&alm_stBikeIn, &alm_stDetect200MSCoef);
    }

    if(power_stPowStateOut.powerstate != POWER_START)
    {
        Can_voMC_Run_200ms();
    }

    //bikelight_voGetBikeLightError(adc_stUpOut.uwU6VPu);
    //bikespeed_voGetBikeSpeedPwrError(adc_stUpOut.uwU5VPu);

    //if( ((switch_flg.SysFault_Flag == TRUE) || (switch_flg.SysWarnning_Flag == TRUE)) && (MC_WorkMode == 1))
    if( ((switch_flg.SysFault_Flag == TRUE) || (switch_flg.SysWarnning_Flag == TRUE)))
    {
         SendData(&UART_TxBuff_Struct_OBC, ID_MC_BC, MODE_REPORT, 0x1104, (uint8_t *)&MC_ErrorCode.Code);
    }
    GearLimitCurHandle();
}

SLONG Signal_detect(void)
{
    SLONG tmp_slGearSpeedRpm = 0;
    
    if(MC_ControlCode.GearSt == 0x01)
    {
        tmp_slGearSpeedRpm = M_R_SPD_RPM*10/25;
    }
    else if(MC_ControlCode.GearSt == 0x02)
    {
        tmp_slGearSpeedRpm = M_R_SPD_RPM*15/25;
    }
    else if(MC_ControlCode.GearSt == 0x03)
    {
        tmp_slGearSpeedRpm = M_R_SPD_RPM*15/25;
    }
    else if(MC_ControlCode.GearSt == 0x04)
    {
        tmp_slGearSpeedRpm = M_R_SPD_RPM;
    }
    else if(MC_ControlCode.GearSt == 0x05)
    {
        tmp_slGearSpeedRpm = M_R_SPD_RPM;
    }
    else
    {
        tmp_slGearSpeedRpm = 0;
    }
    
    return tmp_slGearSpeedRpm;
}


void event_voAssCmd(void)
{
    switch (event_enMode)
    {
        case NoneAss:
            Ass_FSM = StopAssit;
            ass_enCadAssStatus = CadAssStop;
            ass_pvt_stCurLpf.slY.sw.hi = 0;
            ass_CalOut.blAssistflag  = FALSE;
            ass_stCadAssCalOut.blAssistflag = FALSE;
            ass_stCadAssCalOut.blPreStopFlag = FALSE;
            uart_swTorqRefNm = 0;
            event_blCurrentAssFlag = FALSE;

            /* 力矩中轴助力模式判断 */
            if(cp_stBikeRunInfoPara.uwBikeGear > 0)
            {   
                if((cp_stBikeRunInfoPara.uwBikeGear == 0x22)||(bikethrottle_stBikeThrottleOut.uwThrottlePercent > 250))
                {
                    /* 力矩中轴速度模式 */
                    event_enMode = SpdAss;
                }
                else
                {
                    /* 力矩中轴力矩助力模式 */
                    event_enMode = TorqAss;
                }
            }
            else 
            {
                /* 不处理 */
            }
        break;
        
        case TorqAss: 
            /* 力矩模式 */
            /* 清除踏频助力标志（避免电流电压突变） */
            if(ass_stCadAssCalOut.blAssistflag == TRUE)
            {
                ass_stCadAssCalOut.blPreStopFlag = FALSE;
                ass_CalOut.swVoltLimitPu = scm_swUqRefPu;
                ass_CalOut.slUqLimSum = (SLONG)scm_swUqRefPu <<8;
                ass_stCadAssCalOut.blAssistflag = FALSE;
            }
            /* 骑行助力函数 */
            Assist();
            uart_swTorqRefNm = Assist_torqueper;
            ass_stCadAssCalOut.swVoltLimitPu = ass_CalOut.swVoltLimitPu;
            event_blCurrentAssFlag = ass_CalOut.blAssistflag;

            /* 助力模式切换处理 */
            if(cp_stBikeRunInfoPara.uwBikeGear == 0)
            {   
                if(event_blCurrentAssFlag == FALSE)
                {
                    event_enMode = NoneAss;
                }
            }
            else if((cp_stBikeRunInfoPara.uwBikeGear == 0x22) || (bikethrottle_stBikeThrottleOut.uwThrottlePercent > 250))
            {
                event_enMode = SpdAss;
            }
            else 
            {
                /* 不处理 */
            }

        break;

        case SpdAss:  
            /* 速度模式 */
            /* 模式切换处理 */
            if(ass_CalOut.blAssistflag == TRUE)
            {
                ass_enCadAssStatus= CadAssPro;
                ass_stCadAssCalOut.swAssCurrentPu = Assist_torqueper;
                ass_stCadAssCalOut.swVoltLimitPu = scm_swUqRefPu;
                ass_stCadAssParaPro.slVoltageSum = ((SLONG)scm_swUqRefPu <<8);

                Ass_FSM = StopAssit;
                Assist_torqueper = 0;
                ass_pvt_stCurLpf.slY.sw.hi = 0;
                ass_CalOut.blAssistflag = FALSE;
            }

            /* 速度控制 */
            ass_voAssistFunc();
            uart_swTorqRefNm = ass_stCadAssCalOut.swAssCurrentPu;
            event_blCurrentAssFlag = ass_stCadAssCalOut.blAssistflag;

            /* 助力模式切换处理 */
            if(cp_stBikeRunInfoPara.uwBikeGear == 0)
            {   
                if(event_blCurrentAssFlag == FALSE)
                {
                    event_enMode = NoneAss;
                }
            }
            else if((cp_stBikeRunInfoPara.uwBikeGear <=5) && (bikethrottle_stBikeThrottleOut.uwThrottlePercent <= 250))
            {
                event_enMode = TorqAss;
                ass_stCadAssCalOut.swAssCurrentPu= scm_swIqFdbLpfPu;
                uart_swTorqRefNm= scm_swIqFdbLpfPu;
                Assist_torqueper = scm_swIqFdbLpfPu;
                ass_pvt_stCurLpf.slY.sw.hi = scm_swIqFdbLpfPu;
                ass_stCadAssCalOut.blPreStopFlag = TRUE; 
                ass_enCadAssStatus = CadAssStop;
            }
            else 
            {
                /* 不处理 */
            }
            
        break;

        default:
        break;
    }
}

void event_voTorqMovAve(void)
{
    if(cp_stBikeRunInfoPara.BikeSpeedKmH<60)
    {
        MovFiltVault=10;
    }
    else
    {
        if(MovFiltVault<50)
            MovFiltVault++;
    }
    if(MovFiltCnt<=100)
        MovFiltCnt++;
    /* 每两次踏频脉冲记录一次滑动平局力矩值*/
    if ((0 == (cadence_stFreGetOut.uwForwardCnt % 2) && (ass_CalIn.uwcadancelast != ass_CalIn.uwcadance))||(MovFiltCnt > MovFiltVault))
    {
        maf_torque.value = torsensor_stTorSensorOut.uwTorquePu;
        MoveAverageFilter(&maf_torque);
        MovFiltCnt=0;
    }
}

void event_voPowerDownCtrl(void)
{
    /* 车速>10 km/h 电机转速 > 100 且配置停机时间！= 0 时不进入停机及时*/
    UWORD uwTempAutoPowerOffTime;
    uwTempAutoPowerOffTime = ass_ParaCong.uwAutoPowerOffTime & 0x00FF; //根据通信协议后8位为自动关机时间
    if((cp_stBikeRunInfoPara.BikeSpeedKmH <= 10)&&(MC_RunInfo.MotorSpeed <= 100) && uwTempAutoPowerOffTime!=0 && OBC_ButtonStatus.blOBCbuttonPress != TRUE)
    {
        if(Event_ulPowerSleepCnt >= (ULONG)uwTempAutoPowerOffTime*60L*1000L)  //10 minutes
        {
            Event_blPowerSleepFlag = TRUE;
        }
        else
        {
            Event_ulPowerSleepCnt ++;
            Event_blPowerSleepFlag = FALSE;
        }
    }
    else
    {
        OBC_ButtonStatus.blOBCbuttonPress = FALSE;
        Event_ulPowerSleepCnt = 0;
        Event_blPowerSleepFlag = FALSE;
    }
}

ULONG ulAutParaSaveCnt=0;
void event_voAutParaSaveCtrl(void)
{
    /* 每10分钟储存标志立一次，在1ms时间片运行*/
    if((cp_ulSystickCnt-ulAutParaSaveCnt)> 600*1000) // 600：600秒
    {
        ulAutParaSaveCnt = cp_ulSystickCnt;      
        cp_stFlg.ParaHistorySaveEEFinishFlg = FALSE;
    }
}

void event_voAlarmParaSaveCtrl(void)
{  
    cp_stFlg.ParaHistorySaveEEFinishFlg = FALSE;
}


void TimingTaskTimerServer(void)
{
    TimingTaskTimerTick++;
    LoopServerExecutedFlag = 0;
}

void TimingTaskLoopServer(void)
{
    SLONG cnt;
    TimingTaskTimerTickTempOld = TimingTaskTimerTickTemp;
    TimingTaskTimerTickTemp = TimingTaskTimerTick;
    TimingTaskTimerTickPassed = TimingTaskTimerTickTemp - TimingTaskTimerTickTempOld;
    if (LoopServerExecutedFlag == 0)
    {
        for (cnt = 0; cnt < proc_cnt; cnt++)
        {
            Op[cnt].tick -= TimingTaskTimerTickPassed;
            if (Op[cnt].tick <= 0)
            {
                Op[cnt].tick += Op[cnt].timespan;
                Op[cnt].proc();
            }
        }
        LoopServerExecutedFlag = 1;
    }
}


void StartSelectAssistMode(void)
{
  static  UBYTE UpSelecFlg=0;
  static  UWORD UpSelecCnt=0;
  static  UWORD HVoltCnt=0;
  static  UWORD LVoltCnt=0;
  if((UpSelecFlg==0) &&(power_stPowStateOut.blPowerStartupFlg == TRUE) )
  {
   if(++UpSelecCnt<6)
     {
       if( TORQUEVOLREG()>Sel_TORQ_VOL)
       {
           HVoltCnt++;
       }
       else
       {
           LVoltCnt++;
       }
     }
   else
      {
       if(HVoltCnt>LVoltCnt)
         {
           if(cp_stFlg.RunModelSelect!=TorqAssist)
           {
               cp_stFlg.RunModelSelect = TorqAssist;//ClZLOOP; //IFContorl; //CadAssist;
               MC_UpcInfo.stTestParaInfo.RunModelSelect = TorqAssist;
               cadence_stFreGetCof.uwNumbersPulses = cadence_stFreGetCof.uwTorque_NumbersPulses;
               cp_stFlg.ParaAssistUpdateFinishFlg = TRUE; //更新助力参数
               MC_UpcInfo.stSensorInfo.uwSaveFlg=TRUE;//更新到eeprom
               cp_stFlg.ParaSaveEEFlg = TRUE; //关机存flash
               MC_UpcInfo.stTestParaInfo.uwSaveFlg = TRUE ;//存储运行模式
           }
         }
         else
         {
            if((cp_stFlg.RunModelSelect!=CadAssist)
                ||(cadence_stFreGetCof.uwCad_NumbersPulses==0)
                ||(cadence_stFreGetCof.uwCad_NumbersPulses==0xffff))
           {
              cp_stFlg.RunModelSelect = CadAssist; //ClZLOOP; //IFContorl; //CadAssist;
              MC_UpcInfo.stTestParaInfo.RunModelSelect = CadAssist;
              if((cadence_stFreGetCof.uwCad_NumbersPulses==0)
                    ||(cadence_stFreGetCof.uwCad_NumbersPulses==0xffff))
              {
                  cadence_stFreGetCof.uwCad_NumbersPulses=CADENCE_NUMBERS_PULSES; //默认值
              }
              cadence_stFreGetCof.uwNumbersPulses=cadence_stFreGetCof.uwCad_NumbersPulses;
              cp_stFlg.ParaAssistUpdateFinishFlg = TRUE; //更新助力参数
              MC_UpcInfo.stSensorInfo.uwSaveFlg=TRUE;//更新到eeprom
              cp_stFlg.ParaSaveEEFlg = TRUE; //关机存flash
              MC_UpcInfo.stTestParaInfo.uwSaveFlg = TRUE ;
           }
         }

          UpSelecFlg=1;
      }

  }
}

/***************************************************************
 Function: BOKESPEED_Handler;
 Description: calc bike  speed
 Call by:
 Input Variables: N/A
 Output/Return Variables: N/A
 Subroutine Call: N/A;
 Reference: N/A
****************************************************************/
void BOKESPEED_Handler(void)
{
#if TORG4BBTORQU_ENABLE
    bikespeed_stFreGetOut.uwLPFFrequencyPu = (UWORD)((((stTORG4BBInfo.uwFlyWheelSpeed<<20)*6/1000/FBASE)<<20)/36/1048/10);
#else
  /* add user code begin TMR16_GLOBAL_IRQ 0 */
//Bike Speed
   UWORD uwIntSource = 0;

       uwIntSource = 1;
        /* CadenceCal and BikespeedCal */
        bikespeed_voBikeSpeedCal(uwIntSource);

       if(adc_stUpOut.MotorTempReg>1241 )// 1241=1V
       {
           bikespeed_stFreGetOut.blBikeSpeedPort=TRUE;
       }
       else if(adc_stUpOut.MotorTempReg<372)//372-0.3V
       {
           bikespeed_stFreGetOut.blBikeSpeedPort=FALSE;
       }
       if(bikespeed_stFreGetOut.blBikeSpeedPort !=bikespeed_stFreGetOut.blBikeSpeedPortState)
      {
        bikespeed_stFreGetOut.blBikeSpeedPortState=bikespeed_stFreGetOut.blBikeSpeedPort;
           if(bikespeed_stFreGetOut.blBikeSpeedPortState != FALSE )
           {
               uwIntSource = 3;
               bikespeed_voBikeSpeedCal(uwIntSource);
           }
      }
#endif
}
/***************************************************************
 Function: TAPIN_Handler;
 Description: calc bike  speed
 Call by:
 Input Variables: N/A
 Output/Return Variables: N/A
 Subroutine Call: N/A;
 Reference: N/A
****************************************************************/
UWORD cadcntcal=0;
//UWORD uwTaPinPortSta=2;
void TAPIN_Handler(void)
{
  /* add user code begin TMR17_GLOBAL_IRQ 0 */
//韪忛牷
UWORD uwIntSource = 0;


   uwIntSource = 1;
   /* CadenceCal and BikespeedCal */
    cadence_voCadenceCal(uwIntSource);


    if(TORG4BB_Get_CAD_PORT() == RESET)
      {
        if(cadence_stFreGetOut.uwTaPinPortSta!=0)
        {
            cadence_stFreGetOut.uwTaPinPortSta =0;
          uwIntSource = 2;
          cadence_voCadenceCal(uwIntSource);
          cadcntcal++;
        }
      }
    else
    {
        cadence_stFreGetOut.uwTaPinPortSta =1;
    }


}