|
@@ -58,6 +58,7 @@ 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;
|
|
|
+static UWORD ass_pvt_uwSmoothFlg = 0;
|
|
|
/******************************
|
|
|
*
|
|
|
* Function
|
|
@@ -239,6 +240,7 @@ void ass_voAssitEEInit(void)
|
|
|
ass_pvt_uwTorqAccCnt = 0;
|
|
|
ass_pvt_uwTorqDecCnt = 0;
|
|
|
ass_pvt_uwSpd2TorqCnt = 0;
|
|
|
+ ass_pvt_uwSmoothFlg = 0;
|
|
|
AssCnt1ms = 0;
|
|
|
}
|
|
|
|
|
@@ -288,16 +290,9 @@ void ass_voAssitCoef(void)
|
|
|
/*助力系数初始化*/
|
|
|
ass_stCalCoef.StartFlag = 0;
|
|
|
ass_stCalCoef.swSmoothGain = 0; // Q12
|
|
|
+ ass_stCalCoef.swSmoothStopGain = 4096; //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;
|
|
@@ -509,7 +504,6 @@ static void AssitCuvApplPerVolt(void) /* parasoft-suppress METRICS-28 "本项目
|
|
|
AssCnt1ms = 0;
|
|
|
}
|
|
|
|
|
|
- ass_stCalCoef.swTorqFilterGain = Q14_1;
|
|
|
/* 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)); //转矩指令滤波切换,由低通滤波到踏频相关的滑动平均滤波
|
|
@@ -578,11 +572,30 @@ static void AssitCuvApplPerVolt(void) /* parasoft-suppress METRICS-28 "本项目
|
|
|
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)
|
|
|
+ /* 启动系数 */
|
|
|
+ if(ass_pvt_uwSmoothFlg == 0)
|
|
|
+ {
|
|
|
+ ass_stCalCoef.swSmoothGain += ass_stParaSet.uwSpeedAssistIMaxA; ////ass_stCalCoef.uwStartUpGainAddStep;
|
|
|
+ if(ass_stCalCoef.swSmoothGain >= ass_stParaSet.uwStartupCoef)
|
|
|
+ {
|
|
|
+ ass_pvt_uwSmoothFlg = 1;
|
|
|
+ }
|
|
|
+ }
|
|
|
+ else if (ass_pvt_uwSmoothFlg == 1)
|
|
|
+ {
|
|
|
+ if(ass_stCalCoef.swSmoothGain >= Q12_1)
|
|
|
+ {
|
|
|
+ ass_stCalCoef.swSmoothGain -= (ass_stParaSet.uwSpeedAssistIMaxA >> 1);
|
|
|
+ }
|
|
|
+ else
|
|
|
+ {
|
|
|
+ ass_stCalCoef.swSmoothGain = Q12_1;
|
|
|
+ ass_pvt_uwSmoothFlg = 2;
|
|
|
+ }
|
|
|
+ }
|
|
|
+ else
|
|
|
{
|
|
|
- ass_stCalCoef.swSmoothGain = Q12_1;
|
|
|
+ // do nothing
|
|
|
}
|
|
|
|
|
|
swSpdKpPu = 1000; //ass_stParaSet.uwStartUpCadNm;
|
|
@@ -704,15 +717,32 @@ static void AssitCuvApplPerVolt(void) /* parasoft-suppress METRICS-28 "本项目
|
|
|
}
|
|
|
|
|
|
break;
|
|
|
- case TorqueAssit:
|
|
|
-
|
|
|
- /* 启动系数 */
|
|
|
- ass_stCalCoef.swSmoothGain += (SWORD)ass_stParaSet.uwSpeedAssistIMaxA; ////ass_stCalCoef.uwStartUpGainAddStep;
|
|
|
- if(ass_stCalCoef.swSmoothGain >= Q12_1)
|
|
|
+ case TorqueAssit:
|
|
|
+ /* 启动系数 */
|
|
|
+ if(ass_pvt_uwSmoothFlg == 0)
|
|
|
{
|
|
|
- ass_stCalCoef.swSmoothGain = Q12_1;
|
|
|
+ ass_stCalCoef.swSmoothGain += ass_stParaSet.uwSpeedAssistIMaxA; ////ass_stCalCoef.uwStartUpGainAddStep;
|
|
|
+ if(ass_stCalCoef.swSmoothGain >= ass_stParaSet.uwStartupCoef)
|
|
|
+ {
|
|
|
+ ass_pvt_uwSmoothFlg = 1;
|
|
|
+ }
|
|
|
+ }
|
|
|
+ else if (ass_pvt_uwSmoothFlg == 1)
|
|
|
+ {
|
|
|
+ if(ass_stCalCoef.swSmoothGain >= Q12_1)
|
|
|
+ {
|
|
|
+ ass_stCalCoef.swSmoothGain -= (ass_stParaSet.uwSpeedAssistIMaxA >> 1);
|
|
|
+ }
|
|
|
+ else
|
|
|
+ {
|
|
|
+ ass_stCalCoef.swSmoothGain = Q12_1;
|
|
|
+ ass_pvt_uwSmoothFlg = 2;
|
|
|
+ }
|
|
|
+ }
|
|
|
+ else
|
|
|
+ {
|
|
|
+ // do nothing
|
|
|
}
|
|
|
-
|
|
|
/* Reduce Voltage Limit When LPFTorq < Switch1TorqThreshold */
|
|
|
if(0 == (AssCnt1ms%5))
|
|
|
{
|
|
@@ -727,9 +757,9 @@ static void AssitCuvApplPerVolt(void) /* parasoft-suppress METRICS-28 "本项目
|
|
|
// else if (ass_stCalIn.uwtorque <= ass_stCalCoef.uwSwitch1TorqThreshold)
|
|
|
// {
|
|
|
//// ass_stCalOut.swVoltLimitPu -= ass_stCalCoef.uwSpeedConstantCommand;
|
|
|
-//// if (ass_stCalOut.swVoltLimitPu <= (tmpVoltargetPu + ass_ParaSet.uwStartUpCadNm))
|
|
|
+//// if (ass_stCalOut.swVoltLimitPu <= (tmpVoltargetPu + ass_stParaSet.uwStartUpCadNm))
|
|
|
//// {
|
|
|
-//// ass_stCalOut.swVoltLimitPu = tmpVoltargetPu + ass_ParaSet.uwStartUpCadNm;
|
|
|
+//// ass_stCalOut.swVoltLimitPu = tmpVoltargetPu + ass_stParaSet.uwStartUpCadNm;
|
|
|
//// }
|
|
|
// }
|
|
|
// else
|
|
@@ -785,18 +815,22 @@ static void AssitCuvApplPerVolt(void) /* parasoft-suppress METRICS-28 "本项目
|
|
|
|
|
|
case ReduceCurrent:
|
|
|
/* Switch to StopAssit FSM */
|
|
|
- if(ass_stCalCoef.swSmoothGain <= 0)
|
|
|
+ if(ass_stCalCoef.swSmoothStopGain <= 0)
|
|
|
{
|
|
|
+ ass_pvt_uwSmoothFlg = 0;
|
|
|
ass_stCalCoef.swSmoothGain = 0;
|
|
|
+ ass_stCalCoef.swSmoothStopGain = 0;
|
|
|
ass_stCalCoef.swTorqFilterGain = 0;
|
|
|
ass_stCalCoef.swCadanceGain = 0;
|
|
|
+ ass_voMoveAverageFilterClear(&ass_stTorqMafValue);
|
|
|
Ass_FSM = StopAssit;
|
|
|
}
|
|
|
else
|
|
|
{
|
|
|
/* Reduce Curret Coef to Zero*/
|
|
|
- ass_stCalCoef.swSmoothGain -=40;
|
|
|
ass_stCalOut.swVoltLimitPu = scm_swVsDcpLimPu;
|
|
|
+ ass_stCalCoef.swSmoothGain -= ass_stCalCoef.uwStartUpGainAddStep;
|
|
|
+ ass_stCalCoef.swSmoothStopGain -= ass_stCalCoef.uwStartUpGainAddStep;
|
|
|
}
|
|
|
|
|
|
/* Switch to Startup FSM */
|
|
@@ -820,17 +854,21 @@ static void AssitCuvApplPerVolt(void) /* parasoft-suppress METRICS-28 "本项目
|
|
|
{
|
|
|
ass_stCalCoef.sw2StopCNT = 0;
|
|
|
ass_stCalOut.swVoltLimitPu = scm_swVsDcpLimPu;
|
|
|
- ass_pvt_stCurLpf.slY.sw.hi = 0;
|
|
|
+ ass_pvt_stCurLpf.slY.sw.hi = 0;
|
|
|
+ ass_stCalCoef.swSmoothStopGain = Q12_1;
|
|
|
+ ass_stCalCoef.swSmoothGain = ass_stParaSet.uwStartupCruiseCoef;
|
|
|
Ass_FSM = Startup;
|
|
|
}
|
|
|
}
|
|
|
else
|
|
|
{
|
|
|
- if (ass_stCalIn.uwtorquelpf > ((ass_stCalCoef.uwAssThreshold * 3)>>3) && ass_stCalIn.uwtorquePer > ass_stCalCoef.uwAssThreshold && ass_stCalIn.uwcadance > 0)
|
|
|
+ if (ass_stCalIn.uwtorquePer > (ass_stCalCoef.uwAssThreshold >> 1) && ass_stCalIn.uwcadance > 0)
|
|
|
{
|
|
|
ass_stCalCoef.sw2StopCNT = 0;
|
|
|
ass_stCalOut.swVoltLimitPu = scm_swVsDcpLimPu;
|
|
|
- ass_pvt_stCurLpf.slY.sw.hi = 0;
|
|
|
+ ass_pvt_stCurLpf.slY.sw.hi = 0;
|
|
|
+ ass_stCalCoef.swSmoothStopGain = Q12_1;
|
|
|
+ ass_stCalCoef.swSmoothGain = ass_stParaSet.uwStartupCruiseCoef;
|
|
|
Ass_FSM = Startup;
|
|
|
}
|
|
|
}
|
|
@@ -1004,6 +1042,7 @@ static void AssitCuvApplPerVolt(void) /* parasoft-suppress METRICS-28 "本项目
|
|
|
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 = (SLONG)ass_stCalOut.swAssitCurRef * ass_stCalCoef.swSmoothStopGain >> 12;
|
|
|
//ass_stCalOut.swAssitCurRef =ass_stCalOut.swTorAssistCurrent;
|
|
|
}
|
|
|
|
|
@@ -1119,14 +1158,14 @@ 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
|
|
|
- {
|
|
|
+// 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)
|
|
@@ -1142,8 +1181,8 @@ void ass_voMoveAverageFilterClear(MAF_IN *in)
|
|
|
in->uwIndex = 0;
|
|
|
in->slSum = 0;
|
|
|
in->blSecFlag = FALSE;
|
|
|
-
|
|
|
- for (i = 0; i < 64; i++)
|
|
|
+ in->slAverValue = 0;
|
|
|
+ for (i = 0; i < in->uwLength; i++)
|
|
|
{
|
|
|
in->swBuffer[i] = 0;
|
|
|
}
|