/** * @file AssistCurve.c * @author Zhang, Kai(zhangkai71@midea.com) * @brief * @version 0.1 * @date 2021-11-15 * * @copyright Copyright (c) 2021 * */ #include "AssistCurve.h" #include "bikeinformation.h" #include "Cadence.h" #include "FSM_1st.h" #include "flash_master.h" #include "string.h" #include "typedefine.h" #include "user.h" #include "hwsetup.h" #include "UserGpio_Config.h" #include "FuncLayerAPI.h" /****************************** * * constant Parameter * ******************************/ /****************************** * * Parameter * ******************************/ ASS_FSM_STATUS Ass_FSM = StopAssit; ASS_PER_COEF ass_stCalCoef; SWORD Assist_torqueper = 0; ASS_PARA_CONFIGURE ass_ParaCong; ASS_PARA_SET ass_ParaSet; ASS_CURLIM_OUT ass_CurLimOut; ASS_CURLIM_COEF ass_CurLimCoef = ASS_LIM_DEFAULT; ASS_PER_IN ass_CalIn = TORQUE_CAL_IN_DEFAULT; ASS_PER_COEF ass_CalCoef; ASS_PER_OUT ass_CalOut; UWORD LinerAssist[5] = ASS_LINER_TORQUE_DEFAULT; SWORD MAF_buffer[60]; MAF_IN maf_torque = {0, 30, 0, 0, MAF_buffer, 0, FALSE}; TOR2CURRENT_CAL_COEF ass_Tor2CurCalCoef; ASS_LIMIT_ACCORDING_VOL_OUT ass_CurLimitCalBMSOut; ASS_LIMIT_ACCORDING_VOL_COF ass_CurLimCalBMSCoef; ASS_RESERVE_PARA ass_stReservePara; #if(BIKE_TORQUESPDLIMIT_EN!=0) ASS_SpedLinit_COEF Gear_Sped_Cof; #endif /****************************** * * Function * ******************************/ /** * @brief Three order polynomial Y = a*X^3 + b*X^2 + c*x +d * * @param coef polynomial coefficient a, b, c, d * @param Value polynomial input value X * @param Qnum polynomial input Q type * @return UWORD polynomial output Y */ static SLONG Polynomial(POLY_COEF *coef, SWORD *value, UWORD Qnum) { SLONG out; SLONG temp_a, temp_b, temp_c; /* out = a * x ^ 3 + b * x ^ 2 + c * x + d */ temp_a = (((((SQWORD)coef->a * *value >> 12) * *value) >> Qnum) * *value) >> Qnum; // Qx+Q12-Q12+Qx-Qx+Qx-Qx=Qx temp_b = (((SQWORD)coef->b * *value >> 12) * *value) >> Qnum; // Qx+Q12-Q12+Qx-Qx=Qx temp_c = (SQWORD)coef->c * *value >> 12; // Qx+Q12-Q12=Qx out = temp_a + temp_b + temp_c + coef->d; out = (SLONG)out; return out; } /** * @brief Y = z*(x-h)^2 + k to Y = a*X^3 + b*X^2 + c*x +d * * @param coef original point coefficient z, h, k * @return POLY_COEF a, b, c, d */ //static POLY_COEF Polynomial_center(ORIG_COEF *coef) //{ // POLY_COEF out; // /* a = 0; b = z; c = -2ha; d= ah^2 +k*/ // out.a = (SQWORD)0; // Q12 // out.b = (SQWORD)coef->z; // Q12 // out.c = -(((SQWORD)2 * coef->h * coef->z) >> 12); // Q12 // out.d = (((((SQWORD)coef->z * coef->h) >> 12) * coef->h) >> 12) + (SLONG)coef->k; // Q12 // return out; //} /** * @brief Torque to Current when Id = 0; * Te = 1.5p*iq*fai -> iq = te/(1.5*p*fai) * @param coef polynomial coefficient a, b, c, d * @param Value polynomial input value X * @param Qnum polynomial input Q type * @return UWORD polynomial output Y */ static SWORD swCurrentCal(SWORD Tor) { SWORD Current; SWORD MotorTorqueNotPu; MotorTorqueNotPu = (SQWORD)Tor * TORQUEBASE * ass_Tor2CurCalCoef.uwInvMotorBikeRatio >> 22 ; // Q14+Q15-Q22 = Q7 0.1Nm Not Pu Current = ((SLONG)MotorTorqueNotPu * ass_Tor2CurCalCoef.swCalCoefINV) * 10 / IBASE; // Q7+Q7 = Q14; 0.1Nm/0.01A return Current; } /** * @brief Para from EE Init * * @param void * @return void */ void AssitEEInit(void) { ass_ParaCong.uwWheelPerimeter = ASS_BIKE_WHEEL_DIAMETER; // Q0 0.1CM ass_ParaCong.swDeltPerimeter = BIKE_WHEEL_SIZE_ADJUST; ass_ParaCong.uwMechRationMotor = (UWORD)(((ULONG)ASS_BIKE_MECH_RATION*1024)/1000); // Q10 ass_ParaCong.uwThrottleMaxSpdKmH = (ASS_THROTTLE_SPD_LIMIT>>4); //kM/H ass_ParaCong.uwCartSpdKmH = (ASS_CART_SPEED>>4); //kM/H ass_ParaCong.uwNmFrontChainring = BIKE_FRONTTEETH_NMB; // front gear ass_ParaCong.uwNmBackChainring = BIKE_BACKTEETH_NMB; // min number of back gear ass_ParaCong.uwLightConfig = BIKE_LIGHT_CONFIG; ass_ParaCong.uwAutoPowerOffTime = BIKE_AUTO_POWER_OFF_TIME; ass_ParaCong.uwStartMode = BIKE_START_MODE; ass_ParaCong.uwAssistSelect1 = ASSISTMOD_SELECT_MODE1; ass_ParaCong.uwAssistSelect2 = ASSISTMOD_SELECT_MODE2; ass_ParaCong.swDeltaBikeSpeedLimit = BIKE_DELTASPEEDLIMITION; ass_ParaSet.uwStartupCoef = 8194; // Q12 percentage Min 1-4096 1.5-6144 ass_ParaSet.uwStartupCruiseCoef = 4096; // Q12 percentage Min 1-4096 1-6144 ass_ParaSet.uwAssistStartNm = TORQUE_START_THRESHOLD; ass_ParaSet.uwAssistStopNm = TORQUE_STOP_THRESHOLD; ass_ParaSet.uwStartUpGainStep = 4; ass_ParaSet.uwStartUpCadNm = TORQUE_NUMBERS_PULSES >> 1; // 0.5 circle ass_ParaSet.uwTorLPFCadNm = TORQUE_NUMBERS_PULSES >> 1; // 0.5 circle ass_ParaSet.uwSpeedAssistSpdRpm = VolLimCtrKp; ass_ParaSet.uwSpeedAssistIMaxA = VolLimCtrKi; ass_ParaSet.uwAssistLimitBikeSpdStart = BIKE_SPEED_IQLIMIT_THRESHOLD1; ass_ParaSet.uwAssistLimitBikeSpdStop = BIKE_SPEED_IQLIMIT_THRESHOLD2; #if(BIKE_TORQUESPDLIMIT_EN!=0) Gear_Sped_Cof.uwAssistLimitSpdStart[0]=BIKE_SPEED_IQLIMIT_Gear5_S; Gear_Sped_Cof.uwAssistLimitSpdStop[0]=BIKE_SPEED_IQLIMIT_Gear5_E; Gear_Sped_Cof.uwAssistLimitSpdStart[1]=BIKE_SPEED_IQLIMIT_Gear1_S; Gear_Sped_Cof.uwAssistLimitSpdStop[1]=BIKE_SPEED_IQLIMIT_Gear1_E; Gear_Sped_Cof.uwAssistLimitSpdStart[2]=BIKE_SPEED_IQLIMIT_Gear2_S; Gear_Sped_Cof.uwAssistLimitSpdStop[2]=BIKE_SPEED_IQLIMIT_Gear2_E; Gear_Sped_Cof.uwAssistLimitSpdStart[3]=BIKE_SPEED_IQLIMIT_Gear3_S; Gear_Sped_Cof.uwAssistLimitSpdStop[3]=BIKE_SPEED_IQLIMIT_Gear3_E; Gear_Sped_Cof.uwAssistLimitSpdStart[4]=BIKE_SPEED_IQLIMIT_Gear4_S; Gear_Sped_Cof.uwAssistLimitSpdStop[4]=BIKE_SPEED_IQLIMIT_Gear4_E; Gear_Sped_Cof.uwAssistLimitSpdStart[5]=BIKE_SPEED_IQLIMIT_Gear5_S; Gear_Sped_Cof.uwAssistLimitSpdStop[5]=BIKE_SPEED_IQLIMIT_Gear5_E; #endif ass_ParaSet.uwCadenceWeight = 1229; // Q12 percentage ass_ParaSet.uwTorWeight = Q12_1 ; // Q12 percentage ass_ParaSet.uwTorAssAjstGain = 4096;// 4096 // Q12 percentage ass_ParaSet.uwCadenceAssAjstGain = 0; // Q12 percentage ass_ParaSet.uwAsssistSelectNum = 1; ass_ParaSet.swUqLimitKi = 0; } /** * @brief Three order polynomial Y = a*X^3 + b*X^2 + c*x +d * * @param coef polynomial coefficient a, b, c, d * @param Value polynomial input value X * @param Qnum polynomial input Q type * @return UWORD polynomial output Y */ LPF_OUT ass_pvt_stCurLpf; void AssitCoefInit(void) { /*状态机初始化*/ Ass_FSM = StopAssit; /*电机限制初始化*/ ass_ParaCong.uwCofCurMaxPu = (UWORD)(((ULONG)(cp_stMotorPara.swIpeakMaxA) << 14)/IBASE); //q14 ass_ParaCong.uwMotorPoles = cp_stMotorPara.swMotrPolePairs; ass_ParaCong.uwCofTorMaxPu = (((ULONG)cp_stMotorPara.swTorMax << 14) / TORQUEBASE); // Q14 ass_ParaCong.uwBikeAssTorMaxPu = ass_ParaCong.uwCofTorMaxPu * ass_ParaCong.uwMechRationMotor>>10; // Q14+Q10-Q10; /*电流限幅计算*/ ass_CurLimCalBMSCoef.uwIqLimitInitAbs = ass_ParaCong.uwCofCurMaxPu; // Q14 ass_CurLimCalBMSCoef.uwIqLimitStartSoc = 30; ass_CurLimCalBMSCoef.uwIqLimitEndSoc = 5; ass_CurLimCalBMSCoef.swIqLImitK = ((ass_CurLimCalBMSCoef.uwIqLimitInitAbs * 3) >> 2) / ((SWORD)ass_CurLimCalBMSCoef.uwIqLimitStartSoc - ass_CurLimCalBMSCoef.uwIqLimitEndSoc); ass_CurLimCalBMSCoef.uwIqLimitStartIdc = IDCLIMIQSTART; ass_CurLimCalBMSCoef.uwIqLimitEndIdc = IDCLIMIQEND; ass_CurLimCalBMSCoef.swIqLImitIdcK = ass_CurLimCalBMSCoef.uwIqLimitInitAbs / ((SWORD)ass_CurLimCalBMSCoef.uwIqLimitEndIdc - ass_CurLimCalBMSCoef.uwIqLimitStartIdc); /*助力曲线初始化*/ /*根据仪表选型配置整体曲线补偿系数*/ AssistCurveRatio(); /*助力曲线系数计算*/ AssistCurveCoef(&ass_CalCoef.swAssCompCoef); /*助力启动阈值初始化*/ ass_CalCoef.uwAssThreshold = ((ULONG)ass_ParaSet.uwAssistStartNm << 14) / TORQUEBASE; // Q14 ass_CalCoef.uwAssStopThreshold = ((ULONG)ass_ParaSet.uwAssistStopNm << 14) / TORQUEBASE; // Q14; /*助力系数初始化*/ ass_CalCoef.swSmoothGain = 0; // Q12 ass_CalCoef.swSmoothStopGain=4096; // Q12 ass_CalCoef.uwStartUpGainAddStep = ass_ParaSet.uwStartUpGainStep; // 25 Q12 // if (ass_CalCoef.uwStartUpGainAddStep < 1) // { // ass_CalCoef.uwStartUpGainAddStep = 1; // } // if (ass_CalCoef.uwStartUpGainAddStep > 50) // { // ass_CalCoef.uwStartUpGainAddStep = 50; // } /*设置启动到正常助力最少踏频数*/ // ass_CalCoef.uwStartUpTimeCadenceCnt = ass_ParaSet.uwStartUpCadNm; // if (ass_CalCoef.uwStartUpTimeCadenceCnt < (CADENCE_NUMBERS_PULSES >> 3)) // { // ass_CalCoef.uwStartUpTimeCadenceCnt = (CADENCE_NUMBERS_PULSES >> 3); // } // if (ass_CalCoef.uwStartUpTimeCadenceCnt > CADENCE_NUMBERS_PULSES) // { // ass_CalCoef.uwStartUpTimeCadenceCnt = CADENCE_NUMBERS_PULSES; // } /*设置滑动平均滤波踏频数*/ maf_torque.length = ass_ParaSet.uwTorLPFCadNm; ass_CalCoef.swBikeSpeedGain = 0; /*设置电流限幅*/ ass_CalCoef.uwCurrentMaxPu = ass_ParaCong.uwCofCurMaxPu; ass_CalCoef.swCurrentmax_torAssPu =((SLONG)ass_CalCoef.uwCurrentMaxPu * ass_ParaSet.uwTorWeight) >> 12; // Q14 ass_CalCoef.swCurrentmax_cadAssPu = ((SLONG)ass_CalCoef.uwCurrentMaxPu * ass_ParaSet.uwCadenceWeight )>> 12; // Q14 /*初始化标志*/ ass_CalOut.blAssistflag = FALSE; ass_CalOut.swTorAssistSum1 = 0; ass_CalOut.swTorAss2CurrentTemp = 0; ass_CalOut.swCadAss2CurrentTemp = 0; ass_CalOut.swTorAssistCurrentTemp = 0; ass_CalOut.swTorAssistCurrent = 0; ass_CurLimCoef.uwLimitGain[0] = 0; // Q10 percentage of max Current #if(BIKE_TORGEARMAX!=0) ass_CurLimCoef.uwLimitGain[1] = 358; //35% ass_CurLimCoef.uwLimitGain[2] = 512; //50% ass_CurLimCoef.uwLimitGain[3] = 716;//70% ass_CurLimCoef.uwLimitGain[4] = 870;//85% ass_CurLimCoef.uwLimitGain[5] = 1024;//100% #else ass_CurLimCoef.uwLimitGain[1] = 225;//22% ass_CurLimCoef.uwLimitGain[2] = 450;//44% ass_CurLimCoef.uwLimitGain[3] = 796;//78% ass_CurLimCoef.uwLimitGain[4] = 1024;//100% ass_CurLimCoef.uwLimitGain[5] = 1024;//100% #endif #if(BIKE_TORQUESPDLIMIT_EN!=0) /*设置档位限制车速速度*/ Gear_Sped_Cof.uwAssistLimitSpdStart[0]=ass_ParaSet.uwAssistLimitBikeSpdStart+ ass_ParaCong.swDeltPerimeter; Gear_Sped_Cof.uwAssistLimitSpdStop[0]=Gear_Sped_Cof.uwAssistLimitSpdStart[0]+2; Gear_Sped_Cof.uwAssistLimitSpdStart[1]=(ass_ParaSet.uwAssistLimitBikeSpdStart+ass_ParaCong.swDeltPerimeter)*12/25;//48% Gear_Sped_Cof.uwAssistLimitSpdStop[1]=Gear_Sped_Cof.uwAssistLimitSpdStart[1]+2; Gear_Sped_Cof.uwAssistLimitSpdStart[2]=(ass_ParaSet.uwAssistLimitBikeSpdStart+ ass_ParaCong.swDeltPerimeter)*15/25;//60% Gear_Sped_Cof.uwAssistLimitSpdStop[2]=Gear_Sped_Cof.uwAssistLimitSpdStart[2]+2; Gear_Sped_Cof.uwAssistLimitSpdStart[3]=(ass_ParaSet.uwAssistLimitBikeSpdStart+ ass_ParaCong.swDeltPerimeter)*18/25;//72% Gear_Sped_Cof.uwAssistLimitSpdStop[3]=Gear_Sped_Cof.uwAssistLimitSpdStart[3]+2; Gear_Sped_Cof.uwAssistLimitSpdStart[4]=(ass_ParaSet.uwAssistLimitBikeSpdStart+ ass_ParaCong.swDeltPerimeter)*21/25;//84% Gear_Sped_Cof.uwAssistLimitSpdStop[4]=Gear_Sped_Cof.uwAssistLimitSpdStart[4]+2; Gear_Sped_Cof.uwAssistLimitSpdStart[5]=ass_ParaSet.uwAssistLimitBikeSpdStart + ass_ParaCong.swDeltPerimeter;//100% Gear_Sped_Cof.uwAssistLimitSpdStop[5]=Gear_Sped_Cof.uwAssistLimitSpdStart[5]+2; Gear_Sped_Cof.uwLimitdSpeed_S[0] = ((SQWORD)1000 << 20) * Gear_Sped_Cof.uwAssistLimitSpdStart[5] / ((SQWORD)36 * (ass_ParaCong.uwWheelPerimeter + ass_ParaCong.swDeltPerimeter) * FBASE); // Q20 3216 = Q10(3.1415926) Gear_Sped_Cof.uwLimitdSpeed_E[0] = ((SQWORD)1000 << 20) * Gear_Sped_Cof.uwAssistLimitSpdStop[5] / ((SQWORD)36 * (ass_ParaCong.uwWheelPerimeter + ass_ParaCong.swDeltPerimeter) * FBASE); // Q20 3216 = Q10(3.1415926) Gear_Sped_Cof.ulBikeSpdGearDeltInv[0] = (1 << 20) / (Gear_Sped_Cof.uwLimitdSpeed_E[0] - Gear_Sped_Cof.uwLimitdSpeed_S[0]); Gear_Sped_Cof.uwLimitdSpeed_S[1] = ((SQWORD)1000 << 20) * Gear_Sped_Cof.uwAssistLimitSpdStart[1] / ((SQWORD)36 * (ass_ParaCong.uwWheelPerimeter + ass_ParaCong.swDeltPerimeter) * FBASE); // Q20 3216 = Q10(3.1415926) Gear_Sped_Cof.uwLimitdSpeed_E[1] = ((SQWORD)1000 << 20) * Gear_Sped_Cof.uwAssistLimitSpdStop[1] / ((SQWORD)36 * (ass_ParaCong.uwWheelPerimeter + ass_ParaCong.swDeltPerimeter) * FBASE); // Q20 3216 = Q10(3.1415926) Gear_Sped_Cof.ulBikeSpdGearDeltInv[1] = (1 << 20) / (Gear_Sped_Cof.uwLimitdSpeed_E[1] - Gear_Sped_Cof.uwLimitdSpeed_S[1]); Gear_Sped_Cof.uwLimitdSpeed_S[2] = ((SQWORD)1000 << 20) * Gear_Sped_Cof.uwAssistLimitSpdStart[2] / ((SQWORD)36 * (ass_ParaCong.uwWheelPerimeter + ass_ParaCong.swDeltPerimeter) * FBASE); // Q20 3216 = Q10(3.1415926) Gear_Sped_Cof.uwLimitdSpeed_E[2] = ((SQWORD)1000 << 20) * Gear_Sped_Cof.uwAssistLimitSpdStop[2] / ((SQWORD)36 * (ass_ParaCong.uwWheelPerimeter + ass_ParaCong.swDeltPerimeter) * FBASE); // Q20 3216 = Q10(3.1415926) Gear_Sped_Cof.ulBikeSpdGearDeltInv[2] = (1 << 20) / (Gear_Sped_Cof.uwLimitdSpeed_E[2] - Gear_Sped_Cof.uwLimitdSpeed_S[2]); Gear_Sped_Cof.uwLimitdSpeed_S[3] = ((SQWORD)1000 << 20) * Gear_Sped_Cof.uwAssistLimitSpdStart[3] / ((SQWORD)36 * (ass_ParaCong.uwWheelPerimeter + ass_ParaCong.swDeltPerimeter) * FBASE); // Q20 3216 = Q10(3.1415926) Gear_Sped_Cof.uwLimitdSpeed_E[3] = ((SQWORD)1000 << 20) * Gear_Sped_Cof.uwAssistLimitSpdStop[3] / ((SQWORD)36 * (ass_ParaCong.uwWheelPerimeter + ass_ParaCong.swDeltPerimeter) * FBASE); // Q20 3216 = Q10(3.1415926) Gear_Sped_Cof.ulBikeSpdGearDeltInv[3] = (1 << 20) / (Gear_Sped_Cof.uwLimitdSpeed_E[3] - Gear_Sped_Cof.uwLimitdSpeed_S[3]); Gear_Sped_Cof.uwLimitdSpeed_S[4] = ((SQWORD)1000 << 20) * Gear_Sped_Cof.uwAssistLimitSpdStart[4] / ((SQWORD)36 * (ass_ParaCong.uwWheelPerimeter + ass_ParaCong.swDeltPerimeter) * FBASE); // Q20 3216 = Q10(3.1415926) Gear_Sped_Cof.uwLimitdSpeed_E[4] = ((SQWORD)1000 << 20) * Gear_Sped_Cof.uwAssistLimitSpdStop[4] / ((SQWORD)36 * (ass_ParaCong.uwWheelPerimeter + ass_ParaCong.swDeltPerimeter) * FBASE); // Q20 3216 = Q10(3.1415926) Gear_Sped_Cof.ulBikeSpdGearDeltInv[4] = (1 << 20) / (Gear_Sped_Cof.uwLimitdSpeed_E[4] - Gear_Sped_Cof.uwLimitdSpeed_S[4]); Gear_Sped_Cof.uwLimitdSpeed_S[5] = ((SQWORD)1000 << 20) * Gear_Sped_Cof.uwAssistLimitSpdStart[5] / ((SQWORD)36 * (ass_ParaCong.uwWheelPerimeter + ass_ParaCong.swDeltPerimeter) * FBASE); // Q20 3216 = Q10(3.1415926) Gear_Sped_Cof.uwLimitdSpeed_E[5] = ((SQWORD)1000 << 20) * Gear_Sped_Cof.uwAssistLimitSpdStop[5] / ((SQWORD)36 * (ass_ParaCong.uwWheelPerimeter + ass_ParaCong.swDeltPerimeter) * FBASE); // Q20 3216 = Q10(3.1415926) Gear_Sped_Cof.ulBikeSpdGearDeltInv[5] = (1 << 20) / (Gear_Sped_Cof.uwLimitdSpeed_E[5] - Gear_Sped_Cof.uwLimitdSpeed_S[5]); #endif /*设置车速限幅*/ // ass_CurLimCoef.uwBikeSpdThresHold1 = ((SQWORD)10000 << 30) * ass_ParaSet.uwAssistLimitBikeSpdStart / // ((SQWORD)36 * 3216 * ass_ParaCong.uwWheelPerimeter * FBASE); // Q20 3216 = Q10(3.1415926) // ass_CurLimCoef.uwBikeSpdThresHold2 = // ((SQWORD)10000 << 30) * ass_ParaSet.uwAssistLimitBikeSpdStop / ((SQWORD)36 * 3216 * ass_ParaCong.uwWheelPerimeter * FBASE); ass_CurLimCoef.uwBikeSpdThresHold1 = ((SQWORD)1000 << 20) * (ass_ParaSet.uwAssistLimitBikeSpdStart + ass_ParaCong.swDeltaBikeSpeedLimit) / ((SQWORD)360 * (ass_ParaCong.uwWheelPerimeter + ass_ParaCong.swDeltPerimeter) * FBASE); // Q20 3216 = Q10(3.1415926) ass_CurLimCoef.uwBikeSpdThresHold2 = ((SQWORD)1000 << 20) * (ass_ParaSet.uwAssistLimitBikeSpdStop + ass_ParaCong.swDeltaBikeSpeedLimit) / ((SQWORD)360 * (ass_ParaCong.uwWheelPerimeter + ass_ParaCong.swDeltPerimeter) * FBASE); // Q20 3216 = Q10(3.1415926) ass_CurLimCoef.ulBikeSpdDeltInv = (1 << 20) / (ass_CurLimCoef.uwBikeSpdThresHold2 - ass_CurLimCoef.uwBikeSpdThresHold1); /*设置转矩电流标定系数*/ ass_Tor2CurCalCoef.uwMotorFluxWb = cp_stMotorPara.swFluxWb; // 0.001mWb ass_Tor2CurCalCoef.uwMotprPolePairs = ass_ParaCong.uwMotorPoles; ass_Tor2CurCalCoef.uwInvMotorBikeRatio = (UWORD)(32768L*1024L/ass_ParaCong.uwMechRationMotor); ass_Tor2CurCalCoef.swCalCoefINV = (((SLONG)1 << 7) * 1000 * 1000) / (((SLONG)3 * ass_Tor2CurCalCoef.uwMotorFluxWb * ass_Tor2CurCalCoef.uwMotprPolePairs) >> 1); // Q7 Not Pu // 1/(1.5p*fai); mth_voLPFilterCoef(1000000 / 25, EVENT_1MS_HZ, &ass_pvt_stCurLpf.uwKx); //100Hz ass_pvt_stCurLpf.slY.sl = 0; } #if(BIKE_TORQUESPDLIMIT_EN!=0) void Gear_LimitSpd(UWORD Gear) { if(Gear>5) Gear=5; ass_CurLimCoef.uwBikeSpdThresHold1= Gear_Sped_Cof.uwLimitdSpeed_S[Gear]; ass_CurLimCoef.uwBikeSpdThresHold2=Gear_Sped_Cof.uwLimitdSpeed_E[Gear]; ass_CurLimCoef.ulBikeSpdDeltInv=Gear_Sped_Cof.ulBikeSpdGearDeltInv[Gear]; } #endif /*! * @brief ebike assist function implement * * @param void No input parameter * * @return None */ SLONG Te_Tor_Assit_tempPu; /* assist torque curve calculate value */ SLONG Te_Tor_Assit_LinerPu; /* assist torque line curve calculate value */ SLONG Te_Cad_Assit_tempPu; /* cadence curve calculate value */ SWORD Te_Tor_AssitPu1; /* assist torque command*/ SWORD Te_Cad_AssitPu1; /* assist cadence command */ SWORD TorqCmd1=0; /* sampled torque after filtering */ SWORD TorqCmd=0; /* torque command */ SWORD CadCmd; /* cadence command */ UWORD StartUpGainAddStep; SLONG tst_Te_Tor_Assit_tempPu; UWORD test_startflag; UWORD test_stopflag; UWORD test_startflag; UWORD test_AssState; void AssitCuvApplPerVolt(void) { /* 局部变量定义 */ static UWORD CadGpioSta=0; static UWORD exit_pulse_cnt = 200; UWORD uwTmpTorqLpf; SLONG tmp_slBikeSpd2MotSpd = 0; /* bike speed correspond to motor's speed */ // SLONG tmp_Te_Tor_Assit_tempPu = 0; SLONG tmp_slUqLimInit = 0; SLONG tmp_slCurrErr = 0; SLONG tmp_slVoltLimitPu = 0; // SWORD tmp_swUqLimStep = 0; /* 启动标志判断 */ if(ass_CalIn.uwtorquePer > ass_CalCoef.uwAssThreshold) { if(CadGpioSta != Get_CAD_FRE_PORT()) { CadGpioSta = Get_CAD_FRE_PORT(); if(ass_CalIn.uwStartRunPulse < 1000) { ass_CalIn.uwStartRunPulse++; } else { ass_CalIn.uwStartRunPulse = 1000; } } } else if (ass_CalIn.uwtorquePer < ass_CalCoef.uwAssStopThreshold) { if(ass_CalIn.uwStartRunPulse > 0) { ass_CalIn.uwStartRunPulse--; } } else { //滞环，不动作 } //脚踏不动强制关闭 if((ass_CalIn.uwcadLowStopCnt>=200) || (ass_CalIn.uwcadHighStopCnt>=200)) { ass_CalIn.uwStartRunPulse = 0; } if((ass_CalIn.StartAssistEnble == FALSE) && (ass_CalIn.uwStartRunPulse > 2)) { ass_CalIn.StartAssistEnble = TRUE; } else if((ass_CalIn.StartAssistEnble == TRUE) && (ass_CalIn.uwStartRunPulse == 0)) { ass_CalIn.StartAssistEnble = FALSE; } else { //滞环，不动作 } test_startflag = ass_CalIn.StartAssistEnble; /* 停止脚踏标志判断 */ if(cp_stBikeRunInfoPara.BikeSpeedKmH < 30) { exit_pulse_cnt = 250; } else { exit_pulse_cnt = 200; } if (ass_CalIn.uwtorquePer < ass_CalCoef.uwAssStopThreshold) { exit_pulse_cnt = 200; } if( Get_CAD_FRE_PORT() == 0) { if(ass_CalIn.uwcadLowStopCnt<=4000) { ass_CalIn.uwcadLowStopCnt++; } if((ass_CalIn.uwcadLowStopCnt >= exit_pulse_cnt) || (ass_CalIn.uwcadHighStopCnt >= exit_pulse_cnt)) { ass_CalIn.blStopAssistEnble=TRUE; } if((ass_CalIn.uwcadHighStopCnt != 0) && (ass_CalIn.uwcadHighStopCnt < exit_pulse_cnt)) { ass_CalIn.blStopAssistEnble=FALSE; } ass_CalIn.uwcadHighStopCnt=0; } else { if(ass_CalIn.uwcadHighStopCnt<4000) { ass_CalIn.uwcadHighStopCnt++; } if((ass_CalIn.uwcadLowStopCnt >= exit_pulse_cnt) || (ass_CalIn.uwcadHighStopCnt >= exit_pulse_cnt)) { ass_CalIn.blStopAssistEnble=TRUE; } if((ass_CalIn.uwcadLowStopCnt != 0) && (ass_CalIn.uwcadLowStopCnt < exit_pulse_cnt)) { ass_CalIn.blStopAssistEnble=FALSE; } ass_CalIn.uwcadLowStopCnt=0; } test_stopflag = ass_CalIn.blStopAssistEnble; /* 脚踏力矩输入处理 - 滑动平均与低通滤波切换 */ TorqCmd1 = ((ULONG)ass_CalIn.uwtorque * ass_CalCoef.swTorqFilterGain >> 14) + ((ULONG)ass_CalIn.uwtorquelpf * (Q14_1 - ass_CalCoef.swTorqFilterGain) >> 14); /* 脚踏力矩输入处理 - 起动过程平滑处理 */ uwTmpTorqLpf = ((ULONG)TorqCmd1 * ass_CalCoef.swSmoothGain) >> 12; /* 脚踏力矩输入最大值限制 */ if (uwTmpTorqLpf > ass_ParaCong.uwBikeAssTorMaxPu) { TorqCmd = ass_ParaCong.uwBikeAssTorMaxPu; } else { TorqCmd = uwTmpTorqLpf; } /***************** assist curve calculate ******************/ if(ass_CalIn.uwGearSt == 0) { Te_Tor_Assit_tempPu = 0; } else { Te_Tor_Assit_tempPu = AssistCurveCal(&ass_CalCoef.swAssCurCoef[ass_CalIn.uwGearSt-1], &TorqCmd); } if (Te_Tor_Assit_tempPu > ass_ParaCong.uwBikeAssTorMaxPu) // 最大转矩限幅 { Te_Tor_Assit_tempPu = ass_ParaCong.uwBikeAssTorMaxPu; } CadCmd = (((SLONG)ass_CalIn.uwcadance * ass_CalCoef.swSmoothGain) >> 12); // 踏频指令斜坡 Te_Cad_Assit_tempPu = ((SLONG)(Polynomial(&ass_CalCoef.uwCadencAsseGain[ass_CalIn.uwGearSt], &CadCmd, 20))) >> 6; // Q20 - Q6 = Q14 //踏频助力曲线 if (Te_Cad_Assit_tempPu > ass_ParaCong.uwBikeAssTorMaxPu) // 最大转矩限幅 { Te_Cad_Assit_tempPu = ass_ParaCong.uwBikeAssTorMaxPu; } /* 力矩助力根据用户设定比例调整 */ Te_Tor_AssitPu1 = ((((SLONG)Te_Tor_Assit_tempPu) * ass_ParaSet.uwTorAssAjstGain) >> 12); // Q14+Q12-Q12 = Q14; 用户设置转矩比例 /* 踏频助力根据用户设定比例调整 */ Te_Cad_AssitPu1 = ((((SLONG)Te_Cad_Assit_tempPu) * ass_ParaSet.uwCadenceAssAjstGain) >> 12); // Q14+Q12-Q12 = Q14; 用户设置踏频比例 /* 计算力矩和踏频总助力力矩 */ ass_CalOut.swTorAssistSum1 = (Te_Tor_AssitPu1 + Te_Cad_AssitPu1); // Q14 /* 计算力矩助力电流 */ ass_CalOut.swTorAss2CurrentTemp = swCurrentCal(Te_Tor_AssitPu1); // Q14 电流指令计算 /* 计算踏频助力电流 */ ass_CalOut.swCadAss2CurrentTemp = swCurrentCal(Te_Cad_AssitPu1); // Q14 电流指令计算 /* if (ass_CalOut.swTorAss2CurrentTemp > ass_CalCoef.swCurrentmax_torAssPu) { ass_CalOut.swTorAss2CurrentTemp = ass_CalCoef.swCurrentmax_torAssPu; } if (ass_CalOut.swCadAss2CurrentTemp > ass_CalCoef.swCurrentmax_cadAssPu) { ass_CalOut.swCadAss2CurrentTemp = ass_CalCoef.swCurrentmax_cadAssPu; } */ /* 计算力矩和踏频总助力电流 */ ass_CalOut.swTorRefTarget = ass_CalOut.swTorAss2CurrentTemp + ass_CalOut.swCadAss2CurrentTemp; if(ass_CalOut.swTorRefTarget > ass_CalCoef.uwCurrentMaxPu) /* max assist current limiter */ { ass_CalOut.swTorRefTarget = ass_CalCoef.uwCurrentMaxPu; } /* 根据助力方向，对助力电流的正负进行处理 */ ass_CalOut.swTorRefEnd = ass_CalIn.swDirection * ass_CalOut.swTorRefTarget; /* 计算当前时刻车速对应电机转速 */ if(ass_CalIn.uwbikespeed < 95L) //95 - 1Km/h { tmp_slBikeSpd2MotSpd = (((SLONG)95L* ass_ParaCong.uwMechRationMotor >>10) * ass_ParaCong.uwMotorPoles) >> 5; // Q20-Q5= Q15 出力时电机转速计算 } else { tmp_slBikeSpd2MotSpd = (((SLONG)ass_CalIn.uwbikespeed * ass_ParaCong.uwMechRationMotor >>10) * ass_ParaCong.uwMotorPoles) >> 5; // Q20-Q5= Q15 出力时电机转速计算 } if(tmp_slBikeSpd2MotSpd > _IQ15(0.99)) { ass_CalOut.swBikeSpd2MotSpd = _IQ15(0.99); } else if(tmp_slBikeSpd2MotSpd < _IQ15(0.0)) { ass_CalOut.swBikeSpd2MotSpd = _IQ15(0.0); } else { ass_CalOut.swBikeSpd2MotSpd = (SWORD)tmp_slBikeSpd2MotSpd; } /* 计算啮合电机转速对应的电机反电动势，计算起动时刻电压限幅初值 */ if(ass_CalIn.swSpdFbkPu * ass_CalIn.swDirection < ass_CalOut.swBikeSpd2MotSpd) { tmp_slUqLimInit = (SLONG)ass_CalOut.swBikeSpd2MotSpd * (SLONG)ass_CalIn.swDirection *(SLONG)cof_uwFluxPu >> 13;//Q15+Q12-Q13=Q14; } else { tmp_slUqLimInit = (SLONG)ass_CalIn.swSpdFbkPu *(SLONG)cof_uwFluxPu >> 13;//Q15+Q12-Q13=Q14; } // tmp_slUqLimInit = (SLONG)ass_CalIn.swSpdFbkPu *(SLONG)cof_uwFluxPu >> 13;//Q15+Q12-Q13=Q14; tmp_slUqLimInit = tmp_slUqLimInit * 4096 >> 12; //放大1.25倍 if (tmp_slUqLimInit > ((SLONG)scm_swVsDcpLimPu_Assist)) { ass_CalOut.swUqLimInit = scm_swVsDcpLimPu_Assist; } else if(tmp_slUqLimInit < -((SLONG)scm_swVsDcpLimPu_Assist)) { ass_CalOut.swUqLimInit = -scm_swVsDcpLimPu_Assist; } else { ass_CalOut.swUqLimInit = tmp_slUqLimInit; } /* 限速系数计算 */ ass_CalIn.uwbikespeedCal = ass_CalIn.uwbikespeed; /* Bike Speed Limit: use ass_CalCoef.swBikeSpeedGain(0-1) */ #if(BIKE_TORQUESPDLIMIT_EN!=0) Gear_LimitSpd(ass_CalIn.uwGearSt); #endif if (ass_CalIn.uwbikespeedCal <= ass_CurLimCoef.uwBikeSpdThresHold1) { ass_CalCoef.swBikeSpeedGain = Q12_1; // Q12 } else if (ass_CalIn.uwbikespeedCal > ass_CurLimCoef.uwBikeSpdThresHold1 && ass_CalIn.uwbikespeedCal <= ass_CurLimCoef.uwBikeSpdThresHold2) { ass_CalCoef.swBikeSpeedGain = Q12_1 -((((SLONG)ass_CalIn.uwbikespeedCal - (SLONG)ass_CurLimCoef.uwBikeSpdThresHold1) * ass_CurLimCoef.ulBikeSpdDeltInv) >> 8); // Q12 } else { ass_CalCoef.swBikeSpeedGain = 0; } /* 起动平滑系数步进量计算，根据车速自适应变化 */ StartUpGainAddStep = (cp_stBikeRunInfoPara.BikeSpeedKmH >> 4) + ass_CalCoef.uwStartUpGainAddStep; /* 电压限幅步进量计算Q22 */ if(ass_CalIn.swCurRefPu * ass_CalIn.swDirection > VolLimCurrLoopErr) { tmp_slCurrErr = ((SLONG)ass_CalIn.swCurRefPu - (SLONG)ass_CalIn.swCurFdbPu) - VolLimCurrLoopErr * ass_CalIn.swDirection; } else { tmp_slCurrErr = - (SLONG)ass_CalIn.swCurFdbPu; } tmp_slVoltLimitPu = tmp_slCurrErr * (SLONG)ass_ParaSet.uwSpeedAssistSpdRpm >> 6; if(ass_ParaSet.swUqLimitKi < ass_ParaSet.uwSpeedAssistIMaxA) { ass_ParaSet.swUqLimitKi += ass_ParaSet.uwStartUpCadNm; } else { ass_ParaSet.swUqLimitKi = (SWORD)ass_ParaSet.uwSpeedAssistIMaxA; } ass_CalOut.swUqLimStep = (SWORD)(tmp_slCurrErr * (SLONG)ass_ParaSet.swUqLimitKi >> 12); test_AssState = Ass_FSM; /* 力矩助力过程状态机控制 */ switch (Ass_FSM) { case StopAssit: ass_CalOut.swTorAssistCurrentTemp = 0; //停止助力直接将助力电流赋为0 ass_CalOut.blAssistflag = FALSE; /* 启动判断*/ if((ass_CalIn.uwGearSt > 0) && (BikeBrake_blGetstate() == FALSE) &&(BMS_RunInfo.Status.Status_Bit.Charge==FALSE)) { if(ass_CalIn.StartAssistEnble == TRUE)//5启动根据脉冲数 { ass_CalCoef.swTorqFilterGain = 0; ass_CalCoef.swSmoothGain = 0; ass_CalCoef.swSmoothStopGain = Q12_1; ass_CalOut.swVoltLimitPu = ass_CalOut.swUqLimInit; ass_CalOut.slUqLimSum = ((SLONG)ass_CalOut.swUqLimInit)<<8; ass_CalOut.uwPreStartCnt = 0; ass_CalOut.uwPreStartCntMax = (cp_stBikeRunInfoPara.BikeSpeedKmH<<1) + 200; Ass_FSM = PreStart; } } break; case PreStart: /* 力矩助力标志置位 */ ass_CalOut.blAssistflag = TRUE; /* 预启动阶段，助力电流最小值限制 */ // if((ass_CalOut.swTorRefEnd * ass_CalIn.swDirection) > PreStartCurr) // { // ass_CalOut.swTorAssistCurrentTemp = PreStartCurr * ass_CalIn.swDirection; // } // else // { // ass_CalOut.swTorAssistCurrentTemp = ass_CalOut.swTorRefEnd; // } // ass_CalOut.swTorAssistCurrentTemp = PreStartCurr * ass_CalIn.swDirection; ass_CalOut.swTorAssistCurrentTemp = ass_CalOut.swTorRefEnd; /* 平滑系数值计算和限幅 */ ass_CalCoef.swSmoothGain += StartUpGainAddStep;//(SWORD)ass_CalCoef.uwStartUpGainAddStep; ////ass_stCalCoef.uwStartUpGainAddStep; if(ass_CalCoef.swSmoothGain >= Q12_1) { ass_CalCoef.swSmoothGain = Q12_1; } ass_CalCoef.swSmoothStopGain += 64; if(ass_CalCoef.swSmoothStopGain >= Q12_1) { ass_CalCoef.swSmoothStopGain = Q12_1; } /* 力矩切换系数值计算和限幅 */ ass_CalCoef.swTorqFilterGain += 4; // Q14 转矩滤波方式切换系数 if (ass_CalCoef.swTorqFilterGain > Q14_1) { ass_CalCoef.swTorqFilterGain = Q14_1; } /* 电压限幅值计算和限幅 */ // ass_CalOut.slUqLimSum += ass_CalOut.swUqLimStep; ass_ParaSet.swUqLimitKi = 0; ass_CalOut.slUqLimSum = ((SLONG)ass_CalOut.swUqLimInit)<<8; if (ass_CalOut.slUqLimSum > ((SLONG)scm_swVsDcpLimPu_Assist << 8)) { ass_CalOut.slUqLimSum = (SLONG)scm_swVsDcpLimPu_Assist << 8; } else if(ass_CalOut.slUqLimSum < -((SLONG)scm_swVsDcpLimPu_Assist << 8)) { ass_CalOut.slUqLimSum = -((SLONG)scm_swVsDcpLimPu_Assist << 8); } else { //中间不操作 } tmp_slVoltLimitPu = ass_CalOut.slUqLimSum + tmp_slVoltLimitPu; if (tmp_slVoltLimitPu > ((SLONG)scm_swVsDcpLimPu_Assist << 8)) { ass_CalOut.swVoltLimitPu = scm_swVsDcpLimPu_Assist; } else if(tmp_slVoltLimitPu < -((SLONG)scm_swVsDcpLimPu_Assist << 8)) { ass_CalOut.swVoltLimitPu = -scm_swVsDcpLimPu_Assist; } else { ass_CalOut.swVoltLimitPu = (SWORD)(tmp_slVoltLimitPu >> 8); } /* 预启动阶段计时，超时进入助力状态 */ if(ass_CalOut.uwPreStartCnt < ass_CalOut.uwPreStartCntMax) { ass_CalOut.uwPreStartCnt ++; } else { ass_CalOut.uwPreStartCnt = 0; Ass_FSM = TorqueAssit; } /* 预启动阶段，停机判断 */ if((ass_CalIn.uwGearSt == 0) || (BikeBrake_blGetstate() == TRUE) || (BMS_RunInfo.Status.Status_Bit.Charge==TRUE) || \ ((ass_CalIn.blStopAssistEnble == TRUE) || (ass_CalIn.StartAssistEnble == FALSE))) { Ass_FSM = PreStop; } break; case TorqueAssit: /* 力矩助力标志置位 */ ass_CalOut.blAssistflag = TRUE; /* 平滑系数值计算和限幅 */ ass_CalCoef.swSmoothGain += StartUpGainAddStep;//(SWORD)ass_CalCoef.uwStartUpGainAddStep; ////ass_stCalCoef.uwStartUpGainAddStep; if(ass_CalCoef.swSmoothGain >= Q12_1) { ass_CalCoef.swSmoothGain = Q12_1; } ass_CalCoef.swSmoothStopGain += 64; if(ass_CalCoef.swSmoothStopGain >= Q12_1) { ass_CalCoef.swSmoothStopGain = Q12_1; } /* 力矩切换系数值计算和限幅 */ ass_CalCoef.swTorqFilterGain += 4; // Q14 转矩滤波方式切换系数 if (ass_CalCoef.swTorqFilterGain > Q14_1) { ass_CalCoef.swTorqFilterGain = Q14_1; } /* 电压限幅值计算和限幅 */ ass_CalOut.slUqLimSum += ass_CalOut.swUqLimStep; if (ass_CalOut.slUqLimSum > ((SLONG)scm_swVsDcpLimPu_Assist << 8)) { ass_CalOut.slUqLimSum = (SLONG)scm_swVsDcpLimPu_Assist << 8; } else if(ass_CalOut.slUqLimSum < -((SLONG)scm_swVsDcpLimPu_Assist << 8)) { ass_CalOut.slUqLimSum = -((SLONG)scm_swVsDcpLimPu_Assist << 8); } else { //中间不操作 } tmp_slVoltLimitPu = ass_CalOut.slUqLimSum + tmp_slVoltLimitPu; if (tmp_slVoltLimitPu > ((SLONG)scm_swVsDcpLimPu_Assist << 8)) { ass_CalOut.swVoltLimitPu = scm_swVsDcpLimPu_Assist; } else if(tmp_slVoltLimitPu < -((SLONG)scm_swVsDcpLimPu_Assist << 8)) { ass_CalOut.swVoltLimitPu = -scm_swVsDcpLimPu_Assist; } else { ass_CalOut.swVoltLimitPu = (SWORD)(tmp_slVoltLimitPu >> 8); } /* 助力电流读取 */ ass_CalOut.swTorAssistCurrentTemp = ass_CalOut.swTorRefEnd; /* 预启动阶段，停机判断 */ if((ass_CalIn.uwGearSt == 0) || (BikeBrake_blGetstate() == TRUE) || (BMS_RunInfo.Status.Status_Bit.Charge==TRUE) || ((ass_CalIn.blStopAssistEnble == TRUE) || (ass_CalIn.StartAssistEnble == FALSE))) { Ass_FSM = PreStop; } break; case PreStop: /* 力矩助力标志置位 */ ass_CalOut.blAssistflag = TRUE; /* 助力电流读取 */ ass_CalOut.swTorAssistCurrentTemp = ass_CalOut.swTorRefEnd; ass_CalCoef.swSmoothGain -= 64; //降速步进值，修改此值实现降速快慢 if(ass_CalCoef.swSmoothGain <= 0) { ass_CalCoef.swSmoothGain = 0; } ass_CalCoef.swSmoothStopGain -= 64; /* 电压限幅值计算和限幅 */ ass_CalOut.slUqLimSum += ass_CalOut.swUqLimStep; if (ass_CalOut.slUqLimSum > ((SLONG)scm_swVsDcpLimPu_Assist << 8)) { ass_CalOut.slUqLimSum = (SLONG)scm_swVsDcpLimPu_Assist << 8; } else if(ass_CalOut.slUqLimSum < -((SLONG)scm_swVsDcpLimPu_Assist << 8)) { ass_CalOut.slUqLimSum = -((SLONG)scm_swVsDcpLimPu_Assist << 8); } else { //中间不操作 } tmp_slVoltLimitPu = ass_CalOut.slUqLimSum + tmp_slVoltLimitPu; if (tmp_slVoltLimitPu > ((SLONG)scm_swVsDcpLimPu_Assist << 8)) { ass_CalOut.swVoltLimitPu = scm_swVsDcpLimPu_Assist; } else if(tmp_slVoltLimitPu < -((SLONG)scm_swVsDcpLimPu_Assist << 8)) { ass_CalOut.swVoltLimitPu = -scm_swVsDcpLimPu_Assist; } else { ass_CalOut.swVoltLimitPu = (SWORD)(tmp_slVoltLimitPu >> 8); } /* 重新启动判断*/ if((ass_CalIn.uwGearSt > 0) && (BikeBrake_blGetstate() == FALSE) &&(BMS_RunInfo.Status.Status_Bit.Charge==FALSE)) { if(ass_CalIn.StartAssistEnble == TRUE) { Ass_FSM = TorqueAssit; } } /* 预停机完成判断*/ if(ass_CalCoef.swSmoothStopGain <= 0) { ass_CalCoef.swSmoothStopGain = 0; ass_CalCoef.swSmoothGain = 0; ass_CalCoef.swTorqFilterGain = 0; MoveAverageFilterClear(&maf_torque); Ass_FSM = StopAssit; } break; default: break; } /* 指令电流限误差处理 */ if((ass_CalOut.swTorAssistCurrentTemp - ass_CalIn.swCurFdbPu) * ass_CalIn.swDirection > RefCurrErrLim) { ass_CalOut.swTorAssistCurrent = ass_CalIn.swCurFdbPu + RefCurrErrLim * ass_CalIn.swDirection; } else { ass_CalOut.swTorAssistCurrent = ass_CalOut.swTorAssistCurrentTemp; } /* 指令电流滤波处理 */ mth_voLPFilter(ass_CalOut.swTorAssistCurrent, &ass_pvt_stCurLpf); /* 指令电流增量限幅处理 */ // if((ass_CalOut.swTorAssistCurrent - ass_pvt_stCurLpf.slY.sw.hi) * ass_CalIn.swDirection > ass_stCadAssParaPro.swTargetAssCurAcc) // { // ass_pvt_stCurLpf.slY.sw.hi += ass_stCadAssParaPro.swTargetAssCurAcc * ass_CalIn.swDirection; // } // else if((ass_pvt_stCurLpf.slY.sw.hi - ass_CalOut.swTorAssistCurrent) * ass_CalIn.swDirection > (ass_stCadAssParaPro.swTargetAssCurAcc<<1)) // { // ass_pvt_stCurLpf.slY.sw.hi -= (ass_stCadAssParaPro.swTargetAssCurAcc<<1) * ass_CalIn.swDirection; // } // else // { // ass_pvt_stCurLpf.slY.sw.hi = ass_CalOut.swTorAssistCurrent; // } /* 指令电流限速处理 */ Assist_torqueper = ((SLONG)ass_pvt_stCurLpf.slY.sw.hi * ass_CalCoef.swBikeSpeedGain) >> 12; /* 指令电流停机平滑处理 */ Assist_torqueper = (SLONG)Assist_torqueper * ass_CalCoef.swSmoothStopGain >> 12; } /** * @brief Three order polynomial Y = a*X^3 + b*X^2 + c*x +d * * @param coef polynomial coefficient a, b, c, d * @param Value polynomial input value X * @param Qnum polynomial input Q type * @return UWORD polynomial output Y */ void AssitCuvLim(UWORD gear, UWORD uwBikeSpeedHzPu, UWORD uwCurMaxPu) { UWORD uwIqLimitTemp1; if(gear > 5) { gear = 5; } uwIqLimitTemp1 = ((ULONG)ass_CurLimCoef.uwLimitGain[gear] * uwCurMaxPu) >> 10; ass_CurLimOut.uwIqlimit = uwIqLimitTemp1; } /** * @brief Assist function * * @param coef polynomial coefficient a, b, c, d * @param Value polynomial input value X * @param Qnum polynomial input Q type * @return UWORD polynomial output Y @1ms */ void Assist(void) { //////////// Calculate the Iq limit /////////////////// UWORD IqLimitTemp; AssitCuvLim(ass_CalIn.uwGearSt, ass_CalIn.uwbikespeed, ass_ParaCong.uwCofCurMaxPu); IqLimitTemp = (ass_CurLimOut.uwIqlimit < ass_CalIn.swFlxIqLimit) ? (ass_CurLimOut.uwIqlimit < ass_CalIn.swPwrIqLimit ? ass_CurLimOut.uwIqlimit : ass_CalIn.swPwrIqLimit) : (ass_CalIn.swFlxIqLimit < ass_CalIn.swPwrIqLimit ? ass_CalIn.swFlxIqLimit : ass_CalIn.swPwrIqLimit); // IqLimitTemp = (IqLimitTemp < ass_CurLimitCalBMSOut.uwIdcLimIqAbs) ? IqLimitTemp : ass_CurLimitCalBMSOut.uwIdcLimIqAbs; ass_CalCoef.uwCurrentMaxPu = (IqLimitTemp < ass_CurLimitCalBMSOut.uwIqLimitAbs) ? IqLimitTemp : ass_CurLimitCalBMSOut.uwIqLimitAbs; ass_CalCoef.swCurrentmax_torAssPu = ((SLONG)ass_CalCoef.uwCurrentMaxPu * ass_ParaSet.uwTorWeight) >> 12; // Q14 ass_CalCoef.swCurrentmax_cadAssPu = ((SLONG)ass_CalCoef.uwCurrentMaxPu * ass_ParaSet.uwCadenceWeight) >> 12; //////////////// Assist //////////////////////// AssitCuvApplPerVolt(); //助力处理 /////////////// Limit /////////////////////////// if (Assist_torqueper > ass_CalCoef.uwCurrentMaxPu) { Assist_torqueper = ass_CalCoef.uwCurrentMaxPu; } else if(Assist_torqueper < -(SWORD)ass_CalCoef.uwCurrentMaxPu) { Assist_torqueper = -(SWORD)ass_CalCoef.uwCurrentMaxPu; } else { } } void MoveAverageFilter(MAF_IN *in) { in->sum -= in->buffer[in->index]; in->buffer[in->index] = in->value; in->sum += (SQWORD)in->value; if (!in->blSecFlag) { in->AverValue = (SLONG)(in->sum / (in->index + 1)); } else { in->AverValue = (SLONG)(in->sum / in->length); } in->index++; if (in->index >= in->length) { in->blSecFlag = TRUE; in->index = 0; } } void MoveAverageFilterClear(MAF_IN *in) { UWORD i; in->index = 0; in->sum = 0; in->blSecFlag = FALSE; for (i = 0; i < (sizeof(in->buffer) >> 1); i++) { in->buffer[i] = 0; } } void AssistCurveRatio(void) // 上电运行一次or助力参数更新后，AssistCoef需要重新计算 { /* 根据OBC调整补偿系数*/ if (ass_ParaCong.uwStartMode == 1) // { ass_CalCoef.uwAssCurvGain = _IQ12(0.9); } else if (ass_ParaCong.uwStartMode == 2) { ass_CalCoef.uwAssCurvGain = _IQ12(1.0); } else if (ass_ParaCong.uwStartMode == 3) { ass_CalCoef.uwAssCurvGain = _IQ12(1.1); } else { ass_CalCoef.uwAssCurvGain = _IQ12(1.0); } /*根据电机力矩大小配置补偿系数*/ // ass_CalCoef.uwAssistCurveGain = ((SQWORD)cp_stMotorPara.swIpeakMaxA * TORQUR_ASSIST_RATIO * TORQUR_ASSIST_BASE_INV >> 24) + 4096 - TORQUR_ASSIST_RATIO; //Q12+Q16 -Q16 =Q12 // ass_CalCoef.uwAssistCurveGain = (SLONG)ass_CalCoef.uwAssCurvGain * ass_CalCoef.uwAssistCurveGain >> 12; // Q12 ass_CalCoef.uwAssistCurveGain = ass_CalCoef.uwAssCurvGain; /*线性段补偿系数*/ ass_CalCoef.swAssCompCoef.swKHigh = ass_CalCoef.uwAssistCurveGain; /*曲线段补偿系数*/ ass_CalCoef.swAssCompCoef.swKLow = ass_CalCoef.uwAssistCurveGain; memcpy(&ass_CalCoef.uwCadencAsseGain[1], &Syspara2.flash_stPara.slCadAssGain[0], sizeof(Syspara2.flash_stPara.slCadAssGain)); } void AssistCurveCoef(ASS_CURVE_COMP_COEF *comp) { /*传感器输入力矩参考点*/ SWORD swX1 = ASS_CURVE_X1 * TORQUE2PU >> 6; //Q14 SWORD swX2 = ASS_CURVE_X2 * TORQUE2PU >> 6; //Q14 SWORD swX3 = ASS_CURVE_X3 * TORQUE2PU >> 6; //Q14 SWORD swY1,swY2,swY3,swZ; SLONG sltmpk2; /*电机输出力矩参考点*/ memcpy(&ass_CalCoef.slAssCurPre[0], &Syspara2.flash_stPara.slTorqAssGain[0], sizeof(ass_CalCoef.slAssCurPre)); for(UWORD i = 0; i < 5; i++) { swY1 = (ass_CalCoef.slAssCurPre[i].slY1 * TORQUE2PU >> 6) * comp->swKLow >> 12 ; //Q20 - Q6 + Q12 - Q12 = Q14 swY2 = (ass_CalCoef.slAssCurPre[i].slY2 * TORQUE2PU >> 6) * comp->swKLow >> 12; //Q20 - Q6 + Q12 - Q12 = Q14 swY3 = (ass_CalCoef.slAssCurPre[i].slY3 * TORQUE2PU >> 6) * comp->swKHigh >> 12; //Q20 - Q6 + Q12 - Q12 = Q14 swZ = ass_CalCoef.slAssCurPre[i].slZ * TORQUE2PU >> 6; //Q20 -Q16 = Q14 /* k1 =(y2-y1)/(x2-x1) */ ass_CalCoef.swAssCurCoef[i].swk1 = (((SLONG)swY2-(SLONG)swY1)<<10)/(swX2-swX1); // Q10 /* b = y2-k2*x1 */ ass_CalCoef.swAssCurCoef[i].swb = ((SLONG)swY2 - ((SLONG)ass_CalCoef.swAssCurCoef[i].swk1 * swX2 >> 10));// Q14 /* k2 = (y3-k1*x3-b)/(x3-Z)^2 */ sltmpk2 = (SLONG)swY3 - ((SLONG)ass_CalCoef.swAssCurCoef[i].swk1 * swX3 >> 10)- (SLONG)ass_CalCoef.swAssCurCoef[i].swb; //Q14 ass_CalCoef.swAssCurCoef[i].swk2 = ((SQWORD)sltmpk2 << 20 )/((SLONG)(swX3-swZ)*(swX3-swZ)); //Q20 ass_CalCoef.swAssCurCoef[i].swZ = swZ; } } SLONG AssistCurveCal(ASS_CURVE_COEF *coef, SWORD *value) { SLONG out; if(*value < coef->swZ) { /* Y = k1*x+b x< Z */ out = ((SLONG)*value * coef->swk1 >> 10) + coef->swb; } else { /* Y = k1*x+k2*(x-Z)^2+b x>= Z */ out = ((SLONG)*value * coef->swk1 >> 10) + coef->swb + ((SQWORD)coef->swk2 * (*value - coef->swZ) * (*value - coef->swZ) >> 20); } return out; } void ass_voTorqAssistInput(void) { /** Flux current limit input */ if(cp_stFlg.RotateDirectionSelect == ForwardRotate) { ass_CalIn.swDirection = 1; } else { ass_CalIn.swDirection = -1; } /** BMS SOCValue */ ass_CalIn.SOCValue = MC_RunInfo.SOC; /** Flux current limit input */ ass_CalIn.swFlxIqLimit = abs(flx_stCtrlOut.swIqLimPu); /** Power limit current limit input */ ass_CalIn.swPwrIqLimit = abs(pwr_stPwrLimOut2.swIqLimPu); /** Assist gear input */ ass_CalIn.uwGearSt = cp_stBikeRunInfoPara.uwBikeGear; /** Cadence last frequency input */ ass_CalIn.uwcadancelast = ass_CalIn.uwcadance; /** Cadence frequency input */ ass_CalIn.uwcadance = cadence_stFreGetOut.uwFrequencyPu; /** Cadence frequency percentage input */ ass_CalIn.uwcadancePer = cadence_stFreGetOut.uwFreqPercent; /** Cadence forword pulse counter input */ ass_CalIn.uwcadanceFWCnt = cadence_stFreGetOut.uwForwardCnt; /** Bike speed forword pulse counter input */ ass_CalIn.uwbikespeed = bikespeed_stFreGetOut.uwLPFFrequencyPu; /** Motor abs speed input */ ass_CalIn.uwSpdFbkAbsPu = scm_uwSpdFbkLpfAbsPu; /** Motor speed input */ ass_CalIn.swSpdFbkPu = scm_stSpdFbkLpf.slY.sw.hi; /** Motor speed base rpm input */ ass_CalIn.uwBaseSpdrpm = cof_uwVbRpm; /** Torque move average filter input */ ass_CalIn.uwtorque = maf_torque.AverValue; /** Torque low pass filter input */ ass_CalIn.uwtorquelpf = torsensor_stTorSensorOut.uwTorqueLPFPu; /** Instant torque input */ ass_CalIn.uwtorquePer = torsensor_stTorSensorOut.uwTorquePu; /** Current feedback input */ ass_CalIn.swCurFdbPu = scm_stIqFbkforDesat.slY.sw.hi; // ass_CalIn.swCurFdbPu = scm_swIqFdbLpfPu; /** Current reference input */ ass_CalIn.swCurRefPu = scm_swIqRefPu; } void AssistCurrentLimitAccordingBMS(UWORD uwSOCvalue) { if (uwSOCvalue < ass_CurLimCalBMSCoef.uwIqLimitStartSoc && uwSOCvalue > ass_CurLimCalBMSCoef.uwIqLimitEndSoc) { ass_CurLimitCalBMSOut.uwIqLimitAbs = ass_ParaCong.uwCofCurMaxPu - (((SLONG)ass_CurLimCalBMSCoef.uwIqLimitStartSoc - uwSOCvalue) * ass_CurLimCalBMSCoef.swIqLImitK); } else if (uwSOCvalue <= ass_CurLimCalBMSCoef.uwIqLimitEndSoc) { ass_CurLimitCalBMSOut.uwIqLimitAbs = ass_ParaCong.uwCofCurMaxPu - (((SLONG)ass_CurLimCalBMSCoef.uwIqLimitStartSoc - ass_CurLimCalBMSCoef.uwIqLimitEndSoc) * ass_CurLimCalBMSCoef.swIqLImitK); } else { ass_CurLimitCalBMSOut.uwIqLimitAbs = ass_CurLimCalBMSCoef.uwIqLimitInitAbs; } } void AssCurLimAccordIdc(UWORD uwIdcPu) { if (uwIdcPu > ass_CurLimCalBMSCoef.uwIqLimitStartIdc && uwIdcPu < ass_CurLimCalBMSCoef.uwIqLimitEndIdc) { ass_CurLimitCalBMSOut.uwIdcLimIqAbs = ass_CurLimCalBMSCoef.uwIqLimitInitAbs - (((SLONG)uwIdcPu - ass_CurLimCalBMSCoef.uwIqLimitStartIdc) * ass_CurLimCalBMSCoef.swIqLImitIdcK); } else if (uwIdcPu >= ass_CurLimCalBMSCoef.uwIqLimitEndIdc) { ass_CurLimitCalBMSOut.uwIdcLimIqAbs = 0; } else { ass_CurLimitCalBMSOut.uwIdcLimIqAbs = ass_CurLimCalBMSCoef.uwIqLimitInitAbs; } }