/** * @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 { } #if(THROTTLEACCELER==0) spdpid->slPidP = (SLONG)spdpid->uwPidKp * spdpid->slPidErr >> 15; //Q8 * Q15 / Q15 = Q8 #else spdpid->slPidP = (SLONG)spdpid->uwACTPidKp * spdpid->slPidErr >> 15; //Q8 * Q15 / Q15 = Q8 #endif 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+ass_ParaCong.swDeltaBikeSpeedLimit)<<4) + 8; // 骑行助力限速 ass_stCadAssCoef.uwThrottleMaxBikeSpeed = ((ass_ParaCong.uwThrottleMaxSpdKmH+ass_ParaCong.swDeltaBikeSpeedLimit)<<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; #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; #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; #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; #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 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; 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) ||(BMS_RunInfo.Status.Status_Bit.Charge!=0)) { 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=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; acr_stCurIqPIOut.swURefPu=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) { //目标公里数*1000/60/(周长/100)/前飞轮齿轮*后飞轮齿数 ass_stCadAssCoef.uwMaxCadRpm =(UWORD)(166667/(ULONG)ass_stCadAssCoef.uwWheelCircumferenceCm*(ULONG)(ass_ParaSet.uwAssistLimitBikeSpdStart+ass_ParaCong.swDeltaBikeSpeedLimit)/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; } cp_stControlPara.swPwrLimitValWtCALC= GearCur.swAccCurLimitMax; pwr_voPwrLimInit(); pwr_stPwrLimCofIn.swPwrLimW = cp_stControlPara.swPwrLimitValWtCALC; // Q0, unit: 0.1w, Power limit value pwr_stPwrLimCofIn.uwPwrErrW = cp_stControlPara.swPwrLimitErrWt; // Q0, unit: 0.1w, Start power limit when "VAL - ERR" pwr_stPwrLimCofIn.swIqMaxAp = cp_stMotorPara.swIpeakMaxA; // Q0, unit: 0.01A, Max phase current (peak value) pwr_stPwrLimCofIn.uwIBaseAp = IBASE; // Q0,unit: 0.01A, Base Current pwr_stPwrLimCofIn.uwUbVt = VBASE; // Q0,unit: 0.1V, Voltage base pwr_stPwrLimCofIn.uwPwrLimPIKp = cp_stControlPara.swPwrLimitKpPu; pwr_stPwrLimCofIn.uwPwrLimPIKi = cp_stControlPara.swPwrLimitKiPu; pwr_stPwrLimCofIn.uwPwrLimSTARTCe = cp_stControlPara.swAlmPwrLimitStartTempVal; pwr_stPwrLimCofIn.uwPwrLimENDCe = cp_stControlPara.swAlmOverHeatCeVal; pwr_stPwrLimCofIn.uwPwrLimMotTempSTARTCe = cp_stControlPara.swAlmPwrLimitMotorStartTempVal; pwr_stPwrLimCofIn.uwPwrLimMotTempENDCe = cp_stControlPara.swAlmMotorOverHeatCeVal; pwr_stPwrLimCofIn.uwPwrLimStartBatCap = PWRLIM_START_BATCAP; pwr_stPwrLimCofIn.uwPwrLimEndBatCap = PWRLIM_END_BATCAP; pwr_voPwrLimCof(&pwr_stPwrLimCofIn, &pwr_stPwrLimCof); #elif(GEAR_CUR_UART5S!=0) if(stOBC_RunInfo.ucPWM!=GearCur.uwUart5SPWM) { GearCur.uwUart5SPWM=stOBC_RunInfo.ucPWM; GearCur.swAccCurLimitMax= (SWORD)(((SLONG)cp_stControlPara.swPwrLimitValWt * GearCur.uwUart5SPWM)>>8); if(GearCur.swAccCurLimitMax<720) GearCur.swAccCurLimitMax=720; cp_stControlPara.swPwrLimitValWtCALC= GearCur.swAccCurLimitMax; pwr_voPwrLimInit(); pwr_stPwrLimCofIn.swPwrLimW = cp_stControlPara.swPwrLimitValWtCALC; // Q0, unit: 0.1w, Power limit value pwr_stPwrLimCofIn.uwPwrErrW = cp_stControlPara.swPwrLimitErrWt; // Q0, unit: 0.1w, Start power limit when "VAL - ERR" pwr_stPwrLimCofIn.swIqMaxAp = cp_stMotorPara.swIpeakMaxA; // Q0, unit: 0.01A, Max phase current (peak value) pwr_stPwrLimCofIn.uwIBaseAp = IBASE; // Q0,unit: 0.01A, Base Current pwr_stPwrLimCofIn.uwUbVt = VBASE; // Q0,unit: 0.1V, Voltage base pwr_stPwrLimCofIn.uwPwrLimPIKp = cp_stControlPara.swPwrLimitKpPu; pwr_stPwrLimCofIn.uwPwrLimPIKi = cp_stControlPara.swPwrLimitKiPu; pwr_stPwrLimCofIn.uwPwrLimSTARTCe = cp_stControlPara.swAlmPwrLimitStartTempVal; pwr_stPwrLimCofIn.uwPwrLimENDCe = cp_stControlPara.swAlmOverHeatCeVal; pwr_stPwrLimCofIn.uwPwrLimMotTempSTARTCe = cp_stControlPara.swAlmPwrLimitMotorStartTempVal; pwr_stPwrLimCofIn.uwPwrLimMotTempENDCe = cp_stControlPara.swAlmMotorOverHeatCeVal; pwr_stPwrLimCofIn.uwPwrLimStartBatCap = PWRLIM_START_BATCAP; pwr_stPwrLimCofIn.uwPwrLimEndBatCap = PWRLIM_END_BATCAP; pwr_voPwrLimCof(&pwr_stPwrLimCofIn, &pwr_stPwrLimCof); } #endif #else GearCur.swAccCurLimitMax= cp_stControlPara.swPwrLimitValWt; cp_stControlPara.swPwrLimitValWtCALC= GearCur.swAccCurLimitMax; #endif }