增加warning状态

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

@@ -50,6 +50,7 @@ void FSM_voInit(void)
     switch_flg.SysRdy_Flag = FALSE;
     switch_flg.SysRun_Flag = FALSE;
     switch_flg.SysFault_Flag = FALSE;
+    switch_flg.SysWarnning_Flag = FALSE;
 
     signal_state.Sensor = FALSE;
     signal_state.Assist = FALSE;

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

@@ -471,12 +471,12 @@ void  Event_200ms(void)
     /* Upper Computer Info Update */
     Can_voMC_Run_200ms();
 #if(GIANT_ENABLE == 1)
-    if ((switch_flg.SysFault_Flag == TRUE) && (MC_WorkMode > 0))
+    if (((switch_flg.SysFault_Flag == TRUE) || (switch_flg.SysWarnning_Flag == TRUE)) && (MC_WorkMode > 0))
     {
         SendData(ID_MC_BC, MODE_REPORT, 0x1104, (uint8_t *)&MC_ErrorCode.Code);
     }
 #else
-    if (switch_flg.SysFault_Flag == TRUE)
+    if ((switch_flg.SysFault_Flag == TRUE) || (switch_flg.SysWarnning_Flag == TRUE))
     {
         SendData(ID_MC_BC, MODE_REPORT, 0x1104, (uint8_t *)&MC_ErrorCode.Code);
     }

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

@@ -91,11 +91,11 @@ void sysctrl_voThrPhsShrt(void)
     iPwm_SetCompareValue(0, 0, 0);
     iPwm_SetCompareValue(0, 1, 0);
     iPwm_SetCompareValue(0, 2, 0);
+    iPwm_SetCompareValueDelay(0, 0, 0);
+    iPwm_SetCompareValueDelay(0, 1, 0);
     iPwm_SetCompareValueDelay(0, 2, 0);
-    iPwm_SetCompareValueDelay(0, 2, 0);
-    iPwm_SetCompareValueDelay(0, 2, 0);
-    iPwm_SetCompareValueImmediate(0, 2, 0);
-    iPwm_SetCompareValueImmediate(0, 2, 0);
+    iPwm_SetCompareValueImmediate(0, 0, 0);
+    iPwm_SetCompareValueImmediate(0, 1, 0);
     iPwm_SetCompareValueImmediate(0, 2, 0);
 
     sysctrl_voPwmOn();

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

@@ -142,6 +142,7 @@ void tbc_voDownIsr(void)
     }
     
     /* System FSM */
+    switch_flg.SysWarnning_Flag = alm_blWarnOccrFlg;
     switch_flg.SysFault_Flag = alm_blAlmOccrFlg;
 
     if (adc_stDownOut.blADCCalibFlg && adc_stUpOut.blADCCalibFlg)

User project/2.MotorDrive/Include/alarm.h

@@ -470,6 +470,7 @@ _ALARM_EXT UWORD             alm_uwIPMOCResetCnts = 0;
 _ALARM_EXT UWORD             alm_uwOCResetCnts = 0;
 _ALARM_EXT BOOL              alm_blPowerDownFlg = FALSE;
 _ALARM_EXT BOOL              alm_blAlmOccrFlg = FALSE;
+_ALARM_EXT BOOL              alm_blWarnOccrFlg = FALSE;
 _ALARM_EXT BOOL              alm_blAlmSingleRecordDoneFlg = FALSE;
 _ALARM_EXT ALM_RESET1MS_COF  alm_stReset1MSCoef = ALM_RESET1MS_COF_DEFAULT;
 _ALARM_EXT ALM_DETEC200MS_COF alm_stDetect200MSCoef = ALM_DETECT200MS_COF_DEFAULT;
@@ -494,6 +495,7 @@ _ALARM_EXT UWORD             alm_uwIPMOCResetCnts;
 _ALARM_EXT UWORD             alm_uwOCResetCnts;
 _ALARM_EXT BOOL              alm_blPowerDownFlg;
 _ALARM_EXT BOOL              alm_blAlmOccrFlg;
+_ALARM_EXT BOOL              alm_blWarnOccrFlg;
 _ALARM_EXT BOOL              alm_blAlmSingleRecordDoneFlg;
 _ALARM_EXT ALM_RESET1MS_COF  alm_stReset1MSCoef;
 _ALARM_EXT ALM_DETEC200MS_COF alm_stDetect200MSCoef;

User project/2.MotorDrive/Include/spdctrFSM.h

@@ -45,7 +45,8 @@ typedef enum
     StartUp = 3,
     Open2Clz = 4,
     ClzLoop = 5,
-    Stop = 6
+    Stop = 6,
+    Clz2Stop = 7
 } SCM_SPDMDFSM;
 
 typedef struct
@@ -69,6 +70,7 @@ _STARTFSM_EXT void StartUp_TbcupHook(void);
 _STARTFSM_EXT void Open2Clz_TbcupHook(void);
 _STARTFSM_EXT void ClzLoop_TbcupHook(void);
 _STARTFSM_EXT void Stop_TbcupHook(void);
+_STARTFSM_EXT void Clz2Stop_TbcupHook(void);
 
 _STARTFSM_EXT void InitPosDet_TbcdownHook(void);
 _STARTFSM_EXT void ParDet_TbcdownHook(void);
@@ -76,6 +78,7 @@ _STARTFSM_EXT void StartUp_TbcdownHook(void);
 _STARTFSM_EXT void Open2Clz_TbcdownHook(void);
 _STARTFSM_EXT void ClzLoop_TbcdownHook(void);
 _STARTFSM_EXT void Stop_TbcdownHook(void);
+_STARTFSM_EXT void Clz2Stop_TbcdownHook(void);
 
 _STARTFSM_EXT void InitPosDet_TbsHook(void);
 _STARTFSM_EXT void ParDet_TbsHook(void);
@@ -83,6 +86,7 @@ _STARTFSM_EXT void StartUp_TbsHook(void);
 _STARTFSM_EXT void Open2Clz_TbsHook(void);
 _STARTFSM_EXT void ClzLoop_TbsHook(void);
 _STARTFSM_EXT void Stop_TbsHook(void);
+_STARTFSM_EXT void Clz2Stop_TbsHook(void);
 
 _STARTFSM_EXT void scm_voSpdCtrMdFSM(void);
 _STARTFSM_EXT void Switch_speed_FSM(const SPD_STATE_HOOK *in);
@@ -99,6 +103,7 @@ _STARTFSM_EXT SPD_STATE_HOOK StartUp_state = {StartUp, StartUp_TbcupHook, StartU
 _STARTFSM_EXT SPD_STATE_HOOK Open2Clz_state = {Open2Clz, Open2Clz_TbcupHook, Open2Clz_TbcdownHook, Open2Clz_TbsHook};
 _STARTFSM_EXT SPD_STATE_HOOK ClzLoop_state = {ClzLoop, ClzLoop_TbcupHook, ClzLoop_TbcdownHook, ClzLoop_TbsHook};
 _STARTFSM_EXT SPD_STATE_HOOK Stop_state = {Stop, Stop_TbcupHook, Stop_TbcdownHook, Stop_TbsHook};
+_STARTFSM_EXT SPD_STATE_HOOK Clz2Stop_state = {Clz2Stop, Clz2Stop_TbcupHook, Clz2Stop_TbcdownHook, Clz2Stop_TbsHook};
 #else
 _STARTFSM_EXT ULONG          scm_ulStatCt;
 _STARTFSM_EXT SPD_STATE_HOOK InitPosDet_state;
@@ -107,6 +112,7 @@ _STARTFSM_EXT SPD_STATE_HOOK StartUp_state;
 _STARTFSM_EXT SPD_STATE_HOOK Open2Clz_state;
 _STARTFSM_EXT SPD_STATE_HOOK ClzLoop_state;
 _STARTFSM_EXT SPD_STATE_HOOK Stop_state;
+_STARTFSM_EXT SPD_STATE_HOOK Clz2Stop_state;
 #endif
 /************************************************************************
  RAM ALLOCATION (N/A)

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

@@ -51,6 +51,7 @@ static SWORD alm_pvt_swIqRefLpfAbsPu = 0;
 static BOOL  alm_pvt_blTbcFirstFlg = FALSE;
 static BOOL  alm_pvt_blTbsFirstFlg = FALSE;
 static SWORD alm_pvt_swRtLockPwrRatio = 0;
+static ULONG alm_pvt_ulWarn2ErrorCount = 0;
 /************************************************************************
 Constant Table:
 *************************************************************************/
@@ -109,8 +110,10 @@ void alm_voInit(void)
 
     /* Clear alarm flag */
     alm_blAlmOccrFlg = FALSE;
+    alm_blWarnOccrFlg = FALSE;
     alm_blAlmSingleRecordDoneFlg = FALSE;
-
+    alm_pvt_ulWarn2ErrorCount = 0;
+    
     /* Clear alarm count */
     alm_stDecCt.ulIPMOvrHeat = 0;
     alm_stDecCt.ulIPMOvrHeat1 = 0;
@@ -385,9 +388,9 @@ void alm_voDetecTBC(const ALM_IN *in, const ALM_DETECTBC_COF *coef) /* parasoft-
                 alm_stDecCt.ulPhsALoss = coef->ulPhsLossValCt;
                 alm_stDecCt.ulPhsBLoss = coef->ulPhsLossValCt;
                 alm_stDecCt.ulPhsCLoss = coef->ulPhsLossValCt;
-                alm_unAction.bit.PWMOff = 1;
+//                alm_unAction.bit.PWMOff = 1;
                 // alm_unAction.bit.ThrPhsShrt = 1;
-//                alm_unCode.bit.PhsLoss = 1;
+                alm_unCode.bit.PhsLoss = 1;
             }
         }
         else
@@ -433,7 +436,7 @@ void alm_voDetecTBC(const ALM_IN *in, const ALM_DETECTBC_COF *coef) /* parasoft-
                     {
                         alm_stDecCt.slRotorLock = coef->slRotorLockValCt;
                         // alm_unAction.bit.ThrPhsShrt = 1;
-                        alm_unAction.bit.PWMOff = 1;
+//                        alm_unAction.bit.PWMOff = 1;
                         alm_unCode.bit.RotorLock = 1;
                     }
                 }
@@ -461,7 +464,7 @@ void alm_voDetecTBC(const ALM_IN *in, const ALM_DETECTBC_COF *coef) /* parasoft-
         =======================================================================*/
         if(in->blSpiThetaFltFlg)
         {
-            alm_unAction.bit.PWMOff = 1;
+//            alm_unAction.bit.PWMOff = 1;
             alm_unCode.bit.SpiThetaFlt = 1;
         }
 
@@ -472,10 +475,23 @@ void alm_voDetecTBC(const ALM_IN *in, const ALM_DETECTBC_COF *coef) /* parasoft-
     }
 
     /*========================== Alarm flag set ===========================*/
-    if ((alm_unCode.all != 0) || (alm_pvt_blIPMOTReCheckFlg == TRUE) || (alm_unBikeCode.all != 0))
+    if((alm_unCode.bit.OvrCur == 1) || (alm_unCode.bit.IPMFlt == 1) || (alm_unCode.bit.MCUErr == 1))
     {
         alm_blAlmOccrFlg = TRUE;
     }
+    else if ((alm_unCode.all != 0) || (alm_unBikeCode.all != 0))
+    {
+        alm_blWarnOccrFlg = TRUE;
+        
+        if(alm_pvt_ulWarn2ErrorCount > 24000)
+        {
+            alm_blAlmOccrFlg = TRUE;
+        }
+        else
+        {
+            alm_pvt_ulWarn2ErrorCount++;
+        }
+    }
 }
 
 /***************************************************************
@@ -544,7 +560,7 @@ void alm_voDetecTBS(const ALM_IN *in, const ALM_DETECTBS_COF *coef)
 //                alm_stDecCt.ulOvrVltLvl3 = coef->ulOvrVltLvl3ValCt;
 //                // alm_unAction.bit.ThrPhsShrt = 1;
 //                alm_unCode.bit.OvrVlt = 1;
-//                alm_unAction.bit.PWMOff = 1;
+////                alm_unAction.bit.PWMOff = 1;
 //            }
 //        }
 //        else
@@ -561,7 +577,7 @@ void alm_voDetecTBS(const ALM_IN *in, const ALM_DETECTBS_COF *coef)
 //                alm_stDecCt.ulOvrVltLvl2 = coef->ulOvrVltLvl2ValCt;
 //                // alm_unAction.bit.ThrPhsShrt = 1;
 //                alm_unCode.bit.OvrVlt = 1;
-//                alm_unAction.bit.PWMOff = 1;
+////                alm_unAction.bit.PWMOff = 1;
 //            }
 //        }
 //        else
@@ -578,7 +594,7 @@ void alm_voDetecTBS(const ALM_IN *in, const ALM_DETECTBS_COF *coef)
                 alm_stDecCt.ulOvrVltLvl1 = coef->ulOvrVltLvl1ValCt;
                 // alm_unAction.bit.ThrPhsShrt = 1;
                 alm_unCode.bit.OvrVlt = 1;
-                alm_unAction.bit.PWMOff = 1;
+//                alm_unAction.bit.PWMOff = 1;
             }
         }
         else
@@ -595,7 +611,7 @@ void alm_voDetecTBS(const ALM_IN *in, const ALM_DETECTBS_COF *coef)
 //            if (alm_stDecCt.ulUndrVltLvl2 >= coef->ulUndrVltLvl2ValCt) // 1ms
 //            {
 //                alm_stDecCt.ulUndrVltLvl2 = coef->ulUndrVltLvl2ValCt;
-//                alm_unAction.bit.PWMOff = 1;
+////                alm_unAction.bit.PWMOff = 1;
 //                alm_unCode.bit.UndrVlt = 1;
 //            }
 //        }
@@ -611,7 +627,7 @@ void alm_voDetecTBS(const ALM_IN *in, const ALM_DETECTBS_COF *coef)
             if (alm_stDecCt.ulUndrVltLvl1 >= coef->ulUndrVltLvl1ValCt) // 2s
             {
                 alm_stDecCt.ulUndrVltLvl1 = coef->ulUndrVltLvl1ValCt;
-                alm_unAction.bit.PWMOff = 1;
+//                alm_unAction.bit.PWMOff = 1;
                 alm_unCode.bit.UndrVlt = 1;
             }
         }
@@ -653,7 +669,7 @@ void alm_voDetecTBS(const ALM_IN *in, const ALM_DETECTBS_COF *coef)
                 {
                     alm_stDecCt.ulIPMOvrHeat1 = coef->ulIPMOvrHeatValCt;
                     // alm_unAction.bit.ThrPhsShrt = 1;
-                    alm_unAction.bit.PWMOff = 1;
+//                    alm_unAction.bit.PWMOff = 1;
                     alm_unCode.bit.IPMOvrHeat = 1;
                 }
             }
@@ -671,7 +687,7 @@ void alm_voDetecTBS(const ALM_IN *in, const ALM_DETECTBS_COF *coef)
                 {
                     alm_stDecCt.ulIPMOvrHeat = coef->ulIPMOvrHeatValCt;
                     // alm_unAction.bit.ThrPhsShrt = 1;
-                    alm_unAction.bit.PWMOff = 1;
+//                    alm_unAction.bit.PWMOff = 1;
                     alm_unCode.bit.IPMOvrHeat = 1;
                     alm_pvt_blIPMOTReCheckFlg = TRUE;
                 }
@@ -1170,12 +1186,17 @@ void alm_voReset(const ALM_IN *in, const ALM_RESET_COF *coef) /* parasoft-suppre
         alm_stRecCt.ulIPMOvrHeat1 = 0;
         alm_stRecCt.ulIPMOC = 0;
 
-        /* Clear alarm flag */
-        alm_blAlmOccrFlg = FALSE;
-        alm_blAlmSingleRecordDoneFlg = FALSE;
-        alm_blPowerDownFlg = FALSE;
-        /* Clear speed command*/
-        // cmd_swSpdRefPu = 0;
+        if(alm_unBikeCode.all == 0)
+        {
+            /* Clear alarm flag */
+            alm_blAlmOccrFlg = FALSE;
+            alm_blWarnOccrFlg = FALSE;
+            alm_pvt_ulWarn2ErrorCount = 0;
+            alm_blAlmSingleRecordDoneFlg = FALSE;
+            alm_blPowerDownFlg = FALSE;
+            /* Clear speed command*/
+            // cmd_swSpdRefPu = 0;
+        }
     }
 }
 
@@ -1245,13 +1266,13 @@ void alm_voDetec200MS(const ALM_BIKE_IN *in, const ALM_DETEC200MS_COF *coef) /*
             {
                 alm_stDecCt.uwBikeSpdFlt = coef->uwBikeSpdFltCt;
                 alm_unBikeCode.bit.BikeSpdSen = 1;
-                alm_unAction.bit.PWMOff = 1;
+//                alm_unAction.bit.PWMOff = 1;
             }    
         }
         else if(in->blBikeSpdOvrFlg)
         {        
             alm_unBikeCode.bit.BikeSpdSen = 1;
-            alm_unAction.bit.PWMOff = 1;
+//            alm_unAction.bit.PWMOff = 1;
         }
         else
         {
@@ -1275,13 +1296,13 @@ void alm_voDetec200MS(const ALM_BIKE_IN *in, const ALM_DETEC200MS_COF *coef) /*
                 {
                     alm_stDecCt.uwCadenceFlt = coef->uwCadenceFltCt;
                     alm_unBikeCode.bit.CadenceSen = 1;
-                    alm_unAction.bit.PWMOff = 1;
+//                    alm_unAction.bit.PWMOff = 1;
                 }
             }
             else if(in->blCadenceFreqOvrFlg)
             {
                 alm_unBikeCode.bit.CadenceSen = 1;
-                alm_unAction.bit.PWMOff = 1;
+//                alm_unAction.bit.PWMOff = 1;
             }
             else
             {
@@ -1306,7 +1327,7 @@ void alm_voDetec200MS(const ALM_BIKE_IN *in, const ALM_DETEC200MS_COF *coef) /*
         {
             alm_stDecCt.uwTorqFlt = coef->uwTorqFltCt;
             alm_unBikeCode.bit.TorqSen = 1;
-            alm_unAction.bit.PWMOff = 1;
+//            alm_unAction.bit.PWMOff = 1;
         }
     }
     else
@@ -1324,7 +1345,7 @@ void alm_voDetec200MS(const ALM_BIKE_IN *in, const ALM_DETEC200MS_COF *coef) /*
             {
                 alm_stDecCt.uwThrottleFlt = coef->uwThrottleFltCt;
                 alm_unBikeCode.bit.Throttle = 1;
-                alm_unAction.bit.PWMOff = 1;
+//                alm_unAction.bit.PWMOff = 1;
             }
         } 
         else
@@ -1342,7 +1363,7 @@ void alm_voDetec200MS(const ALM_BIKE_IN *in, const ALM_DETEC200MS_COF *coef) /*
         {
             alm_stDecCt.uwPCBNTCFlt = coef->uwPCBNTCFltCt;
             alm_unBikeCode.bit.PCBNTC = 1;
-            alm_unAction.bit.PWMOff = 1;
+//            alm_unAction.bit.PWMOff = 1;
         }
     }
     else
@@ -1360,7 +1381,7 @@ void alm_voDetec200MS(const ALM_BIKE_IN *in, const ALM_DETEC200MS_COF *coef) /*
             {
                 alm_stDecCt.uwMotorNTCFlt = coef->uwMotorNTCFltCt;
                 alm_unBikeCode.bit.MotorNTC = 1;
-                alm_unAction.bit.PWMOff = 1;
+//                alm_unAction.bit.PWMOff = 1;
             }
         }
         else

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

@@ -175,6 +175,42 @@ void ClzLoop_TbcupHook(void)
 void Stop_TbcupHook(void)
 {}
 
+void Clz2Stop_TbcupHook(void)
+{
+    scm_ulStatCt++;
+
+    /*=======================================================================
+                                     Power Limit
+      =======================================================================*/
+    mth_voLPFilter(adc_stUpOut.PCBTemp, &scm_stPCBTempLpf);
+    pwr_stPwrLimIn.swMotorPwrPu = scm_stMotoPwrInLpf.slY.sw.hi; // Q15
+    pwr_stPwrLimIn.swPCBTemp = scm_stPCBTempLpf.slY.sw.hi;  
+    pwr_voPwrLimPI(&pwr_stPwrLimIn, &pwr_stPwrLimCof, &pwr_stPwrLimOut2); // Q14
+
+    /*======================================================================= 
+             Set Iq and Id reference for Constant Voltage Break
+    =======================================================================*/
+    cvb_stBrakeIn.uwVdcLpfPu = adc_stUpOut.uwVdcLpfPu;
+    cvb_stBrakeIn.swIdRefPu = scm_swIdRefPu;
+    cvb_stBrakeIn.swIqRefPu = 0;//swCurRefrompu;            
+    cvb_stBrakeIn.swSpdPu = scm_stSpdFbkLpf.slY.sw.hi;
+    cvb_stBrakeIn.uwAngelPu = scm_uwAngRefPu;
+    cvb_stBrakeIn.uwSpdLpfAbsPu = scm_uwSpdFbkLpfAbsPu;
+    cvb_voBrake(&cvb_stBrakeIn,&cvb_stBrakeCoef,&cvb_stBrakeOut);
+ 
+    /** Idref可变范围-3A~0A且未弱磁，暂不做过多处理；Angle在down中更新，暂不改为改为恒压制动角度 **/
+    if(cvb_stBrakeIn.uwVdcLpfPu >= cvb_stBrakeCoef.uwVdcStartCvbPu)
+    {
+        scm_swIdRefPu = cvb_stBrakeOut.swIdRefPu;
+    }
+    else
+    {
+        scm_swIdRefPu = 0;  //
+    }  
+
+    scm_swIqRefPu = cvb_stBrakeOut.swIqRefPu;
+}
+
 void InitPosDet_TbcdownHook(void)
 {
     /* Get angle for park transformation */
@@ -366,6 +402,85 @@ void Stop_TbcdownHook(void)
     cmfsm_stFlg.blMotorStopFlg = TRUE;
 }
 
+void Clz2Stop_TbcdownHook(void)
+{
+    ULONG ulTmp1;
+    SWORD swAngCompPu = 0; // Q15
+
+/* Speed feedback LPF */
+
+    if(cp_stFlg.ThetaGetModelSelect == ANG_OBSERVER)
+    {
+        obs_stObsCalcIn.swUalphaPu = scm_swUalphaPu;              // Q14
+        obs_stObsCalcIn.swUbetaPu = scm_swUbetaPu;                // Q14
+        obs_stObsCalcIn.swIalphaPu = crd_stCurClarkOut.swAlphaPu; // Q14
+        obs_stObsCalcIn.swIbetaPu = crd_stCurClarkOut.swBetaPu;   // Q14
+        obs_stObsCalcIn.uwVdcPu = adc_stUpOut.uwVdcLpfPu;
+        obs_voObsCalc(&obs_stObsCalcIn, &obs_stObsCoefPu, &obs_stObsOutPu);
+        mth_voLPFilter(obs_stObsOutPu.swElecFreqPu, &scm_stSpdFbkLpf);
+        scm_uwAngRefPu = obs_stObsOutPu.uwElecThetaPu;
+    }
+    else if(cp_stFlg.ThetaGetModelSelect == ANG_RESOLVER)
+    {
+        mth_voLPFilter(spi_stResolverOut.swSpdFbkPu, &scm_stSpdFbkLpf);
+        scm_uwAngRefPu = spi_stResolverOut.uwSpiThetaPu;
+    }
+    else if(cp_stFlg.ThetaGetModelSelect == ANG_SWITCHHALL)
+    {
+        mth_voLPFilter(switchhall_stOut.swLowSpdPu, &scm_stSpdFbkLpf);
+        scm_uwAngRefPu = switchhall_stOut.uwLowThetaPu;
+    }
+    else
+    {
+        //do nothing
+    }
+
+    /* Speed feedback Absolute */
+    scm_uwSpdFbkLpfAbsPu = (UWORD)ABS(scm_stSpdFbkLpf.slY.sw.hi); // Q15
+
+    /* Get angle for park transformation */
+    scm_uwAngParkPu = scm_uwAngRefPu; // Q15
+
+    if(cp_stFlg.ThetaGetModelSelect == ANG_OBSERVER)
+    {
+        swAngCompPu = (SWORD)(((SLONG)obs_stObsOutPu.swElecFreqPu * TBC_TM) >> 10); // Q15
+    }
+    else if(cp_stFlg.ThetaGetModelSelect == ANG_RESOLVER)
+    {
+        swAngCompPu = (SWORD)(((SLONG)spi_stResolverOut.swSpdFbkPu * TBC_TM) >> 10); // Q15
+    }
+    else if(cp_stFlg.ThetaGetModelSelect == ANG_SWITCHHALL)
+    {
+        swAngCompPu = (SWORD)(((SLONG)switchhall_stOut.swLowSpdPu * TBC_TM) >> 10); // Q15
+    }
+    else
+    {
+        //do nothing
+    }
+
+    ulTmp1 = scm_uwAngParkPu + ((swAngCompPu * 3) >> 1) + cof_sl720DegreePu; 
+    scm_uwAngIParkPu = (UWORD)(ulTmp1 & 0x7FFF);
+    /*=======================================================================
+                        Current decoupling
+    =======================================================================*/ 
+    if(cp_stFlg.ThetaGetModelSelect == ANG_OBSERVER)
+    {
+        acr_stUdqDcpIn.swWsPu =  scm_stSpdFbkLpf.slY.sw.hi; // Q15
+    }
+    else if(cp_stFlg.ThetaGetModelSelect == ANG_RESOLVER)
+    {
+        acr_stUdqDcpIn.swWsPu = spi_stResolverOut.swSpdFbkPu; // Q15  swSpdFbkLpfPu spi_stResolverOut.swSpdFbkPu
+    }
+    else
+    {
+        //do nothing
+    }
+    acr_stUdqDcpIn.swIdRefPu = scm_swIdFdbLpfPu;//scm_swIdRefPu  scm_swIdFdbLpfPu          // Q14
+    acr_stUdqDcpIn.swIqRefPu = scm_swIqFdbLpfPu; //scm_swIqRefPu  scm_swIqFdbLpfPu         // Q14  
+    acr_stUdqDcpIn.swUdqLimPu = scm_swVsDcpLimPu;      // Q14
+    acr_voUdqDcp(&acr_stUdqDcpIn, &acr_stUdqDcpCoef, &acr_stUdqDcpOut);
+}
+
 void InitPosDet_TbsHook(void)
 {}
 void ParDet_TbsHook(void)
@@ -401,6 +516,9 @@ void ClzLoop_TbsHook(void)
 void Stop_TbsHook(void)
 {}
 
+void Clz2Stop_TbsHook(void)
+{}
+
 void scm_voSpdCtrMdFSM(void) /* parasoft-suppress METRICS-28 "本项目圈复杂度无法更改，后续避免" */
 {
     switch (curSpeed_state.state)
@@ -417,8 +535,11 @@ void scm_voSpdCtrMdFSM(void) /* parasoft-suppress METRICS-28 "本项目圈复杂
         {
             Switch_speed_FSM(&Stop_state);
         }
-
-        if (scm_ulStatCt < mn_ulAlignRampTbcCt)
+        else if(switch_flg.SysWarnning_Flag == 1)
+        {
+            Switch_speed_FSM(&Clz2Stop_state);
+        }
+        else if(scm_ulStatCt < mn_ulAlignRampTbcCt)
         {}
         else if (scm_ulStatCt >= (mn_ulAlignRampTbcCt + mn_ulAlignHoldTbcCt + 10))
         {
@@ -466,7 +587,11 @@ void scm_voSpdCtrMdFSM(void) /* parasoft-suppress METRICS-28 "本项目圈复杂
         {
             Switch_speed_FSM(&Stop_state);
         }
-        if (scm_StartUpOvrFlg == TRUE)
+        else if(switch_flg.SysWarnning_Flag == 1)
+        {
+            Switch_speed_FSM(&Clz2Stop_state);
+        }
+        else if (scm_StartUpOvrFlg == TRUE)
         {
             if(cp_stFlg.ThetaGetModelSelect == ANG_OBSERVER)
             {
@@ -500,12 +625,13 @@ void scm_voSpdCtrMdFSM(void) /* parasoft-suppress METRICS-28 "本项目圈复杂
         {
             Switch_speed_FSM(&Stop_state);
         }
-        /* Switch over */
-        if (scm_blCurSwitchOvrFlg && scm_blAngSwitchOvrFlg)
+        else if(switch_flg.SysWarnning_Flag == 1)
+        {
+            Switch_speed_FSM(&Clz2Stop_state);
+        }
+        else if (scm_blCurSwitchOvrFlg && scm_blAngSwitchOvrFlg)/* Switch over */
         {
-
             Switch_speed_FSM(&ClzLoop_state);
-
         }
 
         break;
@@ -516,7 +642,11 @@ void scm_voSpdCtrMdFSM(void) /* parasoft-suppress METRICS-28 "本项目圈复杂
         {
             Switch_speed_FSM(&Stop_state);
         }
-        if (!switch_flg.SysRun_Flag)
+        else if(switch_flg.SysWarnning_Flag == 1)
+        {
+            Switch_speed_FSM(&Clz2Stop_state);
+        }
+        else if (!switch_flg.SysRun_Flag)
         {
             /* Go to stop */
             if ((ABS(scm_swSpdRefPu) < mn_swStopSpdRefPu) && (scm_uwSpdFbkLpfAbsPu < mn_swStopSpdRefPu))
@@ -524,7 +654,7 @@ void scm_voSpdCtrMdFSM(void) /* parasoft-suppress METRICS-28 "本项目圈复杂
                 scm_swIdRefPu = 0;
                 scm_swIqRefPu = 0;
 
-                Switch_speed_FSM(&Stop_state);
+                Switch_speed_FSM(&Clz2Stop_state);
             }
             
             if (uart_swTorqRefNm == 0)
@@ -532,15 +662,30 @@ void scm_voSpdCtrMdFSM(void) /* parasoft-suppress METRICS-28 "本项目圈复杂
                 scm_swIdRefPu = 0;
                 scm_swIqRefPu = 0;
 
-                Switch_speed_FSM(&Stop_state);
+                Switch_speed_FSM(&Clz2Stop_state);
             }
         }
         else
         {}
 
+        break;
+    case Clz2Stop:
+        if (switch_flg.SysFault_Flag == 1 || power_stPowStateOut.powerstate == POWER_OFF)
+        {
+            Switch_speed_FSM(&Stop_state);
+        }
+        else if((scm_ulStatCt > 24000) || (scm_uwSpdFbkLpfAbsPu < mn_swStopSpdRefPu))
+        {
+            Switch_speed_FSM(&Stop_state);
+        }
+        else
+        {
+            //do nothing
+        }
+        
         break;
     case Stop:
-        if (switch_flg.SysRun_Flag == TRUE && switch_flg.SysFault_Flag == FALSE) //&& power_stPowStateOut.powerstate == POWER_ON_END
+        if (switch_flg.SysRun_Flag == TRUE && switch_flg.SysFault_Flag == FALSE && switch_flg.SysWarnning_Flag == FALSE) //&& power_stPowStateOut.powerstate == POWER_ON_END
         {
             scm_voSpdCtrMdInit();
             if(cp_stFlg.SpiOffsetFirstSetFlg == 0)
@@ -606,6 +751,7 @@ UWORD StartUp_SpiSensorTest(void)
               AngleGet_count = 2;
               MC_MotorSPD_rpm_Percent = 0;
               cp_stFlg.RunModelSelect = MC_UpcInfo.stTestParaInfo.RunModelSelect;
+              signal_state.Assist = FALSE;
 
               memcpy((UBYTE*)(Angle), (UBYTE*)(&spi_stResolverOut.swSpiThetaOffsetOrignPu), (uint32_t)2);
               memcpy((UBYTE*)(Angle+1), (UBYTE*)(&spi_stResolverOut.swSpiThetaOffsetPu), (uint32_t)2);

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

@@ -166,6 +166,7 @@ typedef struct MC_ERROR_CNT_RECORD
     UWORD Fault_TE_MCUCnt;          ///>TE MCU故障
     UWORD Fault_TE_CircuitCnt;      ///>TE 电路故障
     UWORD Fault_MC_CheckCnt;        ///>MC校验失败
+    UWORD Fault_SpiPosSensorCnt;    ///>SPI位置故障
 } MC_ErrorCode_CNT_RECORD_Struct_t;
 
 /**

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

@@ -370,95 +370,102 @@ void Can_voMC_Run_1ms(void) /* parasoft-suppress METRICS-28 "本项目圈复杂
     }
 
 
-    if (alm_blAlmOccrFlg == TRUE)
+    if ((alm_blAlmOccrFlg == TRUE) || (alm_blWarnOccrFlg == TRUE))
     {
-        if (alm_blAlmSingleRecordDoneFlg == FALSE)
+//        if (alm_blAlmSingleRecordDoneFlg == FALSE)
         {
-            if (alm_unCode.bit.IPMFlt == 1)
+            if ((alm_unCode.bit.IPMFlt == 1) && (MC_ErrorCode.ERROR_Bit.Protect_OverCurrent == 0))
             {
                 MC_ErrorCntRecord.Protect_OverCurrentCnt++;
-                cp_stHistoryPara.uwAlamHOcurTimes++;
+                
                 if (MC_ErrorCntRecord.Protect_OverCurrentCnt == ALAM_DISPLAY_CNT_0LEVEL) // ALAM_DISPLAY_CNT_2LEVEL
                 {
                     MC_ErrorCode.ERROR_Bit.Protect_OverCurrent = 1;
+                    cp_stHistoryPara.uwAlamHOcurTimes++;
                     /*error log updata*/
                     ErrorLog_Updata();
                     que_ubPushIn(&que_stFlashErrorLog, &que_stErrorLog, 1);
                 }
             }
 
-            if (alm_unCode.bit.OvrCur == 1)
+            if ((alm_unCode.bit.OvrCur == 1) && (MC_ErrorCode.ERROR_Bit.Protect_OverCurrent == 0))
             {
                 MC_ErrorCntRecord.Protect_OverCurrentCnt++;
-                cp_stHistoryPara.uwAlamSOcurTimes++;
+                
                 if (MC_ErrorCntRecord.Protect_OverCurrentCnt == ALAM_DISPLAY_CNT_0LEVEL) // ALAM_DISPLAY_CNT_2LEVEL
                 {
                     MC_ErrorCode.ERROR_Bit.Protect_OverCurrent = 1;
+                    cp_stHistoryPara.uwAlamSOcurTimes++;
                     /*error log updata*/
                     ErrorLog_Updata();
                     que_ubPushIn(&que_stFlashErrorLog, &que_stErrorLog, 1);
                 }
             }
 
-            if (alm_unCode.bit.OvrVlt == 1)
+            if ((alm_unCode.bit.OvrVlt == 1) && (MC_ErrorCode.ERROR_Bit.Protect_OverVoltage == 0))
             {
                 MC_ErrorCntRecord.Protect_OverVoltageCnt++;
-                cp_stHistoryPara.uwAlamOVolTimes++;
+                
                 if (MC_ErrorCntRecord.Protect_OverVoltageCnt == ALAM_DISPLAY_CNT_0LEVEL) // ALAM_DISPLAY_CNT_2LEVEL
                 {
                     MC_ErrorCode.ERROR_Bit.Protect_OverVoltage = 1;
+                    cp_stHistoryPara.uwAlamOVolTimes++;
                     /*error log updata*/
                     ErrorLog_Updata();
                     que_ubPushIn(&que_stFlashErrorLog, &que_stErrorLog, 1);
                 }
             }
 
-            if (alm_unCode.bit.UndrVlt == 1)
+            if ((alm_unCode.bit.UndrVlt == 1) && (MC_ErrorCode.ERROR_Bit.Protect_UnderVoltage == 0))
             {
                 MC_ErrorCntRecord.Protect_UnderVoltageCnt++;
-                cp_stHistoryPara.uwAlamUVolTimes++;
+                
                 if (MC_ErrorCntRecord.Protect_UnderVoltageCnt == ALAM_DISPLAY_CNT_0LEVEL) // ALAM_DISPLAY_CNT_2LEVEL
                 {
                     MC_ErrorCode.ERROR_Bit.Protect_UnderVoltage = 1;
+                    cp_stHistoryPara.uwAlamUVolTimes++;
                     /*error log updata*/
                     ErrorLog_Updata();
                     que_ubPushIn(&que_stFlashErrorLog, &que_stErrorLog, 1);
                 }
             }
 
-            if (alm_unCode.bit.RotorLock == 1)
+            if ((alm_unCode.bit.RotorLock == 1) && (MC_ErrorCode.ERROR_Bit.Protect_LockRotor == 0))
             {
                 MC_ErrorCntRecord.Protect_LockRotorCnt++;
-                cp_stHistoryPara.uwAlamRotorLockTimes++;
+                
                 if (MC_ErrorCntRecord.Protect_LockRotorCnt == ALAM_DISPLAY_CNT_0LEVEL) // ALAM_DISPLAY_CNT_2LEVEL
                 {
                     MC_ErrorCode.ERROR_Bit.Protect_LockRotor = 1;
+                    cp_stHistoryPara.uwAlamRotorLockTimes++;
                     /*error log updata*/
                     ErrorLog_Updata();
                     que_ubPushIn(&que_stFlashErrorLog, &que_stErrorLog, 1);
                 }
             }
 
-            if (alm_unCode.bit.IPMOvrHeat == 1)
+            if ((alm_unCode.bit.IPMOvrHeat == 1) && (MC_ErrorCode.ERROR_Bit.Protect_OverTemp == 0))
             {
                 MC_ErrorCntRecord.Protect_OverTempCnt++;
-                cp_stHistoryPara.uwAlamOHeatTimes++;
+                
                 if (MC_ErrorCntRecord.Protect_OverTempCnt == ALAM_DISPLAY_CNT_0LEVEL)
                 {
                     MC_ErrorCode.ERROR_Bit.Protect_OverTemp = 1;
+                    cp_stHistoryPara.uwAlamOHeatTimes++;
                     /*error log updata*/
                     ErrorLog_Updata();
                     que_ubPushIn(&que_stFlashErrorLog, &que_stErrorLog, 1);
                 }
             }
 
-            if (alm_unCode.bit.PhsLoss == 1)
+            if ((alm_unCode.bit.PhsLoss == 1) && (MC_ErrorCode.ERROR_Bit.Fault_PhaseLine == 0))
             {
                 MC_ErrorCntRecord.Fault_PhaseLineCnt++;
-                cp_stHistoryPara.uwAlamPhsLossTimes++;
+                
                 if (MC_ErrorCntRecord.Fault_PhaseLineCnt == ALAM_DISPLAY_CNT_0LEVEL)
                 {
                     MC_ErrorCode.ERROR_Bit.Fault_PhaseLine = 1;
+                    cp_stHistoryPara.uwAlamPhsLossTimes++;
                     /*error log updata*/
                     ErrorLog_Updata();
                     que_ubPushIn(&que_stFlashErrorLog, &que_stErrorLog, 1);
@@ -470,54 +477,80 @@ void Can_voMC_Run_1ms(void) /* parasoft-suppress METRICS-28 "本项目圈复杂
             //     cp_stHistoryPara.uwAlamComOTimeTimes++;
             // }
             
-            if (alm_unCode.bit.SpiThetaFlt == 1)
+            if ((alm_unCode.bit.SpiThetaFlt == 1) && (MC_ErrorCode.ERROR_Bit.Fault_SpiPosSensor == 0))
             {
-                MC_ErrorCode.ERROR_Bit.Fault_SpiPosSensor = 1;
-                /*error log updata*/
-                ErrorLog_Updata();
-                que_ubPushIn(&que_stFlashErrorLog, &que_stErrorLog, 1);
+                MC_ErrorCntRecord.Fault_SpiPosSensorCnt++;
+                if (MC_ErrorCntRecord.Fault_SpiPosSensorCnt == ALAM_DISPLAY_CNT_0LEVEL)
+                {
+                    MC_ErrorCode.ERROR_Bit.Fault_SpiPosSensor = 1;
+                    cp_stHistoryPara.uwPosSensorAlamTimes++;
+                    /*error log updata*/
+                    ErrorLog_Updata();
+                    que_ubPushIn(&que_stFlashErrorLog, &que_stErrorLog, 1);
+                }
             }
             
-            if (alm_unBikeCode.bit.BikeSpdSen == 1)
+            if ((alm_unBikeCode.bit.BikeSpdSen == 1) && (MC_ErrorCode.ERROR_Bit.Fault_BikeSpdSensor == 0))
             {
-                MC_ErrorCode.ERROR_Bit.Fault_BikeSpdSensor = 1;
-                /*error log updata*/
-                ErrorLog_Updata();
-                que_ubPushIn(&que_stFlashErrorLog, &que_stErrorLog, 1);
+                MC_ErrorCntRecord.Fault_SpeedSensorCnt++;
+                if (MC_ErrorCntRecord.Fault_SpeedSensorCnt == ALAM_DISPLAY_CNT_0LEVEL)
+                {
+                    MC_ErrorCode.ERROR_Bit.Fault_BikeSpdSensor = 1;
+                    cp_stHistoryPara.uwBikeSpdSensorAlamTimes++;
+                    /*error log updata*/
+                    ErrorLog_Updata();
+                    que_ubPushIn(&que_stFlashErrorLog, &que_stErrorLog, 1);
+                }
             }
             
-            if (alm_unBikeCode.bit.CadenceSen == 1)
+            if ((alm_unBikeCode.bit.CadenceSen == 1) && (MC_ErrorCode.ERROR_Bit.Fault_CadenceSensor == 0))
             {
-                cp_stHistoryPara.uwCadSensorAlamTimes++;
-                MC_ErrorCode.ERROR_Bit.Fault_CadenceSensor = 1;
-                /*error log updata*/
-                ErrorLog_Updata();
-                que_ubPushIn(&que_stFlashErrorLog, &que_stErrorLog, 1);
+                MC_ErrorCntRecord.Fault_CadenceSensorCnt++;
+                if (MC_ErrorCntRecord.Fault_CadenceSensorCnt == ALAM_DISPLAY_CNT_0LEVEL)
+                {
+                    MC_ErrorCode.ERROR_Bit.Fault_CadenceSensor = 1;
+                    cp_stHistoryPara.uwCadSensorAlamTimes++;
+                    /*error log updata*/
+                    ErrorLog_Updata();
+                    que_ubPushIn(&que_stFlashErrorLog, &que_stErrorLog, 1);
+                }
             }
             
-            if (alm_unBikeCode.bit.PCBNTC == 1)
+            if ((alm_unBikeCode.bit.PCBNTC == 1) && (MC_ErrorCode.ERROR_Bit.Fault_PCBNTC == 0))
             {
-                MC_ErrorCode.ERROR_Bit.Fault_PCBNTC = 1;
-                /*error log updata*/
-                ErrorLog_Updata();
-                que_ubPushIn(&que_stFlashErrorLog, &que_stErrorLog, 1);
+                MC_ErrorCntRecord.Fault_NtcSensorCnt++;
+                if (MC_ErrorCntRecord.Fault_NtcSensorCnt == ALAM_DISPLAY_CNT_0LEVEL)
+                {
+                    MC_ErrorCode.ERROR_Bit.Fault_PCBNTC = 1;
+                    /*error log updata*/
+                    ErrorLog_Updata();
+                    que_ubPushIn(&que_stFlashErrorLog, &que_stErrorLog, 1);
+                }
             }
             
-            if (alm_unBikeCode.bit.Throttle == 1)
+            if ((alm_unBikeCode.bit.Throttle == 1) && (MC_ErrorCode.ERROR_Bit.Fault_GasSensor == 0))
             {
-                MC_ErrorCode.ERROR_Bit.Fault_GasSensor = 1;
-                /*error log updata*/
-                ErrorLog_Updata();
-                que_ubPushIn(&que_stFlashErrorLog, &que_stErrorLog, 1);
+                MC_ErrorCntRecord.Fault_GasSensorCnt++;
+                if (MC_ErrorCntRecord.Fault_GasSensorCnt == ALAM_DISPLAY_CNT_0LEVEL)
+                {
+                    MC_ErrorCode.ERROR_Bit.Fault_GasSensor = 1;
+                    /*error log updata*/
+                    ErrorLog_Updata();
+                    que_ubPushIn(&que_stFlashErrorLog, &que_stErrorLog, 1);
+                }
             }
             
-            if (alm_unBikeCode.bit.TorqSen == 1)
+            if ((alm_unBikeCode.bit.TorqSen == 1) && (MC_ErrorCode.ERROR_Bit.Fault_TorqueSensor == 0))
             {
-                MC_ErrorCode.ERROR_Bit.Fault_TorqueSensor = 1;
-                /*error log updata*/
-                ErrorLog_Updata();
-                que_ubPushIn(&que_stFlashErrorLog, &que_stErrorLog, 1);
-                
+                MC_ErrorCntRecord.Fault_TorqueSensorCnt++;
+                if (MC_ErrorCntRecord.Fault_TorqueSensorCnt == ALAM_DISPLAY_CNT_0LEVEL)
+                {
+                    MC_ErrorCode.ERROR_Bit.Fault_TorqueSensor = 1;
+                    cp_stHistoryPara.uwTorSensorAlamTimes++;
+                    /*error log updata*/
+                    ErrorLog_Updata();
+                    que_ubPushIn(&que_stFlashErrorLog, &que_stErrorLog, 1);
+                }
             }
             
             alm_blAlmSingleRecordDoneFlg = TRUE;
@@ -590,6 +623,7 @@ void Can_voMC_Run_1ms(void) /* parasoft-suppress METRICS-28 "本项目圈复杂
     {
         if (alm_unCode.bit.SpiThetaFlt != 1)
         {
+            MC_ErrorCntRecord.Fault_SpiPosSensorCnt = 0;
             MC_ErrorCode.ERROR_Bit.Fault_SpiPosSensor = 0;
         }
     }
@@ -598,6 +632,7 @@ void Can_voMC_Run_1ms(void) /* parasoft-suppress METRICS-28 "本项目圈复杂
     {
         if (alm_unBikeCode.bit.BikeSpdSen != 1)
         {
+            MC_ErrorCntRecord.Fault_SpeedSensorCnt = 0;
             MC_ErrorCode.ERROR_Bit.Fault_BikeSpdSensor = 0;
         }
     }
@@ -606,6 +641,7 @@ void Can_voMC_Run_1ms(void) /* parasoft-suppress METRICS-28 "本项目圈复杂
     {
         if (alm_unBikeCode.bit.CadenceSen != 1)
         {
+            MC_ErrorCntRecord.Fault_CadenceSensorCnt = 0;
             MC_ErrorCode.ERROR_Bit.Fault_CadenceSensor = 0;
         }
     }
@@ -614,6 +650,7 @@ void Can_voMC_Run_1ms(void) /* parasoft-suppress METRICS-28 "本项目圈复杂
     {
         if (alm_unBikeCode.bit.PCBNTC != 1)
         {
+            MC_ErrorCntRecord.Fault_NtcSensorCnt = 0;
             MC_ErrorCode.ERROR_Bit.Fault_PCBNTC = 0;
         }
     }
@@ -622,6 +659,7 @@ void Can_voMC_Run_1ms(void) /* parasoft-suppress METRICS-28 "本项目圈复杂
     {
         if (alm_unBikeCode.bit.Throttle != 1)
         {
+            MC_ErrorCntRecord.Fault_GasSensorCnt = 0;
             MC_ErrorCode.ERROR_Bit.Fault_GasSensor = 0;
         }
     }
@@ -630,6 +668,7 @@ void Can_voMC_Run_1ms(void) /* parasoft-suppress METRICS-28 "本项目圈复杂
     {
         if (alm_unBikeCode.bit.TorqSen != 1)
         {
+            MC_ErrorCntRecord.Fault_TorqueSensorCnt = 0;
             MC_ErrorCode.ERROR_Bit.Fault_TorqueSensor = 0;
         }
     }