增加踏频响应

User project/2.MotorDrive/Source/spdctrmode.c

@@ -575,7 +575,14 @@ void  scm_voTorqCtrMdTbs(void)
       mth_voLPFilterCoef(1000000 / 50, FTBS_HZ, &scm_stIqLoadLpf.uwKx); 
       mth_voLPFilter(spi_stResolverOut.swSpdFbkPu, &scm_stIqLoadLpf);
       scm_swSpdFbkCompPu = spi_stResolverOut.swSpdFbkPu - scm_stIqLoadLpf.slY.sw.hi;
-      swTmpSpdRate = (SLONG)scm_swSpdFbkCompPu * ass_stParaSet.uwSpeedAssistSpdRpm >> 11;
+      if(Ass_FSM != Prepare)
+      {
+          swTmpSpdRate = (SLONG)scm_swSpdFbkCompPu * ass_stParaSet.uwSpeedAssistSpdRpm >> 11;
+      }
+      else
+      {
+          swTmpSpdRate = (SLONG)scm_swSpdFbkCompPu * ass_stParaSet.uwSpeedAssistSpdRpm >> 12;
+      }
 
       mth_voLPFilterCoef(1000000 / 250, FTBS_HZ, &swTmpSpdRateLpf.uwKx); 
       mth_voLPFilter(swTmpSpdRate, &swTmpSpdRateLpf);

User project/3.BasicFunction/Include/AssistCurve.h

@@ -52,8 +52,8 @@ extern "C" {
 #define BIKE_THROTTLE_SMOOTH           ((BIKE_THROTTLE_SMOOTH_INCREASE<<8)|(BIKE_THROTTLE_SMOOTH_DECREASE))
 #define BIKE_ASSIST_MODE1              0x0155
 #define BIKE_ASSIST_MODE2              0x02AA
-#define TORQUE_START_THRESHOLD   50 // 0.1Nm
-#define TORQUE_STOP_THRESHOLD    50 // 0.1Nm
+#define TORQUE_START_THRESHOLD   70 // 0.1Nm
+#define TORQUE_STOP_THRESHOLD    30 // 0.1Nm
 #define TORQUE_SWITCH2_THRESHOLD 120 // 0.1Nm
 #define TORQUE_SWITCH1_THRESHOLD 100 // 0.1Nm
 
@@ -170,7 +170,8 @@ typedef enum
     Spd2Torq = 3,
     StartupCruise = 4,
     StopAssit = 5,
-    ReduceCurrent = 6
+    ReduceCurrent = 6,
+    Prepare = 7
 } ASS_FSM_STATUS;
 /**
  * @brief AssistCurve Para Configure of Bike

User project/3.BasicFunction/Include/Cadence.h

@@ -115,6 +115,7 @@ typedef struct
     UWORD       uwFreqPercent; // Q14;
     UWORD       uwForwardCnt;  // Q14;
     UWORD       uwOverflowfirst;
+    ULONG       uwCntErrLast;
 } CADENCE_OUT;
 
 /****************************************

User project/3.BasicFunction/Source/AssistCurve.c

@@ -309,8 +309,8 @@ void ass_voAssitCoef(void)
     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.uwSwitch1TorqThreshold = ass_stCalCoef.uwAssStopThreshold;//((ULONG)TORQUE_SWITCH1_THRESHOLD << 14) / TORQUEBASE;
+    ass_stCalCoef.uwSwitch2TorqThreshold = ass_stCalCoef.uwAssThreshold;//((ULONG)TORQUE_SWITCH2_THRESHOLD << 14) / TORQUEBASE;
     ass_stCalCoef.ulStartupDeltInv = ((ULONG)1 << 28) / (ass_stCalCoef.uwSwitch2TorqThreshold - ass_stCalCoef.uwSwitch1TorqThreshold); // Q14;
 
     /*初始化计数*/
@@ -454,7 +454,10 @@ void ass_voAssitCoef(void)
 //    out->swIRefPu = out->slIRefPu >> 15; // Q29-Q15=Q14
 //    out->swErrZ1Pu = (SWORD)slErrPu;
 //}
-
+SWORD swPreCurrentPu;
+SWORD startonce = 0;
+UWORD kcoef =4096; //Q10
+UWORD uwTempStopCnt,StopCad,swMotorCadspd;
 static void AssitCuvApplPerVolt(void) /* parasoft-suppress METRICS-28 "本项目圈复杂度无法更改，后续避免" */
 {  
     SLONG slTeTorAssitTmpPu,slTeTorAssitLinerPu,slTeCadAssitTmpPu;
@@ -466,10 +469,10 @@ static void AssitCuvApplPerVolt(void) /* parasoft-suppress METRICS-28 "本项目
     UWORD uwTorqAccStep = 50,uwTorqDecStep = 80;
     SWORD swCurSwitch = 0;
     SWORD swTmpVoltPu,swTmpVoltPu2;
-    SLONG slSpdErr,slTmpVoltLim;
+    SLONG slSpdErr,slTmpVoltLim, slPreSpderror;
     SWORD swSpdKpPu = 500; //Q10
     UWORD uwVoltAccStep = 1, uwVoltDecStep = 3;
-    UWORD uwTmpStopCnt = 0;
+    SWORD swSpderror;
 //    SLONG slTmp_a1, slTmp_b1, slTmp_c1;
   
     /* Select Torq Growth Rate by Bike Gear */
@@ -515,6 +518,18 @@ static void AssitCuvApplPerVolt(void) /* parasoft-suppress METRICS-28 "本项目
         swTorqCmd = (SWORD)ass_stParaCong.uwBikeAssTorMaxPu;
     }
    
+    if(swTorqCmd < ass_stCalCoef.uwSwitch1TorqThreshold )
+    {
+        ass_stCalCoef.swCadanceGain = 0; //Q12
+    }
+    else if(swTorqCmd >= ass_stCalCoef.uwSwitch1TorqThreshold  && swTorqCmd < ass_stCalCoef.uwSwitch2TorqThreshold)
+    {
+        ass_stCalCoef.swCadanceGain = (((ULONG)swTorqCmd - (ULONG)ass_stCalCoef.uwSwitch1TorqThreshold) * ass_stCalCoef.ulStartupDeltInv )>>16;
+    }
+    else
+    {
+        ass_stCalCoef.swCadanceGain = 4096; //Q12
+    }
     /*  Assist torque Cal using Assist Curve */
     slTeTorAssitTmpPu = (SLONG)(ass_slPolynomial(&ass_stCalCoef.uwTorqueAssGain[ass_stCalIn.uwGearSt], &swTorqCmd, 14)); // Q14  转矩助力曲线
     if(ass_stCalIn.uwGearSt == 5)
@@ -565,6 +580,16 @@ static void AssitCuvApplPerVolt(void) /* parasoft-suppress METRICS-28 "本项目
     {
         swTmpVoltPu = swTmpVoltPu2;
     }
+    swMotorCadspd =(SWORD)((SLONG)(ass_stCalIn.uwSpdFbkAbsPu << 15)/(ass_stParaCong.uwMechRationMotor * ass_stParaCong.uwMotorPoles)>>10);
+    swSpderror = swMotorCadspd - 300;//(SWORD)ass_CalIn.uwcadance;
+    if(swSpderror < 200)
+    {
+       StopCad = 200;
+    }
+    else
+    {
+       StopCad = swSpderror;
+    }
       
     ass_stCalCoef.uwStartupGain =  ass_stParaSet.uwStartupCoef ; //零速启动助力比计算
     ass_stCalCoef.uwStartupCruiseGain = ass_stParaSet.uwStartupCruiseCoef ; //带速启动助力比计算
@@ -572,6 +597,38 @@ static void AssitCuvApplPerVolt(void) /* parasoft-suppress METRICS-28 "本项目
     /*  Assist FSM Control */
     switch (Ass_FSM)
     {
+    case Prepare:
+        
+        UWORD tmpKp = 4000;//ass_ParaSet.uwStartUpCadNm ; //Q10
+        slPreSpderror = (((SLONG)ass_stCalOut.swCadSpd2MotSpd * 800 )>>10) - ass_stCalIn.uwSpdFbkAbsPu; //Q15
+        swPreCurrentPu = (slPreSpderror * tmpKp )>> 11; //Q14
+        
+        if(swPreCurrentPu < 0)
+        {
+            swPreCurrentPu = 0;
+        }
+        
+        if((ass_stCalCoef.swAss2SpdCNT > 3000) || ((ass_stCalIn.uwcadance < StopCad) && (ass_stCalIn.uwtorquePer <= ass_stCalCoef.uwAssStopThreshold))) //踏频小于0.1 hz
+        {
+            Ass_FSM = StopAssit;
+            ass_stCalCoef.swAss2SpdCNT = 0;            
+        }
+        else
+        {
+           ass_stCalCoef.swAss2SpdCNT++;
+            
+        }
+        if((ass_stCalIn.uwtorquePer > ((ass_stCalCoef.uwAssThreshold * 5) >> 3)) && (ass_stCalIn.uwcadance > 300)
+           && ((BikeBrake_blGetstate() | bikegearsensor_blBikeGetState()) == FALSE))
+        {
+           Ass_FSM = Startup;
+           ass_stCalCoef.swAss2SpdCNT=0;
+           startonce = 0;
+        }
+        
+        
+        
+        break;
     case Startup:
          /* 启动系数 */
         if(ass_pvt_uwSmoothFlg == 0)
@@ -591,6 +648,8 @@ static void AssitCuvApplPerVolt(void) /* parasoft-suppress METRICS-28 "本项目
             else
             {
                 ass_stCalCoef.swSmoothGain = Q12_1;
+                Ass_FSM = TorqueAssit;
+                ass_stCalCoef.StartFlag=0;
                 ass_pvt_uwSmoothFlg = 2;
             }        
         }
@@ -599,90 +658,90 @@ static void AssitCuvApplPerVolt(void) /* parasoft-suppress METRICS-28 "本项目
             // do nothing
         }
          
-        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)
+//        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)
 //            {
-//                ass_pvt_swVoltCnt++;  
-//            }    
+//                slTmpVoltLim = scm_swVsDcpLimPu;
+//            }
+//            else if(slTmpVoltLim <= swTmpVoltPu)
+//            {
+//                slTmpVoltLim = swTmpVoltPu;
+//            }
 //            else
 //            {
-//                ass_pvt_swVoltCnt=0;
+//            	//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++;
 //            }
-//            if(ass_pvt_swVoltCnt > 10)
+//            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;
-//                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
-        }
+//            }
+//        } 
+//        else
+//        {
+//        	//do nothing
+//        }
         
         
         /* Switch to ReduceCurrent FSM */
-        if((ass_stCalIn.uwcadancePer == 0) || (ass_stCalIn.uwGearSt == 0) || (BikeBrake_blGetstate() == TRUE) || (bikegearsensor_blBikeGetState() == TRUE))
+        if(((ass_stCalIn.uwcadance < StopCad) && (ass_stCalIn.uwtorquePer <= ass_stCalCoef.uwAssStopThreshold)) || (ass_stCalIn.uwGearSt == 0) || (BikeBrake_blGetstate() == TRUE) || (bikegearsensor_blBikeGetState() == TRUE))
         {
             /* When CandanceFreq=0 or BikeGear=0*/
             ass_stCalCoef.swAss2SpdCNT = 0;
@@ -691,21 +750,17 @@ static void AssitCuvApplPerVolt(void) /* parasoft-suppress METRICS-28 "本项目
         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)
+            uwTempStopCnt = ((ULONG)1*100 << 14)/ ass_stCalIn.uwcadance;//((ULONG)1000<<20)/(ass_CalIn.uwcadance * FBASE) ;
+            if(uwTempStopCnt < 300)
             {
-                uwTmpStopCnt = 200;
+                uwTempStopCnt = 300;
             }
-            else if(uwTmpStopCnt > 500)
+            else if(uwTempStopCnt > 2000)
             {
-                uwTmpStopCnt = 500;
-            }
-            else
-            {
-            	//do nothing
+                uwTempStopCnt = 2000;
             }
             
-            if(ass_stCalCoef.swAss2SpdCNT > uwTmpStopCnt)
+            if(ass_stCalCoef.swAss2SpdCNT > uwTempStopCnt)
             {
                 Ass_FSM = ReduceCurrent;  
                 ass_stCalCoef.swAss2SpdCNT = 0;
@@ -776,35 +831,33 @@ static void AssitCuvApplPerVolt(void) /* parasoft-suppress METRICS-28 "本项目
         }
                  
         /* Switch to ReduceCurrent FSM */
-        if((ass_stCalIn.uwcadancePer == 0) || (ass_stCalIn.uwGearSt == 0) || (BikeBrake_blGetstate() == TRUE) || (bikegearsensor_blBikeGetState() == TRUE))
+        if(((ass_stCalIn.uwcadance < StopCad) && (ass_stCalIn.uwtorquePer <= ass_stCalCoef.uwAssStopThreshold)) || (ass_stCalIn.uwGearSt == 0) || (BikeBrake_blGetstate() == TRUE) || (bikegearsensor_blBikeGetState() == TRUE))
         {
             /* When CandanceFreq=0 or BikeGear=0*/
             ass_stCalOut.blTorqPIFlg = FALSE;
             ass_stCalCoef.swAss2SpdCNT = 0;
             Ass_FSM = ReduceCurrent;
+            ass_stCalCoef.StartFlag = 0;
         }
         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)
+            uwTempStopCnt = ((ULONG)1*100 << 14)/ ass_stCalIn.uwcadance;//((ULONG)1000<<20)/(ass_CalIn.uwcadance * FBASE) ;
+            if(uwTempStopCnt < 300)
             {
-                uwTmpStopCnt = 200;
+                uwTempStopCnt = 300;
             }
-            else if(uwTmpStopCnt > 500)
+            else if(uwTempStopCnt > 2000)
             {
-                uwTmpStopCnt = 500;
-            }
-            else
-            {
-            	//do nothing
+                uwTempStopCnt = 2000;
             }
 
-            if(ass_stCalCoef.swAss2SpdCNT > uwTmpStopCnt)
+            if(ass_stCalCoef.swAss2SpdCNT > uwTempStopCnt)
             {            
                 ass_stCalCoef.swAss2SpdCNT = 0;
                 ass_stCalOut.blTorqPIFlg = FALSE;
                 Ass_FSM = ReduceCurrent;
+                ass_stCalCoef.StartFlag = 0;
             } 
         }
         else
@@ -845,13 +898,18 @@ static void AssitCuvApplPerVolt(void) /* parasoft-suppress METRICS-28 "本项目
         break;
         
     case StopAssit:
+        
+      if(ass_stCalIn.uwcadance == 0)
+      {
+          startonce = 0;
+      }
        ass_stCalOut.swTorSpdLoopCurrentTemp = 0;
        /* Switch to Startup FSM */
        if ((BikeBrake_blGetstate() == FALSE) && (bikegearsensor_blBikeGetState() == FALSE) && (ass_stCalIn.uwGearSt > 0))
        {
-              if (ass_stCalIn.uwbikespeed < 449) // 0.3Hz,  (2.19m轮径下  2.36km/h )
+              if ((ass_stCalIn.uwtorquePer > ass_stCalCoef.uwAssThreshold))
               {
-                  if (ass_stCalIn.uwtorquePer > ass_stCalCoef.uwAssThreshold && ass_stCalIn.uwcadance > 0)
+                  if (ass_stCalIn.uwcadance > 0)
                   {               
                       ass_stCalCoef.sw2StopCNT = 0;
                       ass_stCalOut.swVoltLimitPu = scm_swVsDcpLimPu;
@@ -863,15 +921,17 @@ static void AssitCuvApplPerVolt(void) /* parasoft-suppress METRICS-28 "本项目
               }
               else
               {
-                  if (ass_stCalIn.uwtorquePer > (ass_stCalCoef.uwAssThreshold >> 1) && ass_stCalIn.uwcadance > 0)
+                  if (ass_stCalIn.uwcadance > 300)
                   {            
                       ass_stCalCoef.sw2StopCNT = 0;
                       ass_stCalOut.swVoltLimitPu = scm_swVsDcpLimPu;
                       ass_pvt_stCurLpf.slY.sw.hi = 0; 
                       ass_stCalCoef.swSmoothStopGain = Q12_1;
                       ass_stCalCoef.swSmoothGain = ass_stParaSet.uwStartupCruiseCoef;
-                      Ass_FSM = Startup;
+                      Ass_FSM = Prepare;
+                      startonce = 1;
                   }
+                  swPreCurrentPu = 0;
               }
        }
        
@@ -922,6 +982,11 @@ static void AssitCuvApplPerVolt(void) /* parasoft-suppress METRICS-28 "本项目
     /* Assist Current Output in each FSM */
     switch (Ass_FSM)
     {
+    case Prepare:
+          
+        ass_stCalOut.swTorAssistCurrentTemp =  ass_stCalIn.swDirection * swPreCurrentPu;
+        break;
+        
     case Startup:
         swTeTorAssitPu2 = swTeTorAssitPu1 ; // Q14+Q12-Q12+Q12-Q12=Q14
         swTeCadAssitPu2 = swTeCadAssitPu1 ; // Q14+Q12-Q12+Q12-Q12=Q14
@@ -937,7 +1002,14 @@ static void AssitCuvApplPerVolt(void) /* parasoft-suppress METRICS-28 "本项目
         }
         ass_stCalOut.swTorRefTarget = ass_stCalOut.swTorAss2CurrentTemp + ass_stCalOut.swCadAss2CurrentTemp;
         ass_stCalOut.swTorRefEnd = ass_stCalOut.swTorRefTarget;
-        ass_stCalOut.swTorAssistCurrentTemp = ass_stCalIn.swDirection *ass_stCalOut.swTorRefEnd;
+//        if(swPreCurrentPu < ass_CalOut.swTorRefEnd)
+//        {
+            ass_stCalOut.swTorAssistCurrentTemp = ass_stCalIn.swDirection *ass_stCalOut.swTorRefEnd;
+//        }
+//        else
+//        {
+//            ass_CalOut.swTorAssistCurrentTemp =  (ULONG)ass_CalIn.swDirection * swPreCurrentPu;
+//        }
            
     break;  
     
@@ -984,7 +1056,7 @@ static void AssitCuvApplPerVolt(void) /* parasoft-suppress METRICS-28 "本项目
            ass_stCalOut.swTorRefEnd = ass_stCalOut.swTorRefTarget;
         }
         
-        ass_stCalOut.swTorAssistCurrentTemp = ass_stCalIn.swDirection * ass_stCalOut.swTorRefEnd;
+        ass_stCalOut.swTorAssistCurrentTemp = ass_stCalIn.swDirection * ass_stCalOut.swTorRefEnd * ass_stCalCoef.swCadanceGain >> 12;
         
         /* Torq Clzloop Test */      
 //        if(ass_stCalIn.uwtorquelpf <= ass_stCalCoef.uwSwitch1TorqThreshold)
@@ -1010,7 +1082,7 @@ static void AssitCuvApplPerVolt(void) /* parasoft-suppress METRICS-28 "本项目
 //            ass_stCalOut.swTorAssistCurrentTemp = ass_stCalIn.swDirection * ass_stCalOut.swTorRefEnd;
 //        }
 #else         
-        ass_stCalOut.swTorAssistCurrentTemp = ass_stCalIn.swDirection *(ass_stCalOut.swTorAss2CurrentTemp + ass_stCalOut.swCadAss2CurrentTemp);
+        ass_stCalOut.swTorAssistCurrentTemp = ass_stCalIn.swDirection *(ass_stCalOut.swTorAss2CurrentTemp + ass_stCalOut.swCadAss2CurrentTemp)*ass_CalCoef.swCadanceGain >> 12;
 #endif      
         break;        
     
@@ -1027,7 +1099,7 @@ static void AssitCuvApplPerVolt(void) /* parasoft-suppress METRICS-28 "本项目
         {
             ass_stCalOut.swCadAss2CurrentTemp = ass_stCalCoef.swCurrentmax_cadAssPu;
         }
-        ass_stCalOut.swTorAssistCurrentTemp = ass_stCalIn.swDirection *(ass_stCalOut.swTorAss2CurrentTemp + ass_stCalOut.swCadAss2CurrentTemp);
+        ass_stCalOut.swTorAssistCurrentTemp = ass_stCalIn.swDirection *(ass_stCalOut.swTorAss2CurrentTemp + ass_stCalOut.swCadAss2CurrentTemp)*ass_stCalCoef.swCadanceGain >> 12;
         break; 
  
     case StopAssit:
@@ -1088,7 +1160,7 @@ void ass_voAssist(void)
     ass_stCalIn.uwcadance = (UWORD)(((ULONG)30 << 20) / cof_uwFbHz / 60);
 #endif
     /* Start Assist Jduge */
-    if ((ass_stCalIn.uwtorquePer > ass_stCalCoef.uwAssThreshold && ass_stCalIn.uwcadancePer > 0) && (ass_stCalIn.uwGearSt > 0 && ass_stCalIn.uwGearSt != 0x22))
+    if (((ass_stCalIn.uwcadance > 0) || (ass_stCalIn.uwtorquePer > 3000)) && (ass_stCalIn.uwGearSt > 0 && ass_stCalIn.uwGearSt != 0x22))
     {
         ass_stCalCoef.blAssistflag = TRUE;
     }
@@ -1139,14 +1211,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)