同步 sim20231021版本

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

@@ -51,29 +51,29 @@ static SWORD Event_pvt_swIqRefTarget = 0, Event_pvt_swIqRef = 0, Event_pvt_swIqR
 static BOOL Event_pvt_blBikeThroFlg = FALSE, Event_pvt_blBikeThroFlgZ1 = FALSE;
 /******************************
  *
- *  Functions
+ *  Function
  *
  ******************************/
 void  Event_1ms(void)
 {
-  /* Timing of time slices */
+    /* Timing of time slices */
     TimingTaskTimerServer();
     
-    // 1st FSM control
+    /* 1st FSM control */
     FSM_1st_Main();
     FSM1st_Sys_state.Event_hook();
 
-    // Power Management
+    /* Power control */
     power_voPowerManagement(ass_stParaCong.uwAutoPowerOffTime, cp_ulSystickCnt, OBC_ButtonStatus.ulButtonSetTimeCnt, \
                             MC_RunInfo.Torque, MC_RunInfo.Cadence, MC_RunInfo.BikeSpeed, \
                             cp_stFlg.ParaHistorySaveEEFinishFlg, cp_stFlg.ParaSaveEEFlg);
     
-    // cp_history info update
+    /* cp_history info update */
     Can_voMC_Run_1ms();
 
     if(switch_flg.SysCoef_Flag == TRUE)
     {        
-        // Torq Maf
+        /* Torque move average filter */
         if (cadence_stFreGetOut.uwForwardCnt > 0)
         {
             //torsensor_voCadenceCnt();
@@ -99,7 +99,7 @@ void  Event_1ms(void)
             }
         }
         
-        // Torque info update
+        /* Torque info update */
         torsensor_voTorADC();                 
         // Torq Sensor Offset Update
         torsensor_voOffsetUpdate();
@@ -141,6 +141,7 @@ void  Event_1ms(void)
         ass_stCalIn.swCurRefPu = scm_swIqRefPu;
         ass_voAssist();
 
+        /* Select Bike Torque or Throttle Assist */
         if(Event_pvt_blBikeThroFlg == FALSE)
         {
             if (ass_stCalCoef.blAssistflag == TRUE && cp_stFlg.RunPermitFlg == TRUE && cp_stFlg.SpiOffsetFirstSetFlg ==1)
@@ -239,10 +240,14 @@ void  Event_1ms(void)
                 }
                 
                 //Limit max speed
-//                if(cp_stBikeRunInfoPara.BikeSpeedKmH > (ass_stParaCong.uwThrottleMaxSpdKmH * 10))
-//                {
-//                    uart_slSpdRefRpm = 0;
-//                }
+                if(cp_stBikeRunInfoPara.BikeSpeedKmH > ((ass_stParaCong.uwThrottleMaxSpdKmH + 2) * 10))
+                {
+                    uart_slSpdRefRpm = 0;
+                }
+                else if(cp_stBikeRunInfoPara.BikeSpeedKmH > (ass_stParaCong.uwThrottleMaxSpdKmH * 10))
+                { 
+                    uart_slSpdRefRpm = uart_slSpdRefRpm - uart_slSpdRefRpm * (cp_stBikeRunInfoPara.BikeSpeedKmH - ass_stParaCong.uwThrottleMaxSpdKmH * 10)/20;
+                }
             }  
             else
             {
@@ -265,6 +270,7 @@ void  Event_1ms(void)
 
 void Event_5ms(void)
 {
+    /* Upper Computer Info Update */
     Can_voMC_Run_5ms();
 }
 
@@ -339,7 +345,7 @@ void Event_100ms(void)
 
         /* Bike Throttle Assist */
         //if(((bikethrottle_stBikeThrottleOut.uwThrottlePercent > 200) || Event_pvt_uwAssistCnt == 2)&& (cp_stBikeRunInfoPara.uwBikeGear > 0) && (cp_stFlg.RunPermitFlg == TRUE) && (BikeBrake_blGetstate() == FALSE))
-        if((bikethrottle_stBikeThrottleOut.uwThrottlePercent > 200)  && (cp_stBikeRunInfoPara.uwBikeGear > 0) && (cp_stFlg.RunPermitFlg == TRUE))
+        if((bikethrottle_stBikeThrottleOut.uwThrottlePercent > 200)  && (cp_stBikeRunInfoPara.uwBikeGear > 0) && (cp_stFlg.RunPermitFlg == TRUE) && (BikeBrake_blGetstate() == FALSE))
         {
             Event_pvt_blBikeThroFlg = TRUE;
 
@@ -398,10 +404,12 @@ void Event_100ms(void)
 
 void  Event_200ms(void)
 {
+    /* Upper Computer Info Update */
     Can_voMC_Run_200ms();
     
     if(switch_flg.SysCoef_Flag == TRUE)
     {
+        /* Bike Sesor Suply Voltage Fault Detect */
         display_voGetDisplayError(adc_stUpOut.uwU12VPu);
         bikespeed_voGetBikeSpeedPwrError(adc_stUpOut.uwU5VPu);
         
@@ -435,34 +443,34 @@ void  Event_200ms(void)
 {
     SWORD sign;
 
-    if (cp_stFlg.RotateDirectionSelect == ForwardRotate)
+    if(cp_stFlg.RotateDirectionSelect == ForwardRotate)
     {
-          sign = 1;
+        sign = 1;
     }
     else if(cp_stFlg.RotateDirectionSelect == BackwardRotate)
     {
-          sign = -1;
+        sign = -1;
     }
     else
     {
-          sign = 1;
+        sign = 1;
     }
                   
     if(MC_ControlCode.GearSt == 0x01)
     {
-       uart_slSpdRefRpm = sign *785;
+        uart_slSpdRefRpm = sign *785;
     }
     else if(MC_ControlCode.GearSt == 0x02)
     {
-       uart_slSpdRefRpm = sign *1000;
+        uart_slSpdRefRpm = sign *1000;
     }
     else if(MC_ControlCode.GearSt == 0x03)
     {
-       uart_slSpdRefRpm = sign *3088;     
+        uart_slSpdRefRpm = sign *3088;
     }
     else if(MC_ControlCode.GearSt == 0x04)
     {
-        uart_slSpdRefRpm = sign*3603;    
+        uart_slSpdRefRpm = sign*3603;
     }
     else if(MC_ControlCode.GearSt == 0x33 || MC_ControlCode.GearSt == 0x05)
     {

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

@@ -19,6 +19,7 @@
 #include "power.h"
 #include "STLmain.h"
 #include "display.h"
+#include "enviolo_can.h"
 
 void PeripheralInit();
 void PeripheralStart();
@@ -64,16 +65,15 @@ void AppInit()
     CodeParaInit();
     /* AssitPara Init */
     ass_voAssitEEInit();
-
+    /* PowerInit */
+    power_voPowerInit();
     /* DisplayInit */
     display_voDisplayInit();
 #ifdef RUN_ARCH_SIM
     cp_stFlg.SpiOffsetFirstSetFlg = 1;
 #else
     /* Para Copy */
-    mn_voParaSet();   
-    /* PowerInit */
-    power_voPowerInit();
+    mn_voParaSet();
     //cp_stFlg.ParaFirstSetFlg = 0;  //eeprom save test
     if(cp_stFlg.ParaFirstSetFlg == 0)
     {

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

@@ -106,8 +106,9 @@ int main(void)
 ****************************************************************/
 void mn_voParaSet(void)
 {
+    /* Read system parameter*/  
     i2c_voSysparaReadFromEE(&i2c_stRXCRCOut);
-    
+
     if (i2c_stRXCRCOut.blHistoryParaFltFlg == FALSE)
     {
         ass_stParaSet.uwAsssistSelectNum = I2C_uwHistoryParaRead[0];
@@ -277,134 +278,134 @@ void mn_voParaSet(void)
 ****************************************************************/
 void mn_voParaUpdate(void)
 {
-        if (cp_stFlg.ParaMInfoUpdateFlg == TRUE)
-        {
-            cp_stMotorPara.swMotrPolePairs = (SWORD)MC_UpcInfo.stMotorInfo.uwPolePairs;
-            cp_stMotorPara.swRsOhm = (SWORD)MC_UpcInfo.stMotorInfo.uwRsmOhm;
-            cp_stMotorPara.swLdmH = (SWORD)MC_UpcInfo.stMotorInfo.uwLduH;
-            cp_stMotorPara.swLqmH = (SWORD)MC_UpcInfo.stMotorInfo.uwLquH;
-            cp_stMotorPara.swFluxWb = (SWORD)MC_UpcInfo.stMotorInfo.uwFluxmWb;
-            cp_stMotorPara.swIdMaxA = (SWORD)MC_UpcInfo.stMotorInfo.uwIdMaxA;
-            cp_stMotorPara.swIdMinA = (SWORD)MC_UpcInfo.stMotorInfo.uwIdMinA;
-            cp_stMotorPara.swRSpeedRpm = (SWORD)MC_UpcInfo.stMotorInfo.uwRSpdRpm;
-            cp_stMotorPara.swRPwrWt = (SWORD)MC_UpcInfo.stMotorInfo.uwRPwrWt;
-            cp_stMotorPara.swRIarmsA = (SWORD)MC_UpcInfo.stMotorInfo.uwRCurA;
-            cp_stMotorPara.swRUdcV = (SWORD)MC_UpcInfo.stMotorInfo.uwRVolV;
-            cp_stMotorPara.swJD = (SWORD)MC_UpcInfo.stMotorInfo.uwJD;
-            cp_stMotorPara.swTorMax = (SWORD)MC_UpcInfo.stMotorInfo.uwTorMaxNm;
-
-            cp_stFlg.ParaMotorDriveUpdateFinishFlg = TRUE;
-            cp_stFlg.ParaMInfoUpdateFlg = FALSE;
-        }
+    if (cp_stFlg.ParaMInfoUpdateFlg == TRUE)
+    {
+        cp_stMotorPara.swMotrPolePairs = (SWORD)MC_UpcInfo.stMotorInfo.uwPolePairs;
+        cp_stMotorPara.swRsOhm = (SWORD)MC_UpcInfo.stMotorInfo.uwRsmOhm;
+        cp_stMotorPara.swLdmH = (SWORD)MC_UpcInfo.stMotorInfo.uwLduH;
+        cp_stMotorPara.swLqmH = (SWORD)MC_UpcInfo.stMotorInfo.uwLquH;
+        cp_stMotorPara.swFluxWb = (SWORD)MC_UpcInfo.stMotorInfo.uwFluxmWb;
+        cp_stMotorPara.swIdMaxA = (SWORD)MC_UpcInfo.stMotorInfo.uwIdMaxA;
+        cp_stMotorPara.swIdMinA = (SWORD)MC_UpcInfo.stMotorInfo.uwIdMinA;
+        cp_stMotorPara.swRSpeedRpm = (SWORD)MC_UpcInfo.stMotorInfo.uwRSpdRpm;
+        cp_stMotorPara.swRPwrWt = (SWORD)MC_UpcInfo.stMotorInfo.uwRPwrWt;
+        cp_stMotorPara.swRIarmsA = (SWORD)MC_UpcInfo.stMotorInfo.uwRCurA;
+        cp_stMotorPara.swRUdcV = (SWORD)MC_UpcInfo.stMotorInfo.uwRVolV;
+        cp_stMotorPara.swJD = (SWORD)MC_UpcInfo.stMotorInfo.uwJD;
+        cp_stMotorPara.swTorMax = (SWORD)MC_UpcInfo.stMotorInfo.uwTorMaxNm;
+
+        cp_stFlg.ParaMotorDriveUpdateFinishFlg = TRUE;
+        cp_stFlg.ParaMInfoUpdateFlg = FALSE;
+    }
 
-        if (cp_stFlg.ParaBikeInfoUpdateFlg == TRUE)
-        {
-            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;
-        }
+    if (cp_stFlg.ParaBikeInfoUpdateFlg == TRUE)
+    {
+        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;
+    }
 
-        if (cp_stFlg.ParaMCInfoUpdateFlg == TRUE)
-        {
-            cp_stFlg.ParaFirstSetFlg = MC_UpcInfo.stTestParaInfo.uwEEFirstDefaultSetFlg;
-            cp_stFlg.SpiOffsetFirstSetFlg = MC_UpcInfo.stTestParaInfo.uwSPIOffsetFirstSetFlg;
-            spi_stResolverOut.swSpiThetaOffsetOrignPu = MC_UpcInfo.stMContorlInfo.uwSPIPosOffsetOrigin;
-            spi_stResolverOut.swSpiThetaOffsetPu = MC_UpcInfo.stMContorlInfo.uwSPIPosOffsetNow;
-            cp_stMotorPara.swIpeakMaxA = (SWORD)MC_UpcInfo.stMContorlInfo.uwIPeakMaxA;
-            cp_stControlPara.swAlmOverCurrentVal = (SWORD)MC_UpcInfo.stMContorlInfo.uwAlamOCurA;
-            cp_stControlPara.swAlmOverVolVal3 = (SWORD)MC_UpcInfo.stMContorlInfo.uwAlamOVolV;
-            cp_stControlPara.swAlmUnderVolVal2 = (SWORD)MC_UpcInfo.stMContorlInfo.uwAlamUVolV;
-            cp_stControlPara.swAlmOverSpdVal = (SWORD)MC_UpcInfo.stMContorlInfo.uwAlamOverSpdRpm;
-            cp_stControlPara.swAlmOverHeatCeVal = (SWORD)MC_UpcInfo.stMContorlInfo.uwAlamOverHeatCe;
-            cp_stControlPara.swAlmRecOHeatVal = (SWORD)MC_UpcInfo.stMContorlInfo.uwAlamRecHeatCe;
-            cp_stControlPara.swAlmPwrLimitStartTempVal = (SWORD)MC_UpcInfo.stMContorlInfo.uwPwrLimitStartCe;
-
-            cp_stFlg.ParaMotorDriveUpdateFinishFlg = TRUE;
-            cp_stFlg.ParaMCInfoUpdateFlg = FALSE;
-        }
+    if (cp_stFlg.ParaMCInfoUpdateFlg == TRUE)
+    {
+        cp_stFlg.ParaFirstSetFlg = MC_UpcInfo.stTestParaInfo.uwEEFirstDefaultSetFlg;
+        cp_stFlg.SpiOffsetFirstSetFlg = MC_UpcInfo.stTestParaInfo.uwSPIOffsetFirstSetFlg;
+        spi_stResolverOut.swSpiThetaOffsetOrignPu = MC_UpcInfo.stMContorlInfo.uwSPIPosOffsetOrigin;
+        spi_stResolverOut.swSpiThetaOffsetPu = MC_UpcInfo.stMContorlInfo.uwSPIPosOffsetNow;
+        cp_stMotorPara.swIpeakMaxA = (SWORD)MC_UpcInfo.stMContorlInfo.uwIPeakMaxA;
+        cp_stControlPara.swAlmOverCurrentVal = (SWORD)MC_UpcInfo.stMContorlInfo.uwAlamOCurA;
+        cp_stControlPara.swAlmOverVolVal3 = (SWORD)MC_UpcInfo.stMContorlInfo.uwAlamOVolV;
+        cp_stControlPara.swAlmUnderVolVal2 = (SWORD)MC_UpcInfo.stMContorlInfo.uwAlamUVolV;
+        cp_stControlPara.swAlmOverSpdVal = (SWORD)MC_UpcInfo.stMContorlInfo.uwAlamOverSpdRpm;
+        cp_stControlPara.swAlmOverHeatCeVal = (SWORD)MC_UpcInfo.stMContorlInfo.uwAlamOverHeatCe;
+        cp_stControlPara.swAlmRecOHeatVal = (SWORD)MC_UpcInfo.stMContorlInfo.uwAlamRecHeatCe;
+        cp_stControlPara.swAlmPwrLimitStartTempVal = (SWORD)MC_UpcInfo.stMContorlInfo.uwPwrLimitStartCe;
+
+        cp_stFlg.ParaMotorDriveUpdateFinishFlg = TRUE;
+        cp_stFlg.ParaMCInfoUpdateFlg = FALSE;
+    }
 
-        if (cp_stFlg.ParaSensorInfoUpdateFlg == TRUE)
-        {
-            torsensor_stTorSensorCof.uwTorqueOffsetOrign = MC_UpcInfo.stSensorInfo.uwTorSensorOffsetOrigin;
-            torsensor_stTorSensorCof.uwMaxSensorTorquePu = MC_UpcInfo.stSensorInfo.uwBikeTorMaxNm;
-            torsensor_stTorSensorCof.uwBikeTorStep1RealNm = MC_UpcInfo.stSensorInfo.uwBikeTor1StepRealNm;
-            torsensor_stTorSensorCof.uwBikeTorStep1ADC = MC_UpcInfo.stSensorInfo.uwBikeTor1StepADC;
-            torsensor_stTorSensorCof.uwBikeTorStep2RealNm = MC_UpcInfo.stSensorInfo.uwBikeTor2StepRealNm;
-            torsensor_stTorSensorCof.uwBikeTorStep2ADC = MC_UpcInfo.stSensorInfo.uwBikeTor2StepADC;
-            torsensor_stTorSensorCof.uwBikeTorStep3RealNm = MC_UpcInfo.stSensorInfo.uwBikeTor3StepRealNm;
-            torsensor_stTorSensorCof.uwBikeTorStep3ADC = MC_UpcInfo.stSensorInfo.uwBikeTor3StepADC;
-            torsensor_stTorSensorCof.uwBikeTorStep4RealNm = MC_UpcInfo.stSensorInfo.uwBikeTor4StepRealNm;
-            torsensor_stTorSensorCof.uwBikeTorStep4ADC = MC_UpcInfo.stSensorInfo.uwBikeTor4StepADC;
-            cadence_stFreGetCof.uwNumbersPulses = MC_UpcInfo.stSensorInfo.uwCadSensorPulseNm;
-            bikespeed_stFreGetCof.uwNumbersPulses = MC_UpcInfo.stSensorInfo.uwBikeSpdSensorPulseNm;
-
-            cp_stFlg.ParaAssistUpdateFinishFlg = TRUE;
-            cp_stFlg.ParaSensorInfoUpdateFlg = FALSE;
-        }
+    if (cp_stFlg.ParaSensorInfoUpdateFlg == TRUE)
+    {
+        torsensor_stTorSensorCof.uwTorqueOffsetOrign = MC_UpcInfo.stSensorInfo.uwTorSensorOffsetOrigin;
+        torsensor_stTorSensorCof.uwMaxSensorTorquePu = MC_UpcInfo.stSensorInfo.uwBikeTorMaxNm;
+        torsensor_stTorSensorCof.uwBikeTorStep1RealNm = MC_UpcInfo.stSensorInfo.uwBikeTor1StepRealNm;
+        torsensor_stTorSensorCof.uwBikeTorStep1ADC = MC_UpcInfo.stSensorInfo.uwBikeTor1StepADC;
+        torsensor_stTorSensorCof.uwBikeTorStep2RealNm = MC_UpcInfo.stSensorInfo.uwBikeTor2StepRealNm;
+        torsensor_stTorSensorCof.uwBikeTorStep2ADC = MC_UpcInfo.stSensorInfo.uwBikeTor2StepADC;
+        torsensor_stTorSensorCof.uwBikeTorStep3RealNm = MC_UpcInfo.stSensorInfo.uwBikeTor3StepRealNm;
+        torsensor_stTorSensorCof.uwBikeTorStep3ADC = MC_UpcInfo.stSensorInfo.uwBikeTor3StepADC;
+        torsensor_stTorSensorCof.uwBikeTorStep4RealNm = MC_UpcInfo.stSensorInfo.uwBikeTor4StepRealNm;
+        torsensor_stTorSensorCof.uwBikeTorStep4ADC = MC_UpcInfo.stSensorInfo.uwBikeTor4StepADC;
+        cadence_stFreGetCof.uwNumbersPulses = MC_UpcInfo.stSensorInfo.uwCadSensorPulseNm;
+        bikespeed_stFreGetCof.uwNumbersPulses = MC_UpcInfo.stSensorInfo.uwBikeSpdSensorPulseNm;
+
+        cp_stFlg.ParaAssistUpdateFinishFlg = TRUE;
+        cp_stFlg.ParaSensorInfoUpdateFlg = FALSE;
+    }
 
-        if (cp_stFlg.ParaAInfoUpdateFlg == TRUE)
-        {
-            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;
-        }
+    if (cp_stFlg.ParaAInfoUpdateFlg == TRUE)
+    {
+        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;
+    }
 
-        if(cp_stFlg.TestParaInfoUpdateFlg == TRUE)
-        {
-            cp_stControlPara.swAlignCurAp = (SWORD)MC_UpcInfo.stTestParaInfo.uwInitPosCurAmp;
-            cp_stControlPara.swDragVolAp = (SWORD)MC_UpcInfo.stTestParaInfo.uwVFControlVolAmp;
-            cp_stControlPara.swDragCurAp = (SWORD)MC_UpcInfo.stTestParaInfo.uwIFControlCurAmp;
-            cp_stControlPara.swDragSpdHz = (SWORD)MC_UpcInfo.stTestParaInfo.uwVFIFTargetFreHz;
-
-            cp_stControlPara.swSpeedAccRate = (SWORD)MC_UpcInfo.stTestParaInfo.uwSpeedLoopAccRate;
-            cp_stControlPara.swSpeedDccRate = (SWORD)MC_UpcInfo.stTestParaInfo.uwSpeedLoopDecRate;
-            cp_stControlPara.swAsrPIBandwidth = (SWORD)MC_UpcInfo.stTestParaInfo.uwSpeedLoopBandWidthHz;
-            cp_stControlPara.swAsrPIM = (SWORD)MC_UpcInfo.stTestParaInfo.uwSpeedLoopCoefM;
-
-            cp_stControlPara.swAcrPIBandwidth = (SWORD)MC_UpcInfo.stTestParaInfo.uwCuerrentLoopBandWidthHz;
-            cp_stControlPara.swAcrRaCoef = (SWORD)MC_UpcInfo.stTestParaInfo.uwCurrentLoopCoefM;
-
-            cp_stControlPara.swObsFluxPICrossfreHz = (SWORD)MC_UpcInfo.stTestParaInfo.uwFluxObsBandWidthHz;
-            cp_stControlPara.swObsFluxPIDampratio = (SWORD)MC_UpcInfo.stTestParaInfo.uwFluxObsCoefM;
-            cp_stControlPara.swObsSpdPLLBandWidthHz = (SWORD)MC_UpcInfo.stTestParaInfo.uwThetaObsPLLBandWidthHz;
-            cp_stControlPara.swObsSpdPLLM = (SWORD)MC_UpcInfo.stTestParaInfo.uwThetaObsPLLCoefM;
-
-            cp_stMotorPara.swJD = (SWORD)MC_UpcInfo.stTestParaInfo.uwJm;
-            cp_stControlPara.swPWMMaxDuty = (SWORD)MC_UpcInfo.stTestParaInfo.uwPWMMaxDuty;
-            cp_stControlPara.swPWM7to5Duty = (SWORD)MC_UpcInfo.stTestParaInfo.uwPWM7to5Duty;
-            cp_stControlPara.swPwrLimitValWt = (SWORD)MC_UpcInfo.stTestParaInfo.uwPwrLimit;
-            cp_stControlPara.swPwrLimitErrWt = (SWORD)MC_UpcInfo.stTestParaInfo.uwPwrLimitError;
-            cp_stControlPara.swPwrLimitKpPu = (SWORD)MC_UpcInfo.stTestParaInfo.uwPwrLimitKp;
-            cp_stControlPara.swPwrLimitKiPu = (SWORD)MC_UpcInfo.stTestParaInfo.uwPwrLimitKi;
-
-            cp_stFlg.ParaMotorDriveUpdateFinishFlg = TRUE;
-            // cp_stFlg.TestParaInfoUpdateFlg = FALSE;
-        }
+    if(cp_stFlg.TestParaInfoUpdateFlg == TRUE)
+    {
+        cp_stControlPara.swAlignCurAp = (SWORD)MC_UpcInfo.stTestParaInfo.uwInitPosCurAmp;
+        cp_stControlPara.swDragVolAp = (SWORD)MC_UpcInfo.stTestParaInfo.uwVFControlVolAmp;
+        cp_stControlPara.swDragCurAp = (SWORD)MC_UpcInfo.stTestParaInfo.uwIFControlCurAmp;
+        cp_stControlPara.swDragSpdHz = (SWORD)MC_UpcInfo.stTestParaInfo.uwVFIFTargetFreHz;
+
+        cp_stControlPara.swSpeedAccRate = (SWORD)MC_UpcInfo.stTestParaInfo.uwSpeedLoopAccRate;
+        cp_stControlPara.swSpeedDccRate = (SWORD)MC_UpcInfo.stTestParaInfo.uwSpeedLoopDecRate;
+        cp_stControlPara.swAsrPIBandwidth = (SWORD)MC_UpcInfo.stTestParaInfo.uwSpeedLoopBandWidthHz;
+        cp_stControlPara.swAsrPIM = (SWORD)MC_UpcInfo.stTestParaInfo.uwSpeedLoopCoefM;
+
+        cp_stControlPara.swAcrPIBandwidth = (SWORD)MC_UpcInfo.stTestParaInfo.uwCuerrentLoopBandWidthHz;
+        cp_stControlPara.swAcrRaCoef = (SWORD)MC_UpcInfo.stTestParaInfo.uwCurrentLoopCoefM;
+
+        cp_stControlPara.swObsFluxPICrossfreHz = (SWORD)MC_UpcInfo.stTestParaInfo.uwFluxObsBandWidthHz;
+        cp_stControlPara.swObsFluxPIDampratio = (SWORD)MC_UpcInfo.stTestParaInfo.uwFluxObsCoefM;
+        cp_stControlPara.swObsSpdPLLBandWidthHz = (SWORD)MC_UpcInfo.stTestParaInfo.uwThetaObsPLLBandWidthHz;
+        cp_stControlPara.swObsSpdPLLM = (SWORD)MC_UpcInfo.stTestParaInfo.uwThetaObsPLLCoefM;
+
+        cp_stMotorPara.swJD = (SWORD)MC_UpcInfo.stTestParaInfo.uwJm;
+        cp_stControlPara.swPWMMaxDuty = (SWORD)MC_UpcInfo.stTestParaInfo.uwPWMMaxDuty;
+        cp_stControlPara.swPWM7to5Duty = (SWORD)MC_UpcInfo.stTestParaInfo.uwPWM7to5Duty;
+        cp_stControlPara.swPwrLimitValWt = (SWORD)MC_UpcInfo.stTestParaInfo.uwPwrLimit;
+        cp_stControlPara.swPwrLimitErrWt = (SWORD)MC_UpcInfo.stTestParaInfo.uwPwrLimitError;
+        cp_stControlPara.swPwrLimitKpPu = (SWORD)MC_UpcInfo.stTestParaInfo.uwPwrLimitKp;
+        cp_stControlPara.swPwrLimitKiPu = (SWORD)MC_UpcInfo.stTestParaInfo.uwPwrLimitKi;
+
+        cp_stFlg.ParaMotorDriveUpdateFinishFlg = TRUE;
+        // cp_stFlg.TestParaInfoUpdateFlg = FALSE;
+    }
 }
 /***************************************************************
  Function: mn_voSoftwareInit;

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

@@ -33,7 +33,6 @@ Revising History (ECL of this file):
 #include "queue.h"
 #include "canAppl.h"
 #include "alarm.h"
-#include "torquesensor.h"
 /*************************************************************************
  Exported Functions (N/A)
 *************************************************************************/
@@ -50,27 +49,26 @@ void tbc_voUpIsr(void)
 {
     /* Uart Monitor */
     uart_voAppMonitor();
-   
+        
     if( cp_stFlg.ThetaGetModelSelect == ANG_RESOLVER )
     {
-        /*Resolver lock data*/
         spi_voResolverLock();
-        /* Resolver data read */
         spi_voResolver(&spi_stResolverCoef, &spi_stResolverOut);
     }
     else if( cp_stFlg.ThetaGetModelSelect == ANG_SWITCHHALL )
     {
-        /*Switchhall Position Cal*/
         switchhall_voPosCalTbc();
     }
     else
     {
-        //do nothing
+    	//do noting
     }
 
+    /* 1st FSM */
     FSM1st_Sys_state.Tbcup_hook();
-    
+
 #ifdef RUN_ARCH_SIM
+    /* Inject */
     SWORD elecOmega = 0;
     UWORD gearstate = 0;
     INJ_PT(INT16, uart_slSpdRefRpm, 4);
@@ -78,29 +76,21 @@ void tbc_voUpIsr(void)
     INJ_PT(INT16, elecOmega, 1);
     INJ_PT(INT16, gearstate, 5);
     MC_ControlCode.GearSt =gearstate;
-
-
     spi_stResolverOut.swSpdFbkPu = ((SLONG)elecOmega << 15) / cof_uwWebRadps * 10;
     
+    /* Watch */
     TEST_PT(INT, scm_uwAngParkPu, 0);
     TEST_PT(INT, scm_swSpdRefPu, 1);
     TEST_PT(INT, scm_stSpdFbkLpf.slY.sw.hi, 2);
     TEST_PT(INT, uart_slSpdRefRpm, 3);
     TEST_PT(INT, alm_unCode.all, 4);
     TEST_PT(INT, adc_stUpOut.uwVdcLpfPu, 5);
-
     TEST_PT(INT, scm_swIqRefPu, 6);
     TEST_PT(INT, scm_swIqFdbLpfPu, 7);
-
     TEST_PT(INT, adc_stDownOut.slSampIaPu, 8);
     TEST_PT(INT, adc_stDownOut.swIaPu, 9);
     TEST_PT(INT, adc_stUpOut.swCalibIaPu, 10);
     TEST_PT(INT, crd_stCurParkOut.swQPu, 11);
-    TEST_PT(INT, cadence_stFreGetOut.uwFrequencyPu, 12);
-    TEST_PT(INT, bikespeed_stFreGetOut.uwFrequencyPu, 13);  
-    TEST_PT(INT, torsensor_stTorSensorOut.uwTorquePu, 14); 
-    TEST_PT(INT, cp_stBikeRunInfoPara.uwBikeGear, 15); 
-
 #endif
 }
 

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

@@ -31,10 +31,22 @@ Revising History (ECL of this file):
 #include "cmdgennew.h"
 #include "CodePara.h"
 #include "FSM_2nd.h"
+#include "api.h"
 /************************************************************************
  Constant Table (N/A)
 *************************************************************************/
 
+/************************************************************************
+Private Variables
+*************************************************************************/
+static LPF_OUT swTmpSpdRateLpf;
+static SWORD swTmpSpdRate = 0;
+static SWORD swTmpSpdFbkPuZ1 = 0;
+static SWORD swSpdRateAbsPu;
+static SWORD swTestIqref;
+static UWORD DCPswitch = 0;
+static CRD_PARK_IN  Test_U_in;
+static CRD_PARK_OUT Test_U_out;
 /*************************************************************************
  Exported Functions (N/A)
 *************************************************************************/
@@ -76,10 +88,13 @@ void scm_voSpdCtrMdInit(void)
     /* Contant voltage brake init */
     cvb_voBrakeInit();
     /* switchHall init */
-    switchhall_voInit();
+    //switchhall_voInit();
     /* Align pos startup open2clz clzloop init */
     align_voInit();
+    /* Torque observer init */
+    torqobs_voInit();
 
+    /* Global variablles init */
     scm_stSpdFbkLpf.slY.sw.hi = 0;
     scm_swSpdRefPu = 0;
     scm_swUalphaPu = 0;        // Q14
@@ -135,6 +150,14 @@ void scm_voSpdCtrMdInit(void)
     scm_uwInitPosMdSw = scm_uwInitPosMd;
     scm_uwHfiOvrCnt = 0;
     scm_slIdRefPu = 0;
+    
+    /* Private variables init */
+    swTmpSpdRateLpf.slY.sl = 0;
+    swTmpSpdRate = 0;
+    swTmpSpdFbkPuZ1 = 0;
+    swSpdRateAbsPu = 0;
+    swTestIqref = 0;
+    DCPswitch = 0;
 }
 /***************************************************************
  Function: scm_voSpdCtrMdCoef;
@@ -215,7 +238,7 @@ void scm_voSpdCtrMdCoef(void)
     torqobs_stCoefIn.uwWtcHz = 50; // cp_stControlPara.swAsrPIBandwidth;
     torqobs_stCoefIn.uwRatioJm = cp_stControlPara.swAsrSpdInerRate;
     torqobs_voCoef(&torqobs_stCoefIn, &torqobs_stCoef);    
-    mth_voLPFilterCoef(1000000 / 100, FTBS_HZ, &scm_stIqLoadLpf.uwKx); //50Hz
+    mth_voLPFilterCoef(1000000 / 50, FTBS_HZ, &scm_stIqLoadLpf.uwKx); //50Hz
     /* Id PI coefficient calculate */
     acr_stCurIdPICoefIn.uwFbHz = FBASE;
     acr_stCurIdPICoefIn.uwUbVt = VBASE;
@@ -496,18 +519,11 @@ void scm_voSpdCtrMdTbs(void)
     curSpeed_state.Tbs_hook();
 }
 
-static SWORD deltC, switchCNT, switchflg;
-static BOOL blTorqCompFlg;
-static SWORD swTmpSpdRate = 0;
-static LPF_OUT swTmpSpdRateLpf;
-static SWORD swTmpSpdFbkPuZ1 = 0;
-static SWORD swSpdRateAbsPu;
-static SWORD swTestIqref;
 void  scm_voTorqCtrMdTbs(void)
 {
     SWORD swIqLowerPu;
-/* Speed feedback LPF */
-
+    
+    /* Speed feedback LPF */
     if(cp_stFlg.ThetaGetModelSelect == ANG_OBSERVER)
     {
         mth_voLPFilter(obs_stObsOutPu.swElecFreqPu, &scm_stSpdFbkLpf);
@@ -528,53 +544,64 @@ void  scm_voTorqCtrMdTbs(void)
     /* Speed feedback Absolute */
     scm_uwSpdFbkLpfAbsPu = (UWORD)ABS(scm_stSpdFbkLpf.slY.sw.hi);
 
-    //    /*============================================================
-    //              Speed command generator to generate speed ramp
-    //    =============================================================*/
-    //    cmd_stCmdIn.swSpdCmdRpm = -(((SLONG)cadence_stFreGetOut.uwLPFFrequencyPu * 8000) >> 10) * 6000 >> 15;
-    //    cmd_stCmdIn.swSpdNowPu = scm_stSpdFbkLpf.slY.sw.hi;
-    //    cmd_voCmdOut(&cmd_stCmdIn, &cmd_stCmdCoef, &cmd_stCmdOut);
-    //    /*=======================================================================
-    //                                Get speed command
-    //      =======================================================================*/
-    //    scm_swRotateDir = cmd_stCmdOut.swNewCmdDir;
-    //    scm_swSpdRefPu = cmd_stCmdOut.swIntRefPu; //cmd_stCmdGenOut.Out.swSpdRefPu;
-
-    /*=======================================================================
-                                 Speed PI Controller
-        =======================================================================*/
-    swIqLowerPu = (SWORD)((flx_stCtrlOut.swIqLimPu < ABS(pwr_stPwrLimOut2.swIqLimPu)) ? flx_stCtrlOut.swIqLimPu : ABS(pwr_stPwrLimOut2.swIqLimPu));  
-    /* Torque observe */
-    if (scm_swRotateDir > 0)
-    {
-        torqobs_stCalIn.swIqMaxPu = swIqLowerPu;
-        torqobs_stCalIn.swIqMinPu = -swIqLowerPu;
-    }
-    else
-    {
-        torqobs_stCalIn.swIqMaxPu = swIqLowerPu;
-        torqobs_stCalIn.swIqMinPu = -swIqLowerPu;
-    }
-    torqobs_stCalIn.swIqfbkPu = scm_swIqFdbLpfPu;
-    torqobs_stCalIn.swSpdPu = spi_stResolverOut.swSpdFbkPu;
-    torqobs_voCal(&torqobs_stCalIn, &torqobs_stCoef, &torqobs_stCalOut);
-    mth_voLPFilter((torqobs_stCalOut.swIqLoadPu + scm_swIqFdbLpfPu), &scm_stIqLoadLpf);
+    /* Current limit value */
+    swIqLowerPu = (SWORD)((flx_stCtrlOut.swIqLimPu < ABS(pwr_stPwrLimOut2.swIqLimPu)) ? flx_stCtrlOut.swIqLimPu : ABS(pwr_stPwrLimOut2.swIqLimPu));
     
+    /* Torque observer */
+//    if (scm_swRotateDir > 0)
+//    {
+//        torqobs_stCalIn.swIqMaxPu = swIqLowerPu;
+//        torqobs_stCalIn.swIqMinPu = -swIqLowerPu;
+//    }
+//    else
+//    {
+//        torqobs_stCalIn.swIqMaxPu = swIqLowerPu;
+//        torqobs_stCalIn.swIqMinPu = -swIqLowerPu;
+//    }
+//    torqobs_stCalIn.swIqfbkPu = scm_swIqFdbLpfPu;
+//    torqobs_stCalIn.swSpdPu = spi_stResolverOut.swSpdFbkPu;
+//    torqobs_voCal(&torqobs_stCalIn, &torqobs_stCoef, &torqobs_stCalOut);
+//    mth_voLPFilterCoef(1000000 / 250, FTBS_HZ, &scm_stIqLoadLpf.uwKx); 
+//    mth_voLPFilter((torqobs_stCalOut.swIqLoadPu + scm_swIqFdbLpfPu), &scm_stIqLoadLpf);
     
-//    /* Spd Fbk Compensation Calculate */
+      /* Speed fluctuation compensation */
+      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;
+
+      mth_voLPFilterCoef(1000000 / 250, FTBS_HZ, &swTmpSpdRateLpf.uwKx); 
+      mth_voLPFilter(swTmpSpdRate, &swTmpSpdRateLpf);
+//      swTmpSpdRateLpf.slY.sw.hi = swTmpSpdRate;
+      
+      /* Torque Calculate */
 //    swTmpSpdRate = spi_stResolverOut.swSpdFbkPu - swTmpSpdFbkPuZ1; //Q15
 //    mth_voLPFilterCoef(1000000 / 30, FTBS_HZ, &swTmpSpdRateLpf.uwKx); //30Hz,TBS
 //    mth_voLPFilter(swTmpSpdRate, &swTmpSpdRateLpf);
 //    swTmpSpdFbkPuZ1 = spi_stResolverOut.swSpdFbkPu;
 //    scm_swSpdFbkCompPu = scm_stSpdFbkLpf.slY.sw.hi + (SLONG)swTmpSpdRateLpf.slY.sw.hi * FTBS_HZ / 30; //30Hz,TBS
+      
+//      mth_voLPFilterCoef(1000000 / 250, 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;  // 系数 = J/Tlpf/Pesi 
+//
+////      mth_voLPFilterCoef(1000000 / 250, FTBS_HZ, &swTmpSpdRateLpf.uwKx); 
+////      mth_voLPFilter(swTmpSpdRate, &swTmpSpdRateLpf);
+//      swTmpSpdRateLpf.slY.sw.hi = swTmpSpdRate;
+//    
 
     /* Iqref Compensation */
     if(((uart_swTorqRefNm < -200)||(uart_swTorqRefNm > 200)) && (Ass_FSM !=Spd2Torq) && (Ass_FSM !=SpeedAssit))
     {
-       /* Open Loop */
+       /* Torque Calculate */
        //swTestIqref  = uart_swTorqRefNm - (((SLONG)swTmpSpdRateLpf.slY.sw.hi * cof_uwJmPu * 2 << 11) / cof_uwFluxPu);  //Q15+Q0+Q11-Q12=Q14
        
-       /* Observer */
+       /* Speed fluctuation compensation*/
+//        swTestIqref = uart_swTorqRefNm - swTmpSpdRateLpf.slY.sw.hi;
+         
+       /* Torgque observer */
        swTestIqref  = uart_swTorqRefNm - scm_stIqLoadLpf.slY.sw.hi;
     }
     else
@@ -582,7 +609,7 @@ void  scm_voTorqCtrMdTbs(void)
        swTestIqref = uart_swTorqRefNm;
     }
 
-    
+    /* Current limit */
     if (swTestIqref > swIqLowerPu)
     {
         swTestIqref = swIqLowerPu;
@@ -597,20 +624,7 @@ void  scm_voTorqCtrMdTbs(void)
     }
     swCurRefrompu = swTestIqref;  
     
-
-    
-//    if (uart_swTorqRefNm > swIqLowerPu)
-//    {
-//        uart_swTorqRefNm = swIqLowerPu;
-//    }
-//    else if (uart_swTorqRefNm < -swIqLowerPu)
-//    {
-//        uart_swTorqRefNm = -swIqLowerPu;
-//    }
-//    else
-//    {}
-//    swCurRefrompu = uart_swTorqRefNm;
-
+    /* 3rd FSM*/
     curSpeed_state.Tbs_hook();
 }
 /***************************************************************
@@ -622,8 +636,6 @@ void  scm_voTorqCtrMdTbs(void)
  Subroutine Call: ...;
  Reference: N/A
 ****************************************************************/
-static CRD_PARK_IN  Test_U_in;
-static CRD_PARK_OUT Test_U_out;
 void scm_voSpdCtrMdUpTbc(void)
 {
     /*=======================================================================
@@ -654,7 +666,6 @@ void scm_voSpdCtrMdUpTbc(void)
  Subroutine Call: ...;
  Reference: N/A
 ****************************************************************/
-static UWORD DCPswitch = 0;
 void  scm_voSpdCtrMdDownTbc(void)
 {
     /*=======================================================================
@@ -699,7 +710,7 @@ void  scm_voSpdCtrMdDownTbc(void)
                             Id current PI control
     =======================================================================*/
     //DCPswitch = 0;           //0 with forwardFeedBack    1 without forwardFeedBack
-//      scm_swIdRefPu = 0;
+
     acr_stCurIdPIIn.swCurRefPu = scm_swIdRefPu; // Q14
     acr_stCurIdPIIn.swCurFdbPu = scm_swIdFdbLpfPu;
     if (DCPswitch == 1)
@@ -733,16 +744,20 @@ void  scm_voSpdCtrMdDownTbc(void)
     }
     else if (DCPswitch == 0)
     {        
-        if(FSM2nd_Run_state.state == Assistance)
-        {
-          acr_stCurIqPIIn.swUmaxPu = ass_stCalOut.swVoltLimitPu - acr_stUdqDcpOut.swUqPu;  // Q14
-          acr_stCurIqPIIn.swUminPu = -ass_stCalOut.swVoltLimitPu - acr_stUdqDcpOut.swUqPu; // Q14
-        }
-        else
+//        if(FSM2nd_Run_state.state == Assistance)
+//        {
+//          acr_stCurIqPIIn.swUmaxPu = ass_stCalOut.swVoltLimitPu - acr_stUdqDcpOut.swUqPu;  // Q14
+//          acr_stCurIqPIIn.swUminPu = -ass_stCalOut.swVoltLimitPu - acr_stUdqDcpOut.swUqPu; // Q14
+//        }
+//        else
         {
            acr_stCurIqPIIn.swUmaxPu = scm_swVsDcpLimPu - acr_stUdqDcpOut.swUqPu;  // Q14
            acr_stCurIqPIIn.swUminPu = -scm_swVsDcpLimPu - acr_stUdqDcpOut.swUqPu; // Q14
         } 
+        
+//        scm_swUqLimPu = mth_slSqrt(((SLONG)scm_swVsLimPu * scm_swVsLimPu) - (SLONG)(acr_stCurIdPIOut.swURefPu + acr_stUdqDcpOut.swUdPu) * (acr_stCurIdPIOut.swURefPu + acr_stUdqDcpOut.swUdPu));//Q14
+//        acr_stCurIqPIIn.swUmaxPu = scm_swUqLimPu - acr_stUdqDcpOut.swUqPu;  // Q14
+//        acr_stCurIqPIIn.swUminPu = -scm_swUqLimPu - acr_stUdqDcpOut.swUqPu; // Q14
     }
     else
     {
@@ -823,17 +838,19 @@ void  scm_voSpdCtrMdDownTbc(void)
     pwm_stGenIn.swUbetaPu = scm_swUbetaRefPu;     // Q14
     pwm_stGenIn.uwVdcPu = adc_stUpOut.uwVdcLpfPu; // Q14
     pwm_voGen(&pwm_stGenIn, &pwm_stGenCoef, &pwm_stGenOut);
-        
+
     iPwm_SetCompareGroupValues16(0, pwm_stGenOut.uwNewTIM1COMPR);
     ULONG samplingTick[2];
     samplingTick[0]=pwm_stGenOut.uwSigRTrig;
     samplingTick[1]=pwm_stGenOut.uwRdsonTrig;
     iPwm_SyncMultiSamplingCountUp(0, &samplingTick[0], 2);
-
+    
+    
     Test_U_in.swAlphaPu = pwm_stGenOut.swUalphaPu - scm_swUalphaCompPu; // Q14
     Test_U_in.swBetaPu = pwm_stGenOut.swUbetaPu - scm_swUbetaCompPu;    // Q14
     Test_U_in.uwThetaPu = scm_uwAngIParkPu;                             // Q15
     crd_voPark(&Test_U_in, &Test_U_out);
+
 }
 /*************************************************************************
  Local Functions (N/A)

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

@@ -52,8 +52,8 @@
 
 #define BIKE_ASS_MOTOR_CURRENT_MAX      5000 //5500 // 0.01A
 #define BIKE_ASS_MOTOR_TORQUE_MAX       30   // 0.1Nm
-#define BIKE_SPD_MOTOR_CURRENT_MAX      500  // 0.01A
-#define BIKE_SPD_MOTOR_CONSTANT_COMMAND 5  // RPM
+#define BIKE_SPD_MOTOR_CURRENT_MAX      5  /* 电流系数初始值 */
+#define BIKE_SPD_MOTOR_CONSTANT_COMMAND 4096  /* 速度波动补偿系数初始值 */
 
 #define ASS_LIM_DEFAULT                    \
     {                                      \
@@ -70,7 +70,7 @@
 
 #define ASS_LINER_TORQUE_DEFAULT             \
     {                                  \
-        1638, 2457, 3276, 4096, 2457 \
+        2048, 2867, 4096, 10000, 2867 \
     } // Q12
 //#define TORQUE_ASSIST_DEFAULT                                                                                                                      \
 //    {                                                                                                                                              \
@@ -261,6 +261,7 @@ typedef struct
     UWORD StartFlag;
 
     SWORD swSmoothGain;         // Q12
+    SWORD swSmoothStopGain;         /* 停机电流系数 */
     UWORD uwStartUpGainAddStep; // Q12
     UWORD uwStartUpTargetGain;  // Q12
     UWORD uwStartUpTimeCadenceCnt;

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

@@ -41,7 +41,7 @@ extern "C" {
     }
 #define CADENCE_OUT_DEFAULT                                               \
     {                                                                     \
-        0, 0, 0, 0, 0, 0, 0, FALSE, FALSE, CADENCE_DIR_IDLE, CADENCE_IDLE \
+        0, 0, 0, 0, 0, 0, 0, 0, FALSE, FALSE, CADENCE_DIR_IDLE, CADENCE_IDLE, 0, 0, 0 \
     } // Default value of CADENCE_OUT
 
 #define CADENCESPEED_KMPERH2FREQPU   299   // 200 ms per between two interupte 
@@ -107,6 +107,7 @@ typedef struct
     UWORD       uwCaputureNumCnt;        // The current sequece of the pulse
     UWORD       uwCaputureErrorCnt;      // The current sequece of the pulse
     UWORD       uwCaputureOverflowCnt;   // number of TIM4 CNT Overflow between two capture
+    UWORD       uwCaputureOverflowCnt2;
     BOOL        blCadenceSensorErrorFlg; // The falg of sensor error
     BOOL        blCadenceCalStartState;  // TRUE = START   FALSE = STOP
     CADENCE_DIR cadence_dir;             // Direction of cadence

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

@@ -0,0 +1,133 @@
+/*
+ * File: LoadObsTheta.h
+ *
+ * Code generated for Simulink model 'LoadObsTheta'.
+ *
+ * Model version                  : 8.72
+ * Simulink Coder version         : 9.6 (R2021b) 14-May-2021
+ * C/C++ source code generated on : Sat Nov  5 15:46:39 2022
+ *
+ * Target selection: ert.tlc
+ * Embedded hardware selection: ARM Compatible->ARM Cortex-M
+ * Code generation objectives: Unspecified
+ * Validation result: Not run
+ */
+
+#ifndef RTW_HEADER_LoadObsTheta_h_
+#define RTW_HEADER_LoadObsTheta_h_
+#ifndef LoadObsTheta_COMMON_INCLUDES_
+#define LoadObsTheta_COMMON_INCLUDES_
+#include "rtwtypes2.h"
+#endif                                 /* LoadObsTheta_COMMON_INCLUDES_ */
+
+/* Model Code Variants */
+
+/* Macros for accessing real-time model data structure */
+
+/* Block states (default storage) for system '<Root>' */
+typedef struct
+{
+    int32_T UnitDelay4_DSTATE;         /* '<S3>/Unit Delay4' */
+    int32_T UnitDelay2_DSTATE;         /* '<S3>/Unit Delay2' */
+    int32_T UnitDelay3_DSTATE;         /* '<S3>/Unit Delay3' */
+    int32_T UnitDelay1_DSTATE;         /* '<S3>/Unit Delay1' */
+}
+DW_LoadObsTheta_T;
+
+/* External inputs (root inport signals with default storage) */
+typedef struct
+{
+    uint16_T ThetamPu;                 /* '<Root>/ThetamPu' */
+    int16_T IqFbkPu;                   /* '<Root>/IqFbkPu' */
+}
+ExtU_LoadObsTheta_T;
+
+/* External outputs (root outports fed by signals with default storage) */
+typedef struct
+{
+    int16_T swSpdFbkPu;                /* '<Root>/swSpdFbkPu' */
+    uint16_T uwThetaObsPu;             /* '<Root>/uwThetaObsPu' */
+    int16_T swTLPu;                    /* '<Root>/swTLPu' */
+    int16_T swIqCompPu;                /* '<Root>/swIqCompPu' */
+}
+ExtY_LoadObsTheta_T;
+
+/* Type definition for custom storage class: Struct */
+typedef struct LoadObsTheta_stCoef_tag
+{
+    uint16_T uwCurTs;
+    uint16_T uwFluxPu;
+    uint16_T uwFluxPuInv;
+    uint16_T uwJmPuInv;
+    uint16_T uwK1Pu;
+    uint16_T uwK2Pu;
+    uint16_T uwK3Pu;
+    uint16_T uwTctrPu;
+}
+LoadObsTheta_stCoef_type;
+
+typedef struct LoadObsTheta_stCoefIn_tag
+{
+    uint16_T uwFTbcHz;                 /* '<Root>/uwFTbcHz' */
+    uint16_T uwFbHz;                   /* '<Root>/uwFbHz' */
+    uint16_T uwFluxWb;                 /* '<Root>/uwFluxWb' */
+    uint16_T uwFluxbWb;                /* '<Root>/uwFluxbWb' */
+    uint16_T uwJb;                     /* '<Root>/uwJb' */
+    uint16_T uwMCoef;                  /* '<Root>/uwMCoef' */
+    uint16_T uwMtJm;                   /* '<Root>/uwMtJm' */
+    uint16_T uwWtcHz;                  /* '<Root>/uwWtcHz' */
+}
+LoadObsTheta_stCoefIn_type;
+
+/* Block states (default storage) */
+extern DW_LoadObsTheta_T LoadObsTheta_DW;
+
+/* External inputs (root inport signals with default storage) */
+extern ExtU_LoadObsTheta_T LoadObsTheta_U;
+
+/* External outputs (root outports fed by signals with default storage) */
+extern ExtY_LoadObsTheta_T LoadObsTheta_Y;
+
+/* Model entry point functions */
+extern void LoadObsTheta_initialize(void);
+extern void LoadObsTheta_voCoef(void);
+extern void LoadObsTheta_voInit(void);
+extern void LoadObsTheta_step(void);
+extern void LoadObsTheta_terminate(void);
+
+/* Exported data declaration */
+
+/* Declaration for custom storage class: Struct */
+extern LoadObsTheta_stCoef_type LoadObsTheta_stCoef;
+extern LoadObsTheta_stCoefIn_type LoadObsTheta_stCoefIn;
+
+/*-
+ * The generated code includes comments that allow you to trace directly
+ * back to the appropriate location in the model.  The basic format
+ * is <system>/block_name, where system is the system number (uniquely
+ * assigned by Simulink) and block_name is the name of the block.
+ *
+ * Use the MATLAB hilite_system command to trace the generated code back
+ * to the model.  For example,
+ *
+ * hilite_system('<S3>')    - opens system 3
+ * hilite_system('<S3>/Kp') - opens and selects block Kp which resides in S3
+ *
+ * Here is the system hierarchy for this model
+ *
+ * '<Root>' : 'LoadObsTheta'
+ * '<S1>'   : 'LoadObsTheta/Initialize Function1'
+ * '<S2>'   : 'LoadObsTheta/Initialize Function2'
+ * '<S3>'   : 'LoadObsTheta/LoadObs_Fixdt_Pu'
+ * '<S4>'   : 'LoadObsTheta/LoadObs_Fixdt_Pu/Compare To Constant'
+ * '<S5>'   : 'LoadObsTheta/LoadObs_Fixdt_Pu/Compare To Constant1'
+ * '<S6>'   : 'LoadObsTheta/LoadObs_Fixdt_Pu/Compare To Constant2'
+ * '<S7>'   : 'LoadObsTheta/LoadObs_Fixdt_Pu/Compare To Constant3'
+ */
+#endif                                 /* RTW_HEADER_LoadObsTheta_h_ */
+
+/*
+ * File trailer for generated code.
+ *
+ * [EOF]
+ */

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

@@ -367,7 +367,7 @@
     }
 #define I2CswStartcruiseGain \
     {                        \
-        4096, 0, 0, 0, 0     \
+        0, 0, 0, 0, 0     \
     }
 #define I2CswNmAssistChangeNum \
     {                          \
@@ -522,7 +522,7 @@
     }
 #define I2CuwStartUpGainStep \
     {                        \
-        20, 0, 0, 0, 0       \
+        50, 0, 0, 0, 0       \
     }
 #define I2CuwStartUpCadNm \
     {                     \
@@ -982,7 +982,7 @@ _I2C_MASTER_EXT UWORD I2C_uwBikeParaRead[I2C_BIKE_PARA_N_WORDS];
 _I2C_MASTER_EXT UWORD I2C_uwMControlRead[I2C_MCONTROL_PARA_N_WORDS];
 _I2C_MASTER_EXT UWORD I2C_uwSensorRead[I2C_SENSOR_PARA_N_WORDS];
 _I2C_MASTER_EXT UWORD I2C_uwAssistParaRead[I2C_ASSIST_PARA_N_WORDS];
-_I2C_MASTER_EXT UWORD I2C_uwGearBoxParaRead[I2C_ASSIST_PARA_N_WORDS];
+_I2C_MASTER_EXT UWORD I2C_uwGearBoxParaRead[I2C_GEARBOX_PARA_N_WORDS];
 _I2C_MASTER_EXT UWORD I2C_uwHistoryParaRead[I2C_HISTORY_PARA_N_WORDS];
 
 _I2C_MASTER_EXT UBYTE I2C_ubWriteBuffer[I2C_TX1_NBYTES];

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

@@ -0,0 +1,95 @@
+/*
+ * File: rtwtypes.h
+ *
+ * Code generated for Simulink model 'LoadObsTheta'.
+ *
+ * Model version                  : 8.72
+ * Simulink Coder version         : 9.6 (R2021b) 14-May-2021
+ * C/C++ source code generated on : Sat Nov  5 15:46:39 2022
+ *
+ * Target selection: ert.tlc
+ * Embedded hardware selection: ARM Compatible->ARM Cortex-M
+ * Code generation objectives: Unspecified
+ * Validation result: Not run
+ */
+
+#ifndef RTWTYPES_H
+#define RTWTYPES_H
+
+/* Logical type definitions */
+#if (!defined(__cplusplus))
+#ifndef false
+#define false                          (0U)
+#endif
+
+#ifndef true
+#define true                           (1U)
+#endif
+#endif
+
+/*=======================================================================*
+ * Target hardware information
+ *   Device type: ARM Compatible->ARM Cortex-M
+ *   Number of bits:     char:   8    short:   16    int:  32
+ *                       long:  32    long long:  64
+ *                       native word size:  32
+ *   Byte ordering: LittleEndian
+ *   Signed integer division rounds to: Zero
+ *   Shift right on a signed integer as arithmetic shift: on
+ *=======================================================================*/
+
+/*=======================================================================*
+ * Fixed width word size data types:                                     *
+ *   int8_T, int16_T, int32_T     - signed 8, 16, or 32 bit integers     *
+ *   uint8_T, uint16_T, uint32_T  - unsigned 8, 16, or 32 bit integers   *
+ *=======================================================================*/
+typedef signed char int8_T;
+typedef unsigned char uint8_T;
+typedef short int16_T;
+typedef unsigned short uint16_T;
+typedef int int32_T;
+typedef unsigned int uint32_T;
+typedef long long int64_T;
+typedef unsigned long long uint64_T;
+
+/*===========================================================================*
+ * Generic type definitions: boolean_T, char_T, byte_T, int_T, uint_T,       *
+ *                           ulong_T, ulonglong_T.                           *
+ *===========================================================================*/
+typedef unsigned char boolean_T;
+typedef int int_T;
+typedef unsigned int uint_T;
+typedef unsigned long ulong_T;
+typedef unsigned long long ulonglong_T;
+typedef char char_T;
+typedef unsigned char uchar_T;
+typedef char_T byte_T;
+
+/*=======================================================================*
+ * Min and Max:                                                          *
+ *   int8_T, int16_T, int32_T     - signed 8, 16, or 32 bit integers     *
+ *   uint8_T, uint16_T, uint32_T  - unsigned 8, 16, or 32 bit integers   *
+ *=======================================================================*/
+#define MAX_int8_T                     ((int8_T)(127))
+#define MIN_int8_T                     ((int8_T)(-128))
+#define MAX_uint8_T                    ((uint8_T)(255U))
+#define MAX_int16_T                    ((int16_T)(32767))
+#define MIN_int16_T                    ((int16_T)(-32768))
+#define MAX_uint16_T                   ((uint16_T)(65535U))
+#define MAX_int32_T                    ((int32_T)(2147483647))
+#define MIN_int32_T                    ((int32_T)(-2147483647-1))
+#define MAX_uint32_T                   ((uint32_T)(0xFFFFFFFFU))
+#define MAX_int64_T                    ((int64_T)(9223372036854775807LL))
+#define MIN_int64_T                    ((int64_T)(-9223372036854775807LL-1LL))
+#define MAX_uint64_T                   ((uint64_T)(0xFFFFFFFFFFFFFFFFULL))
+
+/* Block D-Work pointer type */
+typedef void * pointer_T;
+
+#endif                                 /* RTWTYPES_H */
+
+/*
+ * File trailer for generated code.
+ *
+ * [EOF]
+ */

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

@@ -141,7 +141,7 @@ 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_voCadenceCnt(void);
 void torsensor_voDynamicOffset(void);
 void torsensor_voOffsetUpdate(void);
 /************************************************************************/

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

@@ -57,7 +57,8 @@ static UWORD StartUpGainArray[5] = START_GAIN_DEFAULT;
 static UWORD LinerAssist[5] = ASS_LINER_TORQUE_DEFAULT;
 static SWORD ass_pvt_swVoltCnt=0;
 static UWORD ass_pvt_uwTorqAccCnt=0,ass_pvt_uwTorqDecCnt=0,ass_pvt_uwSpd2TorqCnt=0;
-static UWORD AssCnt1ms; 
+static UWORD AssCnt1ms;
+static UWORD ass_pvt_uwSmoothFlg = 0;
 /******************************
  *
  *  Function
@@ -239,6 +240,7 @@ void ass_voAssitEEInit(void)
     ass_pvt_uwTorqAccCnt = 0;
     ass_pvt_uwTorqDecCnt = 0;
     ass_pvt_uwSpd2TorqCnt = 0;
+    ass_pvt_uwSmoothFlg = 0;
     AssCnt1ms = 0; 
 }
 
@@ -288,16 +290,9 @@ void ass_voAssitCoef(void)
     /*助力系数初始化*/
     ass_stCalCoef.StartFlag = 0;
     ass_stCalCoef.swSmoothGain = 0;                                     // Q12
+    ass_stCalCoef.swSmoothStopGain = 4096;                                //Q12
     ass_stCalCoef.uwStartUpTargetGain = 0;                              // Q12
     ass_stCalCoef.uwStartUpGainAddStep = ass_stParaSet.uwStartUpGainStep; // 25 Q12
-    if (ass_stCalCoef.uwStartUpGainAddStep < 1)
-    {
-        ass_stCalCoef.uwStartUpGainAddStep = 1;
-    }
-    if (ass_stCalCoef.uwStartUpGainAddStep > 50)
-    {
-        ass_stCalCoef.uwStartUpGainAddStep = 50;
-    }
 
     /*设置启动到正常助力最少踏频数*/
     ass_stCalCoef.uwStartUpTimeCadenceCnt = ass_stParaSet.uwStartUpCadNm;
@@ -458,14 +453,15 @@ void ass_voAssitCoef(void)
 //    out->swIRefPu = out->slIRefPu >> 15; // Q29-Q15=Q14
 //    out->swErrZ1Pu = (SWORD)slErrPu;
 //}
-SLONG slTeTorAssitTmpPu,slTeTorAssitLinerPu,slTeCadAssitTmpPu;
-SWORD swTeTorAssitPu1, swTeTorAssitPu2;
-SWORD swTeCadAssitPu1, swTeCadAssitPu2;
-SWORD swTmpSpdtoTorqCur;   
-SLONG slTmpSmoothCur;
-SWORD swTorqCmd1, swTorqCmd, swCadCmd;
+
 static void AssitCuvApplPerVolt(void) /* parasoft-suppress METRICS-28 "本项目圈复杂度无法更改，后续避免" */
-{      
+{  
+    SLONG slTeTorAssitTmpPu,slTeTorAssitLinerPu,slTeCadAssitTmpPu;
+    SWORD swTeTorAssitPu1, swTeTorAssitPu2;
+    SWORD swTeCadAssitPu1, swTeCadAssitPu2;
+    SWORD swTmpSpdtoTorqCur;   
+    SLONG slTmpSmoothCur;
+    SWORD swTorqCmd1, swTorqCmd, swCadCmd;
     UWORD uwTorqAccStep = 50,uwTorqDecStep = 80;
     SWORD swCurSwitch = 0;
     SWORD swTmpVoltPu,swTmpVoltPu2;
@@ -473,9 +469,7 @@ static void AssitCuvApplPerVolt(void) /* parasoft-suppress METRICS-28 "本项目
     SWORD swSpdKpPu = 500; //Q10
     UWORD uwVoltAccStep = 1, uwVoltDecStep = 3;
     UWORD uwTmpStopCnt = 0;
-
 //    SLONG slTmp_a1, slTmp_b1, slTmp_c1;
-
   
     /* Select Torq Growth Rate by Bike Gear */
     if (ass_stCalIn.uwGearSt == 1)
@@ -510,10 +504,10 @@ static void AssitCuvApplPerVolt(void) /* parasoft-suppress METRICS-28 "本项目
         AssCnt1ms = 0;
     }
     
-    
     /* Select TorqRef: LPFTorq or MAFTorq */
     swTorqCmd1 = (SWORD)(((SLONG)ass_stCalIn.uwtorque * ass_stCalCoef.swTorqFilterGain >> 14) +
                ((SLONG)ass_stCalIn.uwtorquelpf * (Q14_1 - ass_stCalCoef.swTorqFilterGain) >> 14)); //转矩指令滤波切换，由低通滤波到踏频相关的滑动平均滤波
+
     swTorqCmd = (SWORD)(((SLONG)swTorqCmd1 * ass_stCalCoef.swSmoothGain) >> 12);                             //转矩指令斜坡
     if (swTorqCmd > ass_stParaCong.uwBikeAssTorMaxPu) // 最大转矩限幅
     {
@@ -578,11 +572,30 @@ static void AssitCuvApplPerVolt(void) /* parasoft-suppress METRICS-28 "本项目
     switch (Ass_FSM)
     {
     case Startup:
-//        ass_stCalCoef.swSmoothGain = Q12_1;      
-         ass_stCalCoef.swSmoothGain  += (SWORD)ass_stParaSet.uwSpeedAssistIMaxA; ////ass_stCalCoef.uwStartUpGainAddStep;
-         if(ass_stCalCoef.swSmoothGain >= Q12_1)
+         /* 启动系数 */
+        if(ass_pvt_uwSmoothFlg == 0)
         {
-            ass_stCalCoef.swSmoothGain = Q12_1;
+            ass_stCalCoef.swSmoothGain  += ass_stParaSet.uwSpeedAssistIMaxA; ////ass_stCalCoef.uwStartUpGainAddStep;            
+            if(ass_stCalCoef.swSmoothGain >= ass_stParaSet.uwStartupCoef)
+            {
+                ass_pvt_uwSmoothFlg = 1;
+            }
+        }
+        else if (ass_pvt_uwSmoothFlg == 1)
+        {
+            if(ass_stCalCoef.swSmoothGain >= Q12_1)
+            {
+                ass_stCalCoef.swSmoothGain  -= (ass_stParaSet.uwSpeedAssistIMaxA >> 1);
+            }
+            else
+            {
+                ass_stCalCoef.swSmoothGain = Q12_1;
+                ass_pvt_uwSmoothFlg = 2;
+            }        
+        }
+        else
+        {
+            // do nothing
         }
          
         swSpdKpPu = 1000;  //ass_stParaSet.uwStartUpCadNm;
@@ -610,10 +623,12 @@ static void AssitCuvApplPerVolt(void) /* parasoft-suppress METRICS-28 "本项目
             }          
             ass_stCalOut.swVoltLimitPu = (SWORD)slTmpVoltLim;
             
+            /* 电机与踏频转速差小于阈值启动完成 */
             if(slSpdErr <= 1000)
             {
                 ass_stCalCoef.StartFlag = 1;
             }  
+            /* 根据电流环饱和情况判断启动完成 */
 //            if(ABS(scm_swIqRefPu- scm_swIqFdbLpfPu) > 200)
 //            {
 //                ass_pvt_swVoltCnt++;  
@@ -663,7 +678,8 @@ static void AssitCuvApplPerVolt(void) /* parasoft-suppress METRICS-28 "本项目
         {
         	//do nothing
         }
-
+        
+        
         /* Switch to ReduceCurrent FSM */
         if((ass_stCalIn.uwcadancePer == 0) || (ass_stCalIn.uwGearSt == 0) || BikeBrake_blGetstate() == TRUE)
         {
@@ -690,7 +706,7 @@ static void AssitCuvApplPerVolt(void) /* parasoft-suppress METRICS-28 "本项目
             
             if(ass_stCalCoef.swAss2SpdCNT > uwTmpStopCnt)
             {
-                Ass_FSM = ReduceCurrent;
+                Ass_FSM = ReduceCurrent;  
                 ass_stCalCoef.swAss2SpdCNT = 0;
                 ass_stCalCoef.StartFlag=0;
             } 
@@ -701,15 +717,32 @@ static void AssitCuvApplPerVolt(void) /* parasoft-suppress METRICS-28 "本项目
         }
 
         break;
-    case TorqueAssit:  
-            
-          /* 启动系数 */
-        ass_stCalCoef.swSmoothGain  +=  (SWORD)ass_stParaSet.uwSpeedAssistIMaxA; ////ass_stCalCoef.uwStartUpGainAddStep;
-        if(ass_stCalCoef.swSmoothGain >= Q12_1)
+    case TorqueAssit:            
+         /* 启动系数 */
+        if(ass_pvt_uwSmoothFlg == 0)
         {
-            ass_stCalCoef.swSmoothGain = Q12_1;
+            ass_stCalCoef.swSmoothGain  += ass_stParaSet.uwSpeedAssistIMaxA; ////ass_stCalCoef.uwStartUpGainAddStep;            
+            if(ass_stCalCoef.swSmoothGain >= ass_stParaSet.uwStartupCoef)
+            {
+                ass_pvt_uwSmoothFlg = 1;
+            }
+        }
+        else if (ass_pvt_uwSmoothFlg == 1)
+        {
+            if(ass_stCalCoef.swSmoothGain >= Q12_1)
+            {
+                ass_stCalCoef.swSmoothGain  -= (ass_stParaSet.uwSpeedAssistIMaxA >> 1);
+            }
+            else
+            {
+                ass_stCalCoef.swSmoothGain = Q12_1;
+                ass_pvt_uwSmoothFlg = 2;
+            }        
+        }
+        else
+        {
+            // do nothing
         }
-        
         /* Reduce Voltage Limit When LPFTorq < Switch1TorqThreshold */     
         if(0 == (AssCnt1ms%5))
         {
@@ -724,9 +757,9 @@ static void AssitCuvApplPerVolt(void) /* parasoft-suppress METRICS-28 "本项目
 //            else if (ass_stCalIn.uwtorque <= ass_stCalCoef.uwSwitch1TorqThreshold)
 //            {
 ////                ass_stCalOut.swVoltLimitPu -=  ass_stCalCoef.uwSpeedConstantCommand;
-////                if (ass_stCalOut.swVoltLimitPu <= (tmpVoltargetPu + ass_ParaSet.uwStartUpCadNm))
+////                if (ass_stCalOut.swVoltLimitPu <= (tmpVoltargetPu + ass_stParaSet.uwStartUpCadNm))
 ////                {
-////                    ass_stCalOut.swVoltLimitPu =  tmpVoltargetPu + ass_ParaSet.uwStartUpCadNm;
+////                    ass_stCalOut.swVoltLimitPu =  tmpVoltargetPu + ass_stParaSet.uwStartUpCadNm;
 ////                }
 //            }
 //            else
@@ -782,18 +815,22 @@ static void AssitCuvApplPerVolt(void) /* parasoft-suppress METRICS-28 "本项目
         
     case  ReduceCurrent:
         /* Switch to StopAssit FSM */
-        if(ass_stCalCoef.swSmoothGain <= 0)
+        if(ass_stCalCoef.swSmoothStopGain <= 0)
         {
+            ass_pvt_uwSmoothFlg = 0;
             ass_stCalCoef.swSmoothGain = 0;
+            ass_stCalCoef.swSmoothStopGain = 0;
             ass_stCalCoef.swTorqFilterGain = 0;
             ass_stCalCoef.swCadanceGain = 0;
+            ass_voMoveAverageFilterClear(&ass_stTorqMafValue);
             Ass_FSM = StopAssit;     
         }  
         else
         {
             /* Reduce Curret Coef to Zero*/
-            ass_stCalCoef.swSmoothGain -=40;   
             ass_stCalOut.swVoltLimitPu = scm_swVsDcpLimPu;
+            ass_stCalCoef.swSmoothGain -= ass_stCalCoef.uwStartUpGainAddStep;
+            ass_stCalCoef.swSmoothStopGain -= ass_stCalCoef.uwStartUpGainAddStep;
         }
         
         /* Switch to Startup FSM */
@@ -817,17 +854,21 @@ static void AssitCuvApplPerVolt(void) /* parasoft-suppress METRICS-28 "本项目
                   {               
                       ass_stCalCoef.sw2StopCNT = 0;
                       ass_stCalOut.swVoltLimitPu = scm_swVsDcpLimPu;
-                      ass_pvt_stCurLpf.slY.sw.hi = 0;                
+                      ass_pvt_stCurLpf.slY.sw.hi = 0;  
+                      ass_stCalCoef.swSmoothStopGain = Q12_1;
+                      ass_stCalCoef.swSmoothGain = ass_stParaSet.uwStartupCruiseCoef;
                       Ass_FSM = Startup;
                   }
               }
               else
               {
-                  if (ass_stCalIn.uwtorquelpf > ((ass_stCalCoef.uwAssThreshold * 3)>>3) && ass_stCalIn.uwtorquePer > ass_stCalCoef.uwAssThreshold && ass_stCalIn.uwcadance > 0)
+                  if (ass_stCalIn.uwtorquePer > (ass_stCalCoef.uwAssThreshold >> 1) && ass_stCalIn.uwcadance > 0)
                   {            
                       ass_stCalCoef.sw2StopCNT = 0;
                       ass_stCalOut.swVoltLimitPu = scm_swVsDcpLimPu;
-                      ass_pvt_stCurLpf.slY.sw.hi = 0;               
+                      ass_pvt_stCurLpf.slY.sw.hi = 0; 
+                      ass_stCalCoef.swSmoothStopGain = Q12_1;
+                      ass_stCalCoef.swSmoothGain = ass_stParaSet.uwStartupCruiseCoef;
                       Ass_FSM = Startup;
                   }
               }
@@ -1001,6 +1042,7 @@ static void AssitCuvApplPerVolt(void) /* parasoft-suppress METRICS-28 "本项目
     mth_voLPFilter(ass_stCalOut.swTorAssistCurrent, &ass_pvt_stCurLpf);   
     /* Bikespeed Limit Coef*/
     ass_stCalOut.swAssitCurRef = (SWORD)((SLONG)ass_pvt_stCurLpf.slY.sw.hi * ass_stCalCoef.swBikeSpeedGain >> 12);
+    ass_stCalOut.swAssitCurRef  = (SLONG)ass_stCalOut.swAssitCurRef  * ass_stCalCoef.swSmoothStopGain >> 12; 
     //ass_stCalOut.swAssitCurRef =ass_stCalOut.swTorAssistCurrent;
 }
 
@@ -1116,14 +1158,14 @@ void ass_voMoveAverageFilter(MAF_IN *in)
     in->slSum -= in->swBuffer[in->uwIndex];
     in->swBuffer[in->uwIndex] = in->swValue;
     in->slSum += (SLONG)in->swValue;
-    if (!in->blSecFlag)
-    {
-        in->slAverValue = in->slSum / ((SLONG)in->uwIndex + (SWORD)1);
-    }
-    else
-    {
+//    if (!in->blSecFlag)
+//    {
+//        in->slAverValue = in->slSum / ((SLONG)in->uwIndex + (SWORD)1);
+//    }
+//    else
+//    {
         in->slAverValue = in->slSum / (SLONG)in->uwLength;
-    }
+//    }
     in->uwIndex++;
 
     if (in->uwIndex >= in->uwLength)
@@ -1139,8 +1181,8 @@ void ass_voMoveAverageFilterClear(MAF_IN *in)
     in->uwIndex = 0;
     in->slSum = 0;
     in->blSecFlag = FALSE;
-
-    for (i = 0; i < 64; i++)
+    in->slAverValue = 0;
+    for (i = 0; i < in->uwLength; i++)
     {
         in->swBuffer[i] = 0;
     }

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

@@ -111,12 +111,16 @@ static void cadence_voCadenceIdle(UWORD source)
 ****************************************************************/
 static void cadence_voCadenceHighFrequencyWork(UWORD source)
 {
-    /* 只有踏频一个脉冲会一直在此函数中，对运行无影响，即source==1 && cadence_stFreGetOut.uwCaputureNumCnt==0 的情况未处理 */
+    /* 只有踏频一个脉冲会即source==1 && cadence_stFreGetOut.uwCaputureNumCnt==0，需要处理 */
 
     if (source == 1 && cadence_stFreGetOut.uwCaputureNumCnt == 1) 
     {
         cadence_stFreGetOut.uwCaputureOverflowCnt++;
     }
+    else if (source == 1 && cadence_stFreGetOut.uwCaputureNumCnt == 0)
+    {
+        cadence_stFreGetOut.uwCaputureOverflowCnt2++;
+    }
     else if (source == 2)
     {
         if (cadence_stFreGetOut.uwCaputureNumCnt == 0)
@@ -173,8 +177,9 @@ static void cadence_voCadenceHighFrequencyWork(UWORD source)
         cadence_stFreGetOut.cadence_fsm = CADENCE_ERROR;
         cadence_stFreGetOut.uwFreqPercent = 0;
         cad_pvt_ulCadFreqPu = 0;
+        cadence_stFreGetOut.uwCaputureOverflowCnt2 = 0;
     }
-    else if (cadence_stFreGetOut.uwCaputureOverflowCnt > cadence_stFreGetCof.uwHfMaxTimeCnt)
+    else if (cadence_stFreGetOut.uwCaputureOverflowCnt > cadence_stFreGetCof.uwHfMaxTimeCnt || cadence_stFreGetOut.uwCaputureOverflowCnt2 > cadence_stFreGetCof.uwHfMaxTimeCnt)
     {
         cadence_stFreGetOut.uwFrequencyPu = 0;
         cadence_stFreGetOut.uwLPFFrequencyPu = 0;
@@ -188,6 +193,7 @@ static void cadence_voCadenceHighFrequencyWork(UWORD source)
         cadence_stFreGetOut.cadence_fsm = CADENCE_IDLE;
         cadence_stFreGetOut.uwFreqPercent = 0;
         cad_pvt_ulCadFreqPu = 0;
+        cadence_stFreGetOut.uwCaputureOverflowCnt2 = 0;
     }
     else 
     {
@@ -212,15 +218,16 @@ static void cadence_voCadenceBackword(UWORD source)
     cadence_stFreGetOut.uwFrequencyPu = 0;
     cadence_stFreGetOut.uwLPFFrequencyPu = 0;
     cadence_stFreGetOut.uwFreqPercent = 0;
+    cad_pvt_ulCadFreqPu = 0;
     if (cadence_stFreGetOut.cadence_dir == CADENCE_DIR_FORWARD)
     {
         cadence_stFreGetOut.uwCaputureOverflowCnt = 0;
+        cadence_stFreGetOut.uwCaputureOverflowCnt2 = 0;
         cadence_stFreGetOut.uwCaputureNumCnt = 0;
         cadence_stFreGetOut.uwCaputure2Cnt = 0;
         cadence_stFreGetOut.uwCaputure1Cnt = 0;
         cadence_stFreGetOut.uwFreqPercent = 0;
         cadence_stFreGetOut.cadence_fsm = CADENCE_HFreWork;
-
     }
 }
 /***************************************************************
@@ -283,6 +290,7 @@ void cadence_voCadenceInit(void)
     cadence_stFreGetOut.uwCaputureNumCnt = 0;
     cadence_stFreGetOut.uwCaputureErrorCnt = 0;
     cadence_stFreGetOut.uwCaputureOverflowCnt = 0;
+    cadence_stFreGetOut.uwCaputureOverflowCnt2 = 0;
     cadence_stFreGetOut.blCadenceSensorErrorFlg = FALSE;
     cadence_stFreGetOut.blCadenceCalStartState = FALSE;
     cadence_stFreGetOut.cadence_fsm = CADENCE_IDLE;

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

@@ -0,0 +1,388 @@
+/*
+ * File: LoadObsTheta.c
+ *
+ * Code generated for Simulink model 'LoadObsTheta'.
+ *
+ * Model version                  : 8.72
+ * Simulink Coder version         : 9.6 (R2021b) 14-May-2021
+ * C/C++ source code generated on : Sat Nov  5 15:46:39 2022
+ *
+ * Target selection: ert.tlc
+ * Embedded hardware selection: ARM Compatible->ARM Cortex-M
+ * Code generation objectives: Unspecified
+ * Validation result: Not run
+ */
+
+#include "LoadObsTheta.h"
+#ifndef UCHAR_MAX
+#include <limits.h>
+#endif
+
+#if ( UCHAR_MAX != (0xFFU) ) || ( SCHAR_MAX != (0x7F) )
+#error Code was generated for compiler with different sized uchar/char. \
+Consider adjusting Test hardware word size settings on the \
+Hardware Implementation pane to match your compiler word sizes as \
+defined in limits.h of the compiler. Alternatively, you can \
+select the Test hardware is the same as production hardware option and \
+select the Enable portable word sizes option on the Code Generation > \
+Verification pane for ERT based targets, which will disable the \
+preprocessor word size checks.
+#endif
+
+#if ( USHRT_MAX != (0xFFFFU) ) || ( SHRT_MAX != (0x7FFF) )
+#error Code was generated for compiler with different sized ushort/short. \
+Consider adjusting Test hardware word size settings on the \
+Hardware Implementation pane to match your compiler word sizes as \
+defined in limits.h of the compiler. Alternatively, you can \
+select the Test hardware is the same as production hardware option and \
+select the Enable portable word sizes option on the Code Generation > \
+Verification pane for ERT based targets, which will disable the \
+preprocessor word size checks.
+#endif
+
+#if ( UINT_MAX != (0xFFFFFFFFU) ) || ( INT_MAX != (0x7FFFFFFF) )
+#error Code was generated for compiler with different sized uint/int. \
+Consider adjusting Test hardware word size settings on the \
+Hardware Implementation pane to match your compiler word sizes as \
+defined in limits.h of the compiler. Alternatively, you can \
+select the Test hardware is the same as production hardware option and \
+select the Enable portable word sizes option on the Code Generation > \
+Verification pane for ERT based targets, which will disable the \
+preprocessor word size checks.
+#endif
+
+#if ( ULONG_MAX != (0xFFFFFFFFU) ) || ( LONG_MAX != (0x7FFFFFFF) )
+#error Code was generated for compiler with different sized ulong/long. \
+Consider adjusting Test hardware word size settings on the \
+Hardware Implementation pane to match your compiler word sizes as \
+defined in limits.h of the compiler. Alternatively, you can \
+select the Test hardware is the same as production hardware option and \
+select the Enable portable word sizes option on the Code Generation > \
+Verification pane for ERT based targets, which will disable the \
+preprocessor word size checks.
+#endif
+
+/* Skipping ulong_long/long_long check: insufficient preprocessor integer range. */
+
+/* Exported data definition */
+
+/* Definition for custom storage class: Struct */
+LoadObsTheta_stCoef_type LoadObsTheta_stCoef;
+LoadObsTheta_stCoefIn_type LoadObsTheta_stCoefIn;
+
+/* Block states (default storage) */
+DW_LoadObsTheta_T LoadObsTheta_DW;
+
+/* External inputs (root inport signals with default storage) */
+ExtU_LoadObsTheta_T LoadObsTheta_U;
+
+/* External outputs (root outports fed by signals with default storage) */
+ExtY_LoadObsTheta_T LoadObsTheta_Y;
+void LoadObsTheta_voCoef(void)
+{
+    uint16_T rtb_Divide2;
+
+    /* Outputs for Atomic SubSystem: '<Root>/Initialize Function2' */
+    /* Product: '<S2>/Divide' incorporates:
+     *  DataStoreWrite: '<S2>/Data Store Write'
+     *  Inport: '<Root>/uwFbHz'
+     *  Inport: '<Root>/uwWtcHz'
+     */
+    LoadObsTheta_stCoef.uwK1Pu = (uint16_T)(LoadObsTheta_stCoefIn.uwFbHz == 0U ?
+        MAX_uint32_T : ((uint32_T)LoadObsTheta_stCoefIn.uwWtcHz << 15) /
+        LoadObsTheta_stCoefIn.uwFbHz);
+
+    /* Product: '<S2>/Divide2' incorporates:
+     *  Gain: '<S2>/Gain'
+     *  Inport: '<Root>/uwJb'
+     *  Inport: '<Root>/uwMtJm'
+     */
+    rtb_Divide2 = (uint16_T)(LoadObsTheta_stCoefIn.uwJb == 0U ? MAX_uint32_T :
+        1000U * LoadObsTheta_stCoefIn.uwMtJm / LoadObsTheta_stCoefIn.uwJb);
+
+    /* Product: '<S2>/Product6' incorporates:
+     *  DataStoreWrite: '<S2>/Data Store Write'
+     *  DataStoreWrite: '<S2>/Data Store Write1'
+     *  Product: '<S2>/Divide'
+     *  Product: '<S2>/Product'
+     */
+    LoadObsTheta_stCoef.uwK2Pu = (uint16_T)(((uint64_T)(((uint32_T)
+        LoadObsTheta_stCoef.uwK1Pu * LoadObsTheta_stCoef.uwK1Pu) >> 1) *
+        rtb_Divide2) >> 19);
+
+    /* Product: '<S2>/Divide6' incorporates:
+     *  DataStoreRead: '<S2>/Data Store Read'
+     *  DataStoreWrite: '<S2>/Data Store Write'
+     *  DataStoreWrite: '<S2>/Data Store Write2'
+     *  Inport: '<Root>/uwMCoef'
+     *  Product: '<S2>/Divide'
+     *  Product: '<S2>/Product1'
+     *  Product: '<S2>/Product2'
+     *  Product: '<S2>/Product4'
+     *  Product: '<S2>/Product5'
+     */
+    LoadObsTheta_stCoef.uwK3Pu = (uint16_T)(LoadObsTheta_stCoefIn.uwMCoef == 0U ?
+        MAX_uint32_T : (uint32_T)(uint16_T)(((uint64_T)(uint32_T)(((uint64_T)
+        (uint32_T)(((uint64_T)(((uint32_T)LoadObsTheta_stCoef.uwK1Pu *
+        LoadObsTheta_stCoef.uwK1Pu) >> 1) * LoadObsTheta_stCoef.uwK1Pu) >> 14) *
+        rtb_Divide2) >> 6) * LoadObsTheta_stCoef.uwTctrPu) >> 21) /
+        LoadObsTheta_stCoefIn.uwMCoef);
+
+    /* Product: '<S2>/Divide3' incorporates:
+     *  DataStoreWrite: '<S2>/Data Store Write3'
+     *  Inport: '<Root>/uwFTbcHz'
+     *  Inport: '<Root>/uwFbHz'
+     */
+    LoadObsTheta_stCoef.uwCurTs = (uint16_T)(LoadObsTheta_stCoefIn.uwFTbcHz ==
+        0U ? MAX_uint32_T : ((uint32_T)LoadObsTheta_stCoefIn.uwFbHz << 10) /
+        LoadObsTheta_stCoefIn.uwFTbcHz);
+
+    /* Product: '<S2>/Divide4' incorporates:
+     *  DataStoreWrite: '<S2>/Data Store Write5'
+     */
+    LoadObsTheta_stCoef.uwJmPuInv = (uint16_T)(rtb_Divide2 == 0U ? MAX_uint32_T :
+        16384U / rtb_Divide2);
+
+    /* Product: '<S2>/Product3' incorporates:
+     *  DataStoreWrite: '<S2>/Data Store Write3'
+     *  DataStoreWrite: '<S2>/Data Store Write7'
+     *  Product: '<S2>/Divide3'
+     */
+    LoadObsTheta_stCoef.uwTctrPu = (uint16_T)((LoadObsTheta_stCoef.uwCurTs *
+        3217U) >> 7);
+
+    /* Product: '<S2>/Divide1' incorporates:
+     *  DataStoreWrite: '<S2>/Data Store Write4'
+     *  Inport: '<Root>/uwFluxWb'
+     *  Inport: '<Root>/uwFluxbWb'
+     */
+    LoadObsTheta_stCoef.uwFluxPu = (uint16_T)(LoadObsTheta_stCoefIn.uwFluxbWb ==
+        0U ? MAX_uint32_T : ((uint32_T)LoadObsTheta_stCoefIn.uwFluxWb << 12) /
+        LoadObsTheta_stCoefIn.uwFluxbWb);
+
+    /* Product: '<S2>/Divide5' incorporates:
+     *  DataStoreWrite: '<S2>/Data Store Write4'
+     *  DataStoreWrite: '<S2>/Data Store Write6'
+     *  Product: '<S2>/Divide1'
+     */
+    LoadObsTheta_stCoef.uwFluxPuInv = (uint16_T)(LoadObsTheta_stCoef.uwFluxPu ==
+        0U ? MAX_uint32_T : 16777216U / LoadObsTheta_stCoef.uwFluxPu);
+
+    /* End of Outputs for SubSystem: '<Root>/Initialize Function2' */
+}
+
+void LoadObsTheta_voInit(void)
+{
+    /* Outputs for Atomic SubSystem: '<Root>/Initialize Function1' */
+    /* Outport: '<Root>/swSpdFbkPu' incorporates:
+     *  Constant: '<S1>/Constant'
+     */
+    LoadObsTheta_Y.swSpdFbkPu = 0;
+
+    /* Outport: '<Root>/uwThetaObsPu' incorporates:
+     *  Constant: '<S1>/Constant1'
+     */
+    LoadObsTheta_Y.uwThetaObsPu = 0U;
+
+    /* Outport: '<Root>/swTLPu' incorporates:
+     *  Constant: '<S1>/Constant2'
+     */
+    LoadObsTheta_Y.swTLPu = 0;
+
+    /* Outport: '<Root>/swIqCompPu' incorporates:
+     *  Constant: '<S1>/Constant3'
+     */
+    LoadObsTheta_Y.swIqCompPu = 0;
+
+    /* End of Outputs for SubSystem: '<Root>/Initialize Function1' */
+}
+
+/* Model step function */
+void LoadObsTheta_step(void)
+{
+    int32_T rtb_Sum11;
+    int16_T rtb_Sum5;
+
+    /* Sum: '<S3>/Sum5' incorporates:
+     *  Inport: '<Root>/ThetamPu'
+     *  UnitDelay: '<S3>/Unit Delay4'
+     */
+    rtb_Sum5 = (int16_T)(((LoadObsTheta_U.ThetamPu << 13) -
+                          (LoadObsTheta_DW.UnitDelay4_DSTATE >> 1)) >> 13);
+
+    /* Switch: '<S3>/Switch2' incorporates:
+     *  Constant: '<S3>/Constant2'
+     *  RelationalOperator: '<S6>/Compare'
+     *  RelationalOperator: '<S7>/Compare'
+     *  Sum: '<S3>/Sum1'
+     *  Sum: '<S3>/Sum5'
+     *  Switch: '<S3>/Switch3'
+     */
+    if (rtb_Sum5 >= 16384)
+    {
+        rtb_Sum5 = (int16_T)(rtb_Sum5 - 32767);
+    }
+    else if (rtb_Sum5 < -16384)
+    {
+        /* Switch: '<S3>/Switch2' incorporates:
+         *  Constant: '<S3>/Constant4'
+         *  Sum: '<S3>/Sum4'
+         *  Switch: '<S3>/Switch3'
+         */
+        rtb_Sum5 = (int16_T)(rtb_Sum5 + 32767);
+    }
+
+    /* End of Switch: '<S3>/Switch2' */
+
+    /* Sum: '<S3>/Sum6' incorporates:
+     *  DataStoreRead: '<S3>/Data Store Read3'
+     *  Product: '<S3>/Product2'
+     *  Switch: '<S3>/Switch2'
+     *  UnitDelay: '<S3>/Unit Delay1'
+     */
+    LoadObsTheta_DW.UnitDelay1_DSTATE += LoadObsTheta_stCoef.uwK3Pu * rtb_Sum5;
+
+    /* Sum: '<S3>/Sum11' incorporates:
+     *  DataStoreRead: '<S3>/Data Store Read'
+     *  DataStoreRead: '<S3>/Data Store Read1'
+     *  Inport: '<Root>/IqFbkPu'
+     *  Product: '<S3>/Product1'
+     *  Product: '<S3>/Product7'
+     *  Sum: '<S3>/Sum6'
+     *  Sum: '<S3>/Sum9'
+     *  Switch: '<S3>/Switch2'
+     *  UnitDelay: '<S3>/Unit Delay1'
+     */
+    rtb_Sum11 = (((int16_T)((LoadObsTheta_U.IqFbkPu *
+                    LoadObsTheta_stCoef.uwFluxPu) >> 14) << 13) +
+                 LoadObsTheta_stCoef.uwK2Pu * rtb_Sum5) +
+        (LoadObsTheta_DW.UnitDelay1_DSTATE >> 5);
+
+    /* Sum: '<S3>/Sum10' incorporates:
+     *  DataStoreRead: '<S3>/Data Store Read4'
+     *  DataStoreRead: '<S3>/Data Store Read6'
+     *  Product: '<S3>/Product4'
+     *  Product: '<S3>/Product5'
+     *  Sum: '<S3>/Sum11'
+     *  UnitDelay: '<S3>/Unit Delay2'
+     */
+    LoadObsTheta_DW.UnitDelay2_DSTATE += (int32_T)(((int64_T)
+        LoadObsTheta_stCoef.uwJmPuInv * rtb_Sum11 * LoadObsTheta_stCoef.uwTctrPu)
+        >> 22);
+
+    /* Saturate: '<S3>/Saturation' incorporates:
+     *  Sum: '<S3>/Sum10'
+     *  UnitDelay: '<S3>/Unit Delay2'
+     */
+    if (LoadObsTheta_DW.UnitDelay2_DSTATE > 536870912)
+    {
+        /* Outport: '<Root>/swSpdFbkPu' incorporates:
+         *  DataTypeConversion: '<S3>/Data Type Conversion2'
+         */
+        LoadObsTheta_Y.swSpdFbkPu = MIN_int16_T;
+    }
+    else if (LoadObsTheta_DW.UnitDelay2_DSTATE < -536870912)
+    {
+        /* Outport: '<Root>/swSpdFbkPu' incorporates:
+         *  DataTypeConversion: '<S3>/Data Type Conversion2'
+         */
+        LoadObsTheta_Y.swSpdFbkPu = MIN_int16_T;
+    }
+    else
+    {
+        /* Outport: '<Root>/swSpdFbkPu' incorporates:
+         *  DataTypeConversion: '<S3>/Data Type Conversion2'
+         */
+        LoadObsTheta_Y.swSpdFbkPu = (int16_T)(LoadObsTheta_DW.UnitDelay2_DSTATE >>
+            14);
+    }
+
+    /* End of Saturate: '<S3>/Saturation' */
+
+    /* Sum: '<S3>/Sum8' incorporates:
+     *  DataStoreRead: '<S3>/Data Store Read5'
+     *  DataStoreRead: '<S3>/Data Store Read8'
+     *  Product: '<S3>/Product3'
+     *  Product: '<S3>/Product6'
+     *  Sum: '<S3>/Sum10'
+     *  Sum: '<S3>/Sum7'
+     *  Switch: '<S3>/Switch2'
+     *  UnitDelay: '<S3>/Unit Delay2'
+     *  UnitDelay: '<S3>/Unit Delay3'
+     */
+    LoadObsTheta_DW.UnitDelay3_DSTATE += (int32_T)(((int64_T)
+        (((LoadObsTheta_stCoef.uwK1Pu * rtb_Sum5) >> 1) +
+         LoadObsTheta_DW.UnitDelay2_DSTATE) * LoadObsTheta_stCoef.uwCurTs) >> 10);
+
+    /* Switch: '<S3>/Switch' incorporates:
+     *  Constant: '<S3>/Constant1'
+     *  RelationalOperator: '<S4>/Compare'
+     *  RelationalOperator: '<S5>/Compare'
+     *  Sum: '<S3>/Sum2'
+     *  Sum: '<S3>/Sum8'
+     *  Switch: '<S3>/Switch1'
+     */
+    if (LoadObsTheta_DW.UnitDelay3_DSTATE >= 536870912)
+    {
+        LoadObsTheta_DW.UnitDelay3_DSTATE -= 536870912;
+    }
+    else if (LoadObsTheta_DW.UnitDelay3_DSTATE < 0)
+    {
+        /* Switch: '<S3>/Switch' incorporates:
+         *  Constant: '<S3>/Constant3'
+         *  Sum: '<S3>/Sum3'
+         *  Switch: '<S3>/Switch1'
+         *  UnitDelay: '<S3>/Unit Delay3'
+         */
+        LoadObsTheta_DW.UnitDelay3_DSTATE += 536870912;
+    }
+
+    /* End of Switch: '<S3>/Switch' */
+
+    /* Outport: '<Root>/uwThetaObsPu' incorporates:
+     *  DataTypeConversion: '<S3>/Data Type Conversion1'
+     *  Switch: '<S3>/Switch'
+     *  UnitDelay: '<S3>/Unit Delay3'
+     */
+    LoadObsTheta_Y.uwThetaObsPu = (uint16_T)(LoadObsTheta_DW.UnitDelay3_DSTATE >>
+        14);
+
+    /* Outport: '<Root>/swTLPu' incorporates:
+     *  DataTypeConversion: '<S3>/Data Type Conversion3'
+     *  Sum: '<S3>/Sum6'
+     *  UnitDelay: '<S3>/Unit Delay1'
+     */
+    LoadObsTheta_Y.swTLPu = (int16_T)(LoadObsTheta_DW.UnitDelay1_DSTATE >> 18);
+
+    /* Outport: '<Root>/swIqCompPu' incorporates:
+     *  DataStoreRead: '<S3>/Data Store Read2'
+     *  Product: '<S3>/Product8'
+     *  Sum: '<S3>/Sum11'
+     */
+    LoadObsTheta_Y.swIqCompPu = (int16_T)(((int64_T)
+        LoadObsTheta_stCoef.uwFluxPuInv * rtb_Sum11) >> 23);
+
+    /* Update for UnitDelay: '<S3>/Unit Delay4' incorporates:
+     *  Switch: '<S3>/Switch'
+     *  UnitDelay: '<S3>/Unit Delay3'
+     */
+    LoadObsTheta_DW.UnitDelay4_DSTATE = LoadObsTheta_DW.UnitDelay3_DSTATE;
+}
+
+/* Model initialize function */
+void LoadObsTheta_initialize(void)
+{
+    /* (no initialization code required) */
+}
+
+/* Model terminate function */
+void LoadObsTheta_terminate(void)
+{
+    /* (no terminate code required) */
+}
+
+/*
+ * File trailer for generated code.
+ *
+ * [EOF]
+ */

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

@@ -41,86 +41,84 @@ void bikelight_voGetBikeLightError(UWORD LightPowerVolPu,BOOL Ledsta,BIKELEDCHEC
      //前灯电路检测，开灯故障
     if(Ledsta==TRUE)
     {
-    if(  (LightPowerVolPu <p->LedOnVoltMin  ) || (LightPowerVolPu > p->LedOnVoltMax))
-    {
-        p->blBike_LedOnErrCnt++;
-        if(p->blBike_LedOnErrCnt>LEDRISETIME)
+        if((LightPowerVolPu < p->LedOnVoltMin) || (LightPowerVolPu > p->LedOnVoltMax))
         {
-           if(p->blBike_LedOnErrAddNO< LEDERRTIME)
-           {
-            p->blBike_LedOnErrAddNO++;
-           }
-           else
-           {
-            p->blBike_LedOnErr=   TRUE;
-           }
+            p->blBike_LedOnErrCnt++;
+            if(p->blBike_LedOnErrCnt > LEDRISETIME)
+            {
+               if(p->blBike_LedOnErrAddNO < LEDERRTIME)
+               {
+                   p->blBike_LedOnErrAddNO++;
+               }
+               else
+               {
+                   p->blBike_LedOnErr = TRUE;
+               }
 
-            p->blBike_LedOnErrCnt=0;
+               p->blBike_LedOnErrCnt = 0;
+            }
         }
-
-    }
-    else
-    {
-        p->blBike_LedOnErrDCnt++;
-     if(p->blBike_LedOnErrDCnt >LEDRISETIME)
-     {
-         if(p->blBike_LedOnErrAddNO> 0)
-           {
-            p->blBike_LedOnErrAddNO--;
-           }
-           else
-           {
-            p->blBike_LedOnErr=   FALSE;
-           }
-          p->blBike_LedOnErrDCnt=0;
-     }
-    }
-
-    p->blBike_LedOffErrCnt=0;
-    p->blBike_LedOffErrDCnt=0;
-    }
-    //----------关灯故障-----------
-    else
-    {
-    if(  LightPowerVolPu > p->LedOffVoltMax)
-    {
-        p->blBike_LedOffErrCnt++;
-        if(p->blBike_LedOffErrCnt>LEDFALLTIME)
+        else
         {
-           if(p->blBike_LedOffErrAddNO< LEDERRTIME)
-           {
-            p->blBike_LedOffErrAddNO++;
-           }
-           else
-           {
-            p->blBike_LedOffErr=   TRUE;
-           }
-
-            p->blBike_LedOffErrCnt=0;
+            p->blBike_LedOnErrDCnt++;
+            if(p->blBike_LedOnErrDCnt > LEDRISETIME)
+            {
+                if(p->blBike_LedOnErrAddNO > 0)
+                {
+                    p->blBike_LedOnErrAddNO--;
+                }
+                else
+                {
+                    p->blBike_LedOnErr = FALSE;
+                }
+                
+                p->blBike_LedOnErrDCnt = 0;
+            }
         }
 
+        p->blBike_LedOffErrCnt = 0;
+        p->blBike_LedOffErrDCnt = 0;
     }
-    else
+    else//----------关灯故障-----------
     {
-        p->blBike_LedOffErrDCnt++;
-     if(p->blBike_LedOffErrDCnt >LEDFALLTIME)
-     {
-         if(p->blBike_LedOffErrAddNO> 0)
-           {
-            p->blBike_LedOffErrAddNO--;
-           }
-           else
-           {
-            p->blBike_LedOffErr=   FALSE;
-           }
-          p->blBike_LedOffErrDCnt=0;
-     }
-    }
+        if(LightPowerVolPu > p->LedOffVoltMax)
+        {
+            p->blBike_LedOffErrCnt++;
+            if(p->blBike_LedOffErrCnt > LEDFALLTIME)
+            {
+               if(p->blBike_LedOffErrAddNO < LEDERRTIME)
+               {
+                  p->blBike_LedOffErrAddNO++;
+               }
+               else
+               {
+                  p->blBike_LedOffErr = TRUE;
+               }
 
+               p->blBike_LedOffErrCnt = 0;
+            }
+        }
+        else
+        {
+            p->blBike_LedOffErrDCnt++;
+            if(p->blBike_LedOffErrDCnt > LEDFALLTIME)
+            {
+                if(p->blBike_LedOffErrAddNO > 0)
+                {
+                    p->blBike_LedOffErrAddNO--;
+                }
+                else
+                {
+                    p->blBike_LedOffErr = FALSE;
+                }
+                
+                p->blBike_LedOffErrDCnt = 0;
+            }
+        }
     }
 
-    p->blBike_LedOnErrCnt=0;
-    p->blBike_LedOnErrDCnt=0;
+    p->blBike_LedOnErrCnt = 0;
+    p->blBike_LedOnErrDCnt = 0;
 }
 /***************************************************************
  Function: bikelight_voBikeLightInit;
@@ -410,8 +408,8 @@ void bikelight_voBikeLightControl(UWORD switchAction, BOOL Brate_Sta,UWORD uwLig
     }
 
     //--------------LED电路检测----------
-    bikelight_voGetBikeLightError(adc_stUpOut.uwRU6VPu,BikeLedGloFun.blBike_ForwardLedSta,&BikeLedCheckF); //前灯检测
-       bikelight_voGetBikeLightError(adc_stUpOut.uwFU6VPu,BikeLedGloFun.blBike_BackLedSta,&BikeLedCheckB); //尾灯检测
+    bikelight_voGetBikeLightError(adc_stUpOut.uwFU6VPu,BikeLedGloFun.blBike_ForwardLedSta,&BikeLedCheckF); //前灯检测
+    bikelight_voGetBikeLightError(adc_stUpOut.uwRU6VPu,BikeLedGloFun.blBike_BackLedSta,&BikeLedCheckB); //尾灯检测
 
     if((BikeLedCheckF.blBike_LedOffErr == TRUE) ||(BikeLedCheckF.blBike_LedOnErr==TRUE) )
     {

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

@@ -600,6 +600,7 @@ void DataProcess(UWORD ID, UBYTE Mode, UWORD Cmd, UBYTE Data[]) /* parasoft-supp
         case 0x1901: //写入电机工作模式
         {
             MC_WorkMode = *Data; // 0-关闭定时发送，1-开启定时发送
+            MC_BC_COM = *Data;
             break;
         }
 

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

@@ -224,84 +224,84 @@ void  spi_voResolver(const SPI_RESOLVER_COEF *coef, SPI_RESOLVER_OUT *out)
         out->blSpiThetaFltFlg = TRUE;
     }
 
-     out->swSpdFbkPu = swSpdTmpPu;
+    out->swSpdFbkPu = swSpdTmpPu;
 
-     /*Calculate speed: PLL method*/
-      SLONG slThetaErrPu,slThetaDeltaErrPu,slPllThetaPu;
-      SLONG slKpTmpPu,slKitTmpPu,slPllSpdFbkPu;
-      ULONG ulSpdPllKpPu,ulSpdPllKiPu;
-  
-      slThetaErrPu = (SLONG)out->uwSpiThetaPu - (out->uwPllThetaPu + (((SLONG)out->swPllSpdFbkPu * coef->uwCurTs) >> 10));  //Q15
-      
-      if (slThetaErrPu >= cof_sl180DegreePu)
-      {
-          slThetaErrPu-= cof_sl360DegreePu;
-      }
-      if(slThetaErrPu <= -cof_sl180DegreePu)
-      {
-          slThetaErrPu += cof_sl360DegreePu;
-      }
-      slThetaDeltaErrPu = slThetaErrPu - out->slThetaErrZ1Pu;
-      out->slThetaErrZ1Pu = slThetaErrPu;
-    
-     /* Variable parameter PI，untested*/
-//      if(out->swPllSpdFbkPu < cof_uw1000RpmPu - cof_uw200RpmPu)
+      /*Calculate speed: PLL method*/
+//      SLONG slThetaErrPu,slThetaDeltaErrPu,slPllThetaPu;
+//      SLONG slKpTmpPu,slKitTmpPu,slPllSpdFbkPu;
+//      ULONG ulSpdPllKpPu,ulSpdPllKiPu;
+//  
+//      slThetaErrPu = (SLONG)out->uwSpiThetaPu - (out->uwPllThetaPu + (((SLONG)out->swPllSpdFbkPu * coef->uwCurTs) >> 10));  //Q15
+//      
+//      if (slThetaErrPu >= cof_sl180DegreePu)
 //      {
-//          ulSpdPllKpPu = coef->uwSpdPllKpPu; 
-//          ulSpdPllKiPu = coef->uwSpdPllKiPu; 
+//          slThetaErrPu-= cof_sl360DegreePu;
 //      }
-//      else if((out->swPllSpdFbkPu > cof_uw1000RpmPu) && (out->swPllSpdFbkPu < cof_uw2000RpmPu - cof_uw200RpmPu))
+//      if(slThetaErrPu <= -cof_sl180DegreePu)
 //      {
-//          ulSpdPllKpPu = coef->uwSpdPllKpPu * 2; 
-//          ulSpdPllKiPu = coef->uwSpdPllKiPu * 2 * 2;      
+//          slThetaErrPu += cof_sl360DegreePu;
 //      }
-//      else if((out->swPllSpdFbkPu > cof_uw2000RpmPu) && (out->swPllSpdFbkPu < cof_uw3000RpmPu - cof_uw200RpmPu))
+//      slThetaDeltaErrPu = slThetaErrPu - out->slThetaErrZ1Pu;
+//      out->slThetaErrZ1Pu = slThetaErrPu;
+//    
+//     /* Variable parameter PI，untested*/
+//////      if(out->swPllSpdFbkPu < cof_uw1000RpmPu - cof_uw200RpmPu)
+//////      {
+//////          ulSpdPllKpPu = coef->uwSpdPllKpPu; 
+//////          ulSpdPllKiPu = coef->uwSpdPllKiPu; 
+//////      }
+//////      else if((out->swPllSpdFbkPu > cof_uw1000RpmPu) && (out->swPllSpdFbkPu < cof_uw2000RpmPu - cof_uw200RpmPu))
+//////      {
+//////          ulSpdPllKpPu = coef->uwSpdPllKpPu * 2; 
+//////          ulSpdPllKiPu = coef->uwSpdPllKiPu * 2 * 2;      
+//////      }
+//////      else if((out->swPllSpdFbkPu > cof_uw2000RpmPu) && (out->swPllSpdFbkPu < cof_uw3000RpmPu - cof_uw200RpmPu))
+//////      {
+//////          ulSpdPllKpPu = coef->uwSpdPllKpPu * 4; 
+//////          ulSpdPllKiPu = coef->uwSpdPllKiPu * 4 * 4;        
+//////      }
+//////      else if((out->swPllSpdFbkPu > cof_uw3000RpmPu) && (out->swPllSpdFbkPu < cof_uw4000RpmPu - cof_uw200RpmPu))
+//////      {
+//////          ulSpdPllKpPu = coef->uwSpdPllKpPu * 8; 
+//////          ulSpdPllKiPu = coef->uwSpdPllKiPu * 8 * 8;        
+//////      }
+//////      else if (out->swPllSpdFbkPu > cof_uw4000RpmPu)
+//////      {
+//////          ulSpdPllKpPu = coef->uwSpdPllKpPu * 16; 
+//////          ulSpdPllKiPu = coef->uwSpdPllKiPu * 16 * 16; 
+//////      }
+//    
+//      ulSpdPllKpPu = coef->uwSpdPllKpPu; 
+//      ulSpdPllKiPu = coef->uwSpdPllKiPu; 
+//      
+//      slKpTmpPu = slThetaDeltaErrPu * (SLONG)ulSpdPllKpPu;   //Q15+Q14=Q29
+//      slKitTmpPu = slThetaErrPu * (SLONG)ulSpdPllKiPu;
+//      slPllSpdFbkPu = slKpTmpPu + slKitTmpPu + out->slPllSpdFbkPu;
+//      if(slPllSpdFbkPu >= 0x20000000)
 //      {
-//          ulSpdPllKpPu = coef->uwSpdPllKpPu * 4; 
-//          ulSpdPllKiPu = coef->uwSpdPllKiPu * 4 * 4;        
+//          slPllSpdFbkPu = 0x20000000-1; 																	                    //Q29
 //      }
-//      else if((out->swPllSpdFbkPu > cof_uw3000RpmPu) && (out->swPllSpdFbkPu < cof_uw4000RpmPu - cof_uw200RpmPu))
+//      if(slPllSpdFbkPu <= -0x20000000)
 //      {
-//          ulSpdPllKpPu = coef->uwSpdPllKpPu * 8; 
-//          ulSpdPllKiPu = coef->uwSpdPllKiPu * 8 * 8;        
+//          slPllSpdFbkPu = -0x20000000; 																			            //Q29
 //      }
-//      else if (out->swPllSpdFbkPu > cof_uw4000RpmPu)
+//      out->slPllSpdFbkPu = slPllSpdFbkPu;                                                              //Q29
+//      out->swPllSpdFbkPu = (SWORD)(slPllSpdFbkPu >> 14);                                                   //Q15
+//  
+//      slPllThetaPu = out->slPllThetaPu + (((SLONG)out->swPllSpdFbkPu * coef->uwCurTs) << 4);    //Q15+Q10+Q4=Q29
+//      if(slPllThetaPu >= 0x20000000)
 //      {
-//          ulSpdPllKpPu = coef->uwSpdPllKpPu * 16; 
-//          ulSpdPllKiPu = coef->uwSpdPllKiPu * 16 * 16; 
+//          slPllThetaPu -= 0x20000000;
 //      }
-    
-      ulSpdPllKpPu = coef->uwSpdPllKpPu; 
-      ulSpdPllKiPu = coef->uwSpdPllKiPu; 
-      
-      slKpTmpPu = slThetaDeltaErrPu * (SLONG)ulSpdPllKpPu;   //Q15+Q14=Q29
-      slKitTmpPu = slThetaErrPu * (SLONG)ulSpdPllKiPu;
-      slPllSpdFbkPu = slKpTmpPu + slKitTmpPu + out->slPllSpdFbkPu;
-      if(slPllSpdFbkPu >= 0x20000000)
-      {
-          slPllSpdFbkPu = 0x20000000-1; 																	                    //Q29
-      }
-      if(slPllSpdFbkPu <= -0x20000000)
-      {
-          slPllSpdFbkPu = -0x20000000; 																			            //Q29
-      }
-      out->slPllSpdFbkPu = slPllSpdFbkPu;                                                              //Q29
-      out->swPllSpdFbkPu = (SWORD)(slPllSpdFbkPu >> 14);                                                   //Q15
-  
-      slPllThetaPu = out->slPllThetaPu + (((SLONG)out->swPllSpdFbkPu * coef->uwCurTs) << 4);    //Q15+Q10+Q4=Q29
-      if(slPllThetaPu >= 0x20000000)
-      {
-          slPllThetaPu -= 0x20000000;
-      }
-      if(slPllThetaPu < 0)
-      {
-          slPllThetaPu += 0x20000000;
-      }
-      out->slPllThetaPu = slPllThetaPu;
-      out->uwPllThetaPu = (UWORD)((ULONG)slPllThetaPu >> 14);        //Q15 = Q29 - Q14
-      
-//      spi_pvt_slSpdFbkLpfPu = (SLONG)0x00FF * (out->swPllSpdFbkPu - out->swSpdFbkLpfPu) + spi_pvt_slSpdFbkLpfPu;         //20Hz    Q30
-//      out->swSpdFbkLpfPu = spi_pvt_slSpdFbkLpfPu >> 15;
+//      if(slPllThetaPu < 0)
+//      {
+//          slPllThetaPu += 0x20000000;
+//      }
+//      out->slPllThetaPu = slPllThetaPu;
+//      out->uwPllThetaPu = (UWORD)((ULONG)slPllThetaPu >> 14);        //Q15 = Q29 - Q14
+//      
+////      spi_pvt_slSpdFbkLpfPu = (SLONG)0x00FF * (out->swPllSpdFbkPu - out->swSpdFbkLpfPu) + spi_pvt_slSpdFbkLpfPu;         //20Hz    Q30
+////      out->swSpdFbkLpfPu = spi_pvt_slSpdFbkLpfPu >> 15;
 }
 /*************************************************************************
  Function:
@@ -335,7 +335,7 @@ void spi_voResolverLock(void)
 *************************************************************************/
 void spi_voReadWriteSeneorReg(void) /* parasoft-suppress METRICS-28 "本项目圈复杂度无法更改，后续避免" */
 {
-    UWORD BCTValue = 0x30;
+    UWORD BCTValue = 0x25;
     
     UWORD uwReadBCTReg = 0, uwReadETXY = 0;
     UWORD uwWriteBCTReg = 0, uwWriteETXY = 0, uwWriteRD = 0;

User project/4.BasicHardwSoftwLayer/2.BasicSoftwLayer/Include/notchfilter.h

@@ -0,0 +1,97 @@
+/************************************************************************
+ Project:             Welling Motor Control Paltform
+ Filename:            resctrl.h
+ Partner Filename:    resctrl.c
+ Description:         The header file of asr.c
+ Complier:            IAR Embedded Workbench for ARM 8.40, IAR Systems.
+ CPU TYPE :           STM32F30x
+*************************************************************************
+ Copyright (c) 2022 Welling Motor Technology(Shanghai) Co. Ltd.
+ All rights reserved.
+*************************************************************************
+*************************************************************************
+Revising History (ECL of this file):
+
+************************************************************************/
+
+/************************************************************************
+ Beginning of File, do not put anything above here except notes
+ Compiler Directives:
+*************************************************************************/
+#ifndef NOTCHFILTER_H
+#define NOTCHFILTER_H
+
+#include "typedefine.h"
+/************************************************************************
+ Definitions & Macros (#define ...)
+*************************************************************************/
+
+/************************************************************************
+ global varibles Macros (#define ...)
+*************************************************************************/
+
+/************************************************************************
+ TypeDefs & Structure defines
+*************************************************************************/
+typedef struct
+{
+    ULONG ulKsi;   ///< 陷波宽度系数, Q15
+    ULONG uld;     ///< 陷波深度系数, Q20
+    ULONG ulTs;    ///< 陷波宽度系数, Q25
+    ULONG ulW0;    ///< 陷波中心频率, Q0
+} NOTCHFILTER_COEFIN;
+
+typedef struct
+{
+    SLONG slA0;   ///< Q13
+    SLONG slA1;   ///< Q13   
+    SLONG slA2;   ///< Q13   
+    SLONG slB0;   ///< Q13   
+    SLONG slB1;   ///< Q13   
+} NOTCHFILTER_COEFOUT;
+
+typedef struct
+{
+    NOTCHFILTER_COEFIN stCoefIn;
+    NOTCHFILTER_COEFOUT stCoef;
+    SWORD swXZ1;
+    SWORD swXZ2;
+    SWORD swY;
+    SWORD swYZ1;
+} NOTCHFILTER_OUT;
+
+/************************************************************************
+ TypeDefs & Structure defines
+*************************************************************************/
+
+/************************************************************************
+Constant Table
+*************************************************************************/
+
+/************************************************************************
+ Exported Variables:
+*************************************************************************/
+extern NOTCHFILTER_OUT     notch_stCalOut;
+/************************************************************************
+ RAM ALLOCATION:
+*************************************************************************/
+
+/************************************************************************
+ Exported Function Call Prototypes (N/A)
+*************************************************************************/
+void notch_voInit(NOTCHFILTER_OUT *out);
+void notch_voCoef(NOTCHFILTER_COEFIN *in, NOTCHFILTER_COEFOUT *coef);
+void notch_voCal(SWORD swNfIn,NOTCHFILTER_OUT *out);
+
+/************************************************************************
+ Flag Define (N/A)
+*************************************************************************/
+#endif
+
+/************************************************************************
+ Copyright (c) 2018 Welling Motor Technology(Shanghai) Co. Ltd.
+ All rights reserved.
+*************************************************************************
+ End of this File (EOF)!
+ Do not put anything after this part!
+*************************************************************************/

User project/4.BasicHardwSoftwLayer/2.BasicSoftwLayer/Include/user.h

@@ -47,7 +47,7 @@ Update Time
 #define MOTOR_WELLING_CITY_48V    0x30
 #define MOTOR_WELLING_MTB_48V     0x31
 
-#define MOTOR_ID_SEL              MOTOR_WELLING_MTB_48V    ///< syspar.h中也需要选择36V/48V系统
+#define MOTOR_ID_SEL              MOTOR_WELLING_CITY_36V    ///< syspar.h中也需要选择36V/48V系统
 
 /*======================================================================*
     Motor Parameters

User project/4.BasicHardwSoftwLayer/2.BasicSoftwLayer/Source/notchfilter.c

@@ -0,0 +1,139 @@
+/************************************************************************
+ Project:             Wellg Motor Control Paltform
+ Filename:            resctrl.c
+ Partner Filename:    resctrl.h
+ Description:         Automatic current regulator
+ Complier:            IAR Embedded Workbench for ARM 8.40, IAR Systems.
+ CPU TYPE :           STM32F30x
+*************************************************************************
+ Copyright (c) 2022 Wellg Motor Technology(Shanghai) Co. Ltd.
+ All rights reserved.
+*************************************************************************
+*************************************************************************
+Revisg History (ECL of this file):
+
+************************************************************************/
+
+/************************************************************************
+ Begng of File, do not put anythg above here except notes
+ Compiler Directives:
+*************************************************************************/
+#ifndef _NOTCHFILTER_C_
+#define _NOTCHFILTER_C_
+#endif
+
+/************************************************************************
+ Included File:
+*************************************************************************/
+#include "notchfilter.h"
+/************************************************************************
+ Constant Table:
+*************************************************************************/
+
+/*************************************************************************
+ extern Parameter
+**************************************************************************/
+NOTCHFILTER_COEFIN  notch_stCoefIn;
+NOTCHFILTER_COEFOUT notch_stCoef;
+NOTCHFILTER_OUT     notch_stCalOut;
+/************************************************************************
+ Exported Functions:
+*************************************************************************/
+/***************************************************************
+ Function: ;
+ Description:
+ Call by: functions
+ Input Variables:
+ Output/Return Variables:
+ Subroute Call: N/A;
+ Reference: N/A
+****************************************************************/
+void notch_voInit(NOTCHFILTER_OUT *out)
+{
+    out->swY = 0;
+    out->swYZ1 = 0;
+    out->swXZ1 = 0;
+    out->swXZ2 = 0;
+}
+/***************************************************************
+ Function: ;
+ Description: 
+ Call by: functions 
+ Input Variables: 
+ Output/Return Variables: 
+ Subroute Call: N/A;
+ Reference: N/A
+****************************************************************/
+void notch_voCoef(NOTCHFILTER_COEFIN *in, NOTCHFILTER_COEFOUT *coef)
+{
+//     ->ulKsi = 6554; // 0.2, Q15
+//     ->ulTs = 8389;  // 1/4000, Q25
+//     ->uld = 10486;  // 0.01, Q20
+  
+     SLONG slTmp1,slTmp2,slTmp3,slTmp4;
+     SQWORD sqTmp;
+     SLONG slDivisor;
+     
+     slDivisor = (SLONG)(((SQWORD)in->ulW0 * in->ulW0 * in->ulTs * in->ulTs >> 25)
+                  + ((SQWORD)4 * in->ulKsi  * in->ulW0 * in->ulTs >> 15)+ (4 << 25));  //Q25
+     
+     slTmp1 = (SLONG)(((SQWORD)in->ulW0 * in->ulW0 * in->ulTs * in->ulTs >> 25)
+                  + ((SQWORD)4 * in->ulKsi * in->uld  * in->ulW0  * in->ulTs >> 35)+ (4 << 25));  //Q25
+     
+     slTmp2 =(SLONG)(((SQWORD)2 * in->ulW0 * in->ulW0 * in->ulTs * in->ulTs >> 25)
+                  - (8 << 25));  //Q25
+     slTmp3 =(SLONG)(((SQWORD)in->ulW0 * in->ulW0 * in->ulTs * in->ulTs >> 25)
+                  - ((SQWORD)4 * in->ulKsi * in->uld  * in->ulW0  * in->ulTs >> 35)+ (4 << 25));  //Q25
+     slTmp4 =(SLONG)(((SQWORD)in->ulW0 * in->ulW0 * in->ulTs * in->ulTs >> 25)
+                  - ((SQWORD)4 * in->ulKsi  * in->ulW0 * in->ulTs >> 15)+ (4 << 25));  //Q25
+     
+     coef->slA0 = (SLONG)(((SQWORD)slTmp1 << 13) / slDivisor); //Q13
+     coef->slA1 = (SLONG)(((SQWORD)slTmp2 << 13) / slDivisor); //Q13 //Q13 
+     coef->slA2 = (SLONG)(((SQWORD)slTmp3 << 13) / slDivisor); //Q13 
+     coef->slB0 = coef->slA1;  //Q13
+     coef->slB1 = (SLONG)(((SQWORD)slTmp4 << 13) / slDivisor);  //Q13
+}
+
+/***************************************************************
+ Function: ;
+ Description:
+ Call by: functions
+ Input Variables:
+ Output/Return Variables:
+ Subroute Call: N/A;
+ Reference: N/A
+****************************************************************/
+void notch_voCal(SWORD swNfIn,NOTCHFILTER_OUT *out)
+{
+     SLONG slTmp1,slTmp2,slTmp3,slTmp4,slTmp5;
+     SQWORD sqTmp;
+
+     slTmp1 = out->stCoef.slA0 * swNfIn;
+     slTmp2 = out->stCoef.slA1 * out->swXZ1;
+     slTmp3 = out->stCoef.slA2 * out->swXZ2;
+     slTmp4 = out->stCoef.slB0 * out->swY;
+     slTmp5 = out->stCoef.slB1 * out->swYZ1;
+     
+     out->swXZ2 = out->swXZ1;
+     out->swXZ1 = swNfIn;
+     out->swYZ1 = out->swY;
+     
+     sqTmp = (SQWORD)slTmp1 + slTmp2 + slTmp3 - slTmp4 - slTmp5;
+     out->swY = (SWORD)(sqTmp >> 13);
+
+}
+/*************************************************************************
+ Local Functions (N/A)
+*************************************************************************/
+
+/************************************************************************
+ Copyright (c) 2018 Wellg Motor Technology(Shanghai) Co., Ltd.
+ All rights reserved.
+*************************************************************************/
+#ifdef _RESCTRL_C_
+#undef _RESCTRL_C_
+#endif
+/*************************************************************************
+ End of this File (EOF)!
+ Do not put anythg after this part!
+*************************************************************************/

WLMCP_PACKED.ewp

@@ -2660,12 +2660,18 @@
                 <file>
                     <name>$PROJ_DIR$\User project\3.BasicFunction\Include\InputCapture.h</name>
                 </file>
+                <file>
+                    <name>$PROJ_DIR$\User project\3.BasicFunction\Include\LoadObsTheta.h</name>
+                </file>
                 <file>
                     <name>$PROJ_DIR$\User project\3.BasicFunction\Include\ntc_sensor.h</name>
                 </file>
                 <file>
                     <name>$PROJ_DIR$\User project\3.BasicFunction\Include\power.h</name>
                 </file>
+                <file>
+                    <name>$PROJ_DIR$\User project\3.BasicFunction\Include\rtwtypes2.h</name>
+                </file>
                 <file>
                     <name>$PROJ_DIR$\User project\3.BasicFunction\Include\spi_master.h</name>
                 </file>
@@ -2723,6 +2729,9 @@
                 <file>
                     <name>$PROJ_DIR$\User project\3.BasicFunction\Source\InputCapture.c</name>
                 </file>
+                <file>
+                    <name>$PROJ_DIR$\User project\3.BasicFunction\Source\LoadObsTheta.c</name>
+                </file>
                 <file>
                     <name>$PROJ_DIR$\User project\3.BasicFunction\Source\ntc_sensor.c</name>
                 </file>
@@ -2794,6 +2803,9 @@
                     <file>
                         <name>$PROJ_DIR$\User project\4.BasicHardwSoftwLayer\2.BasicSoftwLayer\Include\mathtool.h</name>
                     </file>
+                    <file>
+                        <name>$PROJ_DIR$\User project\4.BasicHardwSoftwLayer\2.BasicSoftwLayer\Include\notchfilter.h</name>
+                    </file>
                     <file>
                         <name>$PROJ_DIR$\User project\4.BasicHardwSoftwLayer\2.BasicSoftwLayer\Include\queue.h</name>
                     </file>
@@ -2824,6 +2836,9 @@
                     <file>
                         <name>$PROJ_DIR$\User project\4.BasicHardwSoftwLayer\2.BasicSoftwLayer\Source\mathtool.c</name>
                     </file>
+                    <file>
+                        <name>$PROJ_DIR$\User project\4.BasicHardwSoftwLayer\2.BasicSoftwLayer\Source\notchfilter.c</name>
+                    </file>
                     <file>
                         <name>$PROJ_DIR$\User project\4.BasicHardwSoftwLayer\2.BasicSoftwLayer\Source\queue.c</name>
                     </file>