feat: 1.力矩稳态清0 2.车速计算加快 3.转把功能 4.优化电量计算 5.修复踏频中断bug

User project/1.FrameLayer/Source/FSM_1st.c

@@ -115,6 +115,9 @@ void SysInit_hook(void)
         /* Bike Brake Init */
         bikebrake_voBikeBrakeInit();
 
+        /* Bike Cadence Init */
+        cadence_voCadenceInit();
+
         switch_flg.SysRdy_Flag = TRUE;
     }
 }
@@ -133,7 +136,7 @@ void SysRdy_hook(void)
     /* System Coef Cal */
     if (switch_flg.SysCoef_Flag == FALSE)
     {
-        bikelight_voBikeLightCoef(ass_ParaCong.uwLightVoltage);
+        bikelight_voBikeLightCoef(ass_stParaCong.uwLightVoltage);
 
         bikethrottle_voBikeThrottleCof();
 
@@ -158,10 +161,13 @@ void SysRdy_hook(void)
     if (cp_stFlg.ParaAssistUpdateFinishFlg == TRUE && cp_stFlg.ParaUpdateFlg == FALSE)
     {
         if (FSM2nd_Run_state.state == Exit)
-        {
-            ass_voAssitCoef();
-            bikelight_voBikeLightCoef(ass_ParaCong.uwLightVoltage);
+        {                     
+            bikelight_voBikeLightCoef(ass_stParaCong.uwLightVoltage);
+            
             torsensor_voTorSensorCof();
+
+            ass_voAssitCoef();
+            
             cp_stFlg.ParaAssistUpdateFinishFlg = FALSE;
         }
         else

User project/1.FrameLayer/Source/TimeTask_Event.c

@@ -44,6 +44,9 @@ static SQWORD TimingTaskTimerTickPassed = 0;
 static UWORD  LoopServerExecutedFlag = 0;
 static UWORD  AssistCNT = 0;
 static BOOL tstMafClrFlg = FALSE;
+SWORD tstBikeSpdRefTarget = 0, tstBikeSpdRef = 0, tstBikeSpdRefTargetZ1 = 0;
+SWORD tstIqRefTarget = 0, tstIqRef,tstIqRefTargetZ1 = 0;
+BOOL Event_pvt_blBikeThroFlg = FALSE, Event_pvt_blBikeThroFlgZ1 = FALSE;
 /******************************
  *
  *  Function
@@ -56,157 +59,183 @@ void  Event_1ms(void)
     FSM1st_Sys_state.Event_hook();
     
     /* Power control */
-    power_voPowerManagement(ass_ParaCong.uwAutoPowerOffTime * 60, cp_ulSystickCnt, OBC_ButtonStatus.ulButtonSetTimeCnt, \
+    power_voPowerManagement(ass_stParaCong.uwAutoPowerOffTime * 60, cp_ulSystickCnt, OBC_ButtonStatus.ulButtonSetTimeCnt, \
                             MC_RunInfo.Torque, MC_RunInfo.Cadence, MC_RunInfo.BikeSpeed, \
                             cp_stFlg.ParaHistorySaveEEFinishFlg, cp_stFlg.ParaSaveEEFlg);
     
     /* cp_history info update */
     Can_voMC_Run_1ms();
 
-    /* Torque move average filter */
-    if (cadence_stFreGetOut.uwForwardCnt > 0)
+    if(switch_flg.SysCoef_Flag == TRUE)
     {
-        cadence_stFreGetOut.uwForwardCnt = 0;
-        ass_stTorqMafValue.swValue = torsensor_stTorSensorOut.uwTorquePu;
-        ass_voMoveAverageFilter(&ass_stTorqMafValue);
-        
-        /* Iqref maf test, dont add torq obs */
-        if(ass_CalOut.blTorqPIFlg)
+        /* Torque move average filter */
+        if (cadence_stFreGetOut.uwForwardCnt > 0)
         {
-            ass_stUqLimMafValue.swValue = ass_stTorqPIOut.swIRefPu;
-            ass_voMoveAverageFilter(&ass_stUqLimMafValue);
-            tstMafClrFlg = FALSE;
+            cadence_stFreGetOut.uwForwardCnt = 0;
+            ass_stTorqMafValue.swValue = torsensor_stTorSensorOut.uwTorquePu;
+            ass_voMoveAverageFilter(&ass_stTorqMafValue);
+            
+            /* Iqref maf test, dont add torq obs */
+            if(ass_stCalOut.blTorqPIFlg)
+            {
+                ass_stUqLimMafValue.swValue = ass_stTorqPIOut.swIRefPu;
+                ass_voMoveAverageFilter(&ass_stUqLimMafValue);
+                tstMafClrFlg = FALSE;
+            }
+            else if((!ass_stCalOut.blTorqPIFlg) && (tstMafClrFlg == FALSE))
+            {
+                ass_voMoveAverageFilterClear(&ass_stUqLimMafValue);
+                tstMafClrFlg = TRUE;
+            }               
         }
-        else if((!ass_CalOut.blTorqPIFlg) && (tstMafClrFlg == FALSE))
-        {
-            ass_voMoveAverageFilterClear(&ass_stUqLimMafValue);
-            tstMafClrFlg = TRUE;
-        }    
-        
-    }
-
-    /* Torque info update */
-    torsensor_voTorADC();
-
-    /* Torque assist calculation*/
-    //ass_CalIn.SOCValue = 100;
-    ass_CalIn.swFlxIqLimit = MC_RunInfo.SOC;
-    if(cp_stFlg.RunModelSelect == CityBIKE )
-    {
-        ass_CalIn.swDirection = -1;
-    }
-    else if(cp_stFlg.RunModelSelect == MountainBIKE)
-    {
-        ass_CalIn.swDirection = 1;
-    }
-    else
-    {
-        ass_CalIn.swDirection = 1;
-    }
-    ass_CalIn.swFlxIqLimit = abs(flx_stCtrlOut.swIqLimPu);
-    ass_CalIn.swPwrIqLimit = abs(pwr_stPwrLimOut2.swIqLimPu);
-    ass_CalIn.uwbikespeed = bikespeed_stFreGetOut.uwLPFFrequencyPu;
-    ass_CalIn.uwcadancelast = ass_CalIn.uwcadance;
-    ass_CalIn.uwcadance = cadence_stFreGetOut.uwLPFFrequencyPu;
-    ass_CalIn.uwcadancePer = cadence_stFreGetOut.uwFreqPercent;
-    ass_CalIn.uwcadanceFWCnt = cadence_stFreGetOut.uwForwardCnt;
-    ass_CalIn.uwGearSt = (cp_stBikeRunInfoPara.uwBikeGear <= 6) ? cp_stBikeRunInfoPara.uwBikeGear : 0;
-    ass_CalIn.uwSpdFbkAbsPu = scm_uwSpdFbkLpfAbsPu;
-    ass_CalIn.swSpdFbkPu = scm_stSpdFbkLpf.slY.sw.hi;
-    ass_CalIn.uwBaseSpdrpm = cof_uwVbRpm;
-    ass_CalIn.uwtorque = ass_stTorqMafValue.slAverValue; //torsensor_stTorSensorOut.uwTorqueLPFPu;
-    ass_CalIn.uwtorquelpf = torsensor_stTorSensorOut.uwTorqueLPFPu;
-    ass_CalIn.uwtorquePer = torsensor_stTorSensorOut.uwTorquePu;
-    ass_CalIn.swCurFdbPu = scm_swIqFdbLpfPu;
-    ass_CalIn.swCurRefPu = scm_swIqRefPu;
-    ass_voAssist();
-    
-    uart_swTorqRefNm = ass_CalOut.swAssitCurRef;
-    
-    /////////Constant Current Control Test///////////
-//    if((cp_stFlg.RunModelSelect == CityBIKE) || (cp_stFlg.RunModelSelect == MountainBIKE))
-//    {
-//            
-//      ass_CalOut.swVoltLimitPu = scm_swVsDcpLimPu;
-//      uart_swTorqRefNm = ass_CalIn.swDirection * ass_ParaSet.uwSpeedAssistIMaxA;
-//      
-//      if(uart_swTorqRefNm != 0)
-//      {
-//         ass_CalCoef.blAssistflag = TRUE;
-//      }
-//      else
-//      {
-//         ass_CalCoef.blAssistflag = FALSE;
-//      }
-//          
-//    }
-    
-    /* Torque assist mode flag */
-    if (ass_CalCoef.blAssistflag == TRUE && cp_stFlg.RunPermitFlg == TRUE && cp_stFlg.SpiOffsetFirstSetFlg ==1)
-    {
-        signal_state.Sensor = TRUE;
-    }
-    else if( cp_stFlg.SpiOffsetFirstSetFlg == 0 && cp_stFlg.RunPermitFlg == TRUE && cp_stFlg.SpiOffsetFirstSetFinishFlg == FALSE)
-    {
-        signal_state.Sensor = TRUE;
-    }
-    else
-    {
-        signal_state.Sensor = FALSE;
-    }
 
-    /* Speed assist mode flag */
-    if((cp_stFlg.RunModelSelect == CityBIKE) || (cp_stFlg.RunModelSelect == MountainBIKE))
-    {
-        if(cp_stBikeRunInfoPara.uwBikeGear == 0x22)
+        /* Torque info update */
+        torsensor_voTorADC();
+        torsensor_voOffsetUpdate();
+        
+        /* Torque assist calculation*/
+        //ass_stCalIn.SOCValue = 100;
+        ass_stCalIn.swFlxIqLimit = MC_RunInfo.SOC;
+        if(cp_stFlg.RunModelSelect == CityBIKE )
         {
-            AssistCNT ++;
-            if(AssistCNT > 200 && cp_stFlg.RunPermitFlg == TRUE)
-            {
-                signal_state.Assist = TRUE;
-                AssistCNT = 200;
-            }
+            ass_stCalIn.swDirection = -1;
+        }
+        else if(cp_stFlg.RunModelSelect == MountainBIKE)
+        {
+            ass_stCalIn.swDirection = 1;
         }
         else
         {
-            AssistCNT = 0;
-            signal_state.Assist = FALSE;
+            ass_stCalIn.swDirection = 1;
         }
+        ass_stCalIn.swFlxIqLimit = abs(flx_stCtrlOut.swIqLimPu);
+        ass_stCalIn.swPwrIqLimit = abs(pwr_stPwrLimOut2.swIqLimPu);
+        ass_stCalIn.uwbikespeed = bikespeed_stFreGetOut.uwLPFFrequencyPu;
+        ass_stCalIn.uwcadancelast = ass_stCalIn.uwcadance;
+        ass_stCalIn.uwcadance = cadence_stFreGetOut.uwLPFFrequencyPu;
+        ass_stCalIn.uwcadancePer = cadence_stFreGetOut.uwFreqPercent;
+        ass_stCalIn.uwcadanceFWCnt = cadence_stFreGetOut.uwForwardCnt;
+        ass_stCalIn.uwGearSt = (cp_stBikeRunInfoPara.uwBikeGear <= 6) ? cp_stBikeRunInfoPara.uwBikeGear : 0;
+        ass_stCalIn.uwSpdFbkAbsPu = scm_uwSpdFbkLpfAbsPu;
+        ass_stCalIn.swSpdFbkPu = scm_stSpdFbkLpf.slY.sw.hi;
+        ass_stCalIn.uwBaseSpdrpm = cof_uwVbRpm;
+        ass_stCalIn.uwtorque = ass_stTorqMafValue.slAverValue; //torsensor_stTorSensorOut.uwTorqueLPFPu;
+        ass_stCalIn.uwtorquelpf = torsensor_stTorSensorOut.uwTorqueLPFPu;
+        ass_stCalIn.uwtorquePer = torsensor_stTorSensorOut.uwTorquePu;
+        ass_stCalIn.swCurFdbPu = scm_swIqFdbLpfPu;
+        ass_stCalIn.swCurRefPu = scm_swIqRefPu;
+        ass_voAssist();
 
-        if(signal_state.Assist == TRUE)
+        /* Select Bike Torque or Throttle Assist */
+        if(Event_pvt_blBikeThroFlg == FALSE)
         {
-            //6km/H = 100m/min = 1.67m/s
-            if(cp_stFlg.RunModelSelect == CityBIKE)
+            if (ass_stCalCoef.blAssistflag == TRUE && cp_stFlg.RunPermitFlg == TRUE && cp_stFlg.SpiOffsetFirstSetFlg ==1)
             {
-                uart_slSpdRefRpm = -(10000/(ass_ParaCong.uwWheelDiameter))*ass_ParaCong.uwNmBackChainring*ass_ParaCong.uwMechRationMotor/ass_ParaCong.uwNmFrontChainring;
-//                cp_stBikeRunInfoPara.BikeSpeedKmH = 60;      //constant display of 6km/h
+                signal_state.Sensor = TRUE;
             }
-            else if(cp_stFlg.RunModelSelect == MountainBIKE)
+            else if( cp_stFlg.SpiOffsetFirstSetFlg == 0 && cp_stFlg.RunPermitFlg == TRUE && cp_stFlg.SpiOffsetFirstSetFinishFlg == FALSE)
             {
-                uart_slSpdRefRpm = (10000/(ass_ParaCong.uwWheelDiameter))*ass_ParaCong.uwNmBackChainring*ass_ParaCong.uwMechRationMotor/ass_ParaCong.uwNmFrontChainring;
-//                cp_stBikeRunInfoPara.BikeSpeedKmH = 60;      //constant display of 6km/h
+                signal_state.Sensor = TRUE;  //for Spi Theta Offset 
             }
             else
             {
+                signal_state.Sensor = FALSE;
+            }
+
+            /* Throttle to Torque */
+            if(Event_pvt_blBikeThroFlgZ1 == TRUE)
+            {
+                /* Initial Value of Torque Assit Output */
+                ass_stCalOut.swAssitCurRef = scm_swIqFdbLpfPu;    
+                ass_pvt_stCurLpf.slY.sw.hi = scm_swIqFdbLpfPu;
             }
 
+            uart_swTorqRefNm = ass_stCalOut.swAssitCurRef;
         }
         else
         {
-            uart_slSpdRefRpm = 0;
+            signal_state.Sensor = TRUE;
+
+            /* Torque to Throttle */
+            if(Event_pvt_blBikeThroFlgZ1 == FALSE)
+            {
+                /* Initial Value of Throttle Assit Output */
+                tstIqRef = scm_swIqFdbLpfPu;
+                tstIqRefTarget = scm_swIqFdbLpfPu;
+                bikespeed_stPIOut.slIqRefPu = scm_swIqFdbLpfPu << 16;    
+            }
+
+            /* Bike Throttle Assist Iqref Ramp */
+            if(tstIqRef< tstIqRefTarget - 100)
+            {
+                if(tstIqRefTarget >= tstIqRefTargetZ1)
+                {
+                    tstIqRef += 100;
+                }               
+            }
+            else
+            {
+                tstIqRef = tstIqRefTarget;
+            }
+            tstIqRefTargetZ1 = tstIqRefTarget;
+            uart_swTorqRefNm = tstIqRef * ass_stCalIn.swDirection;
         }
-    }
-    else
-    {          
-        if ((uart_slSpdRefRpm >= 10 || uart_slSpdRefRpm <= -10) && cp_stFlg.RunPermitFlg == TRUE )
+        Event_pvt_blBikeThroFlgZ1 = Event_pvt_blBikeThroFlg;
+
+        /* Speed assist mode flag */
+        if((cp_stFlg.RunModelSelect == CityBIKE) || (cp_stFlg.RunModelSelect == MountainBIKE))
         {
-            signal_state.Assist = TRUE;
+            if(cp_stBikeRunInfoPara.uwBikeGear == 0x22)
+            {
+                AssistCNT ++;
+                if(AssistCNT > 200 && cp_stFlg.RunPermitFlg == TRUE)
+                {
+                    signal_state.Assist = TRUE;
+                    AssistCNT = 200;
+                }
+            }
+            else
+            {
+                AssistCNT = 0;
+                signal_state.Assist = FALSE;
+            }
+
+            if(signal_state.Assist == TRUE)
+            {
+                //6km/H = 100m/min = 1.67m/s
+                if(cp_stFlg.RunModelSelect == CityBIKE)
+                {
+                    uart_slSpdRefRpm = -(10000/(ass_stParaCong.uwWheelPerimeter))*ass_stParaCong.uwNmBackChainring*ass_stParaCong.uwMechRationMotor/ass_stParaCong.uwNmFrontChainring;
+    //                cp_stBikeRunInfoPara.BikeSpeedKmH = 60;      //constant display of 6km/h
+                }
+                else if(cp_stFlg.RunModelSelect == MountainBIKE)
+                {
+                    uart_slSpdRefRpm = (10000/(ass_stParaCong.uwWheelPerimeter))*ass_stParaCong.uwNmBackChainring*ass_stParaCong.uwMechRationMotor/ass_stParaCong.uwNmFrontChainring;
+    //                cp_stBikeRunInfoPara.BikeSpeedKmH = 60;      //constant display of 6km/h
+                }
+                else
+                {
+                }
+
+            }
+            else
+            {
+                uart_slSpdRefRpm = 0;
+            }
         }
         else
-        {
-            signal_state.Assist = FALSE;
-        }
-    }  
+        {          
+            if ((uart_slSpdRefRpm >= 10 || uart_slSpdRefRpm <= -10) && cp_stFlg.RunPermitFlg == TRUE )
+            {
+                signal_state.Assist = TRUE;
+            }
+            else
+            {
+                signal_state.Assist = FALSE;
+            }
+        }  
+    }
+
 }
 
 void Event_5ms(void)
@@ -217,43 +246,41 @@ void Event_5ms(void)
 
 void Event_10ms(void)
 {
-    /* Throttle ADC voltage update */
-    bikethrottle_voBikeThrottleADC();
-
-    /*  Speed command set */
-    if(cp_stFlg.RunModelSelect != CityBIKE && cp_stFlg.RunModelSelect != MountainBIKE )
+    if(switch_flg.SysCoef_Flag == TRUE)
     {
-        /* Use instrument */
-//        Signal_detect();
-      
-        /* Use upper computer */
-        if(cp_stFlg.RotateDirectionSelect == ForwardRotate)
-        {
-            uart_slSpdRefRpm = ((SLONG)MC_MotorSPD_rpm_Percent*5000)/100;
-        }
-        else if(cp_stFlg.RotateDirectionSelect == BackwardRotate)
+        /* Throttle ADC voltage update */
+        bikethrottle_voBikeThrottleADC();
+
+        /*  Speed command set */
+        if(cp_stFlg.RunModelSelect != CityBIKE && cp_stFlg.RunModelSelect != MountainBIKE )
         {
-            uart_slSpdRefRpm = -((SLONG)MC_MotorSPD_rpm_Percent*5000)/100;
+            /* Use instrument */
+//           Signal_detect();
+          
+            /* Use upper computer */
+            if(cp_stFlg.RotateDirectionSelect == ForwardRotate)
+            {
+                uart_slSpdRefRpm = ((SLONG)MC_MotorSPD_rpm_Percent*5000)/100;
+            }
+            else if(cp_stFlg.RotateDirectionSelect == BackwardRotate)
+            {
+                uart_slSpdRefRpm = -((SLONG)MC_MotorSPD_rpm_Percent*5000)/100;
+            }
+      
+             if(abs(uart_slSpdRefRpm) < 300)
+             {
+                 uart_slSpdRefRpm = 0;
+             }
         }
-     
-        /*  Use Bike Speed PI */
-//         bikespeed_voPuCal();
-//         bikespeed_voPI(&bikespeed_stPIIn, &bikeSpdPIOut);
-//         ass_CalIn.swDirection =-1;       
-//         uart_slSpdRefRpm = bikeSpdPIOut.swSpdRefRpm * ass_CalIn.swDirection;
+        
+        /* Bike light control */
+        Can_Light_switch();
+        bikelight_voBikeLightControl(cp_stBikeRunInfoPara.uwLightSwitch);
 
-         if(abs(uart_slSpdRefRpm) < 300)
-         {
-             uart_slSpdRefRpm = 0;
-         }
+        /* Trip cal when open */
+        bikespeed_votempTripCal();
     }
-    
-    /* Bike light control */
-    Can_Light_switch();
-    bikelight_voBikeLightControl(cp_stBikeRunInfoPara.uwLightSwitch);
 
-    /* Trip cal when open */
-    bikespeed_votempTripCal();
 }
 
 
@@ -265,10 +292,68 @@ void  Event_20ms(void)
 
 void Event_100ms(void)
 {
-    /* Bike Speed LPF */
-    bikespeed_stFreGetOut.uwLPFFrequencyPu = (bikespeed_stFreGetOut.uwLPFFrequencyPu * bikespeed_stFreGetCof.uwBikeSpeedLPFGain +
-                                              bikespeed_stFreGetOut.uwFrequencyPu * (100 - bikespeed_stFreGetCof.uwBikeSpeedLPFGain)) /
-                                             100;
+    SWORD swIqLowerPu;
+    if(switch_flg.SysCoef_Flag == TRUE)
+    {
+        /* Bike Speed LPF */
+        bikespeed_stFreGetOut.uwLPFFrequencyPu = (bikespeed_stFreGetOut.uwLPFFrequencyPu * bikespeed_stFreGetCof.uwBikeSpeedLPFGain +
+                                                  bikespeed_stFreGetOut.uwFrequencyPu * (100 - bikespeed_stFreGetCof.uwBikeSpeedLPFGain)) /
+                                                 100;
+        /* Bike Throttle Assist */
+        if((bikethrottle_stBikeThrottleOut.uwThrottlePercent > 200)  && (cp_stBikeRunInfoPara.uwBikeGear > 0) && (cp_stFlg.RunPermitFlg == TRUE))
+        {
+            // signal_state.Sensor = TRUE;
+            Event_pvt_blBikeThroFlg = TRUE;
+
+            /* Bike Speed Ref, 200-890Percent: 4-25km/h */
+            tstBikeSpdRefTarget = ((ULONG)(25 - 4) *(bikethrottle_stBikeThrottleOut.uwThrottlePercent - 200)/690 + 4) * BIKESPEED_KMPERH2FREQPU; // Q20
+            
+            /* Bike Speed Ref Ramp */
+            if(tstBikeSpdRef < tstBikeSpdRefTarget - 80)
+            {   
+                if(tstBikeSpdRefTarget >= tstBikeSpdRefTargetZ1)
+                {
+                    tstBikeSpdRef += 80;
+                }             
+            }
+            else if(tstBikeSpdRef > tstBikeSpdRefTarget + 160)
+            {
+                tstBikeSpdRef -= 160;
+            }               
+            else
+            {
+                tstBikeSpdRef = tstBikeSpdRefTarget;
+            }           
+            tstBikeSpdRefTargetZ1 = tstBikeSpdRefTarget;
+            
+            /* Bike Speed Closed Loop */
+            swIqLowerPu = (flx_stCtrlOut.swIqLimPu < abs(pwr_stPwrLimOut2.swIqLimPu)) ? flx_stCtrlOut.swIqLimPu : abs(pwr_stPwrLimOut2.swIqLimPu);  
+            bikespeed_stPIIn.slSpdRefPu = tstBikeSpdRef;
+            bikespeed_stPIIn.slSpdFdkPu = bikespeed_stFreGetOut.uwLPFFrequencyPu; //bikespeed_stFreGetOut.uwFrequencyPu; 
+            bikespeed_stPIIn.swIqMaxPu = swIqLowerPu; // ((SLONG)55 << 14)/60;
+            bikespeed_stPIIn.swIqMinPu = 0;           
+            bikespeed_voPI(&bikespeed_stPIIn, &bikespeed_stPIOut);
+            
+            tstIqRefTarget = bikespeed_stPIOut.swIqRefPu;
+        }
+        else
+        {      
+            // signal_state.Sensor = FALSE;
+            Event_pvt_blBikeThroFlg = FALSE;
+            bikespeed_voPIInit();
+
+            tstIqRef = 0;
+            tstIqRefTarget = 0;
+            tstIqRefTargetZ1 = 0;
+            tstBikeSpdRef = 0;
+            tstBikeSpdRefTarget = 0;
+            tstBikeSpdRefTargetZ1 = 0;
+            
+            // bikespeed_stPIIn.slSpdRefPu = 0; // Q20
+            // bikespeed_stPIIn.slSpdFdkPu = bikespeed_stFreGetOut.uwLPFFrequencyPu; //bikespeed_stFreGetOut.uwFrequencyPu; 
+        }  
+    }
+
 }
 
 void  Event_200ms(void)
@@ -276,33 +361,36 @@ void  Event_200ms(void)
     /* Upper Computer Info Update */
     Can_voMC_Run_200ms();
 
-    /* Bike Sesor Suply Voltage Fault Detect */
-    bikelight_voGetBikeLightError(adc_stUpOut.uwU6VPu);
-    display_voGetDisplayError(adc_stUpOut.uwU12VPu);
-    bikespeed_voGetBikeSpeedPwrError(adc_stUpOut.uwU5VPu);
-
-    /* Bike Sensor Faults Detect */
-//    if((cp_stFlg.RunModelSelect == MountainBIKE) || (cp_stFlg.RunModelSelect == CityBIKE))
-//    {
-//        alm_stBikeIn.uwTroqReg = torsensor_stTorSensorOut.uwTorqueReg;
-//        alm_stBikeIn.uwTroqPu = torsensor_stTorSensorOut.uwTorqueLPFPu;//torsensor_stTorSensorOut.uwTorquePu;
-//        alm_stBikeIn.blBikeSpdOvrFlg = bikespeed_stFreGetOut.blBikeSpeedSensorPwrErrorFlg;
-//        alm_stBikeIn.blCadenceFreqOvrFlg = cadence_stFreGetOut.blCadenceSensorErrorFlg;
-//        alm_stBikeIn.swMotorSpdDir = ass_CalIn.swDirection;
-//        alm_stBikeIn.swMotorSpdPu = scm_stSpdFbkLpf.slY.sw.hi;
-//        alm_stBikeIn.uwBikeSpdPu = bikespeed_stFreGetOut.uwFrequencyPu;
-//        alm_stBikeIn.uwCadenceFreqPu = cadence_stFreGetOut.uwFrequencyPu;
-//        alm_stBikeIn.uwMotorNTCReg = adc_stUpOut.MotorTempReg;
-//        alm_stBikeIn.uwPCBNTCReg = adc_stUpOut.PCBTempReg;
-//        alm_stBikeIn.uwThrottleReg = adc_stUpOut.uwThrottleReg;
-//        alm_stBikeIn.blThrottleExistFlg = FALSE;
-//        alm_stBikeIn.blMotorNTCExistFlg = FALSE;
-//        alm_voDetec200MS(&alm_stBikeIn, &alm_stDetect200MSCoef);   
-//    }
-    
-    if (switch_flg.SysFault_Flag == TRUE)
+    if(switch_flg.SysCoef_Flag == TRUE)
     {
-        SendData(ID_MC_BC, MODE_REPORT, 0x1104, (uint8_t *)&MC_ErrorCode.Code);
+        /* Bike Sesor Suply Voltage Fault Detect */
+        bikelight_voGetBikeLightError(adc_stUpOut.uwU6VPu);
+        display_voGetDisplayError(adc_stUpOut.uwU12VPu);
+        bikespeed_voGetBikeSpeedPwrError(adc_stUpOut.uwU5VPu);
+
+        /* Bike Sensor Faults Detect */
+//        if((cp_stFlg.RunModelSelect == MountainBIKE) || (cp_stFlg.RunModelSelect == CityBIKE))
+//        {
+//            alm_stBikeIn.uwTroqReg = torsensor_stTorSensorOut.uwTorqueReg;
+//            alm_stBikeIn.uwTroqPu = torsensor_stTorSensorOut.uwTorqueLPFPu;//torsensor_stTorSensorOut.uwTorquePu;
+//            alm_stBikeIn.blBikeSpdOvrFlg = bikespeed_stFreGetOut.blBikeSpeedSensorPwrErrorFlg;
+//            alm_stBikeIn.blCadenceFreqOvrFlg = cadence_stFreGetOut.blCadenceSensorErrorFlg;
+//            alm_stBikeIn.swMotorSpdDir = ass_stCalIn.swDirection;
+//            alm_stBikeIn.swMotorSpdPu = scm_stSpdFbkLpf.slY.sw.hi;
+//            alm_stBikeIn.uwBikeSpdPu = bikespeed_stFreGetOut.uwFrequencyPu;
+//            alm_stBikeIn.uwCadenceFreqPu = cadence_stFreGetOut.uwFrequencyPu;
+//            alm_stBikeIn.uwMotorNTCReg = adc_stUpOut.MotorTempReg;
+//            alm_stBikeIn.uwPCBNTCReg = adc_stUpOut.PCBTempReg;
+//            alm_stBikeIn.uwThrottleReg = adc_stUpOut.uwThrottleReg;
+//            alm_stBikeIn.blThrottleExistFlg = FALSE;
+//            alm_stBikeIn.blMotorNTCExistFlg = FALSE;
+//            alm_voDetec200MS(&alm_stBikeIn, &alm_stDetect200MSCoef);   
+//        }
+        
+        if (switch_flg.SysFault_Flag == TRUE)
+        {
+            SendData(ID_MC_BC, MODE_REPORT, 0x1104, (uint8_t *)&MC_ErrorCode.Code);
+        }
     }
 }
 

User project/1.FrameLayer/Source/gd32f30x_it.c

@@ -17,7 +17,7 @@
 #include "TimeTask_Event.h"
 #include "syspar.h"
 #include "can.h"
-
+#include "FSM_1st.h"
 /******************************
  *  Parameter
  ******************************/
@@ -263,34 +263,42 @@ void TIMER1_IRQHandler(void)
     clasB_uwTIM1Cnt++;
     if (timer_interrupt_flag_get(TIMER1, TIMER_INT_FLAG_UP))
     {
-        uwIntSource = 1;
-        cadence_voCadenceCal(uwIntSource);
-        bikespeed_voBikeSpeedCal(uwIntSource);
+        if(switch_flg.SysCoef_Flag == TRUE)
+        {
+            uwIntSource = 1;
+            cadence_voCadenceCal(uwIntSource);
+            bikespeed_voBikeSpeedCal(uwIntSource);
+        }
         timer_interrupt_flag_clear(TIMER1, TIMER_INT_FLAG_UP);
     }
     else if (timer_interrupt_flag_get(TIMER1, TIMER_INT_FLAG_CH2))
     {
-        uwIntSource = 2;        
-        cadence_voCadenceCal(uwIntSource);
-        if(gpio_input_bit_get(GPIOB, GPIO_PIN_10))
-        {
-           /* reset the CH2P and CH2NP bits */
-           TIMER_CHCTL2(TIMER1) &= (~(uint32_t)(TIMER_CHCTL2_CH2P|TIMER_CHCTL2_CH2NP));
-           TIMER_CHCTL2(TIMER1) |= (uint32_t)((uint32_t)(TIMER_IC_POLARITY_FALLING) << 8U);
-        }
-        else
+        if(switch_flg.SysCoef_Flag == TRUE)
         {
-           /* reset the CH2P and CH2NP bits */
-           TIMER_CHCTL2(TIMER1) &= (~(uint32_t)(TIMER_CHCTL2_CH2P|TIMER_CHCTL2_CH2NP));
-           TIMER_CHCTL2(TIMER1) |= (uint32_t)((uint32_t)(TIMER_IC_POLARITY_RISING) << 8U);       
-        }
-        
+            uwIntSource = 2;        
+            cadence_voCadenceCal(uwIntSource);
+            if(gpio_input_bit_get(GPIOB, GPIO_PIN_10))
+            {
+               /* reset the CH2P and CH2NP bits */
+               TIMER_CHCTL2(TIMER1) &= (~(uint32_t)(TIMER_CHCTL2_CH2P|TIMER_CHCTL2_CH2NP));
+               TIMER_CHCTL2(TIMER1) |= (uint32_t)((uint32_t)(TIMER_IC_POLARITY_FALLING) << 8U);
+            }
+            else
+            {
+               /* reset the CH2P and CH2NP bits */
+               TIMER_CHCTL2(TIMER1) &= (~(uint32_t)(TIMER_CHCTL2_CH2P|TIMER_CHCTL2_CH2NP));
+               TIMER_CHCTL2(TIMER1) |= (uint32_t)((uint32_t)(TIMER_IC_POLARITY_RISING) << 8U);       
+            }
+        }      
         timer_interrupt_flag_clear(TIMER1, TIMER_INT_FLAG_CH2);
     }
     else if (timer_interrupt_flag_get(TIMER1, TIMER_INT_FLAG_CH3))
     {
-        uwIntSource = 3;
-        bikespeed_voBikeSpeedCal(uwIntSource);
+        if(switch_flg.SysCoef_Flag == TRUE)
+        {
+            uwIntSource = 3;
+            bikespeed_voBikeSpeedCal(uwIntSource);
+        }
         timer_interrupt_flag_clear(TIMER1, TIMER_INT_FLAG_CH3);
     }
 }

User project/1.FrameLayer/Source/main.c

@@ -184,7 +184,7 @@ void mn_voParaSet(void)
 
     if (i2c_stRXCRCOut.blHistoryParaFltFlg == FALSE)
     {
-        ass_ParaSet.uwAsssistSelectNum = I2C_uwHistoryParaRead[0];
+        ass_stParaSet.uwAsssistSelectNum = I2C_uwHistoryParaRead[0];
         cp_stHistoryPara.uwOpenTimes = I2C_uwHistoryParaRead[1];
         cp_stHistoryPara.ulUsedTime = (((ULONG)I2C_uwHistoryParaRead[2]) << 16) + I2C_uwHistoryParaRead[3];
         cp_stHistoryPara.uwNTCTempMaxCe = I2C_uwHistoryParaRead[4];
@@ -248,18 +248,18 @@ void mn_voParaSet(void)
             {}
             if (i2c_stRXCRCOut.blBikeParaFltFlg == FALSE)
             {
-                ass_ParaCong.uwWheelDiameter = I2C_uwBikeParaRead[0];
-                ass_ParaCong.uwMechRationMotor = I2C_uwBikeParaRead[1];
-                ass_ParaCong.uwAssistMaxSpdKmH = I2C_uwBikeParaRead[2];
-                ass_ParaCong.uwThrottleMaxSpdKmH = I2C_uwBikeParaRead[3];
-                ass_ParaCong.uwNmFrontChainring = I2C_uwBikeParaRead[4];
-                ass_ParaCong.uwNmBackChainring = I2C_uwBikeParaRead[5];
-                ass_ParaCong.uwAssistSelect1 = I2C_uwBikeParaRead[6];
-                ass_ParaCong.uwAssistSelect2 = I2C_uwBikeParaRead[7];
-                ass_ParaCong.uwLightVoltage = I2C_uwBikeParaRead[8];
-                ass_ParaCong.swDeltDiameter = I2C_uwBikeParaRead[9];
-                ass_ParaCong.uwStartMode = I2C_uwBikeParaRead[10];
-                ass_ParaCong.uwAutoPowerOffTime = I2C_uwBikeParaRead[11];
+                ass_stParaCong.uwWheelPerimeter = I2C_uwBikeParaRead[0];
+                ass_stParaCong.uwMechRationMotor = I2C_uwBikeParaRead[1];
+                ass_stParaCong.uwAssistMaxSpdKmH = I2C_uwBikeParaRead[2];
+                ass_stParaCong.uwThrottleMaxSpdKmH = I2C_uwBikeParaRead[3];
+                ass_stParaCong.uwNmFrontChainring = I2C_uwBikeParaRead[4];
+                ass_stParaCong.uwNmBackChainring = I2C_uwBikeParaRead[5];
+                ass_stParaCong.uwAssistSelect1 = I2C_uwBikeParaRead[6];
+                ass_stParaCong.uwAssistSelect2 = I2C_uwBikeParaRead[7];
+                ass_stParaCong.uwLightVoltage = I2C_uwBikeParaRead[8];
+                ass_stParaCong.swDeltPerimeter = I2C_uwBikeParaRead[9];
+                ass_stParaCong.uwStartMode = I2C_uwBikeParaRead[10];
+                ass_stParaCong.uwAutoPowerOffTime = I2C_uwBikeParaRead[11];
             }
             else
             {}
@@ -307,19 +307,19 @@ void mn_voParaSet(void)
             }
             if (i2c_stRXCRCOut.blAssistParaFltFlg == FALSE)
             {
-                ass_ParaSet.uwStartupCoef = I2C_uwAssistParaRead[0];
-                ass_ParaSet.uwStartupCruiseCoef = I2C_uwAssistParaRead[1];
-                ass_ParaSet.uwAssistStartNm = I2C_uwAssistParaRead[2];
-                ass_ParaSet.uwAssistStopNm = I2C_uwAssistParaRead[3];
-                ass_ParaSet.uwStartUpGainStep = I2C_uwAssistParaRead[4];
-                ass_ParaSet.uwStartUpCadNm = I2C_uwAssistParaRead[5];
-                ass_ParaSet.uwTorLPFCadNm = I2C_uwAssistParaRead[6];
-                ass_ParaSet.uwSpeedAssistSpdRpm = I2C_uwAssistParaRead[7];
-                ass_ParaSet.uwSpeedAssistIMaxA = I2C_uwAssistParaRead[8];
-                ass_ParaSet.uwAssistLimitBikeSpdStart = I2C_uwAssistParaRead[9];
-                ass_ParaSet.uwAssistLimitBikeSpdStop = I2C_uwAssistParaRead[10];
-                ass_ParaSet.uwCadenceWeight = I2C_uwAssistParaRead[11];
-//                ass_ParaSet.uwTorWeight = Q12_1 - ass_ParaSet.uwCadenceWeight;
+                ass_stParaSet.uwStartupCoef = I2C_uwAssistParaRead[0];
+                ass_stParaSet.uwStartupCruiseCoef = I2C_uwAssistParaRead[1];
+                ass_stParaSet.uwAssistStartNm = I2C_uwAssistParaRead[2];
+                ass_stParaSet.uwAssistStopNm = I2C_uwAssistParaRead[3];
+                ass_stParaSet.uwStartUpGainStep = I2C_uwAssistParaRead[4];
+                ass_stParaSet.uwStartUpCadNm = I2C_uwAssistParaRead[5];
+                ass_stParaSet.uwTorLPFCadNm = I2C_uwAssistParaRead[6];
+                ass_stParaSet.uwSpeedAssistSpdRpm = I2C_uwAssistParaRead[7];
+                ass_stParaSet.uwSpeedAssistIMaxA = I2C_uwAssistParaRead[8];
+                ass_stParaSet.uwAssistLimitBikeSpdStart = I2C_uwAssistParaRead[9];
+                ass_stParaSet.uwAssistLimitBikeSpdStop = I2C_uwAssistParaRead[10];
+                ass_stParaSet.uwCadenceWeight = I2C_uwAssistParaRead[11];
+//                ass_stParaSet.uwTorWeight = Q12_1 - ass_stParaSet.uwCadenceWeight;
             }
             else
             {}
@@ -365,18 +365,18 @@ void mn_voParaUpdate(void)
 
         if (cp_stFlg.ParaBikeInfoUpdateFlg == TRUE)
         {
-            ass_ParaCong.uwWheelDiameter = MC_UpcInfo.stBikeInfo.uwWheelDiameter;
-            ass_ParaCong.uwMechRationMotor = MC_UpcInfo.stBikeInfo.uwMechRationMotor;
-            ass_ParaCong.uwAssistMaxSpdKmH = MC_UpcInfo.stBikeInfo.uwAssistMaxSpdKmH;
-            ass_ParaCong.uwThrottleMaxSpdKmH = MC_UpcInfo.stBikeInfo.uwThrottleMaxSpdKmH;
-            ass_ParaCong.uwNmFrontChainring = MC_UpcInfo.stBikeInfo.uwNmFrontChainring;
-            ass_ParaCong.uwNmBackChainring = MC_UpcInfo.stBikeInfo.uwNmBackChainring;
-            ass_ParaCong.uwAssistSelect1 = MC_UpcInfo.stBikeInfo.uwAssistSelect1;
-            ass_ParaCong.uwAssistSelect2 = MC_UpcInfo.stBikeInfo.uwAssistSelect2;
-            ass_ParaCong.uwLightVoltage = MC_UpcInfo.stBikeInfo.uwLightVoltage;
-            ass_ParaCong.swDeltDiameter = MC_UpcInfo.stBikeInfo.swWheelSizeAdjust;
-            ass_ParaCong.uwStartMode = MC_UpcInfo.stBikeInfo.uwStartMode;
-            ass_ParaCong.uwAutoPowerOffTime = MC_UpcInfo.stBikeInfo.uwAutoPowerOffTime;
+            ass_stParaCong.uwWheelPerimeter = MC_UpcInfo.stBikeInfo.uwWheelPerimeter;
+            ass_stParaCong.uwMechRationMotor = MC_UpcInfo.stBikeInfo.uwMechRationMotor;
+            ass_stParaCong.uwAssistMaxSpdKmH = MC_UpcInfo.stBikeInfo.uwAssistMaxSpdKmH;
+            ass_stParaCong.uwThrottleMaxSpdKmH = MC_UpcInfo.stBikeInfo.uwThrottleMaxSpdKmH;
+            ass_stParaCong.uwNmFrontChainring = MC_UpcInfo.stBikeInfo.uwNmFrontChainring;
+            ass_stParaCong.uwNmBackChainring = MC_UpcInfo.stBikeInfo.uwNmBackChainring;
+            ass_stParaCong.uwAssistSelect1 = MC_UpcInfo.stBikeInfo.uwAssistSelect1;
+            ass_stParaCong.uwAssistSelect2 = MC_UpcInfo.stBikeInfo.uwAssistSelect2;
+            ass_stParaCong.uwLightVoltage = MC_UpcInfo.stBikeInfo.uwLightVoltage;
+            ass_stParaCong.swDeltPerimeter = MC_UpcInfo.stBikeInfo.swWheelSizeAdjust;
+            ass_stParaCong.uwStartMode = MC_UpcInfo.stBikeInfo.uwStartMode;
+            ass_stParaCong.uwAutoPowerOffTime = MC_UpcInfo.stBikeInfo.uwAutoPowerOffTime;
 
             cp_stFlg.ParaAssistUpdateFinishFlg = TRUE;
             cp_stFlg.ParaBikeInfoUpdateFlg = FALSE;
@@ -422,19 +422,19 @@ void mn_voParaUpdate(void)
 
         if (cp_stFlg.ParaAInfoUpdateFlg == TRUE)
         {
-            ass_ParaSet.uwStartupCoef = MC_UpcInfo.stAssistInfo.swStartupGain;
-            ass_ParaSet.uwStartupCruiseCoef = MC_UpcInfo.stAssistInfo.swStartcruiseGain;
-            ass_ParaSet.uwAssistStartNm = MC_UpcInfo.stAssistInfo.uwAssistStartNm;
-            ass_ParaSet.uwAssistStopNm = MC_UpcInfo.stAssistInfo.uwAssistStopNm;
-            ass_ParaSet.uwStartUpGainStep = MC_UpcInfo.stAssistInfo.uwStartUpGainStep;
-            ass_ParaSet.uwStartUpCadNm = MC_UpcInfo.stAssistInfo.uwStartUpCadNm;
-            ass_ParaSet.uwTorLPFCadNm = MC_UpcInfo.stAssistInfo.uwTorLPFCadNm;
-            ass_ParaSet.uwSpeedAssistSpdRpm = MC_UpcInfo.stAssistInfo.uwSpeedAssistSpdRpm;
-            ass_ParaSet.uwSpeedAssistIMaxA = MC_UpcInfo.stAssistInfo.uwSpeedAssistIMaxA;
-            ass_ParaSet.uwAssistLimitBikeSpdStart = MC_UpcInfo.stAssistInfo.uwAssistLimitBikeSpdStart;
-            ass_ParaSet.uwAssistLimitBikeSpdStop = MC_UpcInfo.stAssistInfo.uwAssistLimitBikeSpdStop;
-            ass_ParaSet.uwCadenceWeight = MC_UpcInfo.stAssistInfo.uwCadenceAssistWeight;
-//            ass_ParaSet.uwTorWeight = Q12_1 - ass_ParaSet.uwCadenceWeight;
+            ass_stParaSet.uwStartupCoef = MC_UpcInfo.stAssistInfo.swStartupGain;
+            ass_stParaSet.uwStartupCruiseCoef = MC_UpcInfo.stAssistInfo.swStartcruiseGain;
+            ass_stParaSet.uwAssistStartNm = MC_UpcInfo.stAssistInfo.uwAssistStartNm;
+            ass_stParaSet.uwAssistStopNm = MC_UpcInfo.stAssistInfo.uwAssistStopNm;
+            ass_stParaSet.uwStartUpGainStep = MC_UpcInfo.stAssistInfo.uwStartUpGainStep;
+            ass_stParaSet.uwStartUpCadNm = MC_UpcInfo.stAssistInfo.uwStartUpCadNm;
+            ass_stParaSet.uwTorLPFCadNm = MC_UpcInfo.stAssistInfo.uwTorLPFCadNm;
+            ass_stParaSet.uwSpeedAssistSpdRpm = MC_UpcInfo.stAssistInfo.uwSpeedAssistSpdRpm;
+            ass_stParaSet.uwSpeedAssistIMaxA = MC_UpcInfo.stAssistInfo.uwSpeedAssistIMaxA;
+            ass_stParaSet.uwAssistLimitBikeSpdStart = MC_UpcInfo.stAssistInfo.uwAssistLimitBikeSpdStart;
+            ass_stParaSet.uwAssistLimitBikeSpdStop = MC_UpcInfo.stAssistInfo.uwAssistLimitBikeSpdStop;
+            ass_stParaSet.uwCadenceWeight = MC_UpcInfo.stAssistInfo.uwCadenceAssistWeight;
+//            ass_stParaSet.uwTorWeight = Q12_1 - ass_stParaSet.uwCadenceWeight;
 
             cp_stFlg.ParaAssistUpdateFinishFlg = TRUE;
             cp_stFlg.ParaAInfoUpdateFlg = FALSE;
@@ -502,21 +502,21 @@ void mn_voEEUperParaUpdate(void)
         Syspara2.stMotorPara.uwJD.uwReal = cp_stMotorPara.swJD;
         Syspara2.stMotorPara.uwTorMaxNm.uwReal = cp_stMotorPara.swTorMax;
     }
-    Syspara2.stBikePara.swDeltDiameter.swReal = ass_ParaCong.swDeltDiameter;
+    Syspara2.stBikePara.swDeltPerimeter.swReal = ass_stParaCong.swDeltPerimeter;
     if (MC_UpcInfo.stBikeInfo.uwSaveFlg == TRUE)
     {
-        Syspara2.stBikePara.uwWheelDiameter.uwReal = ass_ParaCong.uwWheelDiameter;
-        Syspara2.stBikePara.uwMechRationMotor.uwReal = ass_ParaCong.uwMechRationMotor;
-        Syspara2.stBikePara.uwAssistMaxSpdKmH.uwReal = ass_ParaCong.uwAssistMaxSpdKmH;
-        Syspara2.stBikePara.uwThrottleMaxSpdKmH.uwReal = ass_ParaCong.uwThrottleMaxSpdKmH;
-        Syspara2.stBikePara.uwNmFrontChainring.uwReal = ass_ParaCong.uwNmFrontChainring;
-        Syspara2.stBikePara.uwNmBackChainring.uwReal = ass_ParaCong.uwNmBackChainring;
-        Syspara2.stBikePara.uwAssistSelect1.uwReal = ass_ParaCong.uwAssistSelect1;
-        Syspara2.stBikePara.uwAssistSelect2.uwReal = ass_ParaCong.uwAssistSelect2;
-        Syspara2.stBikePara.uwLightVoltage.uwReal = ass_ParaCong.uwLightVoltage;
-        Syspara2.stBikePara.swDeltDiameter.swReal = ass_ParaCong.swDeltDiameter;
-        Syspara2.stBikePara.uwStartMode.uwReal = ass_ParaCong.uwStartMode;
-        Syspara2.stBikePara.uwAutoPowerOffTime.uwReal = ass_ParaCong.uwAutoPowerOffTime;
+        Syspara2.stBikePara.uwWheelPerimeter.uwReal = ass_stParaCong.uwWheelPerimeter;
+        Syspara2.stBikePara.uwMechRationMotor.uwReal = ass_stParaCong.uwMechRationMotor;
+        Syspara2.stBikePara.uwAssistMaxSpdKmH.uwReal = ass_stParaCong.uwAssistMaxSpdKmH;
+        Syspara2.stBikePara.uwThrottleMaxSpdKmH.uwReal = ass_stParaCong.uwThrottleMaxSpdKmH;
+        Syspara2.stBikePara.uwNmFrontChainring.uwReal = ass_stParaCong.uwNmFrontChainring;
+        Syspara2.stBikePara.uwNmBackChainring.uwReal = ass_stParaCong.uwNmBackChainring;
+        Syspara2.stBikePara.uwAssistSelect1.uwReal = ass_stParaCong.uwAssistSelect1;
+        Syspara2.stBikePara.uwAssistSelect2.uwReal = ass_stParaCong.uwAssistSelect2;
+        Syspara2.stBikePara.uwLightVoltage.uwReal = ass_stParaCong.uwLightVoltage;
+        Syspara2.stBikePara.swDeltPerimeter.swReal = ass_stParaCong.swDeltPerimeter;
+        Syspara2.stBikePara.uwStartMode.uwReal = ass_stParaCong.uwStartMode;
+        Syspara2.stBikePara.uwAutoPowerOffTime.uwReal = ass_stParaCong.uwAutoPowerOffTime;
     }
     if (MC_UpcInfo.stMContorlInfo.uwSaveFlg == TRUE)
     {
@@ -554,18 +554,18 @@ void mn_voEEUperParaUpdate(void)
     }
     if (MC_UpcInfo.stAssistInfo.uwSaveFlg == TRUE)
     {
-        Syspara2.stAssistPara.uwStartupGain.uwReal = ass_ParaSet.uwStartupCoef;
-        Syspara2.stAssistPara.uwStartcruiseGain.uwReal = ass_ParaSet.uwStartupCruiseCoef;
-        Syspara2.stAssistPara.uwAssistStartNm.uwReal = ass_ParaSet.uwAssistStartNm;
-        Syspara2.stAssistPara.uwAssistStopNm.uwReal = ass_ParaSet.uwAssistStopNm;
-        Syspara2.stAssistPara.uwStartUpGainStep.uwReal = ass_ParaSet.uwStartUpGainStep;
-        Syspara2.stAssistPara.uwStartUpCadNm.uwReal = ass_ParaSet.uwStartUpCadNm;
-        Syspara2.stAssistPara.uwTorLPFCadNm.uwReal = ass_ParaSet.uwTorLPFCadNm;
-        Syspara2.stAssistPara.uwSpeedAssistSpdRpm.uwReal = ass_ParaSet.uwSpeedAssistSpdRpm;
-        Syspara2.stAssistPara.uwSpeedAssistIMaxA.uwReal = ass_ParaSet.uwSpeedAssistIMaxA;
-        Syspara2.stAssistPara.uwAssistLimitBikeSpdStart.uwReal = ass_ParaSet.uwAssistLimitBikeSpdStart;
-        Syspara2.stAssistPara.uwAssistLimitBikeSpdStop.uwReal = ass_ParaSet.uwAssistLimitBikeSpdStop;
-        Syspara2.stAssistPara.uwCadenceAssistWeight.uwReal = ass_ParaSet.uwCadenceWeight;
+        Syspara2.stAssistPara.uwStartupGain.uwReal = ass_stParaSet.uwStartupCoef;
+        Syspara2.stAssistPara.uwStartcruiseGain.uwReal = ass_stParaSet.uwStartupCruiseCoef;
+        Syspara2.stAssistPara.uwAssistStartNm.uwReal = ass_stParaSet.uwAssistStartNm;
+        Syspara2.stAssistPara.uwAssistStopNm.uwReal = ass_stParaSet.uwAssistStopNm;
+        Syspara2.stAssistPara.uwStartUpGainStep.uwReal = ass_stParaSet.uwStartUpGainStep;
+        Syspara2.stAssistPara.uwStartUpCadNm.uwReal = ass_stParaSet.uwStartUpCadNm;
+        Syspara2.stAssistPara.uwTorLPFCadNm.uwReal = ass_stParaSet.uwTorLPFCadNm;
+        Syspara2.stAssistPara.uwSpeedAssistSpdRpm.uwReal = ass_stParaSet.uwSpeedAssistSpdRpm;
+        Syspara2.stAssistPara.uwSpeedAssistIMaxA.uwReal = ass_stParaSet.uwSpeedAssistIMaxA;
+        Syspara2.stAssistPara.uwAssistLimitBikeSpdStart.uwReal = ass_stParaSet.uwAssistLimitBikeSpdStart;
+        Syspara2.stAssistPara.uwAssistLimitBikeSpdStop.uwReal = ass_stParaSet.uwAssistLimitBikeSpdStop;
+        Syspara2.stAssistPara.uwCadenceAssistWeight.uwReal = ass_stParaSet.uwCadenceWeight;
     }
 }
 /***************************************************************
@@ -579,7 +579,7 @@ void mn_voEEUperParaUpdate(void)
 ****************************************************************/
 void mn_voEEHistoryParaUpdate(void)
 {
-    Syspara2.stHistoryPara.uwAssModSelect.uwReal = ass_ParaSet.uwAsssistSelectNum;
+    Syspara2.stHistoryPara.uwAssModSelect.uwReal = ass_stParaSet.uwAsssistSelectNum;
     Syspara2.stHistoryPara.uwOpenTimes.uwReal = cp_stHistoryPara.uwOpenTimes;
     Syspara2.stHistoryPara.uwUsedTimeH.uwReal = (UWORD)(cp_stHistoryPara.ulUsedTime >> 16);
     Syspara2.stHistoryPara.uwUsedTimeL.uwReal = (UWORD)cp_stHistoryPara.ulUsedTime;
@@ -640,15 +640,10 @@ void mn_voSoftwareInit(void)
     FSM_voInit();
     RUN_FSM_voInit();
     Switch_speed_FSMInit();
-    
     /* TempInit */
     TempInit();
-
+    /* CANInit */
     Can_voInitMC_Run();
-    /* BikeSpeedInit */
-    bikespeed_voBikeSpeedInit();
-     /* CadenceInit */
-    cadence_voCadenceInit();
     /* Alarm init */
     alm_voInit();
     alm_voCoef();

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

@@ -1255,7 +1255,7 @@ void alm_voDetec200MS(ALM_BIKE_IN *in, ALM_DETEC200MS_COF *coef)
     /* Bike cadence sensor fault */
     if(in->uwBikeSpdPu > 0)
     {
-        if(in->uwTroqPu > ass_CalCoef.uwAssThreshold)
+        if(in->uwTroqPu > ass_stCalCoef.uwAssThreshold)
         {
             if(in->uwCadenceFreqPu == 0)
             {

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

@@ -738,8 +738,8 @@ void  scm_voSpdCtrMdDownTbc(void)
       
         if(FSM2nd_Run_state.state == Assistance)
         {
-          acr_stCurIqPIIn.swUmaxPu = ass_CalOut.swVoltLimitPu - acr_stUdqDcpOut.swUqPu;  // Q14
-          acr_stCurIqPIIn.swUminPu = -ass_CalOut.swVoltLimitPu - acr_stUdqDcpOut.swUqPu; // Q14
+          acr_stCurIqPIIn.swUmaxPu = ass_stCalOut.swVoltLimitPu - acr_stUdqDcpOut.swUqPu;  // Q14
+          acr_stCurIqPIIn.swUminPu = -ass_stCalOut.swVoltLimitPu - acr_stUdqDcpOut.swUqPu; // Q14
         }
         else
         {

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

@@ -13,6 +13,7 @@
 #define ASSISTCURVE_H
 
 #include "asr.h"
+#include "mathtool.h"
 /****************************************
  *
  *          Definitions & Macros
@@ -24,7 +25,7 @@
 #define GEAR_NUM                6 // number of gear
 #define CADENCE_PULSES_PER_CIRC 64
 
-#define BIKE_WHEEL_DIAMETER           219 // CM
+#define BIKE_WHEEL_PERIMETER           219 // CM
 #define BIKE_MECH_RATION              35
 #define BIKE_SPEED_IQLIMIT_THRESHOLD1 25                                // Km/h
 #define BIKE_SPEED_IQLIMIT_THRESHOLD2 (BIKE_SPEED_IQLIMIT_THRESHOLD1 + 2) // Km/h
@@ -166,7 +167,7 @@ typedef enum
 typedef struct
 {
     // Bike info
-    UWORD uwWheelDiameter;     // Q0  0.1cm    
+    UWORD uwWheelPerimeter;     // Q0  0.1cm    
     UWORD uwMechRationMotor;
     UWORD uwAssistMaxSpdKmH;
     UWORD uwThrottleMaxSpdKmH;
@@ -175,7 +176,7 @@ typedef struct
     UWORD uwAssistSelect1;
     UWORD uwAssistSelect2;
     UWORD uwLightVoltage;
-    SWORD swDeltDiameter;
+    SWORD swDeltPerimeter;
     UWORD uwStartMode;
     UWORD uwAutoPowerOffTime;
 
@@ -388,17 +389,17 @@ typedef struct
  ****************************************/
 extern ASS_FSM_STATUS Ass_FSM;
 
-extern ASS_PER_IN   ass_CalIn;
-extern ASS_PER_COEF ass_CalCoef;
-extern ASS_PER_OUT  ass_CalOut;
+extern ASS_PER_IN   ass_stCalIn;
+extern ASS_PER_COEF ass_stCalCoef;
+extern ASS_PER_OUT  ass_stCalOut;
 
-extern ASS_PARA_CONFIGURE ass_ParaCong;
-extern ASS_PARA_SET       ass_ParaSet;
+extern ASS_PARA_CONFIGURE ass_stParaCong;
+extern ASS_PARA_SET       ass_stParaSet;
 
-extern ASS_LIMIT_ACCORDING_VOL_COF ass_CurLimCalBMSCoef;
-extern ASS_LIMIT_ACCORDING_VOL_OUT ass_CurLimitCalBMSOut;
-extern ASS_CURLIM_COEF ass_CurLimCoef;
-extern ASS_CURLIM_OUT  ass_CurLimOut;
+extern ASS_LIMIT_ACCORDING_VOL_COF ass_stCurLimCalBMSCoef;
+extern ASS_LIMIT_ACCORDING_VOL_OUT ass_stCurLimitCalBMSOut;
+extern ASS_CURLIM_COEF ass_stCurLimCoef;
+extern ASS_CURLIM_OUT  ass_stCurLimOut;
 
 extern ASR_SPDPI_IN  asr_stTorqSpdPIIn;
 extern ASR_SPDPI_OUT asr_stTorqSpdPIOut;
@@ -412,6 +413,7 @@ extern SWORD  ass_swTorqMafBuf[64];
 extern MAF_IN ass_stTorqMafValue;
 extern SWORD  ass_swUqLimMafBuf[64];
 extern MAF_IN ass_stUqLimMafValue;
+extern LPF_OUT ass_pvt_stCurLpf;
 /************************************************************************
  Ram Allocation
 *************************************************************************/

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

@@ -107,6 +107,7 @@ typedef struct
     CADENCE_FSM cadence_fsm;
     UWORD       uwFreqPercent; // Q14;
     UWORD       uwForwardCnt;  // Q14;
+    UWORD       uwOverflowfirst;
 } CADENCE_OUT;
 
 /****************************************

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

@@ -34,7 +34,8 @@
 #define BIKESPEED_POWER_ERROR_DETECT       4000 // ms
 #define BIKESPEED_POWER_ERROR_RECOVER      4000 // ms
 #define BIKESPEED_POWER_ERROR_TIMEUNIT     200  // ms
-
+#define BIKESPEED_KMPERH2FREQPU            190  // 1km/h to FreqPu : 1/(FBASE * ass_ParaCong.uwWheelPerimeter * 36 /1000 >> 20)  
+   
 #define BIKESPEED_COF_DEFAULT  \
     {                          \
         0, 0, 0, 0, 0, 0, 0, 0 \
@@ -81,7 +82,7 @@ typedef struct
     UWORD uwBikespeedPwrErrorVoltagePuDown; // flag of light error
     UWORD uwBikespeedPwrErrorVoltagePuUp;   // flag of light error
 
-    UWORD uwWheelDiameter;   // the back wheel Diameter 0.1 cm
+    UWORD uwWheelPerimeter;   // the back wheel Diameter 0.1 cm
     UWORD uwMinTriptoUpdate; // the minimum trip to update m
 } BIKESPEED_COF;
 
@@ -117,16 +118,15 @@ typedef struct // Input of "BikeSpdREFPI"
 {
     SLONG slSpdRefPu;  //  bikeSpeed reference,Q0
     SLONG slSpdFdkPu; //  bikeSpeed feedback,Q0
-    SWORD swbikespdMax;      // bikespeed output maximum limit
-    SWORD swbikespdMin;  // bikespeed output minimum limit
+    SWORD swIqMaxPu;
+    SWORD swIqMinPu;  
 } BIKESPDPI_IN;
 
 typedef struct
 {
    SLONG slErrorZ1;
-   SLONG slMotorspeedref;
-   SWORD swMotorSpeedRef;
-   SWORD swSpdRefRpm;
+   SLONG slIqRefPu;
+   SWORD swIqRefPu;
 } BIKESPDPI_OUT;
 
 typedef struct
@@ -140,9 +140,9 @@ typedef struct
  *           Exported variable
  *
  ****************************************/
-
 extern BIKESPEED_COF bikespeed_stFreGetCof;
 extern BIKESPDPI_OUT bikespeed_stPIOut;
+extern BIKESPDPI_IN bikespeed_stPIIn;
 /***************************************
  *
  *          Function  Definations
@@ -155,7 +155,6 @@ void bikespeed_voGetBikeSpeedPwrError(UWORD BikeSpeedPwrVolPu);
 void bikespeed_votempTripCal(void);
 void bikespeed_voPIInit(void);
 void bikespeed_voPICoef(void);
-void bikespeed_voPuCal(void);
 void bikespeed_voPI(BIKESPDPI_IN *in, BIKESPDPI_OUT *out);
 /************************************************************************
  Flag Define (N/A)

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

@@ -354,7 +354,7 @@ typedef struct
 typedef struct
 {
     UWORD uwSaveFlg;
-    UWORD uwWheelDiameter;       //轮胎周长 cm
+    UWORD uwWheelPerimeter;       //轮胎周长 cm
     UWORD uwMechRationMotor;    //传动比 0.1
     UWORD uwAssistMaxSpdKmH;     //限速 1km/h
     UWORD uwThrottleMaxSpdKmH;   //推行模式限速 1km/h
@@ -580,7 +580,7 @@ typedef struct
 
 typedef struct
 {
-    uint8_t uwWheelDiameter; ///>车轮默认长度 1cm
+    uint8_t uwWheelPerimeter; ///>车轮默认长度 1cm
     uint8_t StartUpMod;      ///>1-柔和，2-正常，3-强劲，地址偏移1
     uint8_t BikeSpeedLimit;  /// km/h
     int8_t  DeltDiameter;    ///>有符型，单位1cm，±10.0cm  1Nm，地址偏移0
@@ -614,6 +614,13 @@ typedef struct
     ULONG ulButtonSetTimeCnt;
 } OBC_ButtonStatus_Struct_t;
 
+
+typedef struct
+{
+    SWORD swIdcPu;
+    UWORD uwInterResistpu;
+    UWORD uwVdcCompPu;
+} BMS_VoltEstimat_Struct_t;
 /****************************************
  *
  *           Exported variable
@@ -630,6 +637,7 @@ extern MC_ControlCode_Struct_t          MC_ControlCode;
 extern MC_RunInfoToCDL_Struct_t         MC_RunInfoToCDL;
 extern MC_RS_ASCII_Struct_t             MC_RsASSCII;
 extern MC_UpperPCInfo_Struct_t          MC_UpcInfo;
+extern BMS_VoltEstimat_Struct_t         BMS_VoltEstimat;
 
 extern OBC_SetCustomPara_Struct_t OBC_SetCustomPara;
 extern OBC_ButtonStatus_Struct_t OBC_ButtonStatus;

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

@@ -173,7 +173,7 @@
 /* Bike Para */
 #define I2CuwWheelDiameter                       \
     {                                            \
-        ((UWORD)BIKE_WHEEL_DIAMETER), 0, 0, 0, 0 \
+        ((UWORD)BIKE_WHEEL_PERIMETER), 0, 0, 0, 0 \
     }
 #define I2CuwMechRationMotor                  \
     {                                         \
@@ -783,7 +783,7 @@ typedef struct
 
 typedef struct
 {
-    UWORD_VALUE uwWheelDiameter;
+    UWORD_VALUE uwWheelPerimeter;
     UWORD_VALUE uwMechRationMotor;
     UWORD_VALUE uwAssistMaxSpdKmH;
     UWORD_VALUE uwThrottleMaxSpdKmH;
@@ -792,7 +792,7 @@ typedef struct
     UWORD_VALUE uwAssistSelect1;
     UWORD_VALUE uwAssistSelect2;
     UWORD_VALUE uwLightVoltage;
-    SWORD_VALUE swDeltDiameter;
+    SWORD_VALUE swDeltPerimeter;
     UWORD_VALUE uwStartMode;
     UWORD_VALUE uwAutoPowerOffTime;
     UWORD_VALUE reserve1;

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

@@ -33,7 +33,8 @@
 #define TORQUE_VOLTAGE_PER_NM      30          // 35mV/Nm
 #define TORQUE_SENSOR_LPF_FRQ      1           // Hz
 #define TORQUE_LPF_DISCRETEHZ      1000        // Hz   T=1/f,T is used in discreting LPF s function
-
+#define TORQUE_90S_1MSCNT          90000L 
+#define TORQUE_1S_1MSCNT           1000 
 #define TORQ_OFFSET_NUM 7
 
 #define TORQUESENSOR_OUT_DEFAULT    \
@@ -140,8 +141,9 @@ void torsensor_voTorSensorInit(void);
 void torsensor_voTorADC(void);
 UWORD torsensor_uwTorqOffsetCal(SWORD Temp); 
 UWORD torsensor_uwTorqSencitiveCal(SWORD Temp, SWORD T0);
- void torsensor_voCadenceCnt(void);
- void torsensor_voDynamicOffset(void);
+void torsensor_voCadenceCnt(void);
+void torsensor_voDynamicOffset(void);
+void torsensor_voOffsetUpdate(void);
 /************************************************************************/
 #endif
 /************************************************************************

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

@@ -154,53 +154,125 @@ static void cadence_voCadenceLowFrequencyWork(UWORD source)
  Subroutine Call: N/A;
  Reference: N/A
 ****************************************************************/
-UWORD cadanceCnt;
-ULONG ulCaputureCntErr = 0;
+ULONG ulCaputureCntErr = 0,ulCaputureCntErrLast = 0;
 static void cadence_voCadenceHighFrequencyWork(UWORD source)
-{ 
+{
     if (source == 1 && cadence_stFreGetOut.uwCaputureNumCnt == 1)
     {
         cadence_stFreGetOut.uwCaputureOverflowCnt++;
+        if (timer_interrupt_flag_get(TIMER1, TIMER_INT_FLAG_CH2))
+        {
+           cadence_stFreGetOut.uwCaputure2Cnt =(UWORD)TIMER1_CAP_CANDANCE;
+           if(cadence_stFreGetOut.uwCaputure2Cnt > 9000) // TIMER1 half Period 
+           {
+              cadence_stFreGetOut.uwOverflowfirst= 1;
+           }
+           else
+           {
+              cadence_stFreGetOut.uwOverflowfirst= 2;
+           }   
+           ulCaputureCntErrLast = ulCaputureCntErr;
+            if(cadence_stFreGetOut.uwOverflowfirst == 1)
+            {
+               cadence_stFreGetOut.uwCaputureOverflowCnt -=1;
+               ulCaputureCntErr = ((ULONG)cadence_stFreGetOut.uwCaputureOverflowCnt * 720 * cadence_stFreGetCof.uwTimerUnit) -
+                               cadence_stFreGetOut.uwCaputure1Cnt + cadence_stFreGetOut.uwCaputure2Cnt;
+               cadence_stFreGetOut.uwCaputureOverflowCnt = 1; 
+               cadence_stFreGetOut.uwOverflowfirst = 0;
+              
+            }
+            else if (cadence_stFreGetOut.uwOverflowfirst == 2)
+            {
+                ulCaputureCntErr = ((ULONG)cadence_stFreGetOut.uwCaputureOverflowCnt * 720 * cadence_stFreGetCof.uwTimerUnit) -
+                               cadence_stFreGetOut.uwCaputure1Cnt + cadence_stFreGetOut.uwCaputure2Cnt;
+                cadence_stFreGetOut.uwCaputureOverflowCnt = 0;
+                cadence_stFreGetOut.uwOverflowfirst = 0;
+            }
+            else
+            {
+                ulCaputureCntErr = ((ULONG)cadence_stFreGetOut.uwCaputureOverflowCnt * 720 * cadence_stFreGetCof.uwTimerUnit) -
+                               cadence_stFreGetOut.uwCaputure1Cnt + cadence_stFreGetOut.uwCaputure2Cnt;
+                cadence_stFreGetOut.uwCaputureOverflowCnt = 0;
+            }
+            
+            if (ulCaputureCntErr > 2250)
+            {
+                cadence_stFreGetOut.uwFrequencyPu =
+                    (UWORD)(((SQWORD)720000 << 20) / ((SQWORD)((ulCaputureCntErr + ulCaputureCntErrLast)>>1)* cadence_stFreGetCof.uwNumbersPulses * FBASE));
+                cadence_stFreGetOut.uwLPFFrequencyPu = (cadence_stFreGetOut.uwLPFFrequencyPu * cadence_stFreGetCof.uwCadenceLPFgain +
+                                                        cadence_stFreGetOut.uwFrequencyPu * (100 - cadence_stFreGetCof.uwCadenceLPFgain)) /
+                                                       100;
+                cadence_stFreGetOut.uwFreqPercent = ((ULONG)cadence_stFreGetOut.uwLPFFrequencyPu << 14) / cadence_stFreGetCof.uwMaxCadenceFre; // Q14
+            }
+            cadence_stFreGetOut.uwCaputureNumCnt = 1;
+            cadence_stFreGetOut.uwCaputure1Cnt = cadence_stFreGetOut.uwCaputure2Cnt; 
+            timer_interrupt_flag_clear(TIMER1, TIMER_INT_FLAG_CH2);
+        }
+        else
+        {
+            cadence_stFreGetOut.uwOverflowfirst= 0;
+        }
     }
     else if (source == 2)
     {
+
         if (cadence_stFreGetOut.uwCaputureNumCnt == 0)
         {
             cadence_stFreGetOut.uwCaputureNumCnt = 1;
-            cadence_stFreGetOut.uwCaputure1Cnt = (UWORD)TIMER1_CAP_CANDANCE;
+            cadence_stFreGetOut.uwCaputure1Cnt = (UWORD)TIMER1_CAP_CANDANCE;  
         }
         else if (cadence_stFreGetOut.uwCaputureNumCnt == 1)
         {
             cadence_stFreGetOut.uwForwardCnt++;
-            cadanceCnt++;
             cadence_stFreGetOut.uwCaputureNumCnt = 2;
+
             cadence_stFreGetOut.uwCaputure2Cnt = (UWORD)TIMER1_CAP_CANDANCE;
 
-            ulCaputureCntErr = ((ULONG)cadence_stFreGetOut.uwCaputureOverflowCnt * 720 * cadence_stFreGetCof.uwTimerUnit) -
+            ulCaputureCntErrLast = ulCaputureCntErr;
+            if(cadence_stFreGetOut.uwOverflowfirst == 1)
+            {
+               cadence_stFreGetOut.uwCaputureOverflowCnt -=1;
+               ulCaputureCntErr = ((ULONG)cadence_stFreGetOut.uwCaputureOverflowCnt * 720 * cadence_stFreGetCof.uwTimerUnit) -
+                               cadence_stFreGetOut.uwCaputure1Cnt + cadence_stFreGetOut.uwCaputure2Cnt;
+               cadence_stFreGetOut.uwCaputureOverflowCnt = 1; 
+               cadence_stFreGetOut.uwOverflowfirst = 0;
+              
+            }
+            else if (cadence_stFreGetOut.uwOverflowfirst == 2)
+            {
+                ulCaputureCntErr = ((ULONG)cadence_stFreGetOut.uwCaputureOverflowCnt * 720 * cadence_stFreGetCof.uwTimerUnit) -
                                cadence_stFreGetOut.uwCaputure1Cnt + cadence_stFreGetOut.uwCaputure2Cnt;
-            if (ulCaputureCntErr > 10)
+                cadence_stFreGetOut.uwCaputureOverflowCnt = 0;
+                cadence_stFreGetOut.uwOverflowfirst = 0;
+            }
+            else
+            {
+                ulCaputureCntErr = ((ULONG)cadence_stFreGetOut.uwCaputureOverflowCnt * 720 * cadence_stFreGetCof.uwTimerUnit) -
+                               cadence_stFreGetOut.uwCaputure1Cnt + cadence_stFreGetOut.uwCaputure2Cnt;
+                cadence_stFreGetOut.uwCaputureOverflowCnt = 0;
+            }
+            
+            if (ulCaputureCntErr > 2250)
             {
                 cadence_stFreGetOut.uwFrequencyPu =
-                    (UWORD)(((SQWORD)720000 << 20) / ((SQWORD)ulCaputureCntErr * cadence_stFreGetCof.uwNumbersPulses * FBASE));
+                    (UWORD)(((SQWORD)720000 << 20) / ((SQWORD)((ulCaputureCntErr + ulCaputureCntErrLast)>>1)* cadence_stFreGetCof.uwNumbersPulses * FBASE));
                 cadence_stFreGetOut.uwLPFFrequencyPu = (cadence_stFreGetOut.uwLPFFrequencyPu * cadence_stFreGetCof.uwCadenceLPFgain +
                                                         cadence_stFreGetOut.uwFrequencyPu * (100 - cadence_stFreGetCof.uwCadenceLPFgain)) /
                                                        100;
                 cadence_stFreGetOut.uwFreqPercent = ((ULONG)cadence_stFreGetOut.uwLPFFrequencyPu << 14) / cadence_stFreGetCof.uwMaxCadenceFre; // Q14
             }
-            cadence_stFreGetOut.uwCaputure1Cnt = cadence_stFreGetOut.uwCaputure2Cnt;
-            cadence_stFreGetOut.uwCaputureOverflowCnt = 0;
             cadence_stFreGetOut.uwCaputureNumCnt = 1;
+            cadence_stFreGetOut.uwCaputure1Cnt = cadence_stFreGetOut.uwCaputure2Cnt; 
+            
         }
     }
 
-
-    if (cadence_stFreGetOut.cadence_dir == CADENCE_DIR_BACKWARD) 
+    if (cadence_stFreGetOut.cadence_dir == CADENCE_DIR_BACKWARD) // 100ms
     {
         cadence_stFreGetOut.cadence_fsm = CADENCE_BACKWOR;
     }
-    
-    /* Enter idle state when time out*/
-    if (cadence_stFreGetOut.uwCaputureOverflowCnt >= cadence_stFreGetCof.uwHfMaxTimeCnt) 
+
+    if (cadence_stFreGetOut.uwCaputureOverflowCnt >= cadence_stFreGetCof.uwHfMaxTimeCnt) // 100ms
     {
         cadence_stFreGetOut.uwFrequencyPu = 0;
         cadence_stFreGetOut.uwLPFFrequencyPu = 0;
@@ -214,8 +286,7 @@ static void cadence_voCadenceHighFrequencyWork(UWORD source)
         cadence_stFreGetOut.cadence_fsm = CADENCE_IDLE;
         cadence_stFreGetOut.uwFreqPercent = 0;
     }
-    
-    /* Candence over frequency error */
+
     if (cadence_stFreGetOut.uwLPFFrequencyPu > cadence_stFreGetCof.uwMaxCadenceFre)
     {
         cadence_stFreGetOut.uwFrequencyPu = 0;

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

@@ -59,7 +59,7 @@ void bikespeed_voBikeSpeedCof(void)
     bikespeed_stFreGetCof.uwBikespeedPwrRecoverCnt = BIKESPEED_POWER_ERROR_RECOVER / BIKESPEED_POWER_ERROR_TIMEUNIT;
     bikespeed_stFreGetCof.uwBikespeedPwrErrorVoltagePuDown = ((ULONG)BIKESPEED_POWER_ERROR_VOLTAGE_DOWN << 14) / VBASE;
     bikespeed_stFreGetCof.uwBikespeedPwrErrorVoltagePuUp = ((ULONG)BIKESPEED_POWER_ERROR_VOLTAGE_UP << 14) / VBASE;
-    bikespeed_stFreGetCof.uwWheelDiameter = 600;
+    bikespeed_stFreGetCof.uwWheelPerimeter = 600;
     bikespeed_stFreGetCof.uwMinTriptoUpdate = 100; // 100m
 }
 
@@ -95,8 +95,12 @@ static void bikespeed_voBikeSpeedIdle(UWORD source)
         {
             bikespeed_stFreGetOut.blBikeSpeedCalStartState = TRUE;
             bikespeed_stFreGetOut.uwCaputureOverflowCnt = 0;
-            bikespeed_stFreGetOut.uwCaputureNumCnt = 0;
+//            bikespeed_stFreGetOut.uwCaputureNumCnt = 0;
+            bikespeed_stFreGetOut.uwCaputureNumCnt = 1;
             bikespeed_stFreGetOut.bikespeed_fsm = BIKESPEED_WORK;
+            
+            bikespeed_stFreGetOut.uwFrequencyPu = 1 * BIKESPEED_KMPERH2FREQPU;  
+            bikespeed_stFreGetOut.uwCaputure1Cnt = (UWORD)TIMER1_CAP_BIKESPD;
         }
     }
 }
@@ -250,7 +254,7 @@ void bikespeed_voGetBikeSpeedPwrError(UWORD BikeSpeedPwrVolPu)
 void bikespeed_votempTripCal(void)
 {
     UWORD Temptrip;
-    Temptrip = ((ULONG)bikespeed_stFreGetCof.uwWheelDiameter * 3216 * bikespeed_stFreGetOut.uwBikeForwardCnt / 1000) >> 10; // 3216 = Q10(3.1415926) m
+    Temptrip = ((ULONG)bikespeed_stFreGetCof.uwWheelPerimeter * 3216 * bikespeed_stFreGetOut.uwBikeForwardCnt / 1000) >> 10; // 3216 = Q10(3.1415926) m
     if (Temptrip > bikespeed_stFreGetCof.uwMinTriptoUpdate)
     {
         bikespeed_stFreGetOut.uwBikeForwardCnt = 0;
@@ -323,132 +327,132 @@ void bikespeed_voBikeSpeedCal(UWORD source)
 void bikespeed_voPIInit(void)
 {
     bikespeed_stPIOut.slErrorZ1 = 0;
-    bikespeed_stPIOut.slMotorspeedref = 0;
-    bikespeed_stPIOut.swMotorSpeedRef = 0;
-    bikespeed_stPIOut.swSpdRefRpm = 0 ;   
+    bikespeed_stPIOut.slIqRefPu = 0;
+    bikespeed_stPIOut.swIqRefPu = 0;
 }
 
 void bikespeed_voPICoef(void)
 {
-     bikespeed_stPICof.uwKpPu = 20000 ;   //Q15
-     bikespeed_stPICof.uwKiPu = 8000 ;    //Q15
+     bikespeed_stPICof.uwKpPu = 25000 ;   //Q15
+     bikespeed_stPICof.uwKiPu = 500 ;    //Q15
 }
 
-void bikespeed_voPuCal(void)
+void bikespeed_voPI(BIKESPDPI_IN *in, BIKESPDPI_OUT *out)
 {
-    SWORD swBikeSpeedKmh = 0;  //km/h
-    SLONG slBikeSpdRefPu = 0;  //Q20
+    SLONG  slIqMaxPu, slIqMinPu;  // Q30
+    SLONG  slSpdErrPu, slDeltaErrPu; //Q20
+    SQWORD sqIqRefPu, sqIqpPu, sqIqiPu; //Q30
     
-    if(MC_ControlCode.GearSt == 0x01)
+    slIqMaxPu = (SLONG)in->swIqMaxPu << 16; // Q14+Q16=Q30
+    slIqMinPu = (SLONG)in->swIqMinPu << 16; // Q14+Q16=Q30
+
+    slSpdErrPu = (SLONG)in->slSpdRefPu - in->slSpdFdkPu; // Q20
+    if (slSpdErrPu > 1048576L)
     {
-        swBikeSpeedKmh = 10;
+        slSpdErrPu = 1048576L;
     }
-    else if(MC_ControlCode.GearSt == 0x02)
+    else if (slSpdErrPu < -1048576L)
     {
-        swBikeSpeedKmh = 15;
+        slSpdErrPu = -1048576L;
     }
-    else if(MC_ControlCode.GearSt == 0x03)
+    else
     {
-        swBikeSpeedKmh = 20;
+    /* Nothing */
     }
-    else if(MC_ControlCode.GearSt == 0x04)
+    
+    slDeltaErrPu = slSpdErrPu - out->slErrorZ1;
+    if (slSpdErrPu > 1048576L)
     {
-        swBikeSpeedKmh = 25;
+        slSpdErrPu = 1048576L;
     }
-    else if(MC_ControlCode.GearSt == 0x33 || MC_ControlCode.GearSt == 0x05)
+    else if (slSpdErrPu < -1048576L)
     {
-        swBikeSpeedKmh = 30;
+        slSpdErrPu = -1048576L;
     }
     else
-    {     
+    {
+    /* Nothing */
     }
 
-    slBikeSpdRefPu = swBikeSpeedKmh * 1000 * 29127 / ass_ParaCong.uwWheelDiameter / FBASE ; //f(Q20)
-     
-    bikespeed_stPIIn.slSpdRefPu = slBikeSpdRefPu; // Q20
-    bikespeed_stPIIn.slSpdFdkPu = bikespeed_stFreGetOut.uwLPFFrequencyPu ;  //Q20
-    bikespeed_stPIIn.swbikespdMax= USER_MOTOR_5500RPM2PU  ;  //    Q15
-    bikespeed_stPIIn.swbikespdMin= - USER_MOTOR_5500RPM2PU ;
-}
-
-void bikespeed_voPI(BIKESPDPI_IN *in, BIKESPDPI_OUT *out)
-{
-      SLONG  slbikespeederr; 
-      SLONG  slDeltaErr;
-      SQWORD  motorspeedrefp;
-      SQWORD  motorspeedrefi;
-      SLONG  slmotorspdMax;
-      SLONG  slmotorspdMin;
-      SQWORD  motorspeedref;
-      
-      slmotorspdMax = in-> swbikespdMax << 15; //Q30
-      slmotorspdMin = in-> swbikespdMin << 15; //Q30
-    
-    /* Calculate the error */
-      slbikespeederr =  in->slSpdRefPu - in->slSpdFdkPu ;  //Q20
-      if (slbikespeederr > 1048576L)
-      {
-        slbikespeederr = 1048576L;
-      }
-      else if (slbikespeederr < -1048576L)
-      {
-        slbikespeederr = -1048576L;
-      }
-      else
-      {
-        /* Nothing */
-      }
-      
-    /* Calculate the delta error */
-      slDeltaErr = slbikespeederr - out->slErrorZ1;  //Q20
-      if (slDeltaErr > 1048576L)
-      {
-        slDeltaErr = 1048576L;
-      }
-      else if (slDeltaErr < -1048576L)
-      {
-        slDeltaErr = -1048576L;
-      }
-      else
-      {
-        /* Nothing */
-      }
-  
-    //kp output
-    motorspeedrefp = (SQWORD)slDeltaErr * bikespeed_stPICof.uwKpPu >> 5 ; //Q30
-   
-    /* Calculate the integral output */
-    motorspeedrefi = (SQWORD)slbikespeederr * bikespeed_stPICof.uwKiPu >> 5 ; //Q30
-    
-    /* Calculate the Controller output */
-    motorspeedref = motorspeedrefp + motorspeedrefi + out->slMotorspeedref ; //Q30
-    /* motorspeed output limit */
-    if (motorspeedref > slmotorspdMax)
+    bikespeed_stPICof.uwKpPu = 25000; 
+    if(((SWORD)abs(out->slErrorZ1) - (SWORD)abs(slSpdErrPu)) > 20)   //Fast Approach
     {
-        out->slMotorspeedref = slmotorspdMax;
+        bikespeed_stPICof.uwKiPu = 0; 
+        
+        if(slSpdErrPu < 0 && slSpdErrPu > (SWORD)-3 * BIKESPEED_KMPERH2FREQPU)
+        {
+            bikespeed_stPICof.uwKpPu = 6250;
+            bikespeed_stPICof.uwKiPu = 200;
+        }
+        else if(slSpdErrPu <= (SWORD)-3 * BIKESPEED_KMPERH2FREQPU)
+        {
+            bikespeed_stPICof.uwKpPu = 6250;
+            bikespeed_stPICof.uwKiPu = 400;
+        }
     }
-    else if (motorspeedref < slmotorspdMin)
+    else if(((SWORD)abs(out->slErrorZ1) - (SWORD)abs(slSpdErrPu)) > 0) //Fast Approach
     {
-        out->slMotorspeedref = slmotorspdMin;
+        if(in->slSpdFdkPu<500)
+        {
+            bikespeed_stPICof.uwKiPu = 500;
+        }
+        else
+        {
+            bikespeed_stPICof.uwKiPu = 1000; 
+        }
+        
+        if(slSpdErrPu < 0 && slSpdErrPu > (SWORD)-3 * BIKESPEED_KMPERH2FREQPU)
+        {
+            bikespeed_stPICof.uwKpPu = 6250;
+            bikespeed_stPICof.uwKiPu = 200;
+        }
+        else if(slSpdErrPu <= (SWORD)-3 * BIKESPEED_KMPERH2FREQPU)
+        {
+            bikespeed_stPICof.uwKpPu = 6250;
+            bikespeed_stPICof.uwKiPu = 400;
+        }
     }
-    else
+    else  //Away
     {
-        out->slMotorspeedref = motorspeedref;
+        if(in->slSpdFdkPu<500)
+        {
+            bikespeed_stPICof.uwKiPu = 500;
+        }
+        else
+        {
+            bikespeed_stPICof.uwKiPu = 3000; 
+        }
+            
+        if(slSpdErrPu > (0 * BIKESPEED_KMPERH2FREQPU) && slSpdErrPu <= (3 * BIKESPEED_KMPERH2FREQPU))
+        {
+            bikespeed_stPICof.uwKiPu  = 0;
+        }
     }
     
-    if( (in->slSpdRefPu == 0) && ( in->slSpdFdkPu == 0))
+    if(in->slSpdFdkPu >= (27 * BIKESPEED_KMPERH2FREQPU))
     {
-      out->slMotorspeedref = 0 ;
+        bikespeed_stPICof.uwKiPu  = 6000;
     }
-  
-    out->swMotorSpeedRef = out->slMotorspeedref >> 15 ; // motorspeedrmp,Q15
-       
-    out->slErrorZ1 = slbikespeederr ;
     
-    out->swSpdRefRpm = ((SLONG)out->swMotorSpeedRef * cof_uwVbRpm) >> 15 ;
-    
-    
-   
+    sqIqpPu = ((SQWORD)slDeltaErrPu * bikespeed_stPICof.uwKpPu) << 4; // Q30     
+    sqIqiPu = (SQWORD)slSpdErrPu * bikespeed_stPICof.uwKiPu ; // Q30
+    sqIqRefPu = sqIqpPu + sqIqiPu + (SQWORD)out->slIqRefPu; // Q30
+
+    if (sqIqRefPu > slIqMaxPu)
+    {
+        out->slIqRefPu = slIqMaxPu;
+    }
+    else if (sqIqRefPu < slIqMinPu)
+    {
+        out->slIqRefPu = slIqMinPu;
+    }
+    else
+    {
+        out->slIqRefPu = (SLONG)sqIqRefPu;
+    }
+
+    out->swIqRefPu = out->slIqRefPu >> 16; // Q30-Q16=Q14
+    out->slErrorZ1 = slSpdErrPu;
 }
 /*************************************************************************
  Local Functions (N/A)

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

@@ -404,13 +404,13 @@ void DataProcess(uint16_t ID, uint8_t Mode, uint16_t Cmd, uint8_t *Data)
         }
         case 0x3300: // OBC设置用户参数
         {
-            OBC_SetCustomPara.uwWheelDiameter = ass_ParaCong.uwWheelDiameter;
-            OBC_SetCustomPara.StartUpMod = ass_ParaCong.uwStartMode;
-            OBC_SetCustomPara.BikeSpeedLimit = ass_ParaCong.uwAssistMaxSpdKmH;
-            OBC_SetCustomPara.DeltDiameter = ass_ParaCong.swDeltDiameter;
-            OBC_SetCustomPara.AssistMod = ass_ParaSet.uwAsssistSelectNum;
-            OBC_SetCustomPara.AutoPowerOffTime = ass_ParaCong.uwAutoPowerOffTime;
-            SendData(ID_MC_TO_PBU, MODE_REPORT, 0x5408, (uint8_t *)&OBC_SetCustomPara.uwWheelDiameter);
+            OBC_SetCustomPara.uwWheelPerimeter = ass_stParaCong.uwWheelPerimeter;
+            OBC_SetCustomPara.StartUpMod = ass_stParaCong.uwStartMode;
+            OBC_SetCustomPara.BikeSpeedLimit = ass_stParaCong.uwAssistMaxSpdKmH;
+            OBC_SetCustomPara.DeltDiameter = ass_stParaCong.swDeltPerimeter;
+            OBC_SetCustomPara.AssistMod = ass_stParaSet.uwAsssistSelectNum;
+            OBC_SetCustomPara.AutoPowerOffTime = ass_stParaCong.uwAutoPowerOffTime;
+            SendData(ID_MC_TO_PBU, MODE_REPORT, 0x5408, (uint8_t *)&OBC_SetCustomPara.uwWheelPerimeter);
             break;
         }
         case 0x3408: // OBC设置用户参数
@@ -419,10 +419,10 @@ void DataProcess(uint16_t ID, uint8_t Mode, uint16_t Cmd, uint8_t *Data)
             OBC_SetCustomPara.StartUpMod = Data[1];
             OBC_SetCustomPara.AssistMod = Data[2];
             OBC_SetCustomPara.AutoPowerOffTime = Data[3];
-            ass_ParaCong.swDeltDiameter = OBC_SetCustomPara.DeltDiameter;
-            ass_ParaCong.uwStartMode = OBC_SetCustomPara.StartUpMod;
-            ass_ParaSet.uwAsssistSelectNum = OBC_SetCustomPara.AssistMod;
-            ass_ParaCong.uwAutoPowerOffTime = OBC_SetCustomPara.AutoPowerOffTime;
+            ass_stParaCong.swDeltPerimeter = OBC_SetCustomPara.DeltDiameter;
+            ass_stParaCong.uwStartMode = OBC_SetCustomPara.StartUpMod;
+            ass_stParaSet.uwAsssistSelectNum = OBC_SetCustomPara.AssistMod;
+            ass_stParaCong.uwAutoPowerOffTime = OBC_SetCustomPara.AutoPowerOffTime;
             MC_UpcInfo.stBikeInfo.uwSaveFlg = TRUE;
             MC_UpcInfo.stBikeInfo.swWheelSizeAdjust = OBC_SetCustomPara.DeltDiameter;
             MC_UpcInfo.stBikeInfo.uwStartMode = OBC_SetCustomPara.StartUpMod;
@@ -712,7 +712,7 @@ void DataProcess(uint16_t ID, uint8_t Mode, uint16_t Cmd, uint8_t *Data)
         }
         case 0x3C00: 
         {
-            SendData(ID_MC_TO_CDL, MODE_REPORT, 0xB31A, (uint8_t *)&MC_UpcInfo.stBikeInfo.uwWheelDiameter);
+            SendData(ID_MC_TO_CDL, MODE_REPORT, 0xB31A, (uint8_t *)&MC_UpcInfo.stBikeInfo.uwWheelPerimeter);
             break;
         }
         case 0x3D1C: //写入整车参数

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

@@ -38,6 +38,7 @@ MC_ControlCode_Struct_t          MC_ControlCode;
 MC_RunInfoToCDL_Struct_t         MC_RunInfoToCDL;
 MC_RS_ASCII_Struct_t             MC_RsASSCII;
 MC_UpperPCInfo_Struct_t          MC_UpcInfo;
+BMS_VoltEstimat_Struct_t         BMS_VoltEstimat;
 
 OBC_SetCustomPara_Struct_t OBC_SetCustomPara;
 OBC_ButtonStatus_Struct_t OBC_ButtonStatus = {0,0,0};
@@ -45,6 +46,7 @@ ULONG                      ulOBC_ComTimeOutCount = 0;
 uint8_t                    MC_MotorSPD_rpm_Percent = 0;
 uint8_t                    MC_WorkMode;
 uint8_t                    MC_BC_COM = 1;
+LPF_OUT                    BMS_swCurIdcLpf;
 
 void Can_voUpdateMC_UpcInfo(void)
 {
@@ -62,18 +64,18 @@ void Can_voUpdateMC_UpcInfo(void)
     MC_UpcInfo.stMotorInfo.uwJD = cp_stMotorPara.swJD;
     MC_UpcInfo.stMotorInfo.uwTorMaxNm = cp_stMotorPara.swTorMax;
 
-    MC_UpcInfo.stBikeInfo.uwWheelDiameter = ass_ParaCong.uwWheelDiameter;
-    MC_UpcInfo.stBikeInfo.uwMechRationMotor = ass_ParaCong.uwMechRationMotor;
-    MC_UpcInfo.stBikeInfo.uwAssistMaxSpdKmH = ass_ParaCong.uwAssistMaxSpdKmH;
-    MC_UpcInfo.stBikeInfo.uwThrottleMaxSpdKmH = ass_ParaCong.uwThrottleMaxSpdKmH;
-    MC_UpcInfo.stBikeInfo.uwNmFrontChainring = ass_ParaCong.uwNmFrontChainring;
-    MC_UpcInfo.stBikeInfo.uwNmBackChainring = ass_ParaCong.uwNmBackChainring;
-    MC_UpcInfo.stBikeInfo.uwAssistSelect1 = ass_ParaCong.uwAssistSelect1;
-    MC_UpcInfo.stBikeInfo.uwAssistSelect2 = ass_ParaCong.uwAssistSelect2;
-    MC_UpcInfo.stBikeInfo.uwLightVoltage = ass_ParaCong.uwLightVoltage;
-    MC_UpcInfo.stBikeInfo.swWheelSizeAdjust = ass_ParaCong.swDeltDiameter;
-    MC_UpcInfo.stBikeInfo.uwStartMode = ass_ParaCong.uwStartMode;
-    MC_UpcInfo.stBikeInfo.uwAutoPowerOffTime = ass_ParaCong.uwAutoPowerOffTime;
+    MC_UpcInfo.stBikeInfo.uwWheelPerimeter = ass_stParaCong.uwWheelPerimeter;
+    MC_UpcInfo.stBikeInfo.uwMechRationMotor = ass_stParaCong.uwMechRationMotor;
+    MC_UpcInfo.stBikeInfo.uwAssistMaxSpdKmH = ass_stParaCong.uwAssistMaxSpdKmH;
+    MC_UpcInfo.stBikeInfo.uwThrottleMaxSpdKmH = ass_stParaCong.uwThrottleMaxSpdKmH;
+    MC_UpcInfo.stBikeInfo.uwNmFrontChainring = ass_stParaCong.uwNmFrontChainring;
+    MC_UpcInfo.stBikeInfo.uwNmBackChainring = ass_stParaCong.uwNmBackChainring;
+    MC_UpcInfo.stBikeInfo.uwAssistSelect1 = ass_stParaCong.uwAssistSelect1;
+    MC_UpcInfo.stBikeInfo.uwAssistSelect2 = ass_stParaCong.uwAssistSelect2;
+    MC_UpcInfo.stBikeInfo.uwLightVoltage = ass_stParaCong.uwLightVoltage;
+    MC_UpcInfo.stBikeInfo.swWheelSizeAdjust = ass_stParaCong.swDeltPerimeter;
+    MC_UpcInfo.stBikeInfo.uwStartMode = ass_stParaCong.uwStartMode;
+    MC_UpcInfo.stBikeInfo.uwAutoPowerOffTime = ass_stParaCong.uwAutoPowerOffTime;
 
     MC_UpcInfo.stMContorlInfo.uwSPIPosOffsetOrigin = spi_stResolverOut.swSpiThetaOffsetOrignPu;
     MC_UpcInfo.stMContorlInfo.uwSPIPosOffsetNow = spi_stResolverOut.swSpiThetaOffsetPu;
@@ -103,18 +105,18 @@ void Can_voUpdateMC_UpcInfo(void)
     MC_UpcInfo.stSensorInfo.uwCadSensorPulseNm = cadence_stFreGetCof.uwNumbersPulses;
     MC_UpcInfo.stSensorInfo.uwBikeSpdSensorPulseNm = bikespeed_stFreGetCof.uwNumbersPulses;
 
-    MC_UpcInfo.stAssistInfo.swStartupGain = ass_ParaSet.uwStartupCoef;
-    MC_UpcInfo.stAssistInfo.swStartcruiseGain = ass_ParaSet.uwStartupCruiseCoef;
-    MC_UpcInfo.stAssistInfo.uwAssistStartNm = ass_ParaSet.uwAssistStartNm;
-    MC_UpcInfo.stAssistInfo.uwAssistStopNm = ass_ParaSet.uwAssistStopNm;
-    MC_UpcInfo.stAssistInfo.uwStartUpGainStep = ass_ParaSet.uwStartUpGainStep;
-    MC_UpcInfo.stAssistInfo.uwStartUpCadNm = ass_ParaSet.uwStartUpCadNm;
-    MC_UpcInfo.stAssistInfo.uwTorLPFCadNm = ass_ParaSet.uwTorLPFCadNm;
-    MC_UpcInfo.stAssistInfo.uwSpeedAssistSpdRpm = ass_ParaSet.uwSpeedAssistSpdRpm;
-    MC_UpcInfo.stAssistInfo.uwSpeedAssistIMaxA = ass_ParaSet.uwSpeedAssistIMaxA;
-    MC_UpcInfo.stAssistInfo.uwAssistLimitBikeSpdStart = ass_ParaSet.uwAssistLimitBikeSpdStart;
-    MC_UpcInfo.stAssistInfo.uwAssistLimitBikeSpdStop = ass_ParaSet.uwAssistLimitBikeSpdStop;
-    MC_UpcInfo.stAssistInfo.uwCadenceAssistWeight = ass_ParaSet.uwCadenceWeight;
+    MC_UpcInfo.stAssistInfo.swStartupGain = ass_stParaSet.uwStartupCoef;
+    MC_UpcInfo.stAssistInfo.swStartcruiseGain = ass_stParaSet.uwStartupCruiseCoef;
+    MC_UpcInfo.stAssistInfo.uwAssistStartNm = ass_stParaSet.uwAssistStartNm;
+    MC_UpcInfo.stAssistInfo.uwAssistStopNm = ass_stParaSet.uwAssistStopNm;
+    MC_UpcInfo.stAssistInfo.uwStartUpGainStep = ass_stParaSet.uwStartUpGainStep;
+    MC_UpcInfo.stAssistInfo.uwStartUpCadNm = ass_stParaSet.uwStartUpCadNm;
+    MC_UpcInfo.stAssistInfo.uwTorLPFCadNm = ass_stParaSet.uwTorLPFCadNm;
+    MC_UpcInfo.stAssistInfo.uwSpeedAssistSpdRpm = ass_stParaSet.uwSpeedAssistSpdRpm;
+    MC_UpcInfo.stAssistInfo.uwSpeedAssistIMaxA = ass_stParaSet.uwSpeedAssistIMaxA;
+    MC_UpcInfo.stAssistInfo.uwAssistLimitBikeSpdStart = ass_stParaSet.uwAssistLimitBikeSpdStart;
+    MC_UpcInfo.stAssistInfo.uwAssistLimitBikeSpdStop = ass_stParaSet.uwAssistLimitBikeSpdStop;
+    MC_UpcInfo.stAssistInfo.uwCadenceAssistWeight = ass_stParaSet.uwCadenceWeight;
 
     MC_UpcInfo.stHistoryInfo.uwOpenTimes = cp_stHistoryPara.uwOpenTimes;
     MC_UpcInfo.stHistoryInfo.uwUsedTimeH = (UWORD)(cp_stHistoryPara.ulUsedTime >> 16);
@@ -187,12 +189,12 @@ void Can_voUpdateMC_UpcInfo(void)
     MC_UpcInfo.stTestParaInfo.uwPwrLimitKp = cp_stControlPara.swPwrLimitKpPu;
     MC_UpcInfo.stTestParaInfo.uwPwrLimitKi = cp_stControlPara.swPwrLimitKiPu;
 
-    OBC_SetCustomPara.uwWheelDiameter = ass_ParaCong.uwWheelDiameter;
-    OBC_SetCustomPara.StartUpMod = ass_ParaCong.uwStartMode;
-    OBC_SetCustomPara.BikeSpeedLimit = ass_ParaCong.uwAssistMaxSpdKmH;
-    OBC_SetCustomPara.DeltDiameter = ass_ParaCong.swDeltDiameter;
-    OBC_SetCustomPara.AssistMod = ass_ParaSet.uwAsssistSelectNum;
-    OBC_SetCustomPara.AutoPowerOffTime = ass_ParaCong.uwAutoPowerOffTime;
+    OBC_SetCustomPara.uwWheelPerimeter = ass_stParaCong.uwWheelPerimeter;
+    OBC_SetCustomPara.StartUpMod = ass_stParaCong.uwStartMode;
+    OBC_SetCustomPara.BikeSpeedLimit = ass_stParaCong.uwAssistMaxSpdKmH;
+    OBC_SetCustomPara.DeltDiameter = ass_stParaCong.swDeltPerimeter;
+    OBC_SetCustomPara.AssistMod = ass_stParaSet.uwAsssistSelectNum;
+    OBC_SetCustomPara.AutoPowerOffTime = ass_stParaCong.uwAutoPowerOffTime;
 }
 
 UWORD SizeMCUP;
@@ -213,6 +215,9 @@ void  Can_voInitMC_Run(void)
     memcpy(MC_RsASSCII.CustomASCII1, flash_stPara.ubRsASSCII.CustomASCII1, sizeof(MC_RsASSCII.CustomASCII1));
     memcpy(MC_RsASSCII.CustomASCII2, flash_stPara.ubRsASSCII.CustomASCII2, sizeof(MC_RsASSCII.CustomASCII2));
     memcpy(MC_RsASSCII.CustomASCII3, flash_stPara.ubRsASSCII.CustomASCII3, sizeof(MC_RsASSCII.CustomASCII3));
+
+    BMS_VoltEstimat.uwInterResistpu = ((ULONG)1700 << 15) / cof_uwRbOm; //Q15 0.1mOhm BMS internal resistance 150mOhm + 20mOhm
+    mth_voLPFilterCoef(1000000 / 100, EVENT_1MS_HZ, &BMS_swCurIdcLpf.uwKx); //100Hz
 }
 
 void Can_voMC_Run_1ms(void)
@@ -491,14 +496,23 @@ void Can_voMC_Run_1ms(void)
             MC_ErrorCode.ERROR_Bit.Fault_TorqueSensor = 0;
         }
     }
+
+    BMS_VoltEstimat.swIdcPu = (((ULONG)scm_stMotoPwrInLpf.slY.sw.hi << 13) / adc_stUpOut.uwVdcLpfPu); //Q15+Q13-Q14=Q14  Calculated dc current 
+    if(BMS_VoltEstimat.swIdcPu < 0)
+    {
+      BMS_VoltEstimat.swIdcPu = 0;
+    }
+    mth_voLPFilter(BMS_VoltEstimat.swIdcPu, &BMS_swCurIdcLpf); 
+    BMS_VoltEstimat.uwVdcCompPu = (ULONG)BMS_swCurIdcLpf.slY.sw.hi * BMS_VoltEstimat.uwInterResistpu >> 15; //Q14+Q15-Q15=Q14
 }
 
 void Can_voMC_Run_5ms(void)
 {
     cp_stBikeRunInfoPara.BikeSpeedKmH =
-        (((SQWORD)bikespeed_stFreGetOut.uwLPFFrequencyPu * FBASE * (ass_ParaCong.uwWheelDiameter + ass_ParaCong.swDeltDiameter) * 36 >> 20) * 1048 * 10) >>
+        (((SQWORD)bikespeed_stFreGetOut.uwLPFFrequencyPu * FBASE * (ass_stParaCong.uwWheelPerimeter + ass_stParaCong.swDeltPerimeter) * 36 >> 20) * 1048 * 10) >>
         20; //    1048 = Q20(1/1000)    0.1 km/h
-    MC_RunInfo.BusVoltage = ((SLONG)adc_stUpOut.uwVdcPu * cof_uwUbVt >> 14) * 100;
+    // MC_RunInfo.BusVoltage = ((SLONG)adc_stUpOut.uwVdcPu * cof_uwUbVt >> 14) * 100;
+    MC_RunInfo.BusVoltage = ((ULONG)(adc_stUpOut.uwVdcLpfPu + BMS_VoltEstimat.uwVdcCompPu) * cof_uwUbVt * 100) >> 14;
     MC_RunInfo.MotorSpeed = (SLONG)scm_uwSpdFbkLpfAbsPu * cof_uwVbRpm >> 15;
     MC_RunInfo.BikeSpeed = cp_stBikeRunInfoPara.BikeSpeedKmH;//MC_RunInfo.MotorSpeed / 10;
 }
@@ -520,11 +534,11 @@ void Can_voMC_Run_200ms(void)
         MC_RunInfoToCDL.BusCurrent = (((ULONG)scm_stMotoPwrInLpf.slY.sw.hi << 13) / adc_stUpOut.uwVdcLpfPu * cof_uwIbAp * 100) >> 14; // 1mA
         MC_RunInfoToCDL.MotorSpeed = (SLONG)scm_stSpdFbkLpf.slY.sw.hi * cof_uwVbRpm >> 15;
         //        MC_RunInfoToCDL.BikeSpeed =
-        //            (((SQWORD)bikespeed_stFreGetOut.uwLPFFrequencyPu * cof_uwFbHz * ass_ParaCong.uwWheelDiameter * 3216 * 36 >> 15) * 1050) >>
+        //            (((SQWORD)bikespeed_stFreGetOut.uwLPFFrequencyPu * cof_uwFbHz * ass_stParaCong.uwWheelPerimeter * 3216 * 36 >> 15) * 1050) >>
         //            35; // 600 0.1cm  3216 = Q10(3.1415926)  105 = Q20(1/10000)    0.1 km/h;
         //
         MC_RunInfoToCDL.BikeSpeed =
-            (((SQWORD)bikespeed_stFreGetOut.uwLPFFrequencyPu * FBASE * ass_ParaCong.uwWheelDiameter * 36 >> 20) * 1048 * 10) >> 20;
+            (((SQWORD)bikespeed_stFreGetOut.uwLPFFrequencyPu * FBASE * ass_stParaCong.uwWheelPerimeter * 36 >> 20) * 1048 * 10) >> 20;
         MC_RunInfoToCDL.IqCurrent = ((SLONG)scm_swIqRefPu * cof_uwIbAp * 100) >> 14;
         MC_RunInfoToCDL.AlarmCode = MC_ErrorCode.Code && 0x00FF;
         SendData(ID_MC_TO_CDL, MODE_REPORT, 0xF112, (uint8_t *)&MC_RunInfoToCDL.Torque);
@@ -548,7 +562,7 @@ void Can_voMC_Run_200ms(void)
     {
     }
     MC_RunInfo.Power = scm_swMotorPwrInLpfWt / 10;                                    ///>电功率 1W，地址偏移4
-    MC_RunInfo.BusVoltage = ((ULONG)adc_stUpOut.uwVdcLpfPu * cof_uwUbVt * 100) >> 14; ///>母线电压 1mV，地址偏移6
+    MC_RunInfo.BusVoltage = ((ULONG)(adc_stUpOut.uwVdcLpfPu + BMS_VoltEstimat.uwVdcCompPu) * cof_uwUbVt * 100) >> 14; ///>???? 1mV?????6
     if (scm_stMotoPwrInLpf.slY.sw.hi < 10)
     {
         TempPower = 0;

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

@@ -181,7 +181,7 @@ void i2c_voDefaultWriteBuffer(void)
         I2C_pBuffer += I2C_PBUFFER_NWORDS;
     }
 
-    I2C_pBuffer = &Syspara2.stBikePara.uwWheelDiameter.uwDefault1;
+    I2C_pBuffer = &Syspara2.stBikePara.uwWheelPerimeter.uwDefault1;
     for (i = 0; i < I2C_BIKE_PARA_N_WORDS; i++)
     {
         I2C_ubWriteBuffer[I2C_BIKE_PARA_ARDDR + 2 * i] = *I2C_pBuffer >> 8;
@@ -256,7 +256,7 @@ void i2c_voParaWriteBuffer(void)
         I2C_pBuffer += I2C_PBUFFER_NWORDS;
     }
 
-    I2C_pBuffer = &Syspara2.stBikePara.uwWheelDiameter.uwDefault1;
+    I2C_pBuffer = &Syspara2.stBikePara.uwWheelPerimeter.uwDefault1;
     for (i = 0; i < I2C_BIKE_PARA_N_WORDS; i++)
     {
         I2C_ubWriteBuffer[I2C_BIKE_PARA_ARDDR + 2 * i] = *(I2C_pBuffer + 1) >> 8;
@@ -1191,7 +1191,7 @@ void i2c_voGetValueFrmBuffer(I2C_RXCRC_OUT *out)
     Syspara2.stMotorPara.uwJD.uwReal = I2C_uwMotorParaRead[11];
     Syspara2.stMotorPara.uwTorMaxNm.uwReal = I2C_uwMotorParaRead[12];
 
-    Syspara2.stBikePara.uwWheelDiameter.uwReal = I2C_uwBikeParaRead[0];
+    Syspara2.stBikePara.uwWheelPerimeter.uwReal = I2C_uwBikeParaRead[0];
     Syspara2.stBikePara.uwMechRationMotor.uwReal = I2C_uwBikeParaRead[1];
     Syspara2.stBikePara.uwAssistMaxSpdKmH.uwReal = I2C_uwBikeParaRead[2];
     Syspara2.stBikePara.uwThrottleMaxSpdKmH.uwReal = I2C_uwBikeParaRead[3];
@@ -1200,7 +1200,7 @@ void i2c_voGetValueFrmBuffer(I2C_RXCRC_OUT *out)
     Syspara2.stBikePara.uwAssistSelect1.uwReal = I2C_uwBikeParaRead[6];
     Syspara2.stBikePara.uwAssistSelect2.uwReal = I2C_uwBikeParaRead[7];
     Syspara2.stBikePara.uwLightVoltage.uwReal = I2C_uwBikeParaRead[8];
-    Syspara2.stBikePara.swDeltDiameter.swReal = I2C_uwBikeParaRead[9];
+    Syspara2.stBikePara.swDeltPerimeter.swReal = I2C_uwBikeParaRead[9];
     Syspara2.stBikePara.uwStartMode.uwReal = I2C_uwBikeParaRead[10];
     Syspara2.stBikePara.uwAutoPowerOffTime.uwReal = I2C_uwBikeParaRead[11];
 

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

@@ -257,6 +257,7 @@ void torsensor_voTorSensorInit(void)
 BOOL tstDynCalibflg= TRUE;
 UWORD tstTorqOffset,tstSensitiveset,TorqValue,TorqValuePu, TorqReg;
 SWORD tstTorqTemp,tstTorqTemp111,tstSencitiveOrig;
+
 void torsensor_voCadenceCnt(void)
 {
   if (((cadence_stFreGetCof.uwNumbersPulses>>1)-1) != tsttorqCadCnt)
@@ -358,8 +359,83 @@ void torsensor_voTorADCwithTemp(void)
         
     //TorqValue = ((torsensor_stTorSensorOut.uwTorqueReg - tstTorqOffset) << 12 )/tstSencitiveset;
 }
-
-
+/***************************************************************
+ Function:
+ Description:
+ Call by:
+ Input Variables: N/A
+ Output/Return Variables: N/A
+ Subroutine Call: N/A
+ Reference: N/A
+****************************************************************/
+UWORD tor_pvt_uwOffsetTarget = 0;
+UWORD tor_pvt_uwOffsetMax = 0,tor_pvt_uwOffsetMin = 4096;
+ULONG tor_pvt_ulCnt = 0;
+void torsensor_voOffsetUpdate(void) 
+{
+    SWORD swTorDelta;        
+    if(0 == (++tor_pvt_ulCnt % TORQUE_1S_1MSCNT))
+    {
+        swTorDelta = tor_pvt_uwOffsetMax - tor_pvt_uwOffsetMin;
+        if(swTorDelta > 40)
+        {   
+            tor_pvt_ulCnt = 0;              
+        }
+        tor_pvt_uwOffsetMax = 0;
+        tor_pvt_uwOffsetMin = 4096; 
+    }
+    else
+    {
+        if(tor_pvt_uwOffsetMin > torsensor_stTorSensorOut.uwTorqueReg)
+        {
+            tor_pvt_uwOffsetMin = torsensor_stTorSensorOut.uwTorqueReg;
+        }
+        
+        if(tor_pvt_uwOffsetMax < torsensor_stTorSensorOut.uwTorqueReg)
+        {
+            tor_pvt_uwOffsetMax = torsensor_stTorSensorOut.uwTorqueReg;
+        }    
+    }
+          
+    if(tor_pvt_ulCnt > TORQUE_90S_1MSCNT)
+    {
+        swTorDelta = tor_pvt_uwOffsetMax - tor_pvt_uwOffsetMin;
+        if(swTorDelta < 40)
+        {
+            tor_pvt_uwOffsetTarget = (tor_pvt_uwOffsetMax + tor_pvt_uwOffsetMin) >> 1;
+            if(torsensor_stTorSensorCof.uwTorqueOffset < tor_pvt_uwOffsetTarget - 20)
+            {
+                torsensor_stTorSensorCof.uwTorqueOffset += 20;
+            }
+            else if (torsensor_stTorSensorCof.uwTorqueOffset > tor_pvt_uwOffsetTarget + 20)
+            {
+                torsensor_stTorSensorCof.uwTorqueOffset -= 20;
+            }
+            else
+            {
+               torsensor_stTorSensorCof.uwTorqueOffset = tor_pvt_uwOffsetTarget;
+            }
+            
+            torsensor_stTorSensorCof.uwSensorVolPerTorq1 =
+            (((ULONG)3300 * (torsensor_stTorSensorCof.uwBikeTorStep1ADC - torsensor_stTorSensorCof.uwTorqueOffset)) >> 12) *10 *10/
+            (torsensor_stTorSensorCof.uwBikeTorStep1RealNm - 0);
+        
+            torsensor_stTorSensorCof.uwTorqueReg2Pu1 = (((SQWORD)33 << 24) / 10) / (1 << ADC_RESOLUTION_BIT) / TORQUE_VOLTAGE_SEN2MCUGAIN * 100 * 1000 /
+                                               torsensor_stTorSensorCof.uwSensorVolPerTorq1 / TORQUEBASE *
+                                               10*10; // 3.3/4096/harwaregain/VolPerNm/TorqueBase;           
+        }
+        tor_pvt_ulCnt = 0;   
+    }       
+}
+/***************************************************************
+ Function:
+ Description:
+ Call by:
+ Input Variables: N/A
+ Output/Return Variables: N/A
+ Subroutine Call: N/A
+ Reference: N/A
+****************************************************************/
 void torsensor_voTorADC(void) // need to match ADC_StartConversion(ADC1);
 {