motor_ctl-1/3.BasicFunction/Source/CadAssist.c

/**
 * @file CadAssist.c
 * @author xu, haifeng(xuhf58@midea.com)
 * @brief
 * @version 0.1
 * @date 2023-05-10
 *
 * @copyright Copyright (c) 2023
 *
 */
#include "CadAssist.h"
#include "bikeinformation.h"
#include "Cadence.h"
#include "FSM_1st.h"
#include "Syspar.h"
#include "flash_master.h"
#include "string.h"
#include "typedefine.h"
#include "user.h"
#include "hwsetup.h"
#include "bikethrottle.h"
#include "bikebrake.h"
#include "AssistCurve.h"
#include "FuncLayerAPI.h"
#include "MosResCalib.h"
#include "usart.h"
/******************************
 *
 * constant Parameter
 *
 ******************************/



/******************************
 *
 *  Variable Define and Initial
 *
 ******************************/
CAD_ASS_FSM_STATUS      ass_enCadAssStatus = {CadAssStop};
UWORD                   ass_uwCadAssVoltSatCnt = 0;
UWORD                   ass_uwCadAssStartCnt = 0;
CADASS_PID              ass_stCadAssSpdCtl = {0};
CADASS_COEF             ass_stCadAssCoef = {0};
CADASS_PER_IN           ass_stCadAssParaIn = {0};
CADASS_PER_PRO          ass_stCadAssParaPro = {0};
CADASS_PER_OUT          ass_stCadAssCalOut = {FALSE,FALSE,0,0};
LPF_OUT ass_lpf_SpeedCmd = {0};

GEAR_COEF ass_MaxSpeed;

GEAR_COEF CadAssistSped;

CADPOWER_STRUCT CadSpdPower;
GEAR_CUR_TRUCT GearCur;

/******************************
 *
 *  Function Statement
 *
 ******************************/
void ass_voSpdPidCoefInit(CADASS_PID *spdpid);
void ass_voSpdPidInit(CADASS_PID *spdpid);
void ass_voSpdPidCtrl(CADASS_PID *spdpid);
void ass_voParameterInit(void);
void ass_voAssistCoefCal(void);
void ass_voAssistCmdDeal(void);
void ass_voParameterInput(void);
void ass_voAssMaxCurCal(void);
void ass_voAssInitVoltageCal(void);
void ass_voAssCurCalFunc(void);
void ass_voAssistFunc(void);
void CadSpedLinitPowerInit(void);
void CadSpeedLimitPower(void);
void GearLimitCurInit(void);

UBYTE   SetZeroRunMode=0;
/**
 * @brief  Cad Assist Module Pid Init
 *
 * @param void
 * @return void
 */
extern ASS_PARA_SET       ass_ParaSet;                          /**< For Debug */
void ass_voSpdPidCoefInit(CADASS_PID *spdpid)
{ 
    spdpid->uwPidKp = ASS_PID_KP;                               /**< Pid Kp Initial */
    spdpid->uwACTPidKp=ASS_PID_KP;
    spdpid->swPidLimMax = ASS_VOL_STEP_MAX;                     /**< Pid Limit */
    spdpid->swPidVolDec = ASS_VOL_DEC_STEP;                     /**< Pid Voltage Dec */
}

/**
 * @brief  Cad Assist Module Speed Control Pid Init
 *
 * @param void
 * @return void
 */
void ass_voSpdPidInit(CADASS_PID *spdpid)
{
    spdpid->swPidRef = 0;
    spdpid->swPidFbk = 0;
    spdpid->swIqRef = 0;
    spdpid->swIqFbk = 0;
    spdpid->swDirection = -1;
    spdpid->slPidErr = 0;
    spdpid->slPidP = 0;
    spdpid->swPidOut = 0;
}

/**
 * @brief  Cad Assist Module Pid Control
 *
 * @param void
 * @return void
 */
SWORD test_value1;
SWORD test_value2;
SWORD test_value3;
SWORD test_value4;
SWORD test_value5;
void ass_voSpdPidCtrl(CADASS_PID *spdpid)
{
    SLONG tmp_slSpdErr = 0;
    SLONG tmp_slCurErr = 0;
    
    tmp_slSpdErr = (SLONG)spdpid->swPidRef - (SLONG)spdpid->swPidFbk;  //Q15 - Q15 = Q15
    
    if(spdpid->swDirection == 1)
    {
        if(tmp_slSpdErr > 0)
        {
            if(tmp_slSpdErr > ASS_PID_ERR_MIN)
            {
                tmp_slSpdErr -= ASS_PID_ERR_MIN;
            }
            else
            {
                tmp_slSpdErr = 0;
            }
            spdpid->slPidErr = tmp_slSpdErr;
        }
        else
        {
            spdpid->slPidErr = (tmp_slSpdErr<<1);
        }
    }
    else if(spdpid->swDirection == -1)
    {
        if(tmp_slSpdErr > 0)
        {
            spdpid->slPidErr = (tmp_slSpdErr<<1);
        }
        else
        {
            if(tmp_slSpdErr < -ASS_PID_ERR_MIN)
            {
                tmp_slSpdErr += ASS_PID_ERR_MIN;
            }
            else
            {
                tmp_slSpdErr = 0;
            }
            spdpid->slPidErr = tmp_slSpdErr;
        }
    }
    else
    {
      
    }
    


   // spdpid->slPidP = (SLONG)spdpid->uwPidKp * spdpid->slPidErr >> 15;   //Q8 * Q15 / Q15 = Q8
    spdpid->slPidP = (SLONG)spdpid->uwACTPidKp * spdpid->slPidErr >> 15;   //Q8 * Q15 / Q15 = Q8
    
    if(spdpid->slPidP > (SLONG)spdpid->swPidLimMax)    //Q8
    {
        spdpid->slPidP = (SLONG)spdpid->swPidLimMax;
    }
    else if(spdpid->slPidP < -((SLONG)spdpid->swPidLimMax))       //Q8
    {
        spdpid->slPidP = -((SLONG)spdpid->swPidLimMax);
    }
    else
    {
        
    }
    
    tmp_slCurErr = (SLONG)spdpid->swIqRef - (SLONG)spdpid->swIqFbk;  //Q14
    
    if(spdpid->swDirection == 1)
    {
        if(tmp_slCurErr < ASS_PID_IERR_MIN)
        {
            if(spdpid->swIqRef < (ASS_PID_IERR_MIN<<1))
            {
                spdpid->swPidOut = 0;
            }
            else
            {
                spdpid->swPidOut = -spdpid->swPidVolDec;
            }
        }
        else
        {
            spdpid->swPidOut = spdpid->slPidP;
        }
        
        if((spdpid->swIqFbk < 0) && (spdpid->slPidP < 0))
        {
            spdpid->swPidOut = 0;
        }
    }
    else if(spdpid->swDirection == -1)
    {
        if(tmp_slCurErr > -ASS_PID_IERR_MIN)
        {
            if(spdpid->swIqRef > (-ASS_PID_IERR_MIN<<1))
            {
                spdpid->swPidOut = 0;
            }
            else
            {
                spdpid->swPidOut = spdpid->swPidVolDec;
            }
        }
        else
        {
            spdpid->swPidOut = spdpid->slPidP;
        }
        
        if((spdpid->swIqFbk > 0) && (spdpid->slPidP > 0))
        {
            spdpid->swPidOut = 0;
        }
    }
    else
    {
        //Parameter Err
        spdpid->swPidOut = 0;
    }
}

/**
 * @brief  Cad Assist Module Para Init
 *
 * @param void
 * @return void
 */
void ass_voParameterInit(void)
{
    /** Local variable */
    UWORD tmp_uwCntValue = 0;
    
    /** Speed Control Pid Parameter Initial */
    ass_voSpdPidCoefInit(&ass_stCadAssSpdCtl);
    ass_voSpdPidInit(&ass_stCadAssSpdCtl);

    ass_enCadAssStatus = CadAssStop;
    ass_uwCadAssVoltSatCnt = 0;
    
    /** Module Coefficient Initial */
    ass_stCadAssCoef.uwWheelCircumferenceCm = ASS_BIKE_WHEEL_DIAMETER; 
    ass_stCadAssCoef.uwMechRationMotor = (UWORD)(((ULONG)ASS_BIKE_MECH_RATION*1024)/1000);
    ass_stCadAssCoef.uwAssistMaxBikeSpeed = ASS_SPD_LIMIT + 8;
    ass_stCadAssCoef.uwThrottleMaxBikeSpeed = ASS_THROTTLE_SPD_LIMIT + 8;
    ass_stCadAssCoef.uwCartMaxBikeSpeed = ASS_CART_SPEED + 2;
    ass_stCadAssCoef.uwMaxCadRpm = ASS_CAD_MAX_RPM;
    ass_stCadAssCoef.uwCadencePulses = TORQUE_NUMBERS_PULSES;//CADENCE_NUMBERS_PULSES;
    ass_stCadAssCoef.uwMotorPoles = M_POLE_PAIRS;
    ass_stCadAssCoef.uwCofCurMaxPu = (UWORD)(((ULONG)(M_IS_PEAK_MAX_AP) << 14)/IBASE);  
    ass_stCadAssCoef.uwFluxPu = ((ULONG)M_FLUX_WB << 12) / ((ULONG)VBASE * 1000000 / ((ULONG) 2 * 31416 * FBASE / 1000)); 
    ass_stCadAssCoef.swKmhToMSpdPu = 889L*(SLONG)ass_stCadAssCoef.uwMotorPoles*(SLONG)ass_stCadAssCoef.uwMechRationMotor/((SLONG)ass_stCadAssCoef.uwWheelCircumferenceCm * (SLONG)FBASE);
    ass_stCadAssCoef.slBSpdPuToMSpdPu = ((SLONG)ass_stCadAssCoef.uwMechRationMotor * ass_stCadAssCoef.uwMotorPoles); 
    ass_stCadAssCoef.swMSpdpuLimit = (SWORD)((SLONG)ass_stCadAssCoef.swKmhToMSpdPu*(SLONG)(ass_stCadAssCoef.uwAssistMaxBikeSpeed + ASS_SPD_LIMIT_ERR)  >> 4);
    ass_stCadAssCoef.swAssistSectionComp = _IQ14(1.0/3.0);
    
    /** Module Input Parameter Initial */
    ass_stCadAssParaIn.swFlxIqLimitPu = ass_stCadAssCoef.uwCofCurMaxPu;  
    ass_stCadAssParaIn.swPwrIqLimitPu = ass_stCadAssCoef.uwCofCurMaxPu;  
    ass_stCadAssParaIn.uwBmsIqLimitPu = ass_stCadAssCoef.uwCofCurMaxPu;  
    ass_stCadAssParaIn.swDirection = -1;
    ass_stCadAssParaIn.uwGearSt = 0;
    ass_stCadAssParaIn.uwCadancePer = 0;
    ass_stCadAssParaIn.uwCadanceFwCnt = 0;
    ass_stCadAssParaIn.uwBikespeedPu = 0;
    ass_stCadAssParaIn.swSpdFbkPu = 0;
    ass_stCadAssParaIn.swIqRefpu = 0;
    ass_stCadAssParaIn.swIqFbkpu = 0;
    ass_stCadAssParaIn.swUqOutputpu = 0;
    ass_stCadAssParaIn.uwThrottlePercent = 0;
      
    /** Module Process Parameter Initial */
    ass_stCadAssParaPro.uw1msCnt = 0;
    ass_stCadAssParaPro.uw10msCnt = 0;
    ass_stCadAssParaPro.uwAssitMode = 0;
    ass_stCadAssParaPro.uwAssitModeLast = 0;
    ass_stCadAssParaPro.swAssistSpdCmd = 0;
    ass_stCadAssParaPro.swCurrentMaxPu = (SWORD)ass_stCadAssCoef.uwCofCurMaxPu;
    ass_stCadAssParaPro.swUqLimitInitPu = 0;
    ass_stCadAssParaPro.swVoltageStep = 0;
    ass_stCadAssParaPro.slVoltageSum = 0;
    ass_stCadAssParaPro.swBikeSpd2MotSpdPu = 0;
    for(tmp_uwCntValue=0;tmp_uwCntValue<10;tmp_uwCntValue++)
    {
        ass_stCadAssParaPro.swSpdFbkPuRec[tmp_uwCntValue] = 0;
    }
    ass_stCadAssParaPro.swSpdFbkPuAcc = 0;
    ass_stCadAssParaPro.swTargetAssCurPu = 0;
    ass_stCadAssParaPro.swTargetAssCurAcc = ASS_CUR_STEP;
    
    mth_voLPFilterCoef((ASS_CMD_SPEED_LPF_TIME*333L),1000L,&ass_lpf_SpeedCmd.uwKx);
    
    /** Module Output Parameter Initial */
    ass_stCadAssCalOut.blAssistflag = FALSE;
    ass_stCadAssCalOut.blPreStopFlag = FALSE;
    ass_stCadAssCalOut.swAssCurrentPu = 0;
    ass_stCadAssCalOut.swVoltLimitPu = 0;
    GearLimitCurInit();
}

/**
 * @brief  Cad Assist Module Coefficient Get and Calculate Function
 *
 * @param void
 * @return void
 */
void ass_voAssistCoefCal(void)
{
    /** Local variable */
    SLONG tmp_slAssistSpdLimit = 0;
    
    /** Bike and assist parameter config */
    ass_stCadAssCoef.uwWheelCircumferenceCm = ass_ParaCong.uwWheelPerimeter + ass_ParaCong.swDeltPerimeter; 
    ass_stCadAssCoef.uwMechRationMotor = ass_ParaCong.uwMechRationMotor;
    ass_stCadAssCoef.uwAssistMaxBikeSpeed = (ass_ParaSet.uwAssistLimitBikeSpdStart<<4) + 8; // 骑行助力限速
    ass_stCadAssCoef.uwThrottleMaxBikeSpeed = (ass_ParaCong.uwThrottleMaxSpdKmH<<4) + 8;
    ass_stCadAssCoef.uwCartMaxBikeSpeed = (ass_ParaCong.uwCartSpdKmH<<4) + 2;
    if(cp_stFlg.RunModelSelect == TorqAssist)
    {
        cadence_stFreGetCof.uwNumbersPulses = cadence_stFreGetCof.uwTorque_NumbersPulses;
     }
    else
    {
        cadence_stFreGetCof.uwNumbersPulses=cadence_stFreGetCof.uwCad_NumbersPulses;
    }
    ass_stCadAssCoef.uwCadencePulses = cadence_stFreGetCof.uwNumbersPulses;
    ass_stCadAssCoef.uwMotorPoles = cp_stMotorPara.swMotrPolePairs;
    ass_stCadAssCoef.uwCofCurMaxPu = (UWORD)(((ULONG)(cp_stMotorPara.swIpeakMaxA) << 14)/IBASE); 
    ass_stCadAssCoef.uwFluxPu = ((ULONG)cp_stMotorPara.swFluxWb << 12) / cof_uwFluxbWb;
    
    /** Transform from bikespeed Km/h to motor speed pu Parameter calculate */
    ass_stCadAssCoef.swKmhToMSpdPu = (SWORD)(889L*(SLONG)ass_stCadAssCoef.uwMotorPoles*(SLONG)ass_stCadAssCoef.uwMechRationMotor/((SLONG)ass_stCadAssCoef.uwWheelCircumferenceCm * (SLONG)FBASE));
  
    /** Transform from bikespeed pu to motor speed pu Parameter calculate */
    ass_stCadAssCoef.slBSpdPuToMSpdPu = ((SLONG)ass_stCadAssCoef.uwMechRationMotor * ass_stCadAssCoef.uwMotorPoles);  
    
    /** Motor Max Speed calculate according to bike speed limit */


    tmp_slAssistSpdLimit = (SLONG)ass_stCadAssCoef.swKmhToMSpdPu*((SLONG) ass_stCadAssCoef.uwThrottleMaxBikeSpeed + ASS_SPD_LIMIT_ERR)  >> 4;
    ass_stCadAssCoef.swThrottleMaxspd= (SWORD)tmp_slAssistSpdLimit;
    tmp_slAssistSpdLimit = (SWORD)((SLONG)ass_stCadAssCoef.swKmhToMSpdPu*((SLONG)ass_stCadAssCoef.uwAssistMaxBikeSpeed + ASS_SPD_LIMIT_ERR)  >> 4);
    ass_stCadAssCoef.swAssistMaxSpd=(SWORD)tmp_slAssistSpdLimit;

   #if(THROTTLEGEAR !=0)
     ass_MaxSpeed.uwGearOne  =(UWORD)( ass_stCadAssCoef.uwThrottleMaxBikeSpeed  *ASS_ONE_SPEED_PERCENT/100  );//
    ass_MaxSpeed.uwGearTwo  =(UWORD)(  ass_stCadAssCoef.uwThrottleMaxBikeSpeed  *ASS_TWO_SPEED_PERCENT/100  );
    ass_MaxSpeed.uwGearThree=(UWORD)(  ass_stCadAssCoef.uwThrottleMaxBikeSpeed  *ASS_THREE_SPEED_PERCENT/100  );
    ass_MaxSpeed.uwGearFour =(UWORD)(  ass_stCadAssCoef.uwThrottleMaxBikeSpeed  *ASS_FOUR_SPEED_PERCENT/100  );
    ass_MaxSpeed.uwGearFive =(UWORD)(  ass_stCadAssCoef.uwThrottleMaxBikeSpeed  *ASS_FIVE_SPEED_PERCENT/100 );//
#endif 

#if(CADASSISTGEAR !=0)
    CadAssistSped.uwGearOne=(UWORD)( ass_stCadAssCoef.uwThrottleMaxBikeSpeed  *CAD_ONE_SPEED_PERCENT/100  );//
    CadAssistSped.uwGearTwo=(UWORD)(  ass_stCadAssCoef.uwThrottleMaxBikeSpeed  *CAD_TWO_SPEED_PERCENT/100  );
    CadAssistSped.uwGearThree=(UWORD)(  ass_stCadAssCoef.uwThrottleMaxBikeSpeed  *CAD_THREE_SPEED_PERCENT/100  );
    CadAssistSped.uwGearFour =(UWORD)(  ass_stCadAssCoef.uwThrottleMaxBikeSpeed  *CAD_FOUR_SPEED_PERCENT/100  );
    CadAssistSped.uwGearFive =(UWORD)(  ass_stCadAssCoef.uwThrottleMaxBikeSpeed  *CAD_FIVE_SPEED_PERCENT/100 );//
#endif
    if(ass_ParaSet.uwAssistLimitBikeSpdStart >= 25)
    {
        ass_stCadAssCoef.swAssistSectionComp = ((25L<<14)/(3L*ass_ParaSet.uwAssistLimitBikeSpdStart));
    }
    else
    {
        ass_stCadAssCoef.swAssistSectionComp = _IQ14(1.0/3.0);
    }
    
#if(POWER_INTELLIGENCADGEAR_EN!=0)
         CadSpedLinitPowerInit();
#endif
}

/**
 * @brief  Cad Assist Module Parameter input
 *
 * @param void
 * @return void
 */
void ass_voParameterInput(void)
{
    /** Motor Max Speed calculate according to bike speed limit */
    if( ass_stCadAssParaPro.uwAssitMode == 7)
       {
       ass_stCadAssCoef.swMSpdpuLimit =ass_stCadAssCoef.swThrottleMaxspd;
       }
       else
       {
       ass_stCadAssCoef.swMSpdpuLimit =ass_stCadAssCoef.swAssistMaxSpd;
       }
    /** Flux current limit input */
    ass_stCadAssParaIn.swFlxIqLimitPu = abs(flx_stCtrlOut.swIqLimPu);

    /** Power limit current limit input */
    ass_stCadAssParaIn.swPwrIqLimitPu = abs(pwr_stPwrLimOut2.swIqLimPu);

    /** BMS current limit input */
    ass_stCadAssParaIn.uwBmsIqLimitPu =  ass_CurLimitCalBMSOut.uwIqLimitAbs;
//    (ass_CurLimitCalBMSOut.uwIdcLimIqAbs < ass_CurLimitCalBMSOut.uwIqLimitAbs) ? \
//                                         ass_CurLimitCalBMSOut.uwIdcLimIqAbs : ass_CurLimitCalBMSOut.uwIqLimitAbs;

    /** Assist motor run max voltage limit */
    ass_stCadAssParaIn.swAssMaxVolLimpu = scm_swVsDcpLimPu;
      
    /** Assist motor run direction input */
    if(cp_stFlg.RotateDirectionSelect == ForwardRotate)
    {
        ass_stCadAssParaIn.swDirection = 1;
    }
    else 
    {
        ass_stCadAssParaIn.swDirection = -1;
    }
    

    /** Assist gear input */
    ass_stCadAssParaIn.uwGearSt = cp_stBikeRunInfoPara.uwBikeGear;

    /** Cadence frequency percentage input */
    ass_stCadAssParaIn.uwCadancePer = cadence_stFreGetOut.uwFreqPercent;

    /** Cadence forword pulse counter input */
    ass_stCadAssParaIn.uwCadanceFwCnt = cadence_stFreGetOut.uwForwardCnt;

    /** Bike speed frequency pu input */
    ass_stCadAssParaIn.uwBikespeedPu = bikespeed_stFreGetOut.uwLPFFrequencyPu;

    /** Motor speed frequency pu input */
    ass_stCadAssParaIn.swSpdFbkPu = scm_stSpdFbkLpf.slY.sw.hi;

    /** q axis current reference low pass filter input */
    ass_stCadAssParaIn.swIqRefpu = scm_stIqRefforDesat.slY.sw.hi;

    /** q axis current feedback low pass filter input */
    ass_stCadAssParaIn.swIqFbkpu = scm_stIqFbkforDesat.slY.sw.hi;
    
    /** q axis voltage input */
    ass_stCadAssParaIn.swUqOutputpu = scm_swUqRefPu;

    /** Throttle percentage input */
    ass_stCadAssParaIn.uwThrottlePercent = bikethrottle_stBikeThrottleOut.uwThrottlePercent;
}

/**
 * @brief  Cad Assist Module Cadence Cmd Deal
 *
 * @param void
 * @return SWORD
 */
#if(INTELLIGENCADGEAR_EN!=0)
static SWORD ass_voCadCmdDeal(void)
{
    SLONG tmp_slCadCmd = 0;
    UWORD tmp_uwCadMaxpu = 0;
    
    //tmp_uwCadMaxpu = ((ULONG)ass_stCadAssCoef.uwMaxCadRpm * 3495 >> 20) << 14;// 3495>>20 = 1/(5*60)
    /*小飞最高车速对应踏频（rpm）= 车速*1000/60/（周长/100）/前飞齿数*后飞齿数           车速单位：km/h 周长单位：cm*/ 
    tmp_uwCadMaxpu = (UWORD)(((ULONG)ass_stCadAssCoef.uwMaxCadRpm * 3495) >> 6); //Q14 BASE 5HZ    
    
    if(ass_stCadAssParaIn.uwCadancePer <= 0)
    {
        tmp_slCadCmd = 0;
    }
    else if(ass_stCadAssParaIn.uwCadancePer >= tmp_uwCadMaxpu)
    {
        tmp_slCadCmd = _IQ14(0.999);
    }
    else
    {
        tmp_slCadCmd = ((ULONG)ass_stCadAssParaIn.uwCadancePer << 14) / tmp_uwCadMaxpu;
        if(tmp_slCadCmd <= _IQ14(0.0))
        {
            tmp_slCadCmd = _IQ14(0.0);
        }
        else if(tmp_slCadCmd >= _IQ14(0.999))
        {
            tmp_slCadCmd = _IQ14(0.999);
        }
        else
        {
            
        }
    }
    
    return (SWORD)tmp_slCadCmd;
}
#endif
/**
 * @brief  Cad Assist Module Deal with Command Function
 *
 * @param void
 * @return void
 */

#if(POWER_INTELLIGENCADGEAR_EN!=0)
void ass_voAssistCmdDeal(void)
{
    /** Local variable */

    SLONG tmp_slAssistSpdCmd = 0;
  #if(INTELLIGENCADGEAR_EN!=0) 
    SLONG tmp_slAssistCadSpdCmd = 0;
    SLONG tmp_slMaxMotorSpd = 0;   //Q15 BASE 1400HZ
    SLONG tmp_slAssistSectionComp = 0;  //Q14
    tmp_slMaxMotorSpd = (SLONG)ass_stCadAssCoef.swKmhToMSpdPu*(SLONG)ass_stCadAssCoef.uwAssistMaxBikeSpeed >> 4;
#endif
    /** Record Last assist mode */
    ass_stCadAssParaPro.uwAssitModeLast = ass_stCadAssParaPro.uwAssitMode;
    
    /** Cart Comand */
    if(ass_stCadAssParaIn.uwGearSt == 0x22)
    {
        ass_stCadAssParaPro.uwAssitMode = 6;
        /** Q15 * Q4 / Q4 = Q15 */
         if(MC_WorkMode == 1)
         {
           tmp_slAssistSpdCmd = (SLONG)ass_stCadAssCoef.swKmhToMSpdPu*(SLONG)ass_stCadAssCoef.uwThrottleMaxBikeSpeed >> 4;
           tmp_slAssistSpdCmd= ((SLONG)MC_MotorSPD_rpm_Percent * tmp_slAssistSpdCmd) / 100;
         }
         else
         {
        tmp_slAssistSpdCmd = (SLONG)ass_stCadAssCoef.swKmhToMSpdPu*(SLONG)ass_stCadAssCoef.uwCartMaxBikeSpeed  >> 4;    
    }
    }
    else if((ass_stCadAssParaIn.uwGearSt > 0)&&(ass_stCadAssParaIn.uwGearSt <= 5))
    {
        /** Throttle command */
        if(ass_stCadAssParaIn.uwThrottlePercent > 250)
        {
            ass_stCadAssParaPro.uwAssitMode = 7;
#if(THROTTLEGEAR !=0)
 		if(ass_stCadAssParaIn.uwGearSt == 1)
            {
                ass_MaxSpeed.MaxBikeSpeed =   ass_MaxSpeed.uwGearOne;
            }
            else if(ass_stCadAssParaIn.uwGearSt == 2)
            {
                ass_MaxSpeed.MaxBikeSpeed =   ass_MaxSpeed.uwGearTwo;
            }
            else if(ass_stCadAssParaIn.uwGearSt == 3)
           {
                ass_MaxSpeed.MaxBikeSpeed =   ass_MaxSpeed.uwGearThree;
           }
            else if(ass_stCadAssParaIn.uwGearSt == 4)
           {
                ass_MaxSpeed.MaxBikeSpeed = ass_MaxSpeed.uwGearFour;
           }
            else if((ass_stCadAssParaIn.uwGearSt == 5)  )
            {
                ass_MaxSpeed.MaxBikeSpeed =  ass_stCadAssCoef.uwThrottleMaxBikeSpeed ;
            }
 		if(ass_stCadAssParaIn.uwThrottlePercent < 650)
            {
                /** Q15 * Q4 / Q4 = Q15 */
                tmp_slAssistSpdCmd = ((SLONG)ass_stCadAssCoef.swKmhToMSpdPu*((((SLONG)(ass_stCadAssParaIn.uwThrottlePercent - 250)*(SLONG)(ass_MaxSpeed.MaxBikeSpeed - ASS_THROT_SPEED_MIN))/400)+ASS_THROT_SPEED_MIN)) >> 4;
            }
            else
            {
                tmp_slAssistSpdCmd = (SLONG)ass_stCadAssCoef.swKmhToMSpdPu*(SLONG)ass_MaxSpeed.MaxBikeSpeed>> 4;
            }
#else
            if(ass_stCadAssParaIn.uwThrottlePercent < 650)
            {
                /** Q15 * Q4 / Q4 = Q15 */
                tmp_slAssistSpdCmd = ((SLONG)ass_stCadAssCoef.swKmhToMSpdPu*((((SLONG)(ass_stCadAssParaIn.uwThrottlePercent - 250)*(SLONG)(ass_stCadAssCoef.uwThrottleMaxBikeSpeed - ASS_THROT_SPEED_MIN))/400)+ASS_THROT_SPEED_MIN)) >> 4;
            }
            else
            {
                tmp_slAssistSpdCmd = (SLONG)ass_stCadAssCoef.swKmhToMSpdPu*(SLONG)ass_stCadAssCoef.uwThrottleMaxBikeSpeed >> 4;
            }
#endif
        }
        else
        {
            /** Assist start judge */
            if(ass_stCadAssParaIn.uwCadanceFwCnt > (ass_stCadAssCoef.uwCadencePulses >> 3))
            {
                /** Assist command,gear 1 to 5 */
                if(ass_stCadAssParaIn.uwGearSt == 1)
                {
                    ass_stCadAssParaPro.uwAssitMode = ass_stCadAssParaIn.uwGearSt;
                    /** gear 1, target speed command, Q15 * Q4 / Q4 = Q15 */
               #if(INTELLIGENCADGEAR_EN!=0)   
                    tmp_slAssistCadSpdCmd = (SLONG)ass_voCadCmdDeal();
                    #if(CADASSISTGEAR  !=0)
                        tmp_slAssistSectionComp = _IQ14(1.0 );

                        if(tmp_slAssistCadSpdCmd < (tmp_slAssistSectionComp * (SLONG)_IQ14(0.48)>>14))
                           {
                               tmp_slAssistCadSpdCmd = (tmp_slAssistCadSpdCmd * _IQ14(1.8/4.8) >> 14) + (tmp_slAssistSectionComp * (SLONG)_IQ14(0.3)>>14);
                           }
                           else
                           {
                               tmp_slAssistCadSpdCmd = (tmp_slAssistSectionComp * (SLONG)_IQ14(0.48)>>14);
                           }
                    
                       tmp_slAssistSpdCmd = tmp_slAssistCadSpdCmd * tmp_slMaxMotorSpd >> 14;

                    #else
//                       if(tmp_slAssistCadSpdCmd < _IQ14(1.0))
//                          {
//                              tmp_slAssistCadSpdCmd = (tmp_slAssistCadSpdCmd * _IQ14(8.0/10.0) >> 14) + _IQ14(0.2);
//                          }
//                          else
//                          {
//                              tmp_slAssistCadSpdCmd = _IQ14(1.0);
//                          }
//
//                          tmp_slAssistSpdCmd = tmp_slAssistCadSpdCmd * tmp_slMaxMotorSpd >> 14;

                          tmp_slAssistSpdCmd = (SLONG)ass_stCadAssCoef.swKmhToMSpdPu*((SLONG)ASS_GEAR1_SPEED + 4)  >> 4;
                    #endif
              #else
                    #if(CADASSISTGEAR !=0)
                        tmp_slAssistSpdCmd = (SLONG)ass_stCadAssCoef.swKmhToMSpdPu*((SLONG)CadAssistSped.uwGearOne )  >> 4;
                     #else
                        tmp_slAssistSpdCmd = (SLONG)ass_stCadAssCoef.swKmhToMSpdPu*((SLONG)ASS_GEAR1_SPEED + 4)  >> 4;
                    #endif
                #endif
                }
                else if(ass_stCadAssParaIn.uwGearSt == 2)
                {
                    ass_stCadAssParaPro.uwAssitMode = ass_stCadAssParaIn.uwGearSt;
                    /** gear 2, target speed command, Q15 * Q4 / Q4 = Q15 */
                  #if(INTELLIGENCADGEAR_EN!=0)
                    tmp_slAssistCadSpdCmd = (SLONG)ass_voCadCmdDeal();
                    #if(CADASSISTGEAR !=0)
                    tmp_slAssistSectionComp = _IQ14(1.0 );
                    
                    if(tmp_slAssistCadSpdCmd < (tmp_slAssistSectionComp * (SLONG)_IQ14(0.6)>>14))
                    {
                        tmp_slAssistCadSpdCmd = (tmp_slAssistCadSpdCmd * _IQ14(3.0/6.0) >> 14) + (tmp_slAssistSectionComp * (SLONG)_IQ14(0.3)>>14);
                    }
                    else
                    {
                        tmp_slAssistCadSpdCmd = (tmp_slAssistSectionComp * (SLONG)_IQ14(0.6)>>14);
                    }
                    
                    tmp_slAssistSpdCmd = tmp_slAssistCadSpdCmd * tmp_slMaxMotorSpd >> 14;

                    #else

//                    if(tmp_slAssistCadSpdCmd < _IQ14(1.0))
//                       {
//                           tmp_slAssistCadSpdCmd = (tmp_slAssistCadSpdCmd * _IQ14(8.0/10.0) >> 14) + _IQ14(0.2);
//                       }
//                       else
//                       {
//                           tmp_slAssistCadSpdCmd = _IQ14(1.0);
//                       }
//
//                       tmp_slAssistSpdCmd = tmp_slAssistCadSpdCmd * tmp_slMaxMotorSpd >> 14;
                    tmp_slAssistSpdCmd = (SLONG)ass_stCadAssCoef.swKmhToMSpdPu*((((SLONG)ass_stCadAssCoef.uwAssistMaxBikeSpeed - ASS_GEAR1_SPEED)>>2) + ASS_GEAR1_SPEED)  >> 4;

                    #endif

               #else
                    #if(CADASSISTGEAR !=0)
                        tmp_slAssistSpdCmd = (SLONG)ass_stCadAssCoef.swKmhToMSpdPu*((SLONG)CadAssistSped.uwGearTwo )  >> 4;
                    #else
                        tmp_slAssistSpdCmd = (SLONG)ass_stCadAssCoef.swKmhToMSpdPu*((((SLONG)ass_stCadAssCoef.uwAssistMaxBikeSpeed - ASS_GEAR1_SPEED)>>2) + ASS_GEAR1_SPEED)  >> 4;
                   #endif
               #endif
                }
                else if(ass_stCadAssParaIn.uwGearSt == 3)
                {
                    ass_stCadAssParaPro.uwAssitMode = ass_stCadAssParaIn.uwGearSt;
                    /** gear 3, target speed command, Q15 * Q4 / Q4 = Q15 */
                   #if(INTELLIGENCADGEAR_EN!=0)
                    tmp_slAssistCadSpdCmd = (SLONG)ass_voCadCmdDeal();
                        #if(CADASSISTGEAR !=0)
                            tmp_slAssistSectionComp = _IQ14(1.0 );

                            if(tmp_slAssistCadSpdCmd < (tmp_slAssistSectionComp * (SLONG)_IQ14(0.73) >> 14))
                            {
                                tmp_slAssistCadSpdCmd = (tmp_slAssistCadSpdCmd * _IQ14(4.0/7.3) >> 14) + (tmp_slAssistSectionComp * (SLONG)_IQ14(0.33)>>14);
                            }
                            else
                            {
                                tmp_slAssistCadSpdCmd = (tmp_slAssistSectionComp * (SLONG)_IQ14(0.73)>>14);
                            }


                            tmp_slAssistSpdCmd = tmp_slAssistCadSpdCmd * tmp_slMaxMotorSpd >> 14;

                        #else
//                            if(tmp_slAssistCadSpdCmd < _IQ14(1.0))
//                               {
//                                   tmp_slAssistCadSpdCmd = (tmp_slAssistCadSpdCmd * _IQ14(8.0/10.0) >> 14) + _IQ14(0.2);
//                               }
//                               else
//                               {
//                                   tmp_slAssistCadSpdCmd = _IQ14(1.0);
//                               }
//
//                               tmp_slAssistSpdCmd = tmp_slAssistCadSpdCmd * tmp_slMaxMotorSpd >> 14;

                            tmp_slAssistSpdCmd = (SLONG)ass_stCadAssCoef.swKmhToMSpdPu*((((SLONG)ass_stCadAssCoef.uwAssistMaxBikeSpeed - ASS_GEAR1_SPEED)>>1) + ASS_GEAR1_SPEED)  >> 4;

                    #endif
               #else
                    #if(CADASSISTGEAR !=0)
                        tmp_slAssistSpdCmd = (SLONG)ass_stCadAssCoef.swKmhToMSpdPu*((SLONG)CadAssistSped.uwGearThree )  >> 4;
                    #else
                       tmp_slAssistSpdCmd = (SLONG)ass_stCadAssCoef.swKmhToMSpdPu*((((SLONG)ass_stCadAssCoef.uwAssistMaxBikeSpeed - ASS_GEAR1_SPEED)>>1) + ASS_GEAR1_SPEED)  >> 4;
                    #endif
               #endif
                }
                else if(ass_stCadAssParaIn.uwGearSt == 4)
                {
                    ass_stCadAssParaPro.uwAssitMode = ass_stCadAssParaIn.uwGearSt;
                    /** gear 4, target speed command, Q15 * Q4 / Q4 = Q15 */
                #if(INTELLIGENCADGEAR_EN!=0)
                    tmp_slAssistCadSpdCmd = (SLONG)ass_voCadCmdDeal();

                        #if(CADASSISTGEAR !=0)
                           tmp_slAssistSectionComp = _IQ14(1.0 );

                           if(tmp_slAssistCadSpdCmd < (tmp_slAssistSectionComp * (SLONG)_IQ14(0.84) >> 14))
                             {
                                 tmp_slAssistCadSpdCmd = (tmp_slAssistCadSpdCmd * _IQ14(5.10/8.4) >> 14) + (tmp_slAssistSectionComp * (SLONG)_IQ14(0.33)>>14);
                             }
                             else
                             {
                                 tmp_slAssistCadSpdCmd = (tmp_slAssistSectionComp * (SLONG)_IQ14(0.84)>>14);
                             }


                           tmp_slAssistSpdCmd = tmp_slAssistCadSpdCmd * tmp_slMaxMotorSpd >> 14;

                       #else
//                        if(tmp_slAssistCadSpdCmd < _IQ14(1.0))
//                        {
//                            tmp_slAssistCadSpdCmd = (tmp_slAssistCadSpdCmd * _IQ14(6.0/10.0) >> 14) + _IQ14(0.4);
//                        }
//                        else
//                        {
//                            tmp_slAssistCadSpdCmd = _IQ14(1.0);
//                        }
//
//                        tmp_slAssistSpdCmd = tmp_slAssistCadSpdCmd * tmp_slMaxMotorSpd >> 14;

                           tmp_slAssistSpdCmd = (SLONG)ass_stCadAssCoef.swKmhToMSpdPu*(SLONG)ass_stCadAssCoef.uwAssistMaxBikeSpeed  >> 4;

                    #endif
                #else
                //tmp_slAssistSpdCmd = (SLONG)ass_stCadAssCoef.swKmhToMSpdPu*((SLONG)ass_stCadAssCoef.uwAssistMaxBikeSpeed - (((SLONG)ass_stCadAssCoef.uwAssistMaxBikeSpeed - ASS_GEAR1_SPEED)>>2))  >> 4;
                        #if(CADASSISTGEAR !=0)
                           tmp_slAssistSpdCmd = (SLONG)ass_stCadAssCoef.swKmhToMSpdPu*((SLONG)CadAssistSped.uwGearFour )  >> 4;
                        #else
                           tmp_slAssistSpdCmd = (SLONG)ass_stCadAssCoef.swKmhToMSpdPu*((SLONG)ass_stCadAssCoef.uwAssistMaxBikeSpeed - (((SLONG)ass_stCadAssCoef.uwAssistMaxBikeSpeed - ASS_GEAR1_SPEED)>>2))  >> 4;
                         //    tmp_slAssistSpdCmd = (SLONG)ass_stCadAssCoef.swKmhToMSpdPu*(SLONG)ass_stCadAssCoef.uwAssistMaxBikeSpeed  >> 4;
                        #endif
                    #endif
                }
                else if(ass_stCadAssParaIn.uwGearSt == 5)
                {
                    ass_stCadAssParaPro.uwAssitMode = ass_stCadAssParaIn.uwGearSt;
                    /** gear 5, target speed command, Q15 * Q4 / Q4 = Q15 */
                #if(INTELLIGENCADGEAR_EN!=0)
                    tmp_slAssistCadSpdCmd = (SLONG)ass_voCadCmdDeal();  //Q14
                    #if(CADASSISTGEAR !=0)
                    if(tmp_slAssistCadSpdCmd < _IQ14(0.9))
                           {
                               tmp_slAssistCadSpdCmd = (tmp_slAssistCadSpdCmd * _IQ14(9.0/9.0) >> 14) + _IQ14(0.1);
                           }
                           else
                           {
                               tmp_slAssistCadSpdCmd = _IQ14(1.0);
                           }

                           tmp_slAssistSpdCmd = tmp_slAssistCadSpdCmd * tmp_slMaxMotorSpd >> 14;

                    #else
                        if(tmp_slAssistCadSpdCmd < _IQ14(0.9))
                        {
                            tmp_slAssistCadSpdCmd = (tmp_slAssistCadSpdCmd * _IQ14(9.0/9.0) >> 14) + _IQ14(0.1);
                        }
                        else
                        {
                            tmp_slAssistCadSpdCmd = _IQ14(1.0);
                        }

                        tmp_slAssistSpdCmd = tmp_slAssistCadSpdCmd * tmp_slMaxMotorSpd >> 14;
                        #endif
               #else
                      #if(CADASSISTGEAR !=0)
                          tmp_slAssistSpdCmd = (SLONG)ass_stCadAssCoef.swKmhToMSpdPu*((SLONG)CadAssistSped.uwGearFive )  >> 4;
                      #else
                          tmp_slAssistSpdCmd = (SLONG)ass_stCadAssCoef.swKmhToMSpdPu*(SLONG)ass_stCadAssCoef.uwAssistMaxBikeSpeed  >> 4;
                     #endif
                #endif

                }
                else
                {
                    /** gear err */
                    ass_stCadAssParaPro.uwAssitMode = 0;
                    tmp_slAssistSpdCmd = 0;     /**< Q15 */
                }
            }
            else
    {
        /** Assist stop */
      ass_stCadAssParaPro.uwAssitMode = 0;
      tmp_slAssistSpdCmd = 0;         /**< Q15 */
    }
        }
    }
    else if(ass_stCadAssParaIn.uwGearSt == 0)
    {
        /** Gear 0, Assist stop */
        ass_stCadAssParaPro.uwAssitMode = 0;
        tmp_slAssistSpdCmd = 0;                 /**< Q15 */
    }
    else
    {
        /** gear err */
        ass_stCadAssParaPro.uwAssitMode = 0;
        tmp_slAssistSpdCmd = 0;                 /**< Q15 */
    }

    /** Stop Assist when Bike Brake */
    if((BikeBrake_blGetstate() == TRUE) ||(BMS_RunInfo.Status.Status_Bit.Charge!=0))
    {
        cadence_stFreGetOut.uwForwardCnt = 0;
        ass_stCadAssParaIn.uwCadanceFwCnt = 0;
        ass_stCadAssParaPro.uwAssitMode = 0;
        tmp_slAssistSpdCmd = 0;                 /**< Q15 */
    }
    else
    {

    }

    /** Assist target speed limit and err judge */
    if(tmp_slAssistSpdCmd < 32767)
    {
        /** Judge speed sign by direction */
        ass_stCadAssParaPro.swAssistSpdCmd = (SWORD)tmp_slAssistSpdCmd*ass_stCadAssParaIn.swDirection;
    }
    else
    {
        /** parameter err */
        ass_stCadAssParaPro.swAssistSpdCmd = 0;
    }
}

#else
void ass_voAssistCmdDeal(void)
{
    /** Local variable */

    SLONG tmp_slAssistSpdCmd = 0;
  #if(INTELLIGENCADGEAR_EN!=0) 
    SLONG tmp_slAssistCadSpdCmd = 0;
    SLONG tmp_slMaxMotorSpd = 0;   //Q15 BASE 1400HZ

    tmp_slMaxMotorSpd = (SLONG)ass_stCadAssCoef.swKmhToMSpdPu*(SLONG)ass_stCadAssCoef.uwAssistMaxBikeSpeed >> 4;
  #endif
    /** Record Last assist mode */
    ass_stCadAssParaPro.uwAssitModeLast = ass_stCadAssParaPro.uwAssitMode;
    
    /** Cart Comand */
    if(ass_stCadAssParaIn.uwGearSt == 0x22)
    {
        ass_stCadAssParaPro.uwAssitMode = 6;
        /** Q15 * Q4 / Q4 = Q15 */
         if(MC_WorkMode == 1)
         {
           tmp_slAssistSpdCmd = (SLONG)ass_stCadAssCoef.swKmhToMSpdPu*(SLONG)ass_stCadAssCoef.uwThrottleMaxBikeSpeed >> 4;
           tmp_slAssistSpdCmd= ((SLONG)MC_MotorSPD_rpm_Percent * tmp_slAssistSpdCmd) / 100;

           if(MC_MotorSPD_rpm_Percent<=2)
           {
               tmp_slAssistSpdCmd=0;
               ass_stCadAssParaPro.uwAssitMode =0;
           }
         }
         else
         {
        tmp_slAssistSpdCmd = (SLONG)ass_stCadAssCoef.swKmhToMSpdPu*(SLONG)ass_stCadAssCoef.uwCartMaxBikeSpeed  >> 4;    
    }
    }
    else if((ass_stCadAssParaIn.uwGearSt > 0)&&(ass_stCadAssParaIn.uwGearSt <= 5))
    {
        /** Throttle command */
        if(ass_stCadAssParaIn.uwThrottlePercent > 250)
        {
            ass_stCadAssParaPro.uwAssitMode = 7;
        #if(THROTTLEGEAR !=0)
                if(ass_stCadAssParaIn.uwGearSt == 1)
                    {
                        ass_MaxSpeed.MaxBikeSpeed =   ass_MaxSpeed.uwGearOne;
                    }
                    else if(ass_stCadAssParaIn.uwGearSt == 2)
                    {
                        ass_MaxSpeed.MaxBikeSpeed =   ass_MaxSpeed.uwGearTwo;
                    }
                    else if(ass_stCadAssParaIn.uwGearSt == 3)
                   {
                        ass_MaxSpeed.MaxBikeSpeed =   ass_MaxSpeed.uwGearThree;
                   }
                    else if(ass_stCadAssParaIn.uwGearSt == 4)
                   {
                        ass_MaxSpeed.MaxBikeSpeed = ass_MaxSpeed.uwGearFour;
                   }
                    else if((ass_stCadAssParaIn.uwGearSt == 5)  )
                    {
                        ass_MaxSpeed.MaxBikeSpeed =  ass_stCadAssCoef.uwThrottleMaxBikeSpeed ;
                    }
                if(ass_stCadAssParaIn.uwThrottlePercent < 650)
                    {
                        /** Q15 * Q4 / Q4 = Q15 */
                        tmp_slAssistSpdCmd = ((SLONG)ass_stCadAssCoef.swKmhToMSpdPu*((((SLONG)(ass_stCadAssParaIn.uwThrottlePercent - 250)*(SLONG)(ass_MaxSpeed.MaxBikeSpeed - ASS_THROT_SPEED_MIN))/400)+ASS_THROT_SPEED_MIN)) >> 4;
                    }
                    else
                    {
                        tmp_slAssistSpdCmd = (SLONG)ass_stCadAssCoef.swKmhToMSpdPu*(SLONG)ass_MaxSpeed.MaxBikeSpeed>> 4;
                    }
        #else
            if(ass_stCadAssParaIn.uwThrottlePercent < 650) //850
            {
                /** Q15 * Q4 / Q4 = Q15 */
                tmp_slAssistSpdCmd = ((SLONG)ass_stCadAssCoef.swKmhToMSpdPu*((((SLONG)(ass_stCadAssParaIn.uwThrottlePercent - 250)*(SLONG)(ass_stCadAssCoef.uwThrottleMaxBikeSpeed - ASS_THROT_SPEED_MIN))/400)+ASS_THROT_SPEED_MIN)) >> 4;
            }
            else
            {
                tmp_slAssistSpdCmd = (SLONG)ass_stCadAssCoef.swKmhToMSpdPu*(SLONG)ass_stCadAssCoef.uwThrottleMaxBikeSpeed >> 4;
            }
         #endif
        }
        else
        {
            /** Assist start judge */
            if(ass_stCadAssParaIn.uwCadanceFwCnt > (ass_stCadAssCoef.uwCadencePulses >> 3))
            {
                #if(INTELLIGENCADGEAR_EN!=0) 
                tmp_slAssistCadSpdCmd = (SLONG)ass_voCadCmdDeal();  //Q14
#endif
                /** Assist command,gear 1 to 5 */
                if(ass_stCadAssParaIn.uwGearSt == 1)
                {
                    ass_stCadAssParaPro.uwAssitMode = ass_stCadAssParaIn.uwGearSt;
                    /** gear 1, target speed command, Q15 * Q4 / Q4 = Q15 */
                  #if(INTELLIGENCADGEAR_EN!=0)
                  //  tmp_slAssistCadSpdCmd = (SLONG)ass_voCadCmdDeal();
                        #if(CADASSISTGEAR !=0)
                           tmp_slAssistSpdCmd = (SLONG)ass_stCadAssCoef.swKmhToMSpdPu*((SLONG)CadAssistSped.uwGearOne )  >> 4;
                        #else
                           tmp_slAssistSpdCmd = (SLONG)ass_stCadAssCoef.swKmhToMSpdPu*((SLONG)ASS_GEAR1_SPEED + 4)  >> 4;
                       #endif

                   #else
                        #if(CADASSISTGEAR !=0)
                            tmp_slAssistSpdCmd = (SLONG)ass_stCadAssCoef.swKmhToMSpdPu*((SLONG)CadAssistSped.uwGearOne )  >> 4;
                         #else
                            tmp_slAssistSpdCmd = (SLONG)ass_stCadAssCoef.swKmhToMSpdPu*((SLONG)ASS_GEAR1_SPEED + 4)  >> 4;
                        #endif
                  #endif
                }
                else if(ass_stCadAssParaIn.uwGearSt == 2)
                {
                    ass_stCadAssParaPro.uwAssitMode = ass_stCadAssParaIn.uwGearSt;
                    /** gear 2, target speed command, Q15 * Q4 / Q4 = Q15 */
                  #if(INTELLIGENCADGEAR_EN!=0)   
                   // tmp_slAssistCadSpdCmd = (SLONG)ass_voCadCmdDeal();
                     #if(CADASSISTGEAR !=0)
                           tmp_slAssistSpdCmd = (SLONG)ass_stCadAssCoef.swKmhToMSpdPu*((SLONG)CadAssistSped.uwGearTwo )  >> 4;
                       #else
                           tmp_slAssistSpdCmd = (SLONG)ass_stCadAssCoef.swKmhToMSpdPu*((((SLONG)ass_stCadAssCoef.uwAssistMaxBikeSpeed - ASS_GEAR1_SPEED)>>2) + ASS_GEAR1_SPEED)  >> 4;
                      #endif
                    
                  #else
                        #if(CADASSISTGEAR !=0)
                            tmp_slAssistSpdCmd = (SLONG)ass_stCadAssCoef.swKmhToMSpdPu*((SLONG)CadAssistSped.uwGearTwo )  >> 4;
                        #else
                            tmp_slAssistSpdCmd = (SLONG)ass_stCadAssCoef.swKmhToMSpdPu*((((SLONG)ass_stCadAssCoef.uwAssistMaxBikeSpeed - ASS_GEAR1_SPEED)>>2) + ASS_GEAR1_SPEED)  >> 4;
                       #endif
                #endif
                }
                else if(ass_stCadAssParaIn.uwGearSt == 3)
                {
                    ass_stCadAssParaPro.uwAssitMode = ass_stCadAssParaIn.uwGearSt;
                    /** gear 3, target speed command, Q15 * Q4 / Q4 = Q15 */
                   #if(INTELLIGENCADGEAR_EN!=0)  
                  //  tmp_slAssistCadSpdCmd = (SLONG)ass_voCadCmdDeal();

                        #if(CADASSISTGEAR !=0)
                           tmp_slAssistSpdCmd = (SLONG)ass_stCadAssCoef.swKmhToMSpdPu*((SLONG)CadAssistSped.uwGearThree )  >> 4;
                       #else
                          tmp_slAssistSpdCmd = (SLONG)ass_stCadAssCoef.swKmhToMSpdPu*((((SLONG)ass_stCadAssCoef.uwAssistMaxBikeSpeed - ASS_GEAR1_SPEED)>>1) + ASS_GEAR1_SPEED)  >> 4;
                       #endif
                     #else
                        #if(CADASSISTGEAR !=0)
                            tmp_slAssistSpdCmd = (SLONG)ass_stCadAssCoef.swKmhToMSpdPu*((SLONG)CadAssistSped.uwGearThree )  >> 4;
                        #else
                           tmp_slAssistSpdCmd = (SLONG)ass_stCadAssCoef.swKmhToMSpdPu*((((SLONG)ass_stCadAssCoef.uwAssistMaxBikeSpeed - ASS_GEAR1_SPEED)>>1) + ASS_GEAR1_SPEED)  >> 4;
                        #endif
                   #endif
                }
                else if(ass_stCadAssParaIn.uwGearSt == 4)
                {
                    ass_stCadAssParaPro.uwAssitMode = ass_stCadAssParaIn.uwGearSt;
                    /** gear 4, target speed command, Q15 * Q4 / Q4 = Q15 */
                #if(INTELLIGENCADGEAR_EN!=0)    
                   // tmp_slAssistCadSpdCmd = (SLONG)ass_voCadCmdDeal();
                        #if(CADASSISTGEAR !=0)
                          tmp_slAssistSpdCmd = (SLONG)ass_stCadAssCoef.swKmhToMSpdPu*((SLONG)CadAssistSped.uwGearFour )  >> 4;
                       #else
                         // tmp_slAssistSpdCmd = (SLONG)ass_stCadAssCoef.swKmhToMSpdPu*((SLONG)ass_stCadAssCoef.uwAssistMaxBikeSpeed - (((SLONG)ass_stCadAssCoef.uwAssistMaxBikeSpeed - ASS_GEAR1_SPEED)>>2))  >> 4;
                          tmp_slAssistSpdCmd = (SLONG)ass_stCadAssCoef.swKmhToMSpdPu*(SLONG)ass_stCadAssCoef.uwAssistMaxBikeSpeed  >> 4;
                       #endif
               #else
                    //tmp_slAssistSpdCmd = (SLONG)ass_stCadAssCoef.swKmhToMSpdPu*((SLONG)ass_stCadAssCoef.uwAssistMaxBikeSpeed - (((SLONG)ass_stCadAssCoef.uwAssistMaxBikeSpeed - ASS_GEAR1_SPEED)>>2))  >> 4;
                        #if(CADASSISTGEAR !=0)
                           tmp_slAssistSpdCmd = (SLONG)ass_stCadAssCoef.swKmhToMSpdPu*((SLONG)CadAssistSped.uwGearFour )  >> 4;
                        #else
                           tmp_slAssistSpdCmd = (SLONG)ass_stCadAssCoef.swKmhToMSpdPu*((SLONG)ass_stCadAssCoef.uwAssistMaxBikeSpeed - (((SLONG)ass_stCadAssCoef.uwAssistMaxBikeSpeed - ASS_GEAR1_SPEED)>>2))  >> 4;
                          // tmp_slAssistSpdCmd = (SLONG)ass_stCadAssCoef.swKmhToMSpdPu*(SLONG)ass_stCadAssCoef.uwAssistMaxBikeSpeed  >> 4;
                        #endif
                #endif
                }
                else if(ass_stCadAssParaIn.uwGearSt == 5)
                {
                    ass_stCadAssParaPro.uwAssitMode = ass_stCadAssParaIn.uwGearSt;
                    /** gear 5, target speed command, Q15 * Q4 / Q4 = Q15 */
                #if(INTELLIGENCADGEAR_EN!=0)
                        if(tmp_slAssistCadSpdCmd < _IQ14(0.9))
                        {
                            tmp_slAssistCadSpdCmd = (tmp_slAssistCadSpdCmd * _IQ14(9.0/9.0) >> 14) + _IQ14(0.1);
                        }
                        else
                        {
                            tmp_slAssistCadSpdCmd = _IQ14(1.0);
                        }

                        tmp_slAssistSpdCmd = tmp_slAssistCadSpdCmd * tmp_slMaxMotorSpd >> 14;

               #else
                      #if(CADASSISTGEAR !=0)
                          tmp_slAssistSpdCmd = (SLONG)ass_stCadAssCoef.swKmhToMSpdPu*((SLONG)CadAssistSped.uwGearFive )  >> 4;
                      #else
                          tmp_slAssistSpdCmd = (SLONG)ass_stCadAssCoef.swKmhToMSpdPu*(SLONG)ass_stCadAssCoef.uwAssistMaxBikeSpeed  >> 4;
                     #endif
                #endif
               
                }
                else
                {
                    /** gear err */
                    ass_stCadAssParaPro.uwAssitMode = 0;
                    tmp_slAssistSpdCmd = 0;     /**< Q15 */
                }
            }
            else
            {
                /** Assist stop */
                ass_stCadAssParaPro.uwAssitMode = 0;
                tmp_slAssistSpdCmd = 0;         /**< Q15 */
            }
        }
    }
    else if(ass_stCadAssParaIn.uwGearSt == 0)
    {
        /** Gear 0, Assist stop */
        ass_stCadAssParaPro.uwAssitMode = 0;
        tmp_slAssistSpdCmd = 0;                 /**< Q15 */
    }
    else
    {   
        /** gear err */
        ass_stCadAssParaPro.uwAssitMode = 0;
        tmp_slAssistSpdCmd = 0;                 /**< Q15 */
    }
    
    /** Stop Assist when Bike Brake */
    if(BikeBrake_blGetstate() == TRUE)
    {
        cadence_stFreGetOut.uwForwardCnt = 0;
        ass_stCadAssParaIn.uwCadanceFwCnt = 0;
        ass_stCadAssParaPro.uwAssitMode = 0;
        tmp_slAssistSpdCmd = 0;                 /**< Q15 */
    }
    else
    {
      
    }
#if(STARTRUNATSPEED!=0)
    if(ass_stCadAssParaIn.uwGearSt != 0x22)
    {
        if((bikespeed_stFreGetOut.uwFrequencyPu<RESETTRUNSPEED) &&(ass_stCadAssParaPro.uwAssitMode==0))
        {
            SetZeroRunMode=0;
        }
        else if(cp_stBikeRunInfoPara.BikeSpeedKmH>=STARTRUNSPEED)
        {
            SetZeroRunMode=1;
        }
        if(SetZeroRunMode==0)
        {
           // bikethrottle_stBikeThrottleOut.uwThrottlePercent=0;
            cadence_stFreGetOut.uwForwardCnt = 0;
           ass_stCadAssParaIn.uwCadanceFwCnt = 0;
           ass_stCadAssParaPro.uwAssitMode = 0;
           tmp_slAssistSpdCmd = 0;                 /**< Q15 */
        }
    }
#endif
    /** Assist target speed limit and err judge */
    if(tmp_slAssistSpdCmd < 32767)
    {
        /** Judge speed sign by direction */
        ass_stCadAssParaPro.swAssistSpdCmd = (SWORD)tmp_slAssistSpdCmd*ass_stCadAssParaIn.swDirection;
    }
    else
    {
        /** parameter err */
        ass_stCadAssParaPro.swAssistSpdCmd = 0;
    }
}
#endif
/**
 * @brief  Cad Assist Module Max Assist Current Calculate
 *
 * @param void
 * @return void
 */
void ass_voAssMaxCurCal(void)
{
    /** Local variable */
    /** Calculate the Speed use for speed limit */
    SLONG tmp_slMotorSpd = (SLONG)ass_stCadAssParaIn.swSpdFbkPu * (SLONG)ass_stCadAssParaIn.swDirection;
    /** Calculate the err of current speed and limit speed */
    SLONG tmp_slSpdLimitErr = tmp_slMotorSpd - ((SLONG)ass_stCadAssCoef.swMSpdpuLimit - ((SLONG)ass_stCadAssCoef.swKmhToMSpdPu * ASS_SPD_LIMIT_ERR >> 4));
    SLONG tmp_slSpdLimitCoef = 0;
    
    /** Calculate the q axis current limit by speed limit */
    if (tmp_slSpdLimitErr <= 0)
    {
        ass_stCadAssParaPro.swCurrentMaxPu = ass_stCadAssCoef.uwCofCurMaxPu;
    }
    else if(tmp_slSpdLimitErr < ((SLONG)ass_stCadAssCoef.swKmhToMSpdPu * ASS_SPD_LIMIT_ERR >> 4))
    {
        /** Q15 * Q13 / Q15 = Q13 */
        tmp_slSpdLimitCoef = (tmp_slSpdLimitErr << 13) / ((SLONG)ass_stCadAssCoef.swKmhToMSpdPu * ASS_SPD_LIMIT_ERR >> 4);
        /** Q14 * Q13 / Q13 = Q14 */
        ass_stCadAssParaPro.swCurrentMaxPu = ((SLONG)ass_stCadAssCoef.uwCofCurMaxPu * (SLONG)(8192L - tmp_slSpdLimitCoef)>>13);
    }
    else
    {
        ass_stCadAssParaPro.swCurrentMaxPu = 0;     /**< Q14 */
    }
    
    /** Calculate the min value of speed limit,flux limit,power limit and Bms limit. Q14 */
    ass_stCadAssParaPro.swCurrentMaxPu = (ass_stCadAssParaPro.swCurrentMaxPu < ass_stCadAssParaIn.swPwrIqLimitPu) ? ass_stCadAssParaPro.swCurrentMaxPu : ass_stCadAssParaIn.swPwrIqLimitPu;
    ass_stCadAssParaPro.swCurrentMaxPu = (ass_stCadAssParaPro.swCurrentMaxPu < ass_stCadAssParaIn.swFlxIqLimitPu) ? ass_stCadAssParaPro.swCurrentMaxPu : ass_stCadAssParaIn.swFlxIqLimitPu;
    ass_stCadAssParaPro.swCurrentMaxPu = (ass_stCadAssParaPro.swCurrentMaxPu < ass_stCadAssParaIn.uwBmsIqLimitPu) ? ass_stCadAssParaPro.swCurrentMaxPu : ass_stCadAssParaIn.uwBmsIqLimitPu; 
#if(POWER_INTELLIGENCADGEAR_EN!=0)
    ass_stCadAssParaPro.swCurrentMaxPu = (ass_stCadAssParaPro.swCurrentMaxPu <  CadSpdPower.uwIqCurLimitMax) ? ass_stCadAssParaPro.swCurrentMaxPu :  CadSpdPower.uwIqCurLimitMax;
#endif
}

/**
 * @brief  Cad Assist Module Init Voltage Calculate
 *
 * @param void
 * @return void
 */
void ass_voAssInitVoltageCal(void)
{
    /** Local variable */
    SLONG tmp_slValue1 = 0;
    SLONG tmp_slValue2 = 0;
    
    /** Calculate the Aim Motor Speed by Bike Speed */
    /** Q20 * Q10 / Q15 = Q15  */
    tmp_slValue1 = (SLONG)(((SQWORD)ass_stCadAssParaIn.uwBikespeedPu * ass_stCadAssCoef.slBSpdPuToMSpdPu) >> 15); 
    /** Aim speed limit. Q15 */
    if(tmp_slValue1 > _IQ15(0.9))
    {
        tmp_slValue1 = _IQ15(0.0);
    }
    else if(tmp_slValue1 < _IQ15(0.0))
    {
        tmp_slValue1 = _IQ15(0.0);
    }
    else
    {
        //
    }
    ass_stCadAssParaPro.swBikeSpd2MotSpdPu = tmp_slValue1 * ass_stCadAssParaIn.swDirection;
    
    tmp_slValue2 = (SLONG)ass_stCadAssParaIn.swSpdFbkPu * (SLONG)ass_stCadAssParaIn.swDirection;
    if(tmp_slValue2 < -200)
    {
        tmp_slValue1 = tmp_slValue2 * (SLONG)ass_stCadAssParaIn.swDirection;
    }
    else if(tmp_slValue2 > tmp_slValue1)
    {
        tmp_slValue1 = tmp_slValue2 * (SLONG)ass_stCadAssParaIn.swDirection;
    }
    else
    {
        tmp_slValue1 = ass_stCadAssParaPro.swBikeSpd2MotSpdPu;
    }
    
    /** Calculate the Initial Value of Voltage Limit */
    /** Q15 * Q12 / Q13 = Q14 */
    tmp_slValue1 = (SLONG)tmp_slValue1 *(SLONG)ass_stCadAssCoef.uwFluxPu >> 13;
    /** 加快起动过程 */
    tmp_slValue1 = (tmp_slValue1 * 1126L)>>10;

    /** 对电压限幅初始值进行速度指令限幅处理 */
    if(ass_stCadAssParaPro.uwAssitMode != 0)
    {
        tmp_slValue2 = (SLONG)ass_stCadAssParaPro.swAssistSpdCmd *(SLONG)ass_stCadAssCoef.uwFluxPu >> 13;
        if((tmp_slValue1 * ass_stCadAssParaIn.swDirection) > (tmp_slValue2 * ass_stCadAssParaIn.swDirection))
        {
            tmp_slValue1 = tmp_slValue2;
        }
    } 

    /** Initial voltage limit */
    if(tmp_slValue1 >= ass_stCadAssParaIn.swAssMaxVolLimpu)
    {
        tmp_slValue2 = ass_stCadAssParaIn.swAssMaxVolLimpu;
    }
    else if(tmp_slValue1 <= -ass_stCadAssParaIn.swAssMaxVolLimpu)
    {
        tmp_slValue2 = -ass_stCadAssParaIn.swAssMaxVolLimpu;
    }
    else
    {
        tmp_slValue2 = (SWORD)tmp_slValue1;
    }
    
    ass_stCadAssParaPro.swUqLimitInitPu = tmp_slValue2;
}

/**
 * @brief  Cad Assist Module Assist Current Calculate
 *
 * @param void
 * @return void
 */
void ass_voAssCurCalFunc(void)
{
    SWORD tmp_swAssMeshCur = 0;
    
    /**< Assist current calculate state machine */
    switch (ass_enCadAssStatus)
    {
    /** Stop state */
    case CadAssStop:
        /** Reset the variable */
        ass_stCadAssCalOut.blAssistflag = FALSE;
        ass_stCadAssParaPro.swTargetAssCurPu = 0;
        ass_stCadAssCalOut.swAssCurrentPu = 0;
        ass_stCadAssParaPro.swVoltageStep = 0;
        ass_lpf_SpeedCmd.slY.sl = 0;
        ass_voSpdPidInit(&ass_stCadAssSpdCtl);
        
        if(ass_stCadAssParaPro.uwAssitMode != 0)
        {
            ass_enCadAssStatus = CadAssStartup;
            ass_uwCadAssVoltSatCnt = 0;
            ass_uwCadAssStartCnt = 0;
            
            /** Initial voltage limit */
            ass_stCadAssCalOut.swVoltLimitPu = ass_stCadAssParaPro.swUqLimitInitPu;
            ass_stCadAssParaPro.slVoltageSum = ((SLONG)ass_stCadAssCalOut.swVoltLimitPu << 8);
        }
        
        /** Stop to Start interval */
//        if(ass_uwCadAssStartCnt < 1500)
//        {
//            ass_uwCadAssStartCnt ++;
//            cadence_stFreGetOut.uwForwardCnt = 0;
//            ass_stCadAssParaIn.uwCadanceFwCnt = 0;
//        }
//        else
//        {
//            if(ass_stCadAssParaPro.uwAssitMode != 0)
//            {
//                ass_enCadAssStatus = CadAssStartup;
//                ass_uwCadAssVoltSatCnt = 0;
//                ass_uwCadAssStartCnt = 0;
//            }
//        }
      break;
    /** Startup state for mesh */
    case CadAssStartup:
        /** setting the assist flag */
        ass_stCadAssCalOut.blAssistflag = TRUE;
        ass_uwCadAssVoltSatCnt ++;
        
        tmp_swAssMeshCur = ASS_MESH_CUR;
        if(tmp_swAssMeshCur > ass_stCadAssParaPro.swCurrentMaxPu)
        {
            tmp_swAssMeshCur = ass_stCadAssParaPro.swCurrentMaxPu;
        }
        
        if(ass_stCadAssParaIn.swDirection == 1)
        {
            ass_stCadAssParaPro.swVoltageStep = ASS_VOL_STEP_MAX;
            ass_stCadAssParaPro.swTargetAssCurPu = tmp_swAssMeshCur;
            ass_stCadAssCalOut.swAssCurrentPu = tmp_swAssMeshCur;
        }
        else if(ass_stCadAssParaIn.swDirection == -1)
        {
            ass_stCadAssParaPro.swVoltageStep = -ASS_VOL_STEP_MAX;
            ass_stCadAssParaPro.swTargetAssCurPu = -tmp_swAssMeshCur;
            ass_stCadAssCalOut.swAssCurrentPu = -tmp_swAssMeshCur;
        }
        else
        {
            /** Direction err */
        }

        if(ass_uwCadAssVoltSatCnt >= 250)
        {
            ass_enCadAssStatus = CadAssPro;
        }
        else if((ass_stCadAssParaIn.swSpdFbkPu * ass_stCadAssParaIn.swDirection) >= (ass_stCadAssParaPro.swBikeSpd2MotSpdPu  * ass_stCadAssParaIn.swDirection))
        {
            ass_enCadAssStatus = CadAssPro;
        }
        else if((ass_stCadAssParaIn.swSpdFbkPu * ass_stCadAssParaIn.swDirection) >= (ass_stCadAssParaPro.swAssistSpdCmd  * ass_stCadAssParaIn.swDirection))
        {
            ass_enCadAssStatus = CadAssPro;
        }
        else
        {
            /** 等待 */
        }
        
        ass_stCadAssParaPro.slVoltageSum += ass_stCadAssParaPro.swVoltageStep;
        
        /** voltage limit */
        if(ass_stCadAssParaPro.slVoltageSum >= ((SLONG)ass_stCadAssParaIn.swAssMaxVolLimpu<<8))
        {
            ass_stCadAssParaPro.slVoltageSum = (SLONG)ass_stCadAssParaIn.swAssMaxVolLimpu<<8;
        }
        else if(ass_stCadAssParaPro.slVoltageSum <= -((SLONG)ass_stCadAssParaIn.swAssMaxVolLimpu<<8))
        {
            ass_stCadAssParaPro.slVoltageSum = -(SLONG)ass_stCadAssParaIn.swAssMaxVolLimpu<<8;
        }
        else
        {
            /** Do not deal */
        }
    
        ass_stCadAssCalOut.swVoltLimitPu = (ass_stCadAssParaPro.slVoltageSum >> 8);
        
        if(ass_enCadAssStatus == CadAssPro)
        {
            ass_uwCadAssVoltSatCnt = 0;
            ass_uwCadAssStartCnt = 0;
            
            ass_stCadAssCalOut.swVoltLimitPu = ass_stCadAssParaIn.swUqOutputpu;
            ass_stCadAssParaPro.slVoltageSum = ((SLONG)ass_stCadAssParaIn.swUqOutputpu<<8);
        }
        
        if(ass_stCadAssParaPro.uwAssitMode == 0)
        {
            ass_enCadAssStatus = CadAssStop;
            
            ass_stCadAssCalOut.blAssistflag = FALSE;
            ass_voSpdPidInit(&ass_stCadAssSpdCtl);
            ass_stCadAssParaPro.swTargetAssCurPu = 0;
            ass_stCadAssCalOut.swAssCurrentPu = 0;
            ass_stCadAssParaPro.swVoltageStep = 0;
            ass_lpf_SpeedCmd.slY.sl = 0;
            ass_stCadAssParaIn.uwCadanceFwCnt = 0;
            ass_uwCadAssStartCnt = 0;
            ass_uwCadAssVoltSatCnt = 0;
        }
      break;
    case CadAssPro:
        ass_stCadAssCalOut.blAssistflag = TRUE;
        
        if(ass_stCadAssParaIn.swDirection == 1)
        {
        	#if(INTELLIGENCADGEAR_EN!=0)  
            if((ass_stCadAssParaIn.swIqFbkpu + _IQ14(5.0/60.0)) < ass_stCadAssParaPro.swCurrentMaxPu)
            {
                ass_stCadAssParaPro.swTargetAssCurPu = (ass_stCadAssParaIn.swIqFbkpu + _IQ14(5.0/60.0));
            }
            else
            {
                ass_stCadAssParaPro.swTargetAssCurPu = ass_stCadAssParaPro.swCurrentMaxPu;
            }
            #else
             ass_stCadAssParaPro.swTargetAssCurPu = ass_stCadAssParaPro.swCurrentMaxPu;
            #endif
            
            if(((SLONG)ass_stCadAssCalOut.swAssCurrentPu + (SLONG)ass_stCadAssParaPro.swTargetAssCurAcc) < ass_stCadAssParaPro.swTargetAssCurPu)
            {
                ass_stCadAssCalOut.swAssCurrentPu += ass_stCadAssParaPro.swTargetAssCurAcc;
            }
            else if(((SLONG)ass_stCadAssCalOut.swAssCurrentPu - ((SLONG)ass_stCadAssParaPro.swTargetAssCurAcc<<1)) > ass_stCadAssParaPro.swTargetAssCurPu)
            {
                ass_stCadAssCalOut.swAssCurrentPu -= (ass_stCadAssParaPro.swTargetAssCurAcc<<1);
            }
            else
            {
                ass_stCadAssCalOut.swAssCurrentPu = ass_stCadAssParaPro.swTargetAssCurPu;
            }
        }
        else if(ass_stCadAssParaIn.swDirection == -1)
        {
        	#if(INTELLIGENCADGEAR_EN!=0)  
            if((ass_stCadAssParaIn.swIqFbkpu - _IQ14(5.0/60.0)) > -ass_stCadAssParaPro.swCurrentMaxPu)
            {
                ass_stCadAssParaPro.swTargetAssCurPu = (ass_stCadAssParaIn.swIqFbkpu - _IQ14(5.0/60.0));
            }
            else
            {
                ass_stCadAssParaPro.swTargetAssCurPu = -ass_stCadAssParaPro.swCurrentMaxPu;
            }
            #else
             ass_stCadAssParaPro.swTargetAssCurPu = -ass_stCadAssParaPro.swCurrentMaxPu;
            #endif

            if(((SLONG)ass_stCadAssCalOut.swAssCurrentPu + ((SLONG)ass_stCadAssParaPro.swTargetAssCurAcc<<1)) < ass_stCadAssParaPro.swTargetAssCurPu)
            {
                ass_stCadAssCalOut.swAssCurrentPu += (ass_stCadAssParaPro.swTargetAssCurAcc<<1);
            }
            else if(((SLONG)ass_stCadAssCalOut.swAssCurrentPu - (SLONG)ass_stCadAssParaPro.swTargetAssCurAcc) > ass_stCadAssParaPro.swTargetAssCurPu)
            {
                ass_stCadAssCalOut.swAssCurrentPu -= ass_stCadAssParaPro.swTargetAssCurAcc;
            }
            else
            {
                ass_stCadAssCalOut.swAssCurrentPu = ass_stCadAssParaPro.swTargetAssCurPu;
            }
        }
        else
        {
            //err
        }
        
        ass_stCadAssSpdCtl.swPidRef = ass_stCadAssParaPro.swAssistSpdCmd;
        ass_stCadAssSpdCtl.swPidFbk = ass_stCadAssParaIn.swSpdFbkPu;
        ass_stCadAssSpdCtl.swIqRef = ass_stCadAssParaIn.swIqRefpu;
        ass_stCadAssSpdCtl.swIqFbk = ass_stCadAssParaIn.swIqFbkpu;
//        ass_stCadAssSpdCtl.swPidLimMax = _IQ10(1.0);
        ass_stCadAssSpdCtl.swDirection = ass_stCadAssParaIn.swDirection;
        if(ass_stCadAssParaIn.uwThrottlePercent > 250)
          {
            ass_stCadAssSpdCtl.uwACTPidKp= 10240;//spdpid->uwThrittlePidKp;
          }
          else
          {
              ass_stCadAssSpdCtl.uwACTPidKp= ass_stCadAssSpdCtl.uwPidKp;
          }
        ass_voSpdPidCtrl(&ass_stCadAssSpdCtl);
        ass_stCadAssParaPro.swVoltageStep = ass_stCadAssSpdCtl.swPidOut;
        ass_stCadAssParaPro.slVoltageSum += ass_stCadAssParaPro.swVoltageStep;
        /** voltage limit */
        if(ass_stCadAssParaPro.slVoltageSum >= ((SLONG)ass_stCadAssParaIn.swAssMaxVolLimpu<<8))
        {
            ass_stCadAssParaPro.slVoltageSum = (SLONG)ass_stCadAssParaIn.swAssMaxVolLimpu<<8;
        }
        else if(ass_stCadAssParaPro.slVoltageSum <= -((SLONG)ass_stCadAssParaIn.swAssMaxVolLimpu<<8))
        {
            ass_stCadAssParaPro.slVoltageSum = -(SLONG)ass_stCadAssParaIn.swAssMaxVolLimpu<<8;
        }
        else
        {
            /** Do not deal */
        }
        
        ass_stCadAssCalOut.swVoltLimitPu = (ass_stCadAssParaPro.slVoltageSum >> 8);
        
        if(ass_stCadAssParaPro.uwAssitMode == 0)
        {
            ass_enCadAssStatus = CadAssPreStop;
            ass_stCadAssCalOut.swAssCurrentPu = ass_stCadAssParaIn.swIqFbkpu;
            ass_stCadAssCalOut.blPreStopFlag = TRUE;
        }
      break;
    case CadAssPreStop:
        if(ass_stCadAssParaIn.swDirection == 1)
        {
            ass_stCadAssParaPro.swTargetAssCurPu = 0;
            if(((SLONG)ass_stCadAssCalOut.swAssCurrentPu + (SLONG)ass_stCadAssParaPro.swTargetAssCurAcc) < ass_stCadAssParaPro.swTargetAssCurPu)
            {
                ass_stCadAssCalOut.swAssCurrentPu += (ass_stCadAssParaPro.swTargetAssCurAcc);
            }
            else if(((SLONG)ass_stCadAssCalOut.swAssCurrentPu - ((SLONG)ass_stCadAssParaPro.swTargetAssCurAcc<<1)) > ass_stCadAssParaPro.swTargetAssCurPu)
            {
                ass_stCadAssCalOut.swAssCurrentPu -= (ass_stCadAssParaPro.swTargetAssCurAcc<<1);
            }
            else
            {
                ass_stCadAssCalOut.swAssCurrentPu = ass_stCadAssParaPro.swTargetAssCurPu;
            }
        }
        else if(ass_stCadAssParaIn.swDirection == -1)
        {
            ass_stCadAssParaPro.swTargetAssCurPu = 0;
            if(((SLONG)ass_stCadAssCalOut.swAssCurrentPu + ((SLONG)ass_stCadAssParaPro.swTargetAssCurAcc<<1)) < ass_stCadAssParaPro.swTargetAssCurPu)
            {
                ass_stCadAssCalOut.swAssCurrentPu += (ass_stCadAssParaPro.swTargetAssCurAcc<<1);
            }
            else if(((SLONG)ass_stCadAssCalOut.swAssCurrentPu - (SLONG)ass_stCadAssParaPro.swTargetAssCurAcc) > ass_stCadAssParaPro.swTargetAssCurPu)
            {
                ass_stCadAssCalOut.swAssCurrentPu -= (ass_stCadAssParaPro.swTargetAssCurAcc);
            }
            else
            {
                ass_stCadAssCalOut.swAssCurrentPu = ass_stCadAssParaPro.swTargetAssCurPu;
            }
        }
        else
        {
            //err
        }

        ass_stCadAssCalOut.blPreStopFlag = FALSE;
        
        if(ass_stCadAssParaPro.uwAssitMode != 0)
        {
            ass_enCadAssStatus = CadAssPro;
            
            /** Initial voltage limit */
            ass_stCadAssCalOut.swVoltLimitPu = ass_stCadAssParaIn.swUqOutputpu;
            ass_stCadAssParaPro.slVoltageSum = ((SLONG)ass_stCadAssParaIn.swUqOutputpu<<8);
        }
        
        if(abs(ass_stCadAssCalOut.swAssCurrentPu) <= (ass_stCadAssParaPro.swTargetAssCurAcc<<1))
        {
            ass_enCadAssStatus = CadAssStop;
            ass_stCadAssCalOut.blAssistflag = FALSE;
            ass_voSpdPidInit(&ass_stCadAssSpdCtl);
            ass_stCadAssParaPro.swTargetAssCurPu = 0;
            ass_stCadAssCalOut.swAssCurrentPu = 0;
            ass_stCadAssParaPro.swVoltageStep = 0;
            ass_lpf_SpeedCmd.slY.sl = 0;
            ass_stCadAssParaIn.uwCadanceFwCnt = 0;
        }
      break;
    default:
    	break;
    }
//    ass_stCadAssCalOut.swAssCurrentPu = ass_stCadAssParaPro.swTargetAssCurPu; 
}

/**
 * @brief  Cad Assist Module Function
 *
 * @param void
 * @return void
 */
void ass_voAssistFunc(void)
{
    // 1ms Counter Cycle Count
    if(ass_stCadAssParaPro.uw1msCnt >= 9999)
    {
        ass_stCadAssParaPro.uw1msCnt = 0;
    }
    else
    {
        ass_stCadAssParaPro.uw1msCnt ++;
    }
    
    //get input parameter
    ass_voParameterInput();
    
    //speed limit power
#if(POWER_INTELLIGENCADGEAR_EN!=0)
    CadSpeedLimitPower();
#endif
    //Cmd deal
    ass_voAssistCmdDeal();
   
    //motor speed acc calculate
    ass_stCadAssParaPro.swSpdFbkPuRec[((ass_stCadAssParaPro.uw1msCnt)%10)] = ass_stCadAssParaIn.swSpdFbkPu;
    ass_stCadAssParaPro.swSpdFbkPuAcc = (SWORD)((SLONG)ass_stCadAssParaIn.swSpdFbkPu - (SLONG)ass_stCadAssParaPro.swSpdFbkPuRec[((ass_stCadAssParaPro.uw1msCnt + 1)%10)]);
    
    ass_voAssMaxCurCal(); 
    ass_voAssInitVoltageCal();
    
    //Calculate the Assist Current
    ass_voAssCurCalFunc();        
}

MaTCHCAD_TRUCT  CadSmart;
UWORD  TESTCadnu=0;
void CadSmartDisting(void)
{

    CadSmart.wwCdenceRpm=  ((SQWORD)cadence_stFreGetOut.uwFrequencyPu* cof_uwFbHz * 60) >> 20;// uwFrequencyPu

    CadSmart.uwBikeRpm=  ((SQWORD)bikespeed_stFreGetOut.uwLPFFrequencyPu * FBASE * 6000) >>20; //uwFrequencyPu
  if((CadSmart.blCadChangeFlag==TRUE)&&(CadSmart.wwCdenceRpm>5)) //踏频有更新
  {
      TESTCadnu++;
      CadSmart.uwCadErrNow=  CadSmart.uwBikeRpm/CadSmart.wwCdenceRpm; //

         if(CadSmart.blmatchMode==FALSE)
         {
             if(((CadSmart.uwCadErrNow-CadSmart.uwCadErrBack)<15)&&((CadSmart.uwCadErrBack-CadSmart.uwCadErrNow)<15))
             {
                 CadSmart.blmatchMode=TRUE;
                 CadSmart.uwCadErrdata[0]=CadSmart.uwCadErrNow;
                 CadSmart.uwcnt=1;
                 CadSmart.uwWhellFitCnt=0;
             }
         }
         else
         {
             CadSmart.uwCadErrdata[CadSmart.uwcnt]=CadSmart.uwCadErrNow;
             if(((CadSmart.uwCadErrdata[CadSmart.uwcnt]-CadSmart.uwCadErrdata[CadSmart.uwcnt-1])<15)&&((CadSmart.uwCadErrdata[CadSmart.uwcnt-1]-CadSmart.uwCadErrdata[CadSmart.uwcnt])<15))
             {
                 CadSmart.uwWhellFitCnt++;
             }
             if(CadSmart.uwcnt>=4)
             {
                 if(CadSmart.uwWhellFitCnt==CadSmart.uwcnt)
                 {
                     CadSmart.uwmatchNumb=(CadSmart.uwCadErrdata[1]+CadSmart.uwCadErrdata[2]+CadSmart.uwCadErrdata[3]+CadSmart.uwCadErrdata[4])>>2;
                      if(CadSmart.uwmatchNumb<110)
                      {
                          CadSmart.uwmatchNumb =  110;
                      }
                      else if(CadSmart.uwmatchNumb>510)
                      {
                          CadSmart.uwmatchNumb=510;
                      }
                      if(CadSmart.uwmatchNumb!= CadSmart.uwmatchNumbBack)
                      {

                          ass_stCadAssCoef.uwMaxCadRpm =(UWORD)(166667/(ULONG)ass_stCadAssCoef.uwWheelCircumferenceCm*(ULONG)ass_ParaSet.uwAssistLimitBikeSpdStart/CadSmart.uwmatchNumb);
                          MC_UpcInfo.stAssistInfo.uwMaxCadRpm=ass_stCadAssCoef.uwMaxCadRpm;
                         // MC_UpcInfo.stAssistInfo.uwSaveFlg = 1;
                          cp_stFlg.ParaSaveEEFlg = TRUE;
                         // cp_stFlg.ParaUpdateFlg = TRUE;
                          //cp_stFlg.ParaAInfoUpdateFlg = TRUE;
                         // cp_stFlg.ParaAssistUpdateFinishFlg = TRUE;
                          CadSmart.uwmatchNumbBack=CadSmart.uwmatchNumb;
                      }
                 }
                 CadSmart.uwcnt=0;
                 CadSmart.uwWhellFitCnt=0;
                 CadSmart.blmatchMode=FALSE;
             }
             CadSmart.uwcnt++;

         }


         CadSmart.uwCadErrBack=CadSmart.uwCadErrNow;

     CadSmart.blCadChangeFlag=FALSE;

  }

}

#if(POWER_INTELLIGENCADGEAR_EN!=0)
//速度限功率

void CadSpeedLimitPower(void)
{
    UWORD  CadGear;
    CadSpdPower.uwbikespeedCal = bikespeed_stFreGetOut.uwLPFFrequencyPu;
      /* Bike Speed Limit: use ass_CalCoef.swBikeSpeedGain(0-1) */

    CadGear=ass_stCadAssParaIn.uwGearSt;
    if(CadGear>5)
    {
       CadGear=5;
    }
    CadSpdPower.uwBikeSpdThresHold1= CadSpdPower.uwLimitdSpeed_S[CadGear];
    CadSpdPower.uwBikeSpdThresHold2= CadSpdPower.uwLimitdSpeed_E[CadGear];


  if(CadSpdPower.uwbikespeedCal <= CadSpdPower.uwBikeSpdThresHold1)
      {
          CadSpdPower.uwIqCurLimitMax = ass_ParaCong.uwCofCurMaxPu;
      }
      else if (CadSpdPower.uwbikespeedCal  < CadSpdPower.uwBikeSpdThresHold2 )
      {
          CadSpdPower.uwIqCurLimitMax = (SLONG)(ass_ParaCong.uwCofCurMaxPu-CadSpdPower.uwIqCurLimitMin) *((SLONG)CadSpdPower.uwBikeSpdThresHold2-(SLONG)CadSpdPower.uwbikespeedCal  ) /((SLONG)CadSpdPower.uwBikeSpdThresHold2 -(SLONG)CadSpdPower.uwBikeSpdThresHold1) ; // Q12
          CadSpdPower.uwIqCurLimitMax +=CadSpdPower.uwIqCurLimitMin;
      }
      else
      {
          CadSpdPower.uwIqCurLimitMax = CadSpdPower.uwIqCurLimitMin;
      }

}

void CadSpedLinitPowerInit(void)
{

    CadSpdPower.uwAssistLimitSpdStart[0]=ass_ParaSet.uwAssistLimitBikeSpdStart;
    CadSpdPower.uwAssistLimitSpdStop[0]=CadSpdPower.uwAssistLimitSpdStart[0]+5;

    CadSpdPower.uwAssistLimitSpdStart[1]=ass_ParaSet.uwAssistLimitBikeSpdStart*3/10;//
    CadSpdPower.uwAssistLimitSpdStop[1]=CadSpdPower.uwAssistLimitSpdStart[1]+7;

    CadSpdPower.uwAssistLimitSpdStart[2]=ass_ParaSet.uwAssistLimitBikeSpdStart*45/100;//
    CadSpdPower.uwAssistLimitSpdStop[2]=CadSpdPower.uwAssistLimitSpdStart[2]+7;

    CadSpdPower.uwAssistLimitSpdStart[3]=ass_ParaSet.uwAssistLimitBikeSpdStart*6/10;//
    CadSpdPower.uwAssistLimitSpdStop[3]=CadSpdPower.uwAssistLimitSpdStart[3]+7;

    CadSpdPower.uwAssistLimitSpdStart[4]=ass_ParaSet.uwAssistLimitBikeSpdStart ;//
    CadSpdPower.uwAssistLimitSpdStop[4]=CadSpdPower.uwAssistLimitSpdStart[4]+2;

    CadSpdPower.uwAssistLimitSpdStart[5]=ass_ParaSet.uwAssistLimitBikeSpdStart ;//100%
    CadSpdPower.uwAssistLimitSpdStop[5]=CadSpdPower.uwAssistLimitSpdStart[5]+2;




    CadSpdPower.uwLimitdSpeed_S[0]=((SQWORD)1000 << 20) * CadSpdPower.uwAssistLimitSpdStart[0] /
            ((SQWORD)36 * (ass_ParaCong.uwWheelPerimeter + ass_ParaCong.swDeltPerimeter) * FBASE); // Q20    3216 = Q10(3.1415926)
    CadSpdPower.uwLimitdSpeed_S[1]=((SQWORD)1000 << 20) * CadSpdPower.uwAssistLimitSpdStart[1] /
            ((SQWORD)36 * (ass_ParaCong.uwWheelPerimeter + ass_ParaCong.swDeltPerimeter) * FBASE); // Q20    3216 = Q10(3.1415926)
    CadSpdPower.uwLimitdSpeed_S[2]=((SQWORD)1000 << 20) * CadSpdPower.uwAssistLimitSpdStart[2] /
            ((SQWORD)36 * (ass_ParaCong.uwWheelPerimeter + ass_ParaCong.swDeltPerimeter) * FBASE); // Q20    3216 = Q10(3.1415926)
    CadSpdPower.uwLimitdSpeed_S[3]=((SQWORD)1000 << 20) * CadSpdPower.uwAssistLimitSpdStart[3] /
            ((SQWORD)36 * (ass_ParaCong.uwWheelPerimeter + ass_ParaCong.swDeltPerimeter) * FBASE); // Q20    3216 = Q10(3.1415926)
    CadSpdPower.uwLimitdSpeed_S[4]=((SQWORD)1000 << 20) * CadSpdPower.uwAssistLimitSpdStart[4] /
            ((SQWORD)36 * (ass_ParaCong.uwWheelPerimeter + ass_ParaCong.swDeltPerimeter) * FBASE); // Q20    3216 = Q10(3.1415926)
    CadSpdPower.uwLimitdSpeed_S[5]=((SQWORD)1000 << 20) * CadSpdPower.uwAssistLimitSpdStart[5] /
            ((SQWORD)36 * (ass_ParaCong.uwWheelPerimeter + ass_ParaCong.swDeltPerimeter) * FBASE); // Q20    3216 = Q10(3.1415926)


    CadSpdPower.uwLimitdSpeed_E[0] = ((SQWORD)1000 << 20) * CadSpdPower.uwAssistLimitSpdStop[0] /
                ((SQWORD)36 * (ass_ParaCong.uwWheelPerimeter + ass_ParaCong.swDeltPerimeter) * FBASE); // Q20    3216 = Q10(3.1415926)
    CadSpdPower.uwLimitdSpeed_E[1] = ((SQWORD)1000 << 20) * CadSpdPower.uwAssistLimitSpdStop[1] /
               ((SQWORD)36 * (ass_ParaCong.uwWheelPerimeter + ass_ParaCong.swDeltPerimeter) * FBASE); // Q20    3216 = Q10(3.1415926)
    CadSpdPower.uwLimitdSpeed_E[2] = ((SQWORD)1000 << 20) * CadSpdPower.uwAssistLimitSpdStop[2] /
               ((SQWORD)36 * (ass_ParaCong.uwWheelPerimeter + ass_ParaCong.swDeltPerimeter) * FBASE); // Q20    3216 = Q10(3.1415926)
    CadSpdPower.uwLimitdSpeed_E[3] = ((SQWORD)1000 << 20) * CadSpdPower.uwAssistLimitSpdStop[3] /
               ((SQWORD)36 * (ass_ParaCong.uwWheelPerimeter + ass_ParaCong.swDeltPerimeter) * FBASE); // Q20    3216 = Q10(3.1415926)
    CadSpdPower.uwLimitdSpeed_E[4] = ((SQWORD)1000 << 20) * CadSpdPower.uwAssistLimitSpdStop[4] /
                ((SQWORD)36 * (ass_ParaCong.uwWheelPerimeter + ass_ParaCong.swDeltPerimeter) * FBASE); // Q20    3216 = Q10(3.1415926)
    CadSpdPower.uwLimitdSpeed_E[5] = ((SQWORD)1000 << 20) * CadSpdPower.uwAssistLimitSpdStop[5] /
                ((SQWORD)36 * (ass_ParaCong.uwWheelPerimeter + ass_ParaCong.swDeltPerimeter) * FBASE); // Q20    3216 = Q10(3.1415926)
    CadSpdPower.uwBikeSpdThresHold1=CadSpdPower.uwLimitdSpeed_S[0];
    CadSpdPower.uwBikeSpdThresHold2=CadSpdPower.uwLimitdSpeed_E[0];

     CadSpdPower.uwIqCurLimitMin=  (UWORD)(((ULONG)(CAD_IS_MIN_AP) << 14)/IBASE);
}
#endif


void GearLimitCurInit(void)
{
    GearCur.swAccCurLimitMax= cp_stControlPara.swPwrLimitValWt;
}

void GearLimitCurHandle(void)
{

#if(GEAR_POWERCURLIMIT_EN!=0)

      #if(GEAR_CUR_NORMOL!=0)
        GearCur.uwAccCurLimitGear=ass_stCadAssParaIn.uwGearSt;

        if(GearCur.uwAccCurLimitGear  >5)
        {
            GearCur.uwAccCurLimitGear=5;
        }

        if(GearCur.uwAccCurLimitGear==1)
        {
            GearCur.swAccCurLimitMax=cp_stControlPara.swPwrLimitValWt/100*GEAR_CURVALUT_ONE;
        }
        else if(GearCur.uwAccCurLimitGear==2)
        {
            GearCur.swAccCurLimitMax=cp_stControlPara.swPwrLimitValWt/100*GEAR_CURVALUT_TWO
        }
        else if(GearCur.uwAccCurLimitGear==3)
        {
            GearCur.swAccCurLimitMax=cp_stControlPara.swPwrLimitValWt/100*GEAR_CURVALUT_THR;
        }
        else if(GearCur.uwAccCurLimitGear==4)
        {
            GearCur.swAccCurLimitMax=cp_stControlPara.swPwrLimitValWt/100*GEAR_CURVALUT_FOR;
        }
        else if(GearCur.uwAccCurLimitGear==5)
        {
            GearCur.swAccCurLimitMax=cp_stControlPara.swPwrLimitValWt/100*GEAR_CURVALUT_FIV;
        }
        else
        {
            GearCur.swAccCurLimitMax=cp_stControlPara.swPwrLimitValWt;
        }
    #elif(GEAR_CUR_UART5S!=0)


        if(stOBC_RunInfo.ucPWM!=GearCur.uwUart5SPWM)
        {
        GearCur.uwUart5SPWM=stOBC_RunInfo.ucPWM;
        if(GearCur.uwUart5SPWM>256)
             GearCur.uwUart5SPWM=256;
        GearCur.swAccCurLimitMax= (cp_stControlPara.swPwrLimitValWt>>8)* GearCur.uwUart5SPWM;
        if(GearCur.swAccCurLimitMax<720)
            GearCur.swAccCurLimitMax=720;
        }

    #endif


#else
    GearCur.swAccCurLimitMax= cp_stControlPara.swPwrLimitValWt



#endif



    cp_stControlPara.swPwrLimitValWtCALC= GearCur.swAccCurLimitMax;

}