/** * @file AssistCurve.c * @author Zhang, Kai(zhangkai71@midea.com) * @brief * @version 0.1 * @date 2021-11-15 * * @copyright Copyright (c) 2021 * */ /****************************** * * Include File * ******************************/ #include "string.h" #include "Syspar.h" #include "user.h" #include "AssistCurve.h" #include "bikebrake.h" #include "Cadence.h" #include "flash_master.h" /****************************** * * Parameter * ******************************/ ASS_FSM_STATUS Ass_FSM; ASS_PER_IN ass_stCalIn = TORQUE_CAL_IN_DEFAULT; ASS_PER_COEF ass_stCalCoef; ASS_PER_OUT ass_stCalOut; ASS_PARA_CONFIGURE ass_stParaCong; ASS_PARA_SET ass_stParaSet; ASS_CURLIM_COEF ass_stCurLimCoef = ASS_LIM_DEFAULT; ASS_CURLIM_OUT ass_stCurLimOut; ASS_LIMIT_ACCORDING_VOL_COF ass_stCurLimCalBMSCoef; ASS_LIMIT_ACCORDING_VOL_OUT ass_stCurLimitCalBMSOut; ASR_SPDPI_IN asr_stTorqSpdPIIn; ASR_SPDPI_OUT asr_stTorqSpdPIOut; ASR_SPDPI_COF asr_stTorqSpdPICoef; ASR_SPDPI_COFIN asr_stTorqSpdPICoefIn; ASS_TORQ_PI_IN ass_stTorqPIIn; ASS_TORQ_PI_OUT ass_stTorqPIOut; SWORD ass_swTorqMafBuf[64]; MAF_IN ass_stTorqMafValue = {0, 32, 0, 0, ass_swTorqMafBuf, 0, FALSE}; SWORD ass_swUqLimMafBuf[64]; MAF_IN ass_stUqLimMafValue = {0, 64, 0, 0, ass_swUqLimMafBuf, 0, FALSE}; static TOR2CURRENT_CAL_COEF ass_Tor2CurCalCoef; static UWORD StartUpGainArray[5] = START_GAIN_DEFAULT; static UWORD LinerAssist[5] = ASS_LINER_TORQUE_DEFAULT; static SWORD ass_pvt_swVoltCnt=0; static UWORD ass_pvt_uwTorqAccCnt=0,ass_pvt_uwTorqDecCnt=0,ass_pvt_uwSpd2TorqCnt=0; static UWORD AssCnt1ms; /****************************** * * 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 ass_slPolynomial(const POLY_COEF *coef, const 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 = (SLONG)((((((SQWORD)coef->a * *value >> 12) * *value) >> (SWORD)Qnum) * *value) >> (SWORD)Qnum); // Qx+Q12-Q12+Qx-Qx+Qx-Qx=Qx temp_b = (SLONG)((((SQWORD)coef->b * *value >> 12) * *value) >> (SWORD)Qnum); // Qx+Q12-Q12+Qx-Qx=Qx temp_c = (SLONG)((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 ass_stPolynomialcenter(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 ass_swTorq2CurPu(SWORD Tor) { SWORD swCurrentPu; SWORD swMotorTorqueNotPu; swMotorTorqueNotPu = (SWORD)(((SLONG)Tor * (SWORD)TORQUEBASE / (SWORD)ass_stParaCong.uwMechRationMotor) >> 7); // Q14-Q7 = Q7 0.1Nm Not Pu swCurrentPu = (SWORD)(((SLONG)swMotorTorqueNotPu * ass_Tor2CurCalCoef.swCalCoefINV) * 10 / IBASE); // Q7+Q7 = Q14; 0.1Nm/0.01A return swCurrentPu; } /** * @brief * * @param * @return */ static void ass_voAssistModeSelect(void) // 上电运行一次or助力参数更新后，AssistCoef需要重新计算 { UWORD TempAssit; UWORD TempGear, gear; // if (ass_stParaSet.uwAsssistSelectNum == 1) // OBC：更换成EE参数 // { // TempAssit = ass_stParaCong.uwAssistSelect1; // } // else if (ass_stParaSet.uwAsssistSelectNum == 2) // { // TempAssit = ass_stParaCong.uwAssistSelect2; // } // else // { // TempAssit = ASSISTMOD_SELECT_DEFAULT; // } if (ass_stParaCong.uwStartMode == 1) // OBC：更换成EE参数 { TempAssit = ASSISTMOD_SELECT_DEFAULT; } else if (ass_stParaCong.uwStartMode == 2) { TempAssit = ass_stParaCong.uwAssistSelect1; } else if (ass_stParaCong.uwStartMode == 3) { TempAssit = ass_stParaCong.uwAssistSelect2; } else { TempAssit = ASSISTMOD_SELECT_DEFAULT; } SLONG slTorqGainSum =0; for(UWORD i = 0; i < 4; i++) { slTorqGainSum += flash_stPara.slTorqAssGain[0][i]; } if(slTorqGainSum == 0) { SLONG slTorqAssGain[15][4] = TORQUE_ASSIST_DEFAULT; SLONG slCadAssGain[5][4] = CADENCE_ASSIST_DEFAULT; UWORD a ; a =sizeof( slTorqAssGain); memcpy(&flash_stPara.slTorqAssGain[0], &slTorqAssGain[0], sizeof(slTorqAssGain)); memcpy(&flash_stPara.slCadAssGain[0], &slCadAssGain[0], sizeof(slCadAssGain)); } for (gear = 0; gear < 5; gear++) { TempGear = gear * 3 + ((TempAssit >> (UWORD)(gear << 1)) & 0x0003); memcpy(&ass_stCalCoef.uwTorqueAssGain[(gear + 1)], &flash_stPara.slTorqAssGain[TempGear], sizeof(POLY_COEF)); } memcpy(&ass_stCalCoef.uwCadencAsseGain[1], &flash_stPara.slCadAssGain[0], sizeof(flash_stPara.slCadAssGain)); } /** * @brief Para from EE Init * * @param void * @return void */ void ass_voAssitEEInit(void) { ass_stParaCong.uwWheelPerimeter = BIKE_WHEEL_PERIMETER; // Q0 0.1CM ass_stParaCong.uwCadPulsePerCirc = CADENCE_PULSES_PER_CIRC; ass_stParaCong.uwMechRationMotor = 35; // Q0 ass_stParaCong.uwAssistMaxSpdKmH = BIKE_SPEED_IQLIMIT_THRESHOLD1; ass_stParaCong.uwThrottleMaxSpdKmH = BIKE_SPEED_THROTTLE_MAX; ass_stParaCong.uwNmFrontChainring = BIKE_FRONTTEETH_NMB; // front gear ass_stParaCong.uwNmBackChainring = BIKE_BACKTEETH_NMB; // min number of back gear ass_stParaCong.uwAssistSelect1 = BIKE_ASSIST_MODE1; ass_stParaCong.uwAssistSelect2 = BIKE_ASSIST_MODE2; ass_stParaCong.uwLightVoltage = BIKE_LIGHT_PARA; ass_stParaCong.swDeltPerimeter = BIKE_WHEEL_SIZE_ADJUST; ass_stParaCong.uwStartMode = BIKE_START_MODE; ass_stParaCong.uwAutoPowerOffTime = BIKE_POWER_PARA; ass_stParaSet.uwStartupCoef = 8194; // Q12 percentage Min 1-4096 1.5-6144 ass_stParaSet.uwStartupCruiseCoef = 4096; // Q12 percentage Min 1-4096 1-6144 ass_stParaSet.uwAssistStartNm = TORQUE_START_THRESHOLD; ass_stParaSet.uwAssistStopNm = TORQUE_STOP_THRESHOLD; ass_stParaSet.uwStartUpGainStep = 25; ass_stParaSet.uwStartUpCadNm = CADENCE_NUMBERS_PULSES >> 1; // 0.5 circle ass_stParaSet.uwTorLPFCadNm = CADENCE_NUMBERS_PULSES >> 1; // 0.5 circle ass_stParaSet.uwSpeedAssistSpdRpm = BIKE_SPD_MOTOR_CONSTANT_COMMAND; ass_stParaSet.uwSpeedAssistIMaxA = BIKE_SPD_MOTOR_CURRENT_MAX; ass_stParaSet.uwAssistLimitBikeSpdStart = BIKE_SPEED_IQLIMIT_THRESHOLD1; ass_stParaSet.uwAssistLimitBikeSpdStop = BIKE_SPEED_IQLIMIT_THRESHOLD2; ass_stParaSet.uwCadenceWeight = 1229; // Q12 percentage ass_stParaSet.uwTorWeight = Q12_1 ; // Q12 percentage ass_stParaSet.uwTorAssAjstGain = 4096; // Q12 percentage ass_stParaSet.uwCadenceAssAjstGain = 4094; // Q12 percentage ass_stParaSet.uwAsssistSelectNum = 1; ass_stParaSet.uwSpdRegion[0] = 8192; // Q15 1500rpm ass_stParaSet.uwSpdRegion[1] = 16384; // Q15 3000rpm ass_stParaSet.uwSpdRegion[2] = 21845; // Q15 4000rpm ass_stParaSet.uwSpdRegionGain[0] = 4094; ass_stParaSet.uwSpdRegionGain[1] = 4094; ass_stParaSet.uwSpdRegionGain[2] = 4094; /* 函数私有变量初始化 */ ass_pvt_swVoltCnt = 0; ass_pvt_uwTorqAccCnt = 0; ass_pvt_uwTorqDecCnt = 0; ass_pvt_uwSpd2TorqCnt = 0; AssCnt1ms = 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 ass_voAssitCoef(void) { /*状态机初始化*/ Ass_FSM = StopAssit; /*电机限制初始化*/ ass_stParaCong.uwCofCurMaxPu = ((ULONG)BIKE_ASS_MOTOR_CURRENT_MAX << 14) / IBASE; // Q14 ass_stParaCong.uwMotorPoles = cp_stMotorPara.swMotrPolePairs; ass_stParaCong.uwCofTorMaxPu = (UWORD)(((ULONG)cp_stMotorPara.swTorMax << 14) / TORQUEBASE); // Q14 ass_stParaCong.uwBikeAssTorMaxPu = ass_stParaCong.uwCofTorMaxPu * ass_stParaCong.uwMechRationMotor; // Q14; /*速度环参数初始化*/ asr_stTorqSpdPICoefIn.uwUbVt = VBASE; asr_stTorqSpdPICoefIn.uwIbAp = IBASE; asr_stTorqSpdPICoefIn.uwFbHz = FBASE; asr_stTorqSpdPICoefIn.uwFTbsHz = EVENT_1MS_HZ; asr_stTorqSpdPICoefIn.uwPairs = cp_stMotorPara.swMotrPolePairs; asr_stTorqSpdPICoefIn.uwMtJm = cp_stMotorPara.swJD; asr_stTorqSpdPICoefIn.uwMtFlxWb = cp_stMotorPara.swFluxWb; asr_stTorqSpdPICoefIn.uwMcoef = 5;//cp_stControlPara.swAsrPIM; asr_stTorqSpdPICoefIn.uwWvcHz = 10;//cp_stControlPara.swAsrPIBandwidth; asr_stTorqSpdPICoefIn.uwRatioJm = cp_stControlPara.swAsrSpdInerRate; asr_voSpdPICoef(&asr_stTorqSpdPICoefIn, &asr_stTorqSpdPICoef); /*电流限幅计算*/ ass_stCurLimCalBMSCoef.uwIqLimitInitAbs = ass_stParaCong.uwCofCurMaxPu; // Q14 ass_stCurLimCalBMSCoef.uwIqLimitStartSoc = 35; ass_stCurLimCalBMSCoef.uwIqLimitEndSoc = 0; ass_stCurLimCalBMSCoef.swIqLImitK = (SWORD)ass_stCurLimCalBMSCoef.uwIqLimitInitAbs / ((SWORD)ass_stCurLimCalBMSCoef.uwIqLimitStartSoc - (SWORD)ass_stCurLimCalBMSCoef.uwIqLimitEndSoc); /*助力曲线初始化*/ ass_voAssistModeSelect(); /*助力启动阈值初始化*/ ass_stCalCoef.uwAssThreshold = (UWORD)(((ULONG)ass_stParaSet.uwAssistStartNm << 14) / TORQUEBASE); // Q14 ass_stCalCoef.uwAssStopThreshold = (UWORD)(((ULONG)ass_stParaSet.uwAssistStopNm << 14) / TORQUEBASE); // Q14; /*助力系数初始化*/ ass_stCalCoef.StartFlag = 0; ass_stCalCoef.swSmoothGain = 0; // Q12 ass_stCalCoef.uwStartUpTargetGain = 0; // Q12 ass_stCalCoef.uwStartUpGainAddStep = ass_stParaSet.uwStartUpGainStep; // 25 Q12 if (ass_stCalCoef.uwStartUpGainAddStep < 1) { ass_stCalCoef.uwStartUpGainAddStep = 1; } if (ass_stCalCoef.uwStartUpGainAddStep > 50) { ass_stCalCoef.uwStartUpGainAddStep = 50; } /*设置启动到正常助力最少踏频数*/ ass_stCalCoef.uwStartUpTimeCadenceCnt = ass_stParaSet.uwStartUpCadNm; if (ass_stCalCoef.uwStartUpTimeCadenceCnt < (CADENCE_NUMBERS_PULSES >> 3)) { ass_stCalCoef.uwStartUpTimeCadenceCnt = CADENCE_NUMBERS_PULSES >> 3; } if (ass_stCalCoef.uwStartUpTimeCadenceCnt > CADENCE_NUMBERS_PULSES) { ass_stCalCoef.uwStartUpTimeCadenceCnt = CADENCE_NUMBERS_PULSES; } /*设置滑动平均滤波踏频数*/ ass_stTorqMafValue.uwLength = ass_stParaSet.uwTorLPFCadNm; ass_stCalCoef.swCadanceGain = 0; ass_stCalCoef.uwSwitch1TorqThreshold = ((ULONG)TORQUE_SWITCH1_THRESHOLD << 14) / TORQUEBASE; ass_stCalCoef.uwSwitch2TorqThreshold = ((ULONG)TORQUE_SWITCH2_THRESHOLD << 14) / TORQUEBASE; ass_stCalCoef.ulStartupDeltInv = ((ULONG)1 << 28) / (ass_stCalCoef.uwSwitch2TorqThreshold - ass_stCalCoef.uwSwitch1TorqThreshold); // Q14; /*初始化计数*/ ass_stCalCoef.uwCadencePeriodCNT = 0; ass_stCalCoef.swCadanceCNT = 0; ass_stCalCoef.sw2StopCNT = 0; ass_stCalCoef.swAss2SpdCNT = 0; /*配置速度环参数*/ ass_stCalCoef.uwSpeedConstantCommand = (UWORD)(((ULONG)ass_stParaSet.uwSpeedAssistSpdRpm << 15) / ((ULONG)FBASE * 60 / ass_stParaCong.uwMotorPoles)); ass_stCalCoef.swSpdLoopAbsCurMax = ((SWORD)ass_stParaSet.uwSpeedAssistIMaxA << 14) / IBASE; ass_stCalCoef.swSpeedlimtrpm = -100; ass_stCalCoef.swBikeSpeedGain = 0; /*设置电流限幅*/ ass_stCalCoef.uwCurrentMaxPu = ass_stParaCong.uwCofCurMaxPu; ass_stCalCoef.swCurrentmax_torAssPu = (SWORD)(((SLONG)ass_stCalCoef.uwCurrentMaxPu * (SWORD)ass_stParaSet.uwTorWeight) >> 12); // Q14 ass_stCalCoef.swCurrentmax_cadAssPu = (SWORD)(((SLONG)ass_stCalCoef.uwCurrentMaxPu * (SWORD)ass_stParaSet.uwCadenceWeight )>> 12); // Q14 /*初始化标志*/ ass_stCalCoef.blAssistflag = FALSE; ass_stCalOut.swTorAssistSum1 = 0; ass_stCalOut.swTorAssistSum2 = 0; ass_stCalOut.swTorAss2CurrentTemp = 0; ass_stCalOut.swCadAss2CurrentTemp = 0; ass_stCalOut.swTorAssistCurrentTemp = 0; ass_stCalOut.swTorSpdLoopCurrentTemp = 0; ass_stCalOut.swTorAssistCurrent = 0; ass_stCalOut.swSpeedRef = 0; ass_stCalOut.swCadSpd2MotSpd = 0; ass_stCurLimCoef.uwLimitGain[0] = 0; // Q10 percentage of max Current ass_stCurLimCoef.uwLimitGain[1] = 400; ass_stCurLimCoef.uwLimitGain[2] = 682; ass_stCurLimCoef.uwLimitGain[3] = 910; ass_stCurLimCoef.uwLimitGain[4] = 1024; ass_stCurLimCoef.uwLimitGain[5] = 1024; ass_stCurLimCoef.uwSpdThresHold = 21845; /*设置车速限幅*/ ass_stCurLimCoef.uwBikeSpdThresHold1 = (UWORD)(((UQWORD)1000 << 20) * ass_stParaSet.uwAssistLimitBikeSpdStart / ((UQWORD)36 * (ass_stParaCong.uwWheelPerimeter + (UWORD)ass_stParaCong.swDeltPerimeter) * FBASE)); // Q20 3216 = Q10(3.1415926) ass_stCurLimCoef.uwBikeSpdThresHold2 = (UWORD)(((UQWORD)1000 << 20) * ass_stParaSet.uwAssistLimitBikeSpdStop / ((UQWORD)36 * (ass_stParaCong.uwWheelPerimeter + (UWORD)ass_stParaCong.swDeltPerimeter) * FBASE)); // Q20 3216 = Q10(3.1415926) ass_stCurLimCoef.ulBikeSpdDeltInv = (ULONG)(((UQWORD)1 << 20) / (ass_stCurLimCoef.uwBikeSpdThresHold2 - ass_stCurLimCoef.uwBikeSpdThresHold1)); // Q20; ass_stCurLimCoef.uwBikeSpdIqLimitK = (UWORD)((((ULONG)ass_stCurLimCoef.uwBikeSpdThresHold2 - ass_stCurLimCoef.uwBikeSpdThresHold1) << 8) / ass_stParaCong.uwCofCurMaxPu); // Q28-q14 = Q14; /*设置转矩电流标定系数*/ ass_Tor2CurCalCoef.uwMotorFluxWb = cp_stMotorPara.swFluxWb; // 0.001mWb ass_Tor2CurCalCoef.uwMotprPolePairs = ass_stParaCong.uwMotorPoles; 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; } /** * @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 ass_voAssitTorqPIInit(void) //{ // ass_stTorqPIOut.slIRefPu = 0; // ass_stTorqPIOut.swErrZ1Pu = 0; // ass_stTorqPIOut.swIRefPu = 0; //} // //void ass_voAssitTorqPI(const ASS_TORQ_PI_IN *in, ASS_TORQ_PI_OUT *out) //{ // SLONG slErrPu, slDeltaErrPu; // SLONG slIpPu, slIiPu; // SLONG slImaxPu, slIminPu; // SQWORD sqIRefPu, sqIpPu; // UWORD uwKpPu = 5000, uwKitPu = 0; // uwKpPu(Q12), uwKitPu(Q15) // //// uwKpPu = ass_stParaSet.uwSpeedAssistIMaxA; //// uwKitPu = ass_stParaSet.uwStartUpCadNm; // // slImaxPu = (SLONG)in->swImaxPu << 15; // Q14+Q15=Q29 // slIminPu = (SLONG)in->swIminPu << 15; // Q14+Q15=Q29 // // slErrPu = in->swTorqRefPu - in->swTorqFdbPu; // Q14 // // if (slErrPu > 32767) // { // slErrPu = 32767; // } // else if (slErrPu < -32768) // { // slErrPu = -32768; // } // else // { // /* Nothing */ // } // //slDeltaErrPu = slErrPu - out->swErrZ1Pu; // Q14 // slDeltaErrPu = slErrPu; // if (slDeltaErrPu > 32767) // { // slDeltaErrPu = 32767; // } // else if (slDeltaErrPu < -32768) // { // slDeltaErrPu = -32768; // } // else // { // /* Nothing */ // } // // slIpPu = slDeltaErrPu * uwKpPu; // Q14+Q12=Q26 // sqIpPu = (SQWORD)slIpPu << 3; // // slIiPu = slErrPu * uwKitPu; // Q14+Q15=Q29 // // //sqIRefPu = sqIpPu + (SQWORD)slIiPu + (SQWORD)out->slIRefPu; // Q29 // sqIRefPu = sqIpPu; // // if (sqIRefPu > slImaxPu) // { // out->slIRefPu = slImaxPu; // } // else if (sqIRefPu < slIminPu) // { // out->slIRefPu = slIminPu; // } // else // { // out->slIRefPu = sqIRefPu; // } // out->swIRefPu = out->slIRefPu >> 15; // Q29-Q15=Q14 // out->swErrZ1Pu = (SWORD)slErrPu; //} SLONG slTeTorAssitTmpPu,slTeTorAssitLinerPu,slTeCadAssitTmpPu; SWORD swTeTorAssitPu1, swTeTorAssitPu2; SWORD swTeCadAssitPu1, swTeCadAssitPu2; SWORD swTmpSpdtoTorqCur; SLONG slTmpSmoothCur; SWORD swTorqCmd1, swTorqCmd, swCadCmd; static void AssitCuvApplPerVolt(void) /* parasoft-suppress METRICS-28 "本项目圈复杂度无法更改，后续避免" */ { UWORD uwTorqAccStep = 50,uwTorqDecStep = 80; SWORD swCurSwitch = 0; SWORD swTmpVoltPu,swTmpVoltPu2; SLONG slSpdErr,slTmpVoltLim; SWORD swSpdKpPu = 500; //Q10 UWORD uwVoltAccStep = 1, uwVoltDecStep = 3; UWORD uwTmpStopCnt = 0; // SLONG slTmp_a1, slTmp_b1, slTmp_c1; /* Select Torq Growth Rate by Bike Gear */ if (ass_stCalIn.uwGearSt == 1) { uwTorqAccStep = 50; } else if(ass_stCalIn.uwGearSt == 2) { uwTorqAccStep = 100; } else if(ass_stCalIn.uwGearSt == 3) { uwTorqAccStep = 120; } else if(ass_stCalIn.uwGearSt == 4) { uwTorqAccStep = 150; } else if(ass_stCalIn.uwGearSt == 5) { uwTorqAccStep = 150; } else { //do nothing } uwTorqDecStep = 80; AssCnt1ms ++; if(AssCnt1ms >= 10000) { AssCnt1ms = 0; } /* Select TorqRef: LPFTorq or MAFTorq */ swTorqCmd1 = (SWORD)(((SLONG)ass_stCalIn.uwtorque * ass_stCalCoef.swTorqFilterGain >> 14) + ((SLONG)ass_stCalIn.uwtorquelpf * (Q14_1 - ass_stCalCoef.swTorqFilterGain) >> 14)); //转矩指令滤波切换，由低通滤波到踏频相关的滑动平均滤波 swTorqCmd = (SWORD)(((SLONG)swTorqCmd1 * ass_stCalCoef.swSmoothGain) >> 12); //转矩指令斜坡 if (swTorqCmd > ass_stParaCong.uwBikeAssTorMaxPu) // 最大转矩限幅 { swTorqCmd = (SWORD)ass_stParaCong.uwBikeAssTorMaxPu; } /* Assist torque Cal using Assist Curve */ slTeTorAssitTmpPu = (SLONG)(ass_slPolynomial(&ass_stCalCoef.uwTorqueAssGain[ass_stCalIn.uwGearSt], &swTorqCmd, 14)); // Q14 转矩助力曲线 if(ass_stCalIn.uwGearSt == 5) { slTeTorAssitLinerPu = (((SLONG)swTorqCmd * LinerAssist[ass_stCalIn.uwGearSt-1] )>> 12) + 273; // Q14 转矩助力曲线线性段 } else { slTeTorAssitLinerPu = (((SLONG)swTorqCmd * LinerAssist[ass_stCalIn.uwGearSt-1] )>> 12) + 273; } if (slTeTorAssitTmpPu < slTeTorAssitLinerPu) { slTeTorAssitTmpPu = slTeTorAssitLinerPu; } else { //do nothing; } swCadCmd = (SWORD)((((SLONG)ass_stCalIn.uwcadance * ass_stCalCoef.swSmoothGain) >> 12) * 10); // 踏频指令斜坡 slTeCadAssitTmpPu = ((SLONG)(ass_slPolynomial(&ass_stCalCoef.uwCadencAsseGain[ass_stCalIn.uwGearSt], &swCadCmd, 20))) >> 6; // Q20 - Q6 = Q14 //踏频助力曲线 if (slTeTorAssitTmpPu > ass_stParaCong.uwBikeAssTorMaxPu) // 最大转矩限幅 { slTeTorAssitTmpPu = ass_stParaCong.uwBikeAssTorMaxPu; } if (slTeCadAssitTmpPu > ass_stParaCong.uwBikeAssTorMaxPu) // 最大转矩限幅 { slTeCadAssitTmpPu = ass_stParaCong.uwBikeAssTorMaxPu; } /* Select Assist Percent of Torq and Candence*/ swTeTorAssitPu1 = (SWORD)(slTeTorAssitTmpPu * (SLONG)ass_stParaSet.uwTorAssAjstGain >> 12); // Q14+Q12-Q12 = Q14; 用户可设置转矩比例 swTeCadAssitPu1 = (SWORD)(slTeCadAssitTmpPu * (SLONG)ass_stParaSet.uwCadenceAssAjstGain >> 12); // Q14+Q12-Q12 = Q14; 用户可设置踏频比例 ass_stCalOut.swTorAssistSum1 = swTeTorAssitPu1 + swTeCadAssitPu1; // Q14 /* Candance Speed to Motor Speed*/ ass_stCalOut.swCadSpd2MotSpd = (SWORD)(((SLONG)ass_stCalIn.uwcadance * (SWORD)ass_stParaCong.uwMechRationMotor * (SWORD)ass_stParaCong.uwMotorPoles) >> 5); // Q20-Q5= Q15 出力时电机转速计算 ass_stCalCoef.uwCadencePeriodCNT = (UWORD)(TIME_MS2CNT(((ULONG)1000 << 20) / ((ULONG)ass_stCalIn.uwcadance * FBASE))); //一圈踏频时间计数 /* Back EMF Cal */ swTmpVoltPu = (SWORD)((SLONG)ass_stCalOut.swCadSpd2MotSpd * (SLONG)cof_uwFluxPu >> 13);//Q15+Q12-Q13=Q14; swTmpVoltPu2 = (SWORD)((SLONG)ass_stCalIn.uwSpdFbkAbsPu* (SLONG)cof_uwFluxPu >> 13);//Q15+Q12-Q13=Q14; if (swTmpVoltPu < swTmpVoltPu2) { swTmpVoltPu = swTmpVoltPu2; } ass_stCalCoef.uwStartupGain = ass_stParaSet.uwStartupCoef ; //零速启动助力比计算 ass_stCalCoef.uwStartupCruiseGain = ass_stParaSet.uwStartupCruiseCoef ; //带速启动助力比计算 /* Assist FSM Control */ switch (Ass_FSM) { case Startup: // ass_stCalCoef.swSmoothGain = Q12_1; ass_stCalCoef.swSmoothGain += (SWORD)ass_stParaSet.uwSpeedAssistIMaxA; ////ass_stCalCoef.uwStartUpGainAddStep; if(ass_stCalCoef.swSmoothGain >= Q12_1) { ass_stCalCoef.swSmoothGain = Q12_1; } swSpdKpPu = 1000; //ass_stParaSet.uwStartUpCadNm; slSpdErr = (SLONG)ass_stCalOut.swCadSpd2MotSpd - (SLONG)ass_stCalIn.uwSpdFbkAbsPu; if(slSpdErr < 0) { slSpdErr = 0; } // ass_stCalCoef.StartFlag = 1; /* Open Voltage Limit according SpdErr*/ if(ass_stCalCoef.StartFlag == 0) { slTmpVoltLim= ((slSpdErr * swSpdKpPu )>> 11) + swTmpVoltPu; if(slTmpVoltLim > scm_swVsDcpLimPu) { slTmpVoltLim = scm_swVsDcpLimPu; } else if(slTmpVoltLim <= swTmpVoltPu) { slTmpVoltLim = swTmpVoltPu; } else { //do nothing } ass_stCalOut.swVoltLimitPu = (SWORD)slTmpVoltLim; if(slSpdErr <= 1000) { ass_stCalCoef.StartFlag = 1; } // if(ABS(scm_swIqRefPu- scm_swIqFdbLpfPu) > 200) // { // ass_pvt_swVoltCnt++; // } // else // { // ass_pvt_swVoltCnt=0; // } // if(ass_pvt_swVoltCnt > 10) // { // ass_pvt_swVoltCnt=0; // ass_stCalCoef.StartFlag = 1; // } } else if(ass_stCalCoef.StartFlag ==1 ) { if(0 == (AssCnt1ms%5)) { ass_stCalOut.swVoltLimitPu += ass_stCalCoef.uwStartUpGainAddStep; if (ass_stCalOut.swVoltLimitPu > scm_swVsDcpLimPu) { ass_stCalOut.swVoltLimitPu = scm_swVsDcpLimPu; } } if(slSpdErr <= 100) { ass_pvt_swVoltCnt++; } else { ass_pvt_swVoltCnt--; if(ass_pvt_swVoltCnt < 0) { ass_pvt_swVoltCnt = 0; } } /* Switch to TorqueAssit FSM */ if(ass_pvt_swVoltCnt > 30) { Ass_FSM = TorqueAssit; ass_stCalCoef.StartFlag=0; ass_pvt_swVoltCnt=0; } } else { //do nothing } /* Switch to ReduceCurrent FSM */ if((ass_stCalIn.uwcadancePer == 0) || (ass_stCalIn.uwGearSt == 0) || BikeBrake_blGetstate() == TRUE) { /* When CandanceFreq=0 or BikeGear=0*/ ass_stCalCoef.swAss2SpdCNT = 0; Ass_FSM = ReduceCurrent; } else if(ass_stCalIn.uwtorquePer <= (ass_stCalCoef.uwAssStopThreshold)) { ass_stCalCoef.swAss2SpdCNT++; uwTmpStopCnt = ass_stCalIn.uwcadance;//((ULONG)1000<<20)/(ass_stCalIn.uwcadance * FBASE) ; if(uwTmpStopCnt < 200) { uwTmpStopCnt = 200; } else if(uwTmpStopCnt > 500) { uwTmpStopCnt = 500; } else { //do nothing } if(ass_stCalCoef.swAss2SpdCNT > uwTmpStopCnt) { Ass_FSM = ReduceCurrent; ass_stCalCoef.swAss2SpdCNT = 0; ass_stCalCoef.StartFlag=0; } } else { ass_stCalCoef.swAss2SpdCNT = 0; } break; case TorqueAssit: /* 启动系数 */ ass_stCalCoef.swSmoothGain += (SWORD)ass_stParaSet.uwSpeedAssistIMaxA; ////ass_stCalCoef.uwStartUpGainAddStep; if(ass_stCalCoef.swSmoothGain >= Q12_1) { ass_stCalCoef.swSmoothGain = Q12_1; } /* Reduce Voltage Limit When LPFTorq < Switch1TorqThreshold */ if(0 == (AssCnt1ms%5)) { // if(ass_stCalIn.uwtorque >= ass_stCalCoef.uwSwitch1TorqThreshold) // { ass_stCalOut.swVoltLimitPu += ass_stCalCoef.uwStartUpGainAddStep; if (ass_stCalOut.swVoltLimitPu > scm_swVsDcpLimPu) { ass_stCalOut.swVoltLimitPu = scm_swVsDcpLimPu; } // } // else if (ass_stCalIn.uwtorque <= ass_stCalCoef.uwSwitch1TorqThreshold) // { //// ass_stCalOut.swVoltLimitPu -= ass_stCalCoef.uwSpeedConstantCommand; //// if (ass_stCalOut.swVoltLimitPu <= (tmpVoltargetPu + ass_ParaSet.uwStartUpCadNm)) //// { //// ass_stCalOut.swVoltLimitPu = tmpVoltargetPu + ass_ParaSet.uwStartUpCadNm; //// } // } // else // { // } } /* TorqueRef Select Coef */ ass_stCalCoef.swTorqFilterGain += 4; // Q14 转矩滤波方式切换系数 if (ass_stCalCoef.swTorqFilterGain > Q14_1) { ass_stCalCoef.swTorqFilterGain = Q14_1; } /* Switch to ReduceCurrent FSM */ if((ass_stCalIn.uwcadancePer == 0) || (ass_stCalIn.uwGearSt == 0) || BikeBrake_blGetstate() == TRUE) { /* When CandanceFreq=0 or BikeGear=0*/ ass_stCalOut.blTorqPIFlg = FALSE; ass_stCalCoef.swAss2SpdCNT = 0; Ass_FSM = ReduceCurrent; } else if(ass_stCalIn.uwtorquePer <= (ass_stCalCoef.uwAssStopThreshold)) { ass_stCalCoef.swAss2SpdCNT++; uwTmpStopCnt = ass_stCalIn.uwcadance;//((ULONG)1000<<20)/(ass_stCalIn.uwcadance * FBASE) ; if(uwTmpStopCnt < 200) { uwTmpStopCnt = 200; } else if(uwTmpStopCnt > 500) { uwTmpStopCnt = 500; } else { //do nothing } if(ass_stCalCoef.swAss2SpdCNT > uwTmpStopCnt) { ass_stCalCoef.swAss2SpdCNT = 0; ass_stCalOut.blTorqPIFlg = FALSE; Ass_FSM = ReduceCurrent; } } else { ass_stCalCoef.swAss2SpdCNT = 0; } break; case ReduceCurrent: /* Switch to StopAssit FSM */ if(ass_stCalCoef.swSmoothGain <= 0) { ass_stCalCoef.swSmoothGain = 0; ass_stCalCoef.swTorqFilterGain = 0; ass_stCalCoef.swCadanceGain = 0; Ass_FSM = StopAssit; } else { /* Reduce Curret Coef to Zero*/ ass_stCalCoef.swSmoothGain -=40; ass_stCalOut.swVoltLimitPu = scm_swVsDcpLimPu; } /* Switch to Startup FSM */ // if (ass_stCalIn.uwtorquePer > ((ass_stCalCoef.uwAssThreshold * 3)>>3) && ass_stCalIn.uwcadance > 0) // { // Ass_FSM = Startup; // ass_stCalOut.swSpeedRef = ass_stCalIn.uwSpdFbkAbsPu; // ass_stCalOut.swVoltLimitPu = scm_swVsDcpLimPu; // } break; case StopAssit: ass_stCalOut.swTorSpdLoopCurrentTemp = 0; /* Switch to Startup FSM */ if ((BikeBrake_blGetstate() == FALSE) && (ass_stCalIn.uwGearSt > 0)) { if (ass_stCalIn.uwbikespeed < 449) // 0.3Hz, (2.19m轮径下 2.36km/h ) { if (ass_stCalIn.uwtorquePer > ass_stCalCoef.uwAssThreshold && ass_stCalIn.uwcadance > 0) { ass_stCalCoef.sw2StopCNT = 0; ass_stCalOut.swVoltLimitPu = scm_swVsDcpLimPu; ass_pvt_stCurLpf.slY.sw.hi = 0; Ass_FSM = Startup; } } else { if (ass_stCalIn.uwtorquelpf > ((ass_stCalCoef.uwAssThreshold * 3)>>3) && ass_stCalIn.uwtorquePer > ass_stCalCoef.uwAssThreshold && ass_stCalIn.uwcadance > 0) { ass_stCalCoef.sw2StopCNT = 0; ass_stCalOut.swVoltLimitPu = scm_swVsDcpLimPu; ass_pvt_stCurLpf.slY.sw.hi = 0; Ass_FSM = Startup; } } } /* Assit Exit */ if (ass_stCalIn.uwcadance == 0 || ass_stCalIn.uwtorquelpf < ass_stCalCoef.uwAssStopThreshold) { ass_stCalCoef.sw2StopCNT++; } else { if (ass_stCalCoef.sw2StopCNT >= 1) { ass_stCalCoef.sw2StopCNT--; } } if ((ass_stCalCoef.sw2StopCNT > TIME_MS2CNT(3000)) || (ass_stCalIn.uwGearSt == 0) || (BikeBrake_blGetstate() == TRUE))// 3s { ass_stCalCoef.sw2StopCNT = 0; ass_stCalCoef.blAssistflag = FALSE; } break; default: break; } /* Bikespeed Limit */ if (ass_stCalIn.uwbikespeed <= ass_stCurLimCoef.uwBikeSpdThresHold1) { ass_stCalCoef.swBikeSpeedGain = Q12_1; // Q12 } else if (ass_stCalIn.uwbikespeed > ass_stCurLimCoef.uwBikeSpdThresHold1 && ass_stCalIn.uwbikespeed <= ass_stCurLimCoef.uwBikeSpdThresHold2) { ass_stCalCoef.swBikeSpeedGain = (SWORD)(Q12_1 - ((((SQWORD)ass_stCalIn.uwbikespeed - (SQWORD)ass_stCurLimCoef.uwBikeSpdThresHold1) * (SQWORD)ass_stCurLimCoef.ulBikeSpdDeltInv) >> 8)); // Q12 uwTorqAccStep = 10; uwTorqDecStep = 10; } else { ass_stCalCoef.swBikeSpeedGain = 0; uwTorqAccStep = 10; uwTorqDecStep = 10; } /* Assist Current Output in each FSM */ switch (Ass_FSM) { case Startup: swTeTorAssitPu2 = swTeTorAssitPu1 ; // Q14+Q12-Q12+Q12-Q12=Q14 swTeCadAssitPu2 = swTeCadAssitPu1 ; // Q14+Q12-Q12+Q12-Q12=Q14 ass_stCalOut.swTorAss2CurrentTemp = ass_swTorq2CurPu(swTeTorAssitPu2); // Q14 电流指令计算 ass_stCalOut.swCadAss2CurrentTemp = ass_swTorq2CurPu(swTeCadAssitPu2); // Q14 电流指令计算 if (ass_stCalOut.swTorAss2CurrentTemp > ass_stCalCoef.swCurrentmax_torAssPu) { ass_stCalOut.swTorAss2CurrentTemp = ass_stCalCoef.swCurrentmax_torAssPu; } if (ass_stCalOut.swCadAss2CurrentTemp > ass_stCalCoef.swCurrentmax_cadAssPu) { ass_stCalOut.swCadAss2CurrentTemp = ass_stCalCoef.swCurrentmax_cadAssPu; } ass_stCalOut.swTorRefTarget = ass_stCalOut.swTorAss2CurrentTemp + ass_stCalOut.swCadAss2CurrentTemp; ass_stCalOut.swTorRefEnd = ass_stCalOut.swTorRefTarget; ass_stCalOut.swTorAssistCurrentTemp = ass_stCalIn.swDirection *ass_stCalOut.swTorRefEnd; break; case TorqueAssit: swTeTorAssitPu2 = swTeTorAssitPu1 ; // Q14+Q12-Q12+Q12-Q12=Q14 swTeCadAssitPu2 = swTeCadAssitPu1 ; // Q14+Q12-Q12+Q12-Q12=Q14 ass_stCalOut.swTorAss2CurrentTemp = ass_swTorq2CurPu(swTeTorAssitPu2); // Q14 电流指令计算 ass_stCalOut.swCadAss2CurrentTemp = ass_swTorq2CurPu(swTeCadAssitPu2); // Q14 电流指令计算 if (ass_stCalOut.swTorAss2CurrentTemp > ass_stCalCoef.swCurrentmax_torAssPu) { ass_stCalOut.swTorAss2CurrentTemp = ass_stCalCoef.swCurrentmax_torAssPu; } if (ass_stCalOut.swCadAss2CurrentTemp > ass_stCalCoef.swCurrentmax_cadAssPu) { ass_stCalOut.swCadAss2CurrentTemp = ass_stCalCoef.swCurrentmax_cadAssPu; } #if CURSWITCH /* Ajust CurrentRef growth and decline rate */ ass_stCalOut.swTorRefTarget = ass_stCalOut.swTorAss2CurrentTemp + ass_stCalOut.swCadAss2CurrentTemp; if((ass_stCalOut.swTorRefTarget - ass_stCalOut.swTorRefEnd) > 2) { ass_pvt_uwTorqAccCnt++; if(ass_pvt_uwTorqAccCnt >= 2) { ass_stCalOut.swTorRefEnd += (SWORD)uwTorqAccStep; ass_pvt_uwTorqAccCnt = 0; } } else if((ass_stCalOut.swTorRefTarget - ass_stCalOut.swTorRefEnd) < -1) { if (ass_stCalIn.uwcadance != ass_stCalIn.uwcadancelast) { ass_stCalOut.swTorRefEnd -= (SWORD)uwTorqDecStep; } // ass_pvt_uwTorqDecCnt++; // if(ass_pvt_uwTorqDecCnt >= 10) // { // ass_stCalOut.swTorRefEnd += uwTorqAccStep; // ass_pvt_uwTorqDecCnt = 0; // } } else { ass_stCalOut.swTorRefEnd = ass_stCalOut.swTorRefTarget; } ass_stCalOut.swTorAssistCurrentTemp = ass_stCalIn.swDirection * ass_stCalOut.swTorRefEnd; /* Torq Clzloop Test */ // if(ass_stCalIn.uwtorquelpf <= ass_stCalCoef.uwSwitch1TorqThreshold) // { // if(!ass_stCalOut.blTorqPIFlg) // { // /* Initial value */ // ass_stTorqPIOut.slIRefPu = 0; // swCurSwitch = ABS(ass_stCalOut.swTorRefTarget); //ABS(ass_stCalOut.swAssitCurRef); // ass_stCalOut.blTorqPIFlg = TRUE; // } // // ass_stTorqPIIn.swTorqRefPu = ass_stCalIn.uwtorquelpf ; //torsensor_test_Lpf.slY.sw.hi ; //ass_stCalIn.uwtorque; // ass_stTorqPIIn.swTorqFdbPu = ass_stCalCoef.uwSwitch1TorqThreshold; // ass_stTorqPIIn.swImaxPu = 0; // ass_stTorqPIIn.swIminPu = -swCurSwitch; // ass_voAssitTorqPI(&ass_stTorqPIIn,&ass_stTorqPIOut); // ass_stCalOut.swTorAssistCurrentTemp = ass_stCalIn.swDirection *(swCurSwitch + ass_stUqLimMafValue.slAverValue); // } // else // { // ass_stCalOut.blTorqPIFlg = FALSE; // ass_stCalOut.swTorAssistCurrentTemp = ass_stCalIn.swDirection * ass_stCalOut.swTorRefEnd; // } #else ass_stCalOut.swTorAssistCurrentTemp = ass_stCalIn.swDirection *(ass_stCalOut.swTorAss2CurrentTemp + ass_stCalOut.swCadAss2CurrentTemp); #endif break; case ReduceCurrent: swTeTorAssitPu2 = swTeTorAssitPu1; // Q14+Q12-Q12+Q12-Q12=Q14 swTeCadAssitPu2 = swTeCadAssitPu1; // Q14+Q12-Q12+Q12-Q12=Q14 ass_stCalOut.swTorAss2CurrentTemp = ass_swTorq2CurPu(swTeTorAssitPu2); // Q14 电流指令计算 ass_stCalOut.swCadAss2CurrentTemp = ass_swTorq2CurPu(swTeCadAssitPu2); // Q14 电流指令计算 if (ass_stCalOut.swTorAss2CurrentTemp > ass_stCalCoef.swCurrentmax_torAssPu) { ass_stCalOut.swTorAss2CurrentTemp = ass_stCalCoef.swCurrentmax_torAssPu; } if (ass_stCalOut.swCadAss2CurrentTemp > ass_stCalCoef.swCurrentmax_cadAssPu) { ass_stCalOut.swCadAss2CurrentTemp = ass_stCalCoef.swCurrentmax_cadAssPu; } ass_stCalOut.swTorAssistCurrentTemp = ass_stCalIn.swDirection *(ass_stCalOut.swTorAss2CurrentTemp + ass_stCalOut.swCadAss2CurrentTemp); break; case StopAssit: ass_stCalOut.swTorAssistCurrentTemp = 0; ass_stCalOut.swTorRefEnd = 0; break; default: break; } /* Assist Iqref Output */ ass_stCalOut.swTorAssistCurrent = ass_stCalOut.swTorAssistCurrentTemp; mth_voLPFilter(ass_stCalOut.swTorAssistCurrent, &ass_pvt_stCurLpf); /* Bikespeed Limit Coef*/ ass_stCalOut.swAssitCurRef = (SWORD)((SLONG)ass_pvt_stCurLpf.slY.sw.hi * ass_stCalCoef.swBikeSpeedGain >> 12); //ass_stCalOut.swAssitCurRef =ass_stCalOut.swTorAssistCurrent; } /** * @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 void ass_voAssitCurLim(UWORD gear, UWORD uwBikeSpeedHzPu, UWORD uwCurMaxPu) { /* Limit the Output Current according to Bike Gear */ UWORD uwIqLimitTemp1; if(gear > 5) { gear = 5; } uwIqLimitTemp1 = (UWORD)(((ULONG)ass_stCurLimCoef.uwLimitGain[gear] * uwCurMaxPu) >> 10); ass_stCurLimOut.uwIqlimit = uwIqLimitTemp1; } /** * @brief * * @param * @return */ static void ass_voAssistCurLimBMS(UWORD uwSOCvalue) { /* Limit the Output Current according to Bike SOC */ if (uwSOCvalue < ass_stCurLimCalBMSCoef.uwIqLimitStartSoc && uwSOCvalue > ass_stCurLimCalBMSCoef.uwIqLimitEndSoc) { ass_stCurLimitCalBMSOut.uwIqLimitAbs = ass_stCurLimCalBMSCoef.uwIqLimitInitAbs - ((ass_stCurLimCalBMSCoef.uwIqLimitStartSoc - uwSOCvalue) * ass_stCurLimCalBMSCoef.swIqLImitK); } else if (uwSOCvalue <= ass_stCurLimCalBMSCoef.uwIqLimitEndSoc) { ass_stCurLimitCalBMSOut.uwIqLimitAbs = 0; } else { ass_stCurLimitCalBMSOut.uwIqLimitAbs = ass_stCurLimCalBMSCoef.uwIqLimitInitAbs; } } /** * @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 */ void ass_voAssist(void) { /* Start Assist Jduge */ if ((ass_stCalIn.uwtorquePer > ass_stCalCoef.uwAssThreshold && ass_stCalIn.uwcadancePer > 0) && (ass_stCalIn.uwGearSt > 0 && ass_stCalIn.uwGearSt != 0x22)) { ass_stCalCoef.blAssistflag = TRUE; } if (ass_stCalCoef.blAssistflag == TRUE) { /* Calculate Iqref Limit */ UWORD uwIqLimitTemp; ass_voAssitCurLim(ass_stCalIn.uwGearSt, ass_stCalIn.uwbikespeed, ass_stParaCong.uwCofCurMaxPu); ass_voAssistCurLimBMS(ass_stCalIn.SOCValue); uwIqLimitTemp = (ass_stCurLimOut.uwIqlimit < ass_stCalIn.swFlxIqLimit) ? (ass_stCurLimOut.uwIqlimit < ass_stCalIn.swPwrIqLimit ? ass_stCurLimOut.uwIqlimit : ass_stCalIn.swPwrIqLimit) : (ass_stCalIn.swFlxIqLimit < ass_stCalIn.swPwrIqLimit ? ass_stCalIn.swFlxIqLimit : ass_stCalIn.swPwrIqLimit); ass_stCalCoef.uwCurrentMaxPu = (uwIqLimitTemp < ass_stCurLimitCalBMSOut.uwIqLimitAbs) ? uwIqLimitTemp : ass_stCurLimitCalBMSOut.uwIqLimitAbs; ass_stCalCoef.swCurrentmax_torAssPu = (SWORD)(((SLONG)ass_stCalCoef.uwCurrentMaxPu * (SWORD)ass_stParaSet.uwTorWeight) >> 12); // Q14 ass_stCalCoef.swCurrentmax_cadAssPu = (SWORD)(((SLONG)ass_stCalCoef.uwCurrentMaxPu * (SWORD)ass_stParaSet.uwCadenceWeight) >> 12); /* Calculate Assist Current, Iqref*/ AssitCuvApplPerVolt(); /* Iqref Limit */ if (ass_stCalOut.swAssitCurRef > (SWORD)ass_stCalCoef.uwCurrentMaxPu) { ass_stCalOut.swAssitCurRef = (SWORD)ass_stCalCoef.uwCurrentMaxPu; } else if(ass_stCalOut.swAssitCurRef < -(SWORD)ass_stCalCoef.uwCurrentMaxPu) { ass_stCalOut.swAssitCurRef = -(SWORD)ass_stCalCoef.uwCurrentMaxPu; } else { //do nothing } } else { ass_stCalOut.swAssitCurRef = 0; } } /** * @brief * * @param * @return */ void ass_voMoveAverageFilter(MAF_IN *in) { in->slSum -= in->swBuffer[in->uwIndex]; in->swBuffer[in->uwIndex] = in->swValue; in->slSum += (SLONG)in->swValue; if (!in->blSecFlag) { in->slAverValue = in->slSum / ((SLONG)in->uwIndex + (SWORD)1); } else { in->slAverValue = in->slSum / (SLONG)in->uwLength; } in->uwIndex++; if (in->uwIndex >= in->uwLength) { in->blSecFlag = TRUE; in->uwIndex = 0; } } void ass_voMoveAverageFilterClear(MAF_IN *in) { UWORD i; in->uwIndex = 0; in->slSum = 0; in->blSecFlag = FALSE; for (i = 0; i < 64; i++) { in->swBuffer[i] = 0; } }