|
|
@@ -288,9 +288,9 @@ void AssitCoefInit(void)
|
|
|
ass_CalOut.swCadSpd2MotSpd = 0;
|
|
|
|
|
|
ass_CurLimCoef.uwLimitGain[0] = 0; // Q10 percentage of max Current
|
|
|
- ass_CurLimCoef.uwLimitGain[1] = 716;
|
|
|
- ass_CurLimCoef.uwLimitGain[2] = 870;
|
|
|
- ass_CurLimCoef.uwLimitGain[3] = 1024;
|
|
|
+ ass_CurLimCoef.uwLimitGain[1] = 400;
|
|
|
+ ass_CurLimCoef.uwLimitGain[2] = 682;
|
|
|
+ ass_CurLimCoef.uwLimitGain[3] = 910;
|
|
|
ass_CurLimCoef.uwLimitGain[4] = 1024;
|
|
|
ass_CurLimCoef.uwLimitGain[5] = 1024;
|
|
|
ass_CurLimCoef.uwSpdThresHold = 21845;
|
|
|
@@ -319,6 +319,8 @@ void AssitCoefInit(void)
|
|
|
|
|
|
mth_voLPFilterCoef(1000000 / 25, EVENT_1MS_HZ, &ass_pvt_stCurLpf.uwKx); //100Hz
|
|
|
ass_pvt_stCurLpf.slY.sl = 0;
|
|
|
+
|
|
|
+
|
|
|
}
|
|
|
|
|
|
/**
|
|
|
@@ -342,10 +344,37 @@ ORIG_COEF Stop_Orig_Coef = {-100, 0, 0};
|
|
|
SLONG TempSmooth;
|
|
|
SWORD TorqCmd1, TorqCmd, CadCmd;
|
|
|
SLONG temp_a1, temp_b1, temp_c1;
|
|
|
+ UWORD TorqueAccStep=0;
|
|
|
+ UWORD TorqueDecStep=80;
|
|
|
+ UWORD TorquAccCnt=0,TorquDecCnt=0;
|
|
|
SWORD AssitCuvApplPer(void)
|
|
|
{
|
|
|
- /////////////////////////// Assist torque Cal using Assist Curve ////////////////////////////////
|
|
|
+ /////////////////////////// Assist torque Cal using Assist Curve ////////////////////////////////
|
|
|
+ if (ass_CalIn.uwGearSt == 1)
|
|
|
+ {
|
|
|
+ TorqueAccStep = 50;
|
|
|
+ }
|
|
|
+ else if(ass_CalIn.uwGearSt == 2)
|
|
|
+ {
|
|
|
+ TorqueAccStep = 100;
|
|
|
+ }
|
|
|
+ else if(ass_CalIn.uwGearSt == 3)
|
|
|
+ {
|
|
|
+ TorqueAccStep = 120;
|
|
|
+ }
|
|
|
+ else if(ass_CalIn.uwGearSt == 4)
|
|
|
+ {
|
|
|
+ TorqueAccStep = 150;
|
|
|
+ }
|
|
|
+ else if(ass_CalIn.uwGearSt == 5)
|
|
|
+ {
|
|
|
+ TorqueAccStep = 150;
|
|
|
+ }
|
|
|
+ else
|
|
|
+ {
|
|
|
|
|
|
+ }
|
|
|
+
|
|
|
TorqCmd1 = ((ULONG)ass_CalIn.uwtorque * ass_CalCoef.swTorqFilterGain >> 14) +
|
|
|
((ULONG)ass_CalIn.uwtorquelpf * (Q14_1 - ass_CalCoef.swTorqFilterGain) >> 14); //转矩指令滤波切换，由低通滤波到踏频相关的滑动平均滤波
|
|
|
TorqCmd = ((ULONG)TorqCmd1 * ass_CalCoef.swSmoothGain) >> 12; //转矩指令斜坡
|
|
|
@@ -364,8 +393,8 @@ SWORD AssitCuvApplPer(void)
|
|
|
{
|
|
|
//do nothing;
|
|
|
}
|
|
|
- // CadCmd = (((SLONG)ass_CalIn.uwcadance * ass_CalCoef.swCadanceGain) >> 12)*10; // 踏频指令斜坡
|
|
|
- CadCmd = (((SLONG)ass_CalIn.uwcadance * ass_CalCoef.swSmoothGain) >> 12)*10;
|
|
|
+
|
|
|
+ CadCmd = (((SLONG)ass_CalIn.uwcadance * ass_CalCoef.swSmoothGain) >> 12)*10; // 踏频指令斜坡
|
|
|
Te_Cad_Assit_tempPu = ((SLONG)(Polynomial(&ass_CalCoef.uwCadencAsseGain[ass_CalIn.uwGearSt], &CadCmd, 20))) >> 6; // Q20 - Q6 = Q14 //踏频助力曲线
|
|
|
|
|
|
if (Te_Tor_Assit_tempPu > ass_ParaCong.uwBikeAssTorMaxPu) // 最大转矩限幅
|
|
|
@@ -871,10 +900,11 @@ SWORD AssitCuvApplPer(void)
|
|
|
}
|
|
|
else
|
|
|
{
|
|
|
- if (ass_CalIn.uwtorquelpf > ((ass_CalCoef.uwAssThreshold * 5)>>3) && ass_CalIn.uwtorquePer > ass_CalCoef.uwAssThreshold && ass_CalIn.uwcadance > 0)
|
|
|
+ if (ass_CalIn.uwtorquelpf > ((ass_CalCoef.uwAssThreshold * 3)>>3) && ass_CalIn.uwtorquePer > ass_CalCoef.uwAssThreshold && ass_CalIn.uwcadance > 0)
|
|
|
{
|
|
|
// hw_voPWMOn();
|
|
|
ass_CalCoef.swTorqFilterGain = 0;
|
|
|
+ uwspeed2torqCnt = 0;
|
|
|
Ass_FSM = Spd2Torq;
|
|
|
ass_CalOut.swSpeedRef = ass_CalIn.uwSpdFbkAbsPu;
|
|
|
ass_CalCoef.sw2StopCNT = 0;
|
|
|
@@ -924,12 +954,15 @@ SWORD AssitCuvApplPer(void)
|
|
|
if (ass_CalIn.uwbikespeed <= ass_CurLimCoef.uwBikeSpdThresHold1)
|
|
|
{
|
|
|
ass_CalCoef.swBikeSpeedGain = Q12_1; // Q12
|
|
|
+
|
|
|
}
|
|
|
else if (ass_CalIn.uwbikespeed > ass_CurLimCoef.uwBikeSpdThresHold1 && ass_CalIn.uwbikespeed <= ass_CurLimCoef.uwBikeSpdThresHold2)
|
|
|
{
|
|
|
ass_CalCoef.swBikeSpeedGain =
|
|
|
Q12_1 -
|
|
|
((((SQWORD)ass_CalIn.uwbikespeed - (SQWORD)ass_CurLimCoef.uwBikeSpdThresHold1) * (SQWORD)ass_CurLimCoef.ulBikeSpdDeltInv) >> 28); // Q12
|
|
|
+ TorqueAccStep = 10;
|
|
|
+ TorqueDecStep = 10;
|
|
|
}
|
|
|
else
|
|
|
{
|
|
|
@@ -939,8 +972,26 @@ SWORD AssitCuvApplPer(void)
|
|
|
switch (Ass_FSM)
|
|
|
{
|
|
|
case Startup:
|
|
|
+ Te_Tor_AssitPu2 = ((SLONG)Te_Tor_AssitPu1 * ass_CalCoef.swBikeSpeedGain) >> 12; // Q14+Q12-Q12+Q12-Q12=Q14
|
|
|
+ Te_Cad_AssitPu2 = ((SLONG)Te_Cad_AssitPu1 * ass_CalCoef.swBikeSpeedGain) >> 12; // Q14+Q12-Q12+Q12-Q12=Q14
|
|
|
+ ass_CalOut.swTorAss2CurrentTemp = swCurrentCal(Te_Tor_AssitPu2); // Q14 电流指令计算
|
|
|
+ ass_CalOut.swCadAss2CurrentTemp = swCurrentCal(Te_Cad_AssitPu2); // 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;
|
|
|
+ ass_CalOut.swTorRefEnd = ass_CalOut.swTorRefTarget;
|
|
|
+ ass_CalOut.swTorAssistCurrentTemp = ass_CalIn.swDirection *ass_CalOut.swTorRefEnd;
|
|
|
+
|
|
|
+ break;
|
|
|
+
|
|
|
case TorqueAssit:
|
|
|
- case ReduceCurrent:
|
|
|
+
|
|
|
Te_Tor_AssitPu2 = ((SLONG)Te_Tor_AssitPu1 * ass_CalCoef.swBikeSpeedGain) >> 12; // Q14+Q12-Q12+Q12-Q12=Q14
|
|
|
Te_Cad_AssitPu2 = ((SLONG)Te_Cad_AssitPu1 * ass_CalCoef.swBikeSpeedGain) >> 12; // Q14+Q12-Q12+Q12-Q12=Q14
|
|
|
ass_CalOut.swTorAss2CurrentTemp = swCurrentCal(Te_Tor_AssitPu2); // Q14 电流指令计算
|
|
|
@@ -953,7 +1004,38 @@ SWORD AssitCuvApplPer(void)
|
|
|
{
|
|
|
ass_CalOut.swCadAss2CurrentTemp = ass_CalCoef.swCurrentmax_cadAssPu;
|
|
|
}
|
|
|
+#if CURSWITCH
|
|
|
+ ass_CalOut.swTorRefTarget = ass_CalOut.swTorAss2CurrentTemp + ass_CalOut.swCadAss2CurrentTemp;
|
|
|
+
|
|
|
+ if((ass_CalOut.swTorRefTarget - ass_CalOut.swTorRefEnd) > 2)
|
|
|
+ {
|
|
|
+ TorquAccCnt++;
|
|
|
+ if(TorquAccCnt >= 20)
|
|
|
+ {
|
|
|
+ ass_CalOut.swTorRefEnd += TorqueAccStep;
|
|
|
+ TorquAccCnt = 0;
|
|
|
+ }
|
|
|
+ }
|
|
|
+ else if(((ass_CalOut.swTorRefTarget - ass_CalOut.swTorRefEnd) < (-1)))
|
|
|
+ {
|
|
|
+ if (ass_CalIn.uwcadance != ass_CalIn.uwcadancelast)
|
|
|
+ {
|
|
|
+
|
|
|
+ ass_CalOut.swTorRefEnd -= TorqueDecStep;
|
|
|
+
|
|
|
+ }
|
|
|
+
|
|
|
+ }
|
|
|
+ else
|
|
|
+ {
|
|
|
+ ass_CalOut.swTorRefEnd = ass_CalOut.swTorRefTarget;
|
|
|
+ }
|
|
|
+ ass_CalOut.swTorAssistCurrentTemp = ass_CalIn.swDirection * ass_CalOut.swTorRefEnd;
|
|
|
+
|
|
|
+#else
|
|
|
+
|
|
|
ass_CalOut.swTorAssistCurrentTemp = ass_CalIn.swDirection *(ass_CalOut.swTorAss2CurrentTemp + ass_CalOut.swCadAss2CurrentTemp);
|
|
|
+#endif
|
|
|
break;
|
|
|
|
|
|
case StartupCruise:
|
|
|
@@ -969,6 +1051,43 @@ SWORD AssitCuvApplPer(void)
|
|
|
{
|
|
|
ass_CalOut.swCadAss2CurrentTemp = ass_CalCoef.swCurrentmax_cadAssPu;
|
|
|
}
|
|
|
+
|
|
|
+#if CURSWITCH
|
|
|
+
|
|
|
+ ass_CalOut.swTorRefTarget = ass_CalOut.swTorAss2CurrentTemp + ass_CalOut.swCadAss2CurrentTemp;
|
|
|
+ if((ass_CalOut.swTorRefTarget - ass_CalOut.swTorRefEnd) > 2)
|
|
|
+ {
|
|
|
+ TorquAccCnt++;
|
|
|
+ if(TorquAccCnt >= 20)
|
|
|
+ {
|
|
|
+ ass_CalOut.swTorRefEnd += TorqueAccStep;
|
|
|
+ TorquAccCnt = 0;
|
|
|
+ }
|
|
|
+ }
|
|
|
+ else if(((ass_CalOut.swTorRefTarget - ass_CalOut.swTorRefEnd) < (-1)))
|
|
|
+ {
|
|
|
+ if (ass_CalIn.uwcadance != ass_CalIn.uwcadancelast)
|
|
|
+ {
|
|
|
+ ass_CalOut.swTorRefEnd -= TorqueDecStep;
|
|
|
+ }
|
|
|
+ }
|
|
|
+ else
|
|
|
+ {
|
|
|
+ ass_CalOut.swTorRefEnd = ass_CalOut.swTorRefTarget;
|
|
|
+ }
|
|
|
+
|
|
|
+ if(ass_CalOut.swTorRefEnd < ass_CalOut.swTorSpdLoopCurrentTemp)
|
|
|
+ {
|
|
|
+ ass_CalOut.swTorRefEnd = ass_CalOut.swTorSpdLoopCurrentTemp;
|
|
|
+ //ass_CalOut.swTorSpdLoopCurrentTemp = 0; // 启动前电流最小为速度环电流，启动后最小电流为0
|
|
|
+ }
|
|
|
+ else
|
|
|
+ {
|
|
|
+ ass_CalOut.swTorRefEnd = ass_CalOut.swTorRefEnd;
|
|
|
+ }
|
|
|
+ ass_CalOut.swTorAssistCurrentTemp = ass_CalIn.swDirection * ass_CalOut.swTorRefEnd;
|
|
|
+
|
|
|
+#else
|
|
|
ass_CalOut.swTorAssistCurrentTemp = ass_CalOut.swTorAss2CurrentTemp + ass_CalOut.swCadAss2CurrentTemp;
|
|
|
if(ass_CalOut.swTorAssistCurrentTemp < ass_CalOut.swTorSpdLoopCurrentTemp)
|
|
|
{
|
|
|
@@ -979,16 +1098,34 @@ SWORD AssitCuvApplPer(void)
|
|
|
{
|
|
|
ass_CalOut.swTorAssistCurrentTemp = ass_CalIn.swDirection * ass_CalOut.swTorAssistCurrentTemp;
|
|
|
}
|
|
|
-
|
|
|
- break;
|
|
|
+#endif
|
|
|
+
|
|
|
+ break;
|
|
|
+ case ReduceCurrent:
|
|
|
+ Te_Tor_AssitPu2 = ((SLONG)Te_Tor_AssitPu1 * ass_CalCoef.swBikeSpeedGain) >> 12; // Q14+Q12-Q12+Q12-Q12=Q14
|
|
|
+ Te_Cad_AssitPu2 = ((SLONG)Te_Cad_AssitPu1 * ass_CalCoef.swBikeSpeedGain) >> 12; // Q14+Q12-Q12+Q12-Q12=Q14
|
|
|
+ ass_CalOut.swTorAss2CurrentTemp = swCurrentCal(Te_Tor_AssitPu2); // Q14 电流指令计算
|
|
|
+ ass_CalOut.swCadAss2CurrentTemp = swCurrentCal(Te_Cad_AssitPu2); // 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.swTorAssistCurrentTemp = ass_CalIn.swDirection *(ass_CalOut.swTorAss2CurrentTemp + ass_CalOut.swCadAss2CurrentTemp);
|
|
|
+ break;
|
|
|
case SpeedAssit:
|
|
|
ass_CalOut.swTorAssistCurrentTemp = asr_stTorqSpdPIOut.swIqRefPu; // ass_CalOut.swTorSpdLoopCurrentTemp;
|
|
|
break;
|
|
|
case Spd2Torq:
|
|
|
ass_CalOut.swTorAssistCurrentTemp = asr_stTorqSpdPIOut.swIqRefPu; // ass_CalOut.swTorSpdLoopCurrentTemp;
|
|
|
+ ass_CalOut.swTorRefEnd = ass_CalOut.swTorAssistCurrentTemp;
|
|
|
break;
|
|
|
case StopAssit:
|
|
|
ass_CalOut.swTorAssistCurrentTemp = 0;
|
|
|
+ ass_CalOut.swTorRefEnd = 0;
|
|
|
break;
|
|
|
default:
|
|
|
break;
|
|
|
@@ -1002,156 +1139,954 @@ SWORD AssitCuvApplPer(void)
|
|
|
return Assist_torqueper;
|
|
|
}
|
|
|
|
|
|
-/**
|
|
|
- * @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;
|
|
|
-
|
|
|
- uwIqLimitTemp1 = ((ULONG)ass_CurLimCoef.uwLimitGain[gear] * uwCurMaxPu) >> 10;
|
|
|
|
|
|
- ass_CurLimOut.uwIqlimit = uwIqLimitTemp1;
|
|
|
-}
|
|
|
+SWORD tmpVoltargetPu,VoltCnt;
|
|
|
+UWORD tempVoltage;
|
|
|
|
|
|
-/**
|
|
|
- * @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 Assist(void)
|
|
|
+ SLONG slSpderror,sltmpvoltagelimit;
|
|
|
+ SWORD SpdKp = 500; //Q10;
|
|
|
+SWORD AssitCuvApplPerVolt(void)
|
|
|
{
|
|
|
-
|
|
|
- if (((ass_CalIn.uwtorquePer > ass_CalCoef.uwAssThreshold && ass_CalIn.uwcadancePer > 0) || ass_CalIn.uwtorquePer > 3000) && ass_CalIn.uwGearSt > 0)
|
|
|
+ /////////////////////////// Assist torque Cal using Assist Curve ////////////////////////////////
|
|
|
+ if (ass_CalIn.uwGearSt == 1)
|
|
|
{
|
|
|
- ass_CalCoef.blAssistflag = TRUE;
|
|
|
+ TorqueAccStep = 50;
|
|
|
}
|
|
|
- if (ass_CalCoef.blAssistflag == TRUE)
|
|
|
+ else if(ass_CalIn.uwGearSt == 2)
|
|
|
{
|
|
|
- //////////// Calculate the Iq limit ///////////////////
|
|
|
- UWORD IqLimitTemp;
|
|
|
- AssitCuvLim(ass_CalIn.uwGearSt, ass_CalIn.uwbikespeed, ass_ParaCong.uwCofCurMaxPu);
|
|
|
- AssistCurrentLimitAccordingBMS(ass_CalIn.SOCValue);
|
|
|
-
|
|
|
- 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);
|
|
|
-
|
|
|
- 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 ////////////////////////
|
|
|
-
|
|
|
- AssitCuvApplPer();
|
|
|
-
|
|
|
- /////////////// Limit ///////////////////////////
|
|
|
- if (Assist_torqueper > ass_CalCoef.uwCurrentMaxPu)
|
|
|
- {
|
|
|
- Assist_torqueper = ass_CalCoef.uwCurrentMaxPu;
|
|
|
- }
|
|
|
+ TorqueAccStep = 100;
|
|
|
}
|
|
|
- else
|
|
|
- {
|
|
|
- Assist_torqueper = 0;
|
|
|
- }
|
|
|
-}
|
|
|
-
|
|
|
-void MoveAverageFilter(MAF_IN *in)
|
|
|
-{
|
|
|
- in->sum -= in->buffer[in->index];
|
|
|
- in->buffer[in->index] = in->value;
|
|
|
- in->sum += (SQWORD)in->value;
|
|
|
-// if (in->buffer[in->length - 1] == 0)
|
|
|
-// {
|
|
|
-// in->AverValue = (SLONG)(in->sum / (in->index + 1));
|
|
|
-// }
|
|
|
-// else
|
|
|
-// {
|
|
|
- in->AverValue = (SLONG)(in->sum / in->length);
|
|
|
-// }
|
|
|
- in->index++;
|
|
|
-
|
|
|
- if (in->index >= in->length)
|
|
|
+ else if(ass_CalIn.uwGearSt == 3)
|
|
|
{
|
|
|
- in->index = 0;
|
|
|
+ TorqueAccStep = 120;
|
|
|
}
|
|
|
-}
|
|
|
-void MoveAverageFilterClear(MAF_IN *in)
|
|
|
-{
|
|
|
- UWORD i;
|
|
|
- in->index = 0;
|
|
|
- in->sum = 0;
|
|
|
- // memset((UBYTE*)in->buffer, 0, sizeof(in->buffer));
|
|
|
-
|
|
|
- // in->buffer[(1 << in->length)-1]=0;
|
|
|
- for (i = 0; i < 64; i++)
|
|
|
+ else if(ass_CalIn.uwGearSt == 4)
|
|
|
{
|
|
|
- in->buffer[i] = 0;
|
|
|
- }
|
|
|
-}
|
|
|
-
|
|
|
-void AssistMode_Select(void) // 上电运行一次or助力参数更新后，AssistCoef需要重新计算
|
|
|
-{
|
|
|
- UWORD TempAssit;
|
|
|
- UWORD TempGear, gear;
|
|
|
-// if (ass_ParaSet.uwAsssistSelectNum == 1) // OBC：更换成EE参数
|
|
|
-// {
|
|
|
-// TempAssit = ass_ParaCong.uwAssistSelect1;
|
|
|
-// }
|
|
|
-// else if (ass_ParaSet.uwAsssistSelectNum == 2)
|
|
|
-// {
|
|
|
-// TempAssit = ass_ParaCong.uwAssistSelect2;
|
|
|
-// }
|
|
|
-// else
|
|
|
-// {
|
|
|
-// TempAssit = ASSISTMOD_SELECT_DEFAULT;
|
|
|
-// }
|
|
|
- if (ass_ParaCong.uwStartMode == 1) // OBC：更换成EE参数
|
|
|
+ TorqueAccStep = 150;
|
|
|
+ }
|
|
|
+ else if(ass_CalIn.uwGearSt == 5)
|
|
|
{
|
|
|
- TempAssit = ASSISTMOD_SELECT_DEFAULT;
|
|
|
+ TorqueAccStep = 150;
|
|
|
}
|
|
|
- else if (ass_ParaCong.uwStartMode == 2)
|
|
|
+ else
|
|
|
{
|
|
|
- TempAssit = ass_ParaCong.uwAssistSelect1;
|
|
|
+
|
|
|
}
|
|
|
- else if (ass_ParaCong.uwStartMode == 3)
|
|
|
+ TorqueDecStep = 80;
|
|
|
+ TorqCmd1 = ((ULONG)ass_CalIn.uwtorque * ass_CalCoef.swTorqFilterGain >> 14) +
|
|
|
+ ((ULONG)ass_CalIn.uwtorquelpf * (Q14_1 - ass_CalCoef.swTorqFilterGain) >> 14); //转矩指令滤波切换，由低通滤波到踏频相关的滑动平均滤波
|
|
|
+ TorqCmd = ((ULONG)TorqCmd1 * ass_CalCoef.swSmoothGain) >> 12; //转矩指令斜坡
|
|
|
+ if (TorqCmd > ass_ParaCong.uwBikeAssTorMaxPu) // 最大转矩限幅
|
|
|
{
|
|
|
- TempAssit = ass_ParaCong.uwAssistSelect2;
|
|
|
+ TorqCmd = ass_ParaCong.uwBikeAssTorMaxPu;
|
|
|
}
|
|
|
- else
|
|
|
+
|
|
|
+ Te_Tor_Assit_tempPu = (SLONG)(Polynomial(&ass_CalCoef.uwTorqueAssGain[ass_CalIn.uwGearSt], &TorqCmd, 14)); // Q14 转矩助力曲线
|
|
|
+ Te_Tor_Assit_LinerPu = (SLONG)(((TorqCmd * LinerAssist[ass_CalIn.uwGearSt] )>> 12) + 136);
|
|
|
+ if (Te_Tor_Assit_tempPu < Te_Tor_Assit_LinerPu)
|
|
|
{
|
|
|
- TempAssit = ASSISTMOD_SELECT_DEFAULT;
|
|
|
+ Te_Tor_Assit_tempPu = Te_Tor_Assit_LinerPu;
|
|
|
}
|
|
|
-
|
|
|
- for (gear = 0; gear < 5; gear++)
|
|
|
+ else
|
|
|
{
|
|
|
- TempGear = gear * 3 + ((TempAssit >> (gear << 1)) & 0x0003);
|
|
|
- memcpy(&ass_CalCoef.uwTorqueAssGain[(gear + 1)], &flash_stPara.slTorqAssGain[TempGear], sizeof(POLY_COEF));
|
|
|
+ //do nothing;
|
|
|
}
|
|
|
- memcpy(&ass_CalCoef.uwCadencAsseGain[1], &flash_stPara.slCadAssGain[0], sizeof(flash_stPara.slCadAssGain));
|
|
|
-}
|
|
|
+
|
|
|
+ CadCmd = (((SLONG)ass_CalIn.uwcadance * ass_CalCoef.swSmoothGain) >> 12)*10; // 踏频指令斜坡
|
|
|
+ Te_Cad_Assit_tempPu = ((SLONG)(Polynomial(&ass_CalCoef.uwCadencAsseGain[ass_CalIn.uwGearSt], &CadCmd, 20))) >> 6; // Q20 - Q6 = Q14 //踏频助力曲线
|
|
|
|
|
|
-void AssistCurrentLimitAccordingBMS(UWORD uwSOCvalue)
|
|
|
-{
|
|
|
- if (uwSOCvalue < ass_CurLimCalBMSCoef.uwIqLimitStartSoc && uwSOCvalue > ass_CurLimCalBMSCoef.uwIqLimitEndSoc)
|
|
|
+ if (Te_Tor_Assit_tempPu > ass_ParaCong.uwBikeAssTorMaxPu) // 最大转矩限幅
|
|
|
{
|
|
|
- ass_CurLimitCalBMSOut.uwIqLimitAbs =
|
|
|
- ass_CurLimCalBMSCoef.uwIqLimitInitAbs - (((SLONG)ass_CurLimCalBMSCoef.uwIqLimitStartSoc - uwSOCvalue) * ass_CurLimCalBMSCoef.swIqLImitK);
|
|
|
+ Te_Tor_Assit_tempPu = ass_ParaCong.uwBikeAssTorMaxPu;
|
|
|
}
|
|
|
- else if (uwSOCvalue <= ass_CurLimCalBMSCoef.uwIqLimitEndSoc)
|
|
|
+
|
|
|
+ if (Te_Cad_Assit_tempPu > ass_ParaCong.uwBikeAssTorMaxPu) // 最大转矩限幅
|
|
|
{
|
|
|
- ass_CurLimitCalBMSOut.uwIqLimitAbs = 0;
|
|
|
+ Te_Cad_Assit_tempPu = ass_ParaCong.uwBikeAssTorMaxPu;
|
|
|
}
|
|
|
- else
|
|
|
+
|
|
|
+ // paraset gain of user
|
|
|
+ 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
|
|
|
+
|
|
|
+ ////////////////////////////// Dadence para cal /////////////////////////////////////////////
|
|
|
+ ass_CalOut.swCadSpd2MotSpd =
|
|
|
+ ((SLONG)ass_CalIn.uwcadance * ass_ParaCong.uwMechRationMotor * ass_ParaCong.uwMotorPoles) >> 5; // Q20-Q5= Q15 出力时电机转速计算
|
|
|
+ ass_CalCoef.uwCadencePeriodCNT = TIME_MS2CNT(((ULONG)1000 << 20) / ((ULONG)ass_CalIn.uwcadance * FBASE)); //一圈踏频时间计数
|
|
|
+
|
|
|
+
|
|
|
+ tmpVoltargetPu = (SLONG)ass_CalOut.swCadSpd2MotSpd *(SLONG)cof_uwFluxPu >> 13;//Q15+Q12-Q13=Q14;
|
|
|
+
|
|
|
+ ass_CalCoef.uwStartupGain = ass_ParaSet.uwStartupCoef ; //零速启动助力比计算
|
|
|
+ ass_CalCoef.uwStartupCruiseGain = ass_ParaSet.uwStartupCruiseCoef ; //带速启动助力比计算
|
|
|
+ ////////////////////////////// Assist FSM Control ///////////////////////////////////////////
|
|
|
+ switch (Ass_FSM)
|
|
|
{
|
|
|
- ass_CurLimitCalBMSOut.uwIqLimitAbs = ass_CurLimCalBMSCoef.uwIqLimitInitAbs;
|
|
|
- }
|
|
|
-}
|
|
|
+ case Startup:
|
|
|
+ /*code*/
|
|
|
+ ass_CalCoef.swSmoothGain = Q12_1;
|
|
|
+
|
|
|
+ SpdKp = 500;//ass_ParaSet.uwStartUpCadNm;
|
|
|
+ slSpderror = (SLONG)ass_CalOut.swCadSpd2MotSpd - (SLONG)ass_CalIn.uwSpdFbkAbsPu;
|
|
|
+
|
|
|
+ if(ass_CalCoef.StartFlag == 0)
|
|
|
+ {
|
|
|
+ sltmpvoltagelimit= ((slSpderror * SpdKp )>> 11) + tmpVoltargetPu;
|
|
|
+ if(sltmpvoltagelimit > scm_swVsDcpLimPu)
|
|
|
+ {
|
|
|
+ sltmpvoltagelimit = scm_swVsDcpLimPu;
|
|
|
+ }
|
|
|
+ else if(sltmpvoltagelimit <= 0)
|
|
|
+ {
|
|
|
+ sltmpvoltagelimit =0;
|
|
|
+ }
|
|
|
+ ass_CalOut.swVoltLimitPu = sltmpvoltagelimit;
|
|
|
+
|
|
|
+ if(slSpderror <= 1500 )
|
|
|
+ {
|
|
|
+ ass_CalCoef.StartFlag=1;
|
|
|
+ }
|
|
|
+ }
|
|
|
+ else if(ass_CalCoef.StartFlag ==1 )
|
|
|
+ {
|
|
|
+ ass_CalOut.swVoltLimitPu += 3;//ass_CalCoef.uwStartUpGainAddStep;
|
|
|
+ if(slSpderror <= 100)
|
|
|
+ {
|
|
|
+ VoltCnt++;
|
|
|
+ }
|
|
|
+ else
|
|
|
+ {
|
|
|
+ VoltCnt--;
|
|
|
+ if (VoltCnt<0)
|
|
|
+ {
|
|
|
+ VoltCnt=0;
|
|
|
+ }
|
|
|
+ }
|
|
|
+ if(VoltCnt > 30)
|
|
|
+ {
|
|
|
+ Ass_FSM = TorqueAssit;
|
|
|
+ ass_CalCoef.StartFlag=0;
|
|
|
+ }
|
|
|
+ }
|
|
|
+
|
|
|
+
|
|
|
+// if(ass_CalCoef.StartFlag ==0 )
|
|
|
+// {
|
|
|
+//
|
|
|
+// VoltCnt++;
|
|
|
+// ass_CalOut.swVoltLimitPu = tmpVoltargetPu;
|
|
|
+// if(VoltCnt>=30)
|
|
|
+// {
|
|
|
+// ass_CalCoef.StartFlag=1;
|
|
|
+// VoltCnt=0;
|
|
|
+// }
|
|
|
+//// if (ass_CalOut.swVoltLimitPu < (tmpVoltargetPu-200))
|
|
|
+//// {
|
|
|
+//// ass_CalOut.swVoltLimitPu +=200;
|
|
|
+////
|
|
|
+//// }
|
|
|
+//// else
|
|
|
+//// {
|
|
|
+//// ass_CalOut.swVoltLimitPu = tmpVoltargetPu;
|
|
|
+//// ass_CalCoef.StartFlag=1;
|
|
|
+//// }
|
|
|
+// }
|
|
|
+// else if(ass_CalCoef.StartFlag ==1)
|
|
|
+// {
|
|
|
+// ass_CalOut.swVoltLimitPu += ass_CalCoef.uwStartUpGainAddStep;
|
|
|
+// if(abs(ass_CalIn.swCurRefPu - ass_CalIn.swCurFdbPu)<200)
|
|
|
+// {
|
|
|
+// VoltCnt++;
|
|
|
+// }
|
|
|
+// else
|
|
|
+// {
|
|
|
+// VoltCnt--;
|
|
|
+// if (VoltCnt<0)
|
|
|
+// {
|
|
|
+// VoltCnt=0;
|
|
|
+// }
|
|
|
+// }
|
|
|
+// if(VoltCnt > 30)
|
|
|
+// {
|
|
|
+// Ass_FSM = TorqueAssit;
|
|
|
+// ass_CalCoef.StartFlag=0;
|
|
|
+// VoltCnt = (((SLONG)abs(ass_CalOut.swVoltLimitPu - tmpVoltargetPu))<<12)/scm_swVsDcpLimPu;
|
|
|
+// }
|
|
|
+// else
|
|
|
+// {
|
|
|
+// }
|
|
|
+// }
|
|
|
+//
|
|
|
+
|
|
|
+ /*启动停机判断，瞬时转矩小于停机值持续1/4圈*/
|
|
|
+ if(ass_CalIn.uwcadancePer == 0)
|
|
|
+ {
|
|
|
+ ass_CalOut.swSpeedRef = ass_CalIn.uwSpdFbkAbsPu; // Q15 Q10 (9.625)
|
|
|
+ asr_stTorqSpdPIOut.slIqRefPu = ((SLONG)ass_CalOut.swTorAssistCurrent) << 16;
|
|
|
+ asr_stTorqSpdPIOut.swIqRefPu = ass_CalOut.swTorAssistCurrent;
|
|
|
+ ass_CalCoef.swAss2SpdCNT = 0;
|
|
|
+ ass_CalCoef.sw2StopCNT = 0;
|
|
|
+ Ass_FSM = ReduceCurrent;
|
|
|
+ }
|
|
|
+ else if(ass_CalIn.uwtorquePer <= (ass_CalCoef.uwAssStopThreshold))
|
|
|
+ {
|
|
|
+ if (ass_CalIn.uwcadance != ass_CalIn.uwcadancelast)
|
|
|
+ {
|
|
|
+ ass_CalCoef.swAss2SpdCNT++;
|
|
|
+ }
|
|
|
+ if (ass_CalCoef.swAss2SpdCNT > (ass_ParaCong.uwCadPulsePerCirc >> 1))
|
|
|
+ {
|
|
|
+ ass_CalOut.swSpeedRef = ass_CalIn.uwSpdFbkAbsPu; // Q15 Q10 (9.625)
|
|
|
+ asr_stTorqSpdPIOut.slIqRefPu = ((SLONG)ass_CalOut.swTorAssistCurrent) << 16;
|
|
|
+ asr_stTorqSpdPIOut.swIqRefPu = ass_CalOut.swTorAssistCurrent;
|
|
|
+ ass_CalCoef.swAss2SpdCNT = 0;
|
|
|
+ ass_CalCoef.sw2StopCNT = 0;
|
|
|
+ Ass_FSM = ReduceCurrent;
|
|
|
+ }
|
|
|
+ }
|
|
|
+ else
|
|
|
+ {
|
|
|
+
|
|
|
+ ass_CalCoef.swAss2SpdCNT = 0;
|
|
|
+
|
|
|
+ }
|
|
|
+
|
|
|
+ break;
|
|
|
+
|
|
|
+ case TorqueAssit:
|
|
|
+
|
|
|
+ VoltCnt += 3;
|
|
|
+ if(VoltCnt > Q12_1)
|
|
|
+ {
|
|
|
+ VoltCnt = Q12_1;
|
|
|
+ }
|
|
|
+ else
|
|
|
+ {
|
|
|
+
|
|
|
+ }
|
|
|
+
|
|
|
+// if(ass_CalIn.uwtorquelpf >= ass_CalCoef.uwSwitch2TorqThreshold)
|
|
|
+// {
|
|
|
+// ass_CalCoef.swCadanceGain = Q12_1;
|
|
|
+// }
|
|
|
+// else if (ass_CalIn.uwtorquelpf > ass_CalCoef.uwSwitch1TorqThreshold && ass_CalIn.uwtorquelpf <= ass_CalCoef.uwSwitch2TorqThreshold)
|
|
|
+// {
|
|
|
+// ass_CalCoef.swCadanceGain = ((SLONG)ass_CalIn.uwtorquelpf - (SLONG)ass_CalCoef.uwSwitch1TorqThreshold) * ass_CalCoef.ulStartupDeltInv >> 16;//Q14+Q14-Q16=Q12;
|
|
|
+// }
|
|
|
+// else
|
|
|
+// {
|
|
|
+// ass_CalCoef.swCadanceGain = 0;
|
|
|
+// }
|
|
|
+// ass_CalCoef.swCadanceGain=Q12_1;
|
|
|
+// tempVoltage =(SLONG)scm_swVsDcpLimPu * ass_CalCoef.swCadanceGain >> 12;
|
|
|
+// ass_CalOut.swVoltLimitPu = (SLONG)tmpVoltargetPu + ((SLONG)tempVoltage * VoltCnt >> 12);
|
|
|
+
|
|
|
+ ///////////test/////////
|
|
|
+
|
|
|
+ if(ass_CalIn.uwtorquePer >= ass_CalCoef.uwSwitch1TorqThreshold)
|
|
|
+ {
|
|
|
+ ass_CalOut.swVoltLimitPu += ass_CalCoef.uwStartUpGainAddStep;
|
|
|
+ }
|
|
|
+ else if (ass_CalIn.uwtorquePer <= ass_CalCoef.uwSwitch1TorqThreshold)
|
|
|
+ {
|
|
|
+ ass_CalOut.swVoltLimitPu -= ass_CalCoef.uwSpeedConstantCommand;
|
|
|
+ }
|
|
|
+ else
|
|
|
+ {}
|
|
|
+ if (ass_CalOut.swVoltLimitPu > scm_swVsDcpLimPu)
|
|
|
+ {
|
|
|
+ ass_CalOut.swVoltLimitPu = scm_swVsDcpLimPu;
|
|
|
+ }
|
|
|
+ else if (ass_CalOut.swVoltLimitPu <= (tmpVoltargetPu + ass_ParaSet.uwStartUpCadNm))
|
|
|
+ {
|
|
|
+ ass_CalOut.swVoltLimitPu = tmpVoltargetPu + ass_ParaSet.uwStartUpCadNm;
|
|
|
+ }
|
|
|
+
|
|
|
+
|
|
|
+ /* Torque stop*/
|
|
|
+ ass_CalCoef.swTorqFilterGain += 4; // Q14 转矩滤波方式切换系数
|
|
|
+ if (ass_CalCoef.swTorqFilterGain > Q14_1)
|
|
|
+ {
|
|
|
+ ass_CalCoef.swTorqFilterGain = Q14_1;
|
|
|
+ }
|
|
|
+//ass_CalCoef.swTorqFilterGain = 0;
|
|
|
+
|
|
|
+ /*停机判断，瞬时转矩小于停机值持续1/4圈*/
|
|
|
+
|
|
|
+ if(ass_CalIn.uwcadancePer == 0)
|
|
|
+ {
|
|
|
+ VoltCnt=0;
|
|
|
+ ass_CalOut.swSpeedRef = ass_CalIn.uwSpdFbkAbsPu; // Q15 Q10 (9.625)
|
|
|
+ asr_stTorqSpdPIOut.slIqRefPu = ((SLONG)ass_CalOut.swTorAssistCurrent) << 16;
|
|
|
+ asr_stTorqSpdPIOut.swIqRefPu = ass_CalOut.swTorAssistCurrent;
|
|
|
+ ass_CalCoef.swAss2SpdCNT = 0;
|
|
|
+ ass_CalCoef.sw2StopCNT = 0;
|
|
|
+ Stop_Orig_Coef.k = (SLONG)ass_CalCoef.swSmoothGain;
|
|
|
+ Stop_Coef = Polynomial_center(&Stop_Orig_Coef);
|
|
|
+ Ass_FSM = ReduceCurrent;
|
|
|
+
|
|
|
+ ass_CalOut.blAssHoldFlag = 0;
|
|
|
+ }
|
|
|
+ else if(ass_CalIn.uwtorquePer <= (ass_CalCoef.uwAssStopThreshold))
|
|
|
+ {
|
|
|
+ if (ass_CalIn.uwcadance != ass_CalIn.uwcadancelast)
|
|
|
+ {
|
|
|
+ ass_CalCoef.swAss2SpdCNT++;
|
|
|
+ }
|
|
|
+ if (ass_CalCoef.swAss2SpdCNT > (ass_ParaCong.uwCadPulsePerCirc >> 1 ) || ass_CalIn.uwcadancePer == 0)
|
|
|
+ {
|
|
|
+ TorquAccCnt=0;
|
|
|
+ ass_CalOut.swSpeedRef = ass_CalIn.uwSpdFbkAbsPu; // Q15 Q10 (9.625)
|
|
|
+ asr_stTorqSpdPIOut.slIqRefPu = ((SLONG)ass_CalOut.swTorAssistCurrent) << 16;
|
|
|
+ asr_stTorqSpdPIOut.swIqRefPu = ass_CalOut.swTorAssistCurrent;
|
|
|
+ ass_CalCoef.swAss2SpdCNT = 0;
|
|
|
+ ass_CalCoef.sw2StopCNT = 0;
|
|
|
+ Ass_FSM = ReduceCurrent;
|
|
|
+ Stop_Orig_Coef.k = (SLONG)ass_CalCoef.swSmoothGain;
|
|
|
+ Stop_Coef = Polynomial_center(&Stop_Orig_Coef);
|
|
|
+
|
|
|
+ ass_CalOut.blAssHoldFlag = 0;
|
|
|
+ }
|
|
|
+ }
|
|
|
+ else
|
|
|
+ {
|
|
|
+ ass_CalCoef.swAss2SpdCNT = 0;
|
|
|
+
|
|
|
+ }
|
|
|
+
|
|
|
+ break;
|
|
|
+
|
|
|
+ case SpeedAssit:
|
|
|
+
|
|
|
+ ass_CalOut.swVoltLimitPu = scm_swVsDcpLimPu;
|
|
|
+ /*电机速度指令斜坡，保证低速运行不停机*/
|
|
|
+ if (ass_CalOut.swSpeedRef >= 0)//ass_CalCoef.uwSpeedConstantCommand) // Q15
|
|
|
+ {
|
|
|
+ ass_CalOut.swSpeedRef -= 100;
|
|
|
+ }
|
|
|
+ else
|
|
|
+ {
|
|
|
+ ass_CalOut.swSpeedRef += 10;
|
|
|
+ }
|
|
|
+ if(ass_CalOut.swSpeedRef < 0)
|
|
|
+ {
|
|
|
+ ass_CalOut.swSpeedRef = 0;
|
|
|
+ }
|
|
|
+
|
|
|
+ asr_stTorqSpdPIIn.swSpdRefPu = ass_CalIn.swDirection*ass_CalOut.swSpeedRef;
|
|
|
+ asr_stTorqSpdPIIn.swSpdFdbPu = ass_CalIn.swSpdFbkPu; // Q15
|
|
|
+ asr_stTorqSpdPIIn.swIqMaxPu = ass_CalCoef.swSpdLoopAbsCurMax; // ass_CalCoef.uwCurrentMaxPu;
|
|
|
+ asr_stTorqSpdPIIn.swIqMinPu = - ass_CalCoef.swSpdLoopAbsCurMax; // ass_CalCoef.uwCurrentMaxPu;
|
|
|
+ asr_voSpdPI(&asr_stTorqSpdPIIn, &asr_stTorqSpdPICoef, &asr_stTorqSpdPIOut);
|
|
|
+ ass_CalOut.swTorSpdLoopCurrentTemp = abs(asr_stTorqSpdPIOut.swIqRefPu);
|
|
|
+
|
|
|
+
|
|
|
+ if(abs(ass_CalIn.swSpdFbkPu)< 400)
|
|
|
+ {
|
|
|
+ ass_CalCoef.sw2StopCNT = 0;
|
|
|
+ ass_CalCoef.StartFlag = 0;
|
|
|
+ ass_CalCoef.uwStartUpTargetGain = 0;
|
|
|
+ Ass_FSM = StopAssit;
|
|
|
+ MoveAverageFilterClear(&maf_torque);
|
|
|
+ Stop_Orig_Coef.k = 0;
|
|
|
+ Stop_Coef = Polynomial_center(&Stop_Orig_Coef);
|
|
|
+ ass_CalCoef.swCoefStep = 0;
|
|
|
+ }
|
|
|
+ break;
|
|
|
+ case Spd2Torq:
|
|
|
+ /*加速啮合，速度指令斜坡快速追踪踏频*/
|
|
|
+ if (ass_CalIn.uwSpdFbkAbsPu < ((ass_CalOut.swCadSpd2MotSpd*3) >> 1))
|
|
|
+ {
|
|
|
+ ass_CalOut.swSpeedRef += 100;
|
|
|
+ }
|
|
|
+ else
|
|
|
+ {
|
|
|
+// ass_CalOut.swSpeedRef -= 4;
|
|
|
+ }
|
|
|
+ if (ass_CalOut.swSpeedRef > ((ass_CalOut.swCadSpd2MotSpd*3) >> 1))
|
|
|
+ {
|
|
|
+ ass_CalOut.swSpeedRef = ((ass_CalOut.swCadSpd2MotSpd*3) >> 1);
|
|
|
+ }
|
|
|
+ asr_stTorqSpdPIIn.swSpdRefPu = ass_CalIn.swDirection*ass_CalOut.swSpeedRef;
|
|
|
+ asr_stTorqSpdPIIn.swSpdFdbPu = ass_CalIn.swSpdFbkPu; // Q15
|
|
|
+ asr_stTorqSpdPIIn.swIqMaxPu = ass_CalCoef.swSpdLoopAbsCurMax; // Q14
|
|
|
+ asr_stTorqSpdPIIn.swIqMinPu = -ass_CalCoef.swSpdLoopAbsCurMax; // Q14
|
|
|
+ asr_voSpdPI(&asr_stTorqSpdPIIn, &asr_stTorqSpdPICoef, &asr_stTorqSpdPIOut);
|
|
|
+ ass_CalOut.swTorSpdLoopCurrentTemp = abs(asr_stTorqSpdPIOut.swIqRefPu);
|
|
|
+ /*啮合后切换至带速启动*/
|
|
|
+ uwspeed2torqCnt++;
|
|
|
+ if (uwspeed2torqCnt > 150)//ass_CalIn.uwSpdFbkAbsPu > ass_CalOut.swCadSpd2MotSpd )// && ass_CalIn.uwtorquePer > ass_CalCoef.uwAssStopThreshold) // Q15
|
|
|
+ {
|
|
|
+ ass_CalCoef.StartFlag = 0;
|
|
|
+ ass_CalCoef.uwStartUpTargetGain = 0;
|
|
|
+ TempSpeedtoTorque = swCurrentCal(Te_Tor_AssitPu1);
|
|
|
+ TempSmooth = ((ULONG)abs(asr_stTorqSpdPIOut.swIqRefPu) << 12) /
|
|
|
+ TempSpeedtoTorque; // abs(asr_stTorqSpdPIOut.swIqRefPu)/swCurrentCal(Te_Tor_AssitPu1)
|
|
|
+ if (TempSmooth > Q12_1)
|
|
|
+ {
|
|
|
+ TempSmooth = Q12_1;
|
|
|
+ }
|
|
|
+ else
|
|
|
+ {}
|
|
|
+ ass_CalCoef.swSmoothGain = 0;//Q12_1 >> 1;
|
|
|
+ uwspeed2torqCnt = 0;
|
|
|
+
|
|
|
+ Ass_FSM = StartupCruise;
|
|
|
+ }
|
|
|
+
|
|
|
+ /* 停机判断*/
|
|
|
+ if( ass_CalIn.uwcadance == 0)
|
|
|
+ {
|
|
|
+ ass_CalOut.swSpeedRef = ass_CalIn.uwSpdFbkAbsPu; // Q15 Q10 (9.625)
|
|
|
+ asr_stTorqSpdPIOut.slIqRefPu = ((SLONG)ass_CalOut.swTorAssistCurrent) << 16;
|
|
|
+ asr_stTorqSpdPIOut.swIqRefPu = ass_CalOut.swTorAssistCurrent;
|
|
|
+ ass_CalCoef.swAss2SpdCNT = 0;
|
|
|
+ Ass_FSM = ReduceCurrent;
|
|
|
+ Stop_Orig_Coef.k = (SLONG)ass_CalCoef.swSmoothGain;
|
|
|
+ Stop_Coef = Polynomial_center(&Stop_Orig_Coef);
|
|
|
+ }
|
|
|
+ else if ((ass_CalIn.uwtorquePer <= ass_CalCoef.uwAssStopThreshold)) // Q14
|
|
|
+ {
|
|
|
+ if (ass_CalIn.uwcadance != ass_CalIn.uwcadancelast)
|
|
|
+ {
|
|
|
+ ass_CalCoef.swAss2SpdCNT++;
|
|
|
+ }
|
|
|
+ if (ass_CalCoef.swAss2SpdCNT > (ass_ParaCong.uwCadPulsePerCirc >> 1)|| ass_CalIn.uwcadance == 0)
|
|
|
+ {
|
|
|
+ ass_CalOut.swSpeedRef = ass_CalIn.uwSpdFbkAbsPu; // Q15 Q10 (9.625)
|
|
|
+ asr_stTorqSpdPIOut.slIqRefPu = ((SLONG)ass_CalOut.swTorAssistCurrent) << 16;
|
|
|
+ asr_stTorqSpdPIOut.swIqRefPu = ass_CalOut.swTorAssistCurrent;
|
|
|
+ ass_CalCoef.swAss2SpdCNT = 0;
|
|
|
+ Ass_FSM = ReduceCurrent;
|
|
|
+ Stop_Orig_Coef.k = (SLONG)ass_CalCoef.swSmoothGain;
|
|
|
+ Stop_Coef = Polynomial_center(&Stop_Orig_Coef);
|
|
|
+ }
|
|
|
+ }
|
|
|
+ else
|
|
|
+ {
|
|
|
+ ass_CalCoef.swAss2SpdCNT = 0;
|
|
|
+ }
|
|
|
+ break;
|
|
|
+
|
|
|
+ case StartupCruise:
|
|
|
+
|
|
|
+ if (ass_CalCoef.StartFlag == 0)
|
|
|
+ {
|
|
|
+ ass_CalCoef.swSmoothGain += ass_CalCoef.uwStartUpGainAddStep;// / ass_CalIn.uwGearSt; //助力比斜坡，与用户设置以及档位相关
|
|
|
+ if (ass_CalCoef.swSmoothGain >= ass_CalCoef.uwStartupCruiseGain)
|
|
|
+ {
|
|
|
+ ass_CalCoef.StartFlag = 1;
|
|
|
+ }
|
|
|
+ }
|
|
|
+ else if (ass_CalCoef.StartFlag == 1)
|
|
|
+ {
|
|
|
+ ass_CalCoef.swSmoothGain -= ass_CalCoef.uwStartUpGainAddStep;// / ass_CalIn.uwGearSt; //助力比斜坡，与用户设置以及档位相关
|
|
|
+ if (ass_CalIn.uwcadance != ass_CalIn.uwcadancelast)
|
|
|
+ {
|
|
|
+ ass_CalCoef.swCadanceCNT++;
|
|
|
+ }
|
|
|
+ if (ass_CalCoef.swSmoothGain < Q12_1)
|
|
|
+ {
|
|
|
+ ass_CalCoef.swSmoothGain = Q12_1;
|
|
|
+ if (ass_CalCoef.swCadanceCNT > ass_CalCoef.uwStartUpTimeCadenceCnt)
|
|
|
+ {
|
|
|
+ Ass_FSM = TorqueAssit;
|
|
|
+ ass_CalCoef.swAss2SpdCNT = 0;
|
|
|
+ ass_CalCoef.swCadanceCNT = 0;
|
|
|
+ ass_CalCoef.sw2StopCNT = 0;
|
|
|
+ }
|
|
|
+ }
|
|
|
+ }
|
|
|
+
|
|
|
+ if(ass_CalIn.uwcadancePer == 0)
|
|
|
+ {
|
|
|
+ ass_CalOut.swSpeedRef = ass_CalIn.uwSpdFbkAbsPu; // Q15 Q10 (9.625)
|
|
|
+ asr_stTorqSpdPIOut.slIqRefPu = ((SLONG)ass_CalOut.swTorAssistCurrent) << 16;
|
|
|
+ asr_stTorqSpdPIOut.swIqRefPu = ass_CalOut.swTorAssistCurrent;
|
|
|
+ ass_CalCoef.swAss2SpdCNT = 0;
|
|
|
+ ass_CalCoef.sw2StopCNT = 0;
|
|
|
+ Ass_FSM = ReduceCurrent;
|
|
|
+ Stop_Orig_Coef.k = (SLONG)ass_CalCoef.swSmoothGain;
|
|
|
+ Stop_Coef = Polynomial_center(&Stop_Orig_Coef);
|
|
|
+ }
|
|
|
+ else if(ass_CalIn.uwtorquePer <= (ass_CalCoef.uwAssStopThreshold))
|
|
|
+ {
|
|
|
+ if (ass_CalIn.uwcadance != ass_CalIn.uwcadancelast)
|
|
|
+ {
|
|
|
+ ass_CalCoef.swAss2SpdCNT++;
|
|
|
+ }
|
|
|
+ if (ass_CalCoef.swAss2SpdCNT > (ass_ParaCong.uwCadPulsePerCirc >> 1) || ass_CalIn.uwcadancePer == 0)
|
|
|
+ {
|
|
|
+ ass_CalOut.swSpeedRef = ass_CalIn.uwSpdFbkAbsPu; // Q15 Q10 (9.625)
|
|
|
+ asr_stTorqSpdPIOut.slIqRefPu = ((SLONG)ass_CalOut.swTorAssistCurrent) << 16;
|
|
|
+ asr_stTorqSpdPIOut.swIqRefPu = ass_CalOut.swTorAssistCurrent;
|
|
|
+ ass_CalCoef.swAss2SpdCNT = 0;
|
|
|
+ ass_CalCoef.sw2StopCNT = 0;
|
|
|
+ Ass_FSM = ReduceCurrent;
|
|
|
+ Stop_Orig_Coef.k = (SLONG)ass_CalCoef.swSmoothGain;
|
|
|
+ Stop_Coef = Polynomial_center(&Stop_Orig_Coef);
|
|
|
+ }
|
|
|
+ }
|
|
|
+ else
|
|
|
+ {
|
|
|
+
|
|
|
+ ass_CalCoef.swAss2SpdCNT = 0;
|
|
|
+ }
|
|
|
+
|
|
|
+
|
|
|
+ break;
|
|
|
+
|
|
|
+ case ReduceCurrent:
|
|
|
+ if ((ass_CalCoef.swSmoothGain <= 0) && (abs(ass_CalIn.swSpdFbkPu)< 400))
|
|
|
+ {
|
|
|
+ ass_CalCoef.swSmoothGain = 0;
|
|
|
+ ass_CalCoef.swTorqFilterGain = 0;
|
|
|
+ MoveAverageFilterClear(&maf_torque);
|
|
|
+ ass_CalCoef.swCadanceGain = 0;
|
|
|
+ ass_CalOut.swSpeedRef = ass_CalIn.uwSpdFbkAbsPu; // Q15 Q10 (9.625)
|
|
|
+ asr_stTorqSpdPIOut.slIqRefPu = ((SLONG)ass_CalOut.swTorAssistCurrent) << 16;
|
|
|
+ asr_stTorqSpdPIOut.swIqRefPu = ass_CalOut.swTorAssistCurrent;
|
|
|
+ Ass_FSM = StopAssit;
|
|
|
+
|
|
|
+ }
|
|
|
+ else
|
|
|
+ {
|
|
|
+ ass_CalCoef.swSmoothGain -= 40;
|
|
|
+ ass_CalOut.swVoltLimitPu -= 20;
|
|
|
+ if(ass_CalOut.swVoltLimitPu <= 0)
|
|
|
+ {
|
|
|
+ ass_CalOut.swVoltLimitPu = 0;
|
|
|
+ }
|
|
|
+
|
|
|
+ }
|
|
|
+ break;
|
|
|
+ case StopAssit:
|
|
|
+
|
|
|
+ ass_CalOut.swTorSpdLoopCurrentTemp = 0;
|
|
|
+ /* 启动判断*/
|
|
|
+ if (ass_CalIn.uwbikespeed < 449) // 0.3Hz, (2.19m轮径下 2.36km/h )
|
|
|
+ {
|
|
|
+ if (ass_CalIn.uwtorquePer > ass_CalCoef.uwAssThreshold && ass_CalIn.uwcadance > 0)
|
|
|
+ {
|
|
|
+// hw_voPWMOn();
|
|
|
+ Ass_FSM = Startup;
|
|
|
+ ass_CalCoef.swTorqFilterGain = 0;
|
|
|
+ ass_CalCoef.sw2StopCNT = 0;
|
|
|
+ VoltCnt=0;
|
|
|
+ ass_CalOut.swVoltLimitPu=0;
|
|
|
+ }
|
|
|
+ }
|
|
|
+ else
|
|
|
+ {
|
|
|
+ if (ass_CalIn.uwtorquelpf > ((ass_CalCoef.uwAssThreshold * 3)>>3) && ass_CalIn.uwtorquePer > ass_CalCoef.uwAssThreshold && ass_CalIn.uwcadance > 0)
|
|
|
+ {
|
|
|
+// hw_voPWMOn();
|
|
|
+ ass_CalCoef.swTorqFilterGain = 0;
|
|
|
+ uwspeed2torqCnt = 0;
|
|
|
+ Ass_FSM = Startup;
|
|
|
+ ass_CalOut.swSpeedRef = ass_CalIn.uwSpdFbkAbsPu;
|
|
|
+ ass_CalCoef.sw2StopCNT = 0;
|
|
|
+ VoltCnt=0;
|
|
|
+ ass_CalOut.swVoltLimitPu=0;
|
|
|
+ }
|
|
|
+ }
|
|
|
+
|
|
|
+// if (ass_CalIn.uwtorquelpf > ass_CalCoef.uwAssThreshold && ass_CalIn.uwcadance > 0)
|
|
|
+// {
|
|
|
+//// hw_voPWMOn();
|
|
|
+// if (ass_CalIn.uwbikespeed == 0)
|
|
|
+// {
|
|
|
+// Ass_FSM = Startup;
|
|
|
+// ass_CalCoef.swTorqFilterGain = 0;
|
|
|
+// }
|
|
|
+// else
|
|
|
+// {
|
|
|
+// ass_CalCoef.swTorqFilterGain = 0;
|
|
|
+// Ass_FSM = Spd2Torq;
|
|
|
+// ass_CalOut.swSpeedRef = ass_CalIn.uwSpdFbkAbsPu;
|
|
|
+// }
|
|
|
+// ass_CalCoef.sw2StopCNT = 0;
|
|
|
+// }
|
|
|
+ /* 退出助力判断*/
|
|
|
+ if (ass_CalIn.uwcadance == 0 || ass_CalIn.uwtorquelpf < ass_CalCoef.uwAssStopThreshold)
|
|
|
+ {
|
|
|
+ ass_CalCoef.sw2StopCNT++;
|
|
|
+ }
|
|
|
+ else
|
|
|
+ {
|
|
|
+ if (ass_CalCoef.sw2StopCNT >= 1)
|
|
|
+ {
|
|
|
+ ass_CalCoef.sw2StopCNT--;
|
|
|
+ }
|
|
|
+ }
|
|
|
+ if (ass_CalCoef.sw2StopCNT > TIME_MS2CNT(3000)) // 3s
|
|
|
+ {
|
|
|
+ ass_CalCoef.sw2StopCNT = 0;
|
|
|
+ ass_CalCoef.blAssistflag = FALSE;
|
|
|
+
|
|
|
+ }
|
|
|
+ break;
|
|
|
+
|
|
|
+ default:
|
|
|
+ break;
|
|
|
+ }
|
|
|
+
|
|
|
+ ////////////////////////////// Bikespeed limt of Assist //////////////////////////////////
|
|
|
+ if (ass_CalIn.uwbikespeed <= ass_CurLimCoef.uwBikeSpdThresHold1)
|
|
|
+ {
|
|
|
+ ass_CalCoef.swBikeSpeedGain = Q12_1; // Q12
|
|
|
+
|
|
|
+ }
|
|
|
+ else if (ass_CalIn.uwbikespeed > ass_CurLimCoef.uwBikeSpdThresHold1 && ass_CalIn.uwbikespeed <= ass_CurLimCoef.uwBikeSpdThresHold2)
|
|
|
+ {
|
|
|
+ ass_CalCoef.swBikeSpeedGain =
|
|
|
+ Q12_1 -
|
|
|
+ ((((SQWORD)ass_CalIn.uwbikespeed - (SQWORD)ass_CurLimCoef.uwBikeSpdThresHold1) * (SQWORD)ass_CurLimCoef.ulBikeSpdDeltInv) >> 28); // Q12
|
|
|
+ TorqueAccStep = 10;
|
|
|
+ TorqueDecStep = 10;
|
|
|
+ }
|
|
|
+ else
|
|
|
+ {
|
|
|
+ ass_CalCoef.swBikeSpeedGain = 0;
|
|
|
+ }
|
|
|
+
|
|
|
+ switch (Ass_FSM)
|
|
|
+ {
|
|
|
+ case Startup:
|
|
|
+ Te_Tor_AssitPu2 = ((SLONG)Te_Tor_AssitPu1 * ass_CalCoef.swBikeSpeedGain) >> 12; // Q14+Q12-Q12+Q12-Q12=Q14
|
|
|
+ Te_Cad_AssitPu2 = ((SLONG)Te_Cad_AssitPu1 * ass_CalCoef.swBikeSpeedGain) >> 12; // Q14+Q12-Q12+Q12-Q12=Q14
|
|
|
+ ass_CalOut.swTorAss2CurrentTemp = swCurrentCal(Te_Tor_AssitPu2); // Q14 电流指令计算
|
|
|
+ ass_CalOut.swCadAss2CurrentTemp = swCurrentCal(Te_Cad_AssitPu2); // 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;
|
|
|
+ ass_CalOut.swTorRefEnd = ass_CalOut.swTorRefTarget;
|
|
|
+ if (ass_CalCoef.StartFlag==0)
|
|
|
+ {
|
|
|
+ ass_CalOut.swTorAssistCurrentTemp = ass_CalIn.swDirection * 13000;
|
|
|
+ }
|
|
|
+ else
|
|
|
+ {
|
|
|
+ ass_CalOut.swTorAssistCurrentTemp = ass_CalIn.swDirection *ass_CalOut.swTorRefEnd;
|
|
|
+ }
|
|
|
+
|
|
|
+
|
|
|
+ break;
|
|
|
+
|
|
|
+ case TorqueAssit:
|
|
|
+
|
|
|
+ Te_Tor_AssitPu2 = ((SLONG)Te_Tor_AssitPu1 * ass_CalCoef.swBikeSpeedGain) >> 12; // Q14+Q12-Q12+Q12-Q12=Q14
|
|
|
+ Te_Cad_AssitPu2 = ((SLONG)Te_Cad_AssitPu1 * ass_CalCoef.swBikeSpeedGain) >> 12; // Q14+Q12-Q12+Q12-Q12=Q14
|
|
|
+ ass_CalOut.swTorAss2CurrentTemp = swCurrentCal(Te_Tor_AssitPu2); // Q14 电流指令计算
|
|
|
+ ass_CalOut.swCadAss2CurrentTemp = swCurrentCal(Te_Cad_AssitPu2); // 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;
|
|
|
+ }
|
|
|
+#if CURSWITCH
|
|
|
+ ass_CalOut.swTorRefTarget = ass_CalOut.swTorAss2CurrentTemp + ass_CalOut.swCadAss2CurrentTemp;
|
|
|
+
|
|
|
+ if((ass_CalOut.swTorRefTarget - ass_CalOut.swTorRefEnd) > 2)
|
|
|
+ {
|
|
|
+ TorquAccCnt++;
|
|
|
+ if(TorquAccCnt >= 2)
|
|
|
+ {
|
|
|
+ ass_CalOut.swTorRefEnd += TorqueAccStep;
|
|
|
+ TorquAccCnt = 0;
|
|
|
+ }
|
|
|
+ }
|
|
|
+ else if(((ass_CalOut.swTorRefTarget - ass_CalOut.swTorRefEnd) < (-1)))
|
|
|
+ {
|
|
|
+ if (ass_CalIn.uwcadance != ass_CalIn.uwcadancelast)
|
|
|
+ {
|
|
|
+
|
|
|
+ ass_CalOut.swTorRefEnd -= TorqueDecStep;
|
|
|
+
|
|
|
+ }
|
|
|
+// TorquDecCnt++;
|
|
|
+// if(TorquDecCnt >= 10)
|
|
|
+// {
|
|
|
+// ass_CalOut.swTorRefEnd += TorqueAccStep;
|
|
|
+// TorquDecCnt = 0;
|
|
|
+// }
|
|
|
+ }
|
|
|
+ else
|
|
|
+ {
|
|
|
+ ass_CalOut.swTorRefEnd = ass_CalOut.swTorRefTarget;
|
|
|
+ }
|
|
|
+ ass_CalOut.swTorAssistCurrentTemp = ass_CalIn.swDirection * ass_CalOut.swTorRefEnd;
|
|
|
+
|
|
|
+#else
|
|
|
+
|
|
|
+ ass_CalOut.swTorAssistCurrentTemp = ass_CalIn.swDirection *(ass_CalOut.swTorAss2CurrentTemp + ass_CalOut.swCadAss2CurrentTemp);
|
|
|
+#endif
|
|
|
+ break;
|
|
|
+
|
|
|
+ case StartupCruise:
|
|
|
+ Te_Tor_AssitPu2 = ((SLONG)Te_Tor_AssitPu1 * ass_CalCoef.swBikeSpeedGain) >> 12; // Q14+Q12-Q12+Q12-Q12=Q14
|
|
|
+ Te_Cad_AssitPu2 = ((SLONG)Te_Cad_AssitPu1 * ass_CalCoef.swBikeSpeedGain) >> 12; // Q14+Q12-Q12+Q12-Q12=Q14
|
|
|
+ ass_CalOut.swTorAss2CurrentTemp = swCurrentCal(Te_Tor_AssitPu2); // Q14 电流指令计算
|
|
|
+ ass_CalOut.swCadAss2CurrentTemp = swCurrentCal(Te_Cad_AssitPu2); // 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;
|
|
|
+ }
|
|
|
+
|
|
|
+#if CURSWITCH
|
|
|
+
|
|
|
+ ass_CalOut.swTorRefTarget = ass_CalOut.swTorAss2CurrentTemp + ass_CalOut.swCadAss2CurrentTemp;
|
|
|
+ if((ass_CalOut.swTorRefTarget - ass_CalOut.swTorRefEnd) > 2)
|
|
|
+ {
|
|
|
+ TorquAccCnt++;
|
|
|
+ if(TorquAccCnt >= 1)
|
|
|
+ {
|
|
|
+ ass_CalOut.swTorRefEnd += TorqueAccStep;
|
|
|
+ TorquAccCnt = 0;
|
|
|
+ }
|
|
|
+ }
|
|
|
+ else if(((ass_CalOut.swTorRefTarget - ass_CalOut.swTorRefEnd) < (-1)))
|
|
|
+ {
|
|
|
+ if (ass_CalIn.uwcadance != ass_CalIn.uwcadancelast)
|
|
|
+ {
|
|
|
+ ass_CalOut.swTorRefEnd -= TorqueDecStep;
|
|
|
+ }
|
|
|
+ }
|
|
|
+ else
|
|
|
+ {
|
|
|
+ ass_CalOut.swTorRefEnd = ass_CalOut.swTorRefTarget;
|
|
|
+ }
|
|
|
+
|
|
|
+ if(ass_CalOut.swTorRefEnd < ass_CalOut.swTorSpdLoopCurrentTemp)
|
|
|
+ {
|
|
|
+ ass_CalOut.swTorRefEnd = ass_CalOut.swTorSpdLoopCurrentTemp;
|
|
|
+ //ass_CalOut.swTorSpdLoopCurrentTemp = 0; // 启动前电流最小为速度环电流，启动后最小电流为0
|
|
|
+ }
|
|
|
+ else
|
|
|
+ {
|
|
|
+ ass_CalOut.swTorRefEnd = ass_CalOut.swTorRefEnd;
|
|
|
+ }
|
|
|
+ ass_CalOut.swTorAssistCurrentTemp = ass_CalIn.swDirection * ass_CalOut.swTorRefEnd;
|
|
|
+
|
|
|
+#else
|
|
|
+ ass_CalOut.swTorAssistCurrentTemp = ass_CalOut.swTorAss2CurrentTemp + ass_CalOut.swCadAss2CurrentTemp;
|
|
|
+ if(ass_CalOut.swTorAssistCurrentTemp < ass_CalOut.swTorSpdLoopCurrentTemp)
|
|
|
+ {
|
|
|
+ ass_CalOut.swTorAssistCurrentTemp = ass_CalIn.swDirection*ass_CalOut.swTorSpdLoopCurrentTemp;
|
|
|
+ //ass_CalOut.swTorSpdLoopCurrentTemp = 0; // 启动前电流最小为速度环电流，启动后最小电流为0
|
|
|
+ }
|
|
|
+ else
|
|
|
+ {
|
|
|
+ ass_CalOut.swTorAssistCurrentTemp = ass_CalIn.swDirection * ass_CalOut.swTorAssistCurrentTemp;
|
|
|
+ }
|
|
|
+#endif
|
|
|
+
|
|
|
+ break;
|
|
|
+ case ReduceCurrent:
|
|
|
+ Te_Tor_AssitPu2 = ((SLONG)Te_Tor_AssitPu1 * ass_CalCoef.swBikeSpeedGain) >> 12; // Q14+Q12-Q12+Q12-Q12=Q14
|
|
|
+ Te_Cad_AssitPu2 = ((SLONG)Te_Cad_AssitPu1 * ass_CalCoef.swBikeSpeedGain) >> 12; // Q14+Q12-Q12+Q12-Q12=Q14
|
|
|
+ ass_CalOut.swTorAss2CurrentTemp = swCurrentCal(Te_Tor_AssitPu2); // Q14 电流指令计算
|
|
|
+ ass_CalOut.swCadAss2CurrentTemp = swCurrentCal(Te_Cad_AssitPu2); // 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.swTorAssistCurrentTemp = ass_CalIn.swDirection *(ass_CalOut.swTorAss2CurrentTemp + ass_CalOut.swCadAss2CurrentTemp);
|
|
|
+ break;
|
|
|
+ case SpeedAssit:
|
|
|
+ ass_CalOut.swTorAssistCurrentTemp = asr_stTorqSpdPIOut.swIqRefPu; // ass_CalOut.swTorSpdLoopCurrentTemp;
|
|
|
+ break;
|
|
|
+ case Spd2Torq:
|
|
|
+ ass_CalOut.swTorAssistCurrentTemp = asr_stTorqSpdPIOut.swIqRefPu; // ass_CalOut.swTorSpdLoopCurrentTemp;
|
|
|
+ ass_CalOut.swTorRefEnd = ass_CalOut.swTorAssistCurrentTemp;
|
|
|
+ break;
|
|
|
+ case StopAssit:
|
|
|
+ ass_CalOut.swTorAssistCurrentTemp = 0;
|
|
|
+ ass_CalOut.swTorRefEnd = 0;
|
|
|
+ break;
|
|
|
+ default:
|
|
|
+ break;
|
|
|
+ }
|
|
|
+
|
|
|
+ //////////////////////////// output Iqref ///////////////////////////////////////
|
|
|
+ ass_CalOut.swTorAssistCurrent = ass_CalOut.swTorAssistCurrentTemp;
|
|
|
+ mth_voLPFilter(ass_CalOut.swTorAssistCurrent, &ass_pvt_stCurLpf);
|
|
|
+ Assist_torqueper = ass_pvt_stCurLpf.slY.sw.hi;
|
|
|
+// Assist_torqueper =ass_CalOut.swTorAssistCurrent;
|
|
|
+ return Assist_torqueper;
|
|
|
+}
|
|
|
+
|
|
|
+/**
|
|
|
+ * @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;
|
|
|
+
|
|
|
+ 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
|
|
|
+ */
|
|
|
+
|
|
|
+void Assist(void)
|
|
|
+{
|
|
|
+
|
|
|
+ if ((ass_CalIn.uwtorquePer > ass_CalCoef.uwAssThreshold && ass_CalIn.uwcadancePer > 0) && ass_CalIn.uwGearSt > 0)
|
|
|
+ {
|
|
|
+ ass_CalCoef.blAssistflag = TRUE;
|
|
|
+ }
|
|
|
+ if (ass_CalCoef.blAssistflag == TRUE)
|
|
|
+ {
|
|
|
+ //////////// Calculate the Iq limit ///////////////////
|
|
|
+ UWORD IqLimitTemp;
|
|
|
+ AssitCuvLim(ass_CalIn.uwGearSt, ass_CalIn.uwbikespeed, ass_ParaCong.uwCofCurMaxPu);
|
|
|
+ AssistCurrentLimitAccordingBMS(ass_CalIn.SOCValue);
|
|
|
+
|
|
|
+ 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);
|
|
|
+
|
|
|
+ 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 ////////////////////////
|
|
|
+
|
|
|
+// AssitCuvApplPer();
|
|
|
+ AssitCuvApplPerVolt();
|
|
|
+ /////////////// Limit ///////////////////////////
|
|
|
+ if (Assist_torqueper > ass_CalCoef.uwCurrentMaxPu)
|
|
|
+ {
|
|
|
+ Assist_torqueper = ass_CalCoef.uwCurrentMaxPu;
|
|
|
+ }
|
|
|
+ }
|
|
|
+ else
|
|
|
+ {
|
|
|
+ Assist_torqueper = 0;
|
|
|
+ }
|
|
|
+}
|
|
|
+
|
|
|
+void MoveAverageFilter(MAF_IN *in)
|
|
|
+{
|
|
|
+ in->sum -= in->buffer[in->index];
|
|
|
+ in->buffer[in->index] = in->value;
|
|
|
+ in->sum += (SQWORD)in->value;
|
|
|
+// if (in->buffer[in->length - 1] == 0)
|
|
|
+// {
|
|
|
+// in->AverValue = (SLONG)(in->sum / (in->index + 1));
|
|
|
+// }
|
|
|
+// else
|
|
|
+// {
|
|
|
+ in->AverValue = (SLONG)(in->sum / in->length);
|
|
|
+// }
|
|
|
+ in->index++;
|
|
|
+
|
|
|
+ if (in->index >= in->length)
|
|
|
+ {
|
|
|
+ in->index = 0;
|
|
|
+ }
|
|
|
+}
|
|
|
+void MoveAverageFilterClear(MAF_IN *in)
|
|
|
+{
|
|
|
+ UWORD i;
|
|
|
+ in->index = 0;
|
|
|
+ in->sum = 0;
|
|
|
+ // memset((UBYTE*)in->buffer, 0, sizeof(in->buffer));
|
|
|
+
|
|
|
+ // in->buffer[(1 << in->length)-1]=0;
|
|
|
+ for (i = 0; i < 64; i++)
|
|
|
+ {
|
|
|
+ in->buffer[i] = 0;
|
|
|
+ }
|
|
|
+}
|
|
|
+
|
|
|
+void AssistMode_Select(void) // 上电运行一次or助力参数更新后，AssistCoef需要重新计算
|
|
|
+{
|
|
|
+ UWORD TempAssit;
|
|
|
+ UWORD TempGear, gear;
|
|
|
+// if (ass_ParaSet.uwAsssistSelectNum == 1) // OBC：更换成EE参数
|
|
|
+// {
|
|
|
+// TempAssit = ass_ParaCong.uwAssistSelect1;
|
|
|
+// }
|
|
|
+// else if (ass_ParaSet.uwAsssistSelectNum == 2)
|
|
|
+// {
|
|
|
+// TempAssit = ass_ParaCong.uwAssistSelect2;
|
|
|
+// }
|
|
|
+// else
|
|
|
+// {
|
|
|
+// TempAssit = ASSISTMOD_SELECT_DEFAULT;
|
|
|
+// }
|
|
|
+ if (ass_ParaCong.uwStartMode == 1) // OBC：更换成EE参数
|
|
|
+ {
|
|
|
+ TempAssit = ASSISTMOD_SELECT_DEFAULT;
|
|
|
+ }
|
|
|
+ else if (ass_ParaCong.uwStartMode == 2)
|
|
|
+ {
|
|
|
+ TempAssit = ass_ParaCong.uwAssistSelect1;
|
|
|
+ }
|
|
|
+ else if (ass_ParaCong.uwStartMode == 3)
|
|
|
+ {
|
|
|
+ TempAssit = ass_ParaCong.uwAssistSelect2;
|
|
|
+ }
|
|
|
+ else
|
|
|
+ {
|
|
|
+ TempAssit = ASSISTMOD_SELECT_DEFAULT;
|
|
|
+ }
|
|
|
+
|
|
|
+ for (gear = 0; gear < 5; gear++)
|
|
|
+ {
|
|
|
+ TempGear = gear * 3 + ((TempAssit >> (gear << 1)) & 0x0003);
|
|
|
+ memcpy(&ass_CalCoef.uwTorqueAssGain[(gear + 1)], &flash_stPara.slTorqAssGain[TempGear], sizeof(POLY_COEF));
|
|
|
+ }
|
|
|
+ memcpy(&ass_CalCoef.uwCadencAsseGain[1], &flash_stPara.slCadAssGain[0], sizeof(flash_stPara.slCadAssGain));
|
|
|
+}
|
|
|
+
|
|
|
+void AssistCurrentLimitAccordingBMS(UWORD uwSOCvalue)
|
|
|
+{
|
|
|
+ if (uwSOCvalue < ass_CurLimCalBMSCoef.uwIqLimitStartSoc && uwSOCvalue > ass_CurLimCalBMSCoef.uwIqLimitEndSoc)
|
|
|
+ {
|
|
|
+ ass_CurLimitCalBMSOut.uwIqLimitAbs =
|
|
|
+ ass_CurLimCalBMSCoef.uwIqLimitInitAbs - (((SLONG)ass_CurLimCalBMSCoef.uwIqLimitStartSoc - uwSOCvalue) * ass_CurLimCalBMSCoef.swIqLImitK);
|
|
|
+ }
|
|
|
+ else if (uwSOCvalue <= ass_CurLimCalBMSCoef.uwIqLimitEndSoc)
|
|
|
+ {
|
|
|
+ ass_CurLimitCalBMSOut.uwIqLimitAbs = 0;
|
|
|
+ }
|
|
|
+ else
|
|
|
+ {
|
|
|
+ ass_CurLimitCalBMSOut.uwIqLimitAbs = ass_CurLimCalBMSCoef.uwIqLimitInitAbs;
|
|
|
+ }
|
|
|
+}
|
|
|
+
|
|
|
+
|
CN\guohui27