C51 COMPILER V9.52.0.0   ADDFUNCTION                                                       09/21/2018 09:06:09 PAGE 1   


C51 COMPILER V9.52.0.0, COMPILATION OF MODULE ADDFUNCTION
OBJECT MODULE PLACED IN .\Output\AddFunction.obj
COMPILER INVOKED BY: C:\Keil\C51\BIN\C51.EXE ..\User\Source\Application\AddFunction.c LARGE OMF2 WARNINGLEVEL(0) OPTIMIZ
                    -E(5,SPEED) BROWSE INCDIR(..\Library\FU68xx_Hardware_Driver\Include;..\User\Include) DEBUG PRINT(.\Listing\AddFunction.ls
                    -t) TABS(2) OBJECT(.\Output\AddFunction.obj)

line level    source

   1          /**************************** (C) COPYRIGHT 2017 Fortiortech shenzhen *****************************
   2          * File Name          : AddFunction.c
   3          * Author             : Fortiortech  Appliction Team
   4          * Version            : V1.0
   5          * Date               : 2017-12-26
   6          * Description        : This file contains all the add function used for Motor Control.
   7          ***************************************************************************************************
   8          * All Rights Reserved
   9          **************************************************************************************************/
  10          
  11          
  12          /* Includes -------------------------------------------------------------------------------------*/
  13          // #include <AddFunction.h>
  14          #include <FU68xx_2.h>
  15          #include <Myproject.h>
  16          
  17          /* Private variables ---------------------------------------------------------*/
  18          FaultStateType             mcFaultSource;
  19          PWMINPUTCAL        idata   mcPwmInput;
  20          FaultVarible       idata   mcFaultDect;
  21          MotorRSDTypeDef    idata   RSDDetect;
  22          CurrentVarible     idata   mcCurVarible;
  23          ProtectVarible     idata   mcProtectTime;
  24          
  25          OUTLOOP            xdata   SpeedPICtrl;
  26          FOCCTRL            xdata   mcFocCtrl;
  27          ADCSample          xdata   AdcSampleValue;
  28          ONVarible          xdata   ONOFFTest;
  29          MCLedDisplay       xdata   mcLedDisplay;
  30          MCRAMP             xdata   mcSpeedRamp;
  31          SLEEPMODE          xdata   SleepSet;
  32          MotorFRTypeDef     xdata   mcFRState;
  33          int16              xdata   VSP;
  34          
  35          const int16  PowerGiven[10] ={0,500,1000,2200,2900,4300,5900,7300,7900,8550};
  36          
  37          /*---------------------------------------------------------------------------*/
  38          /* Name     :   void OutLoopParameterSet(void)
  39          /* Input    :   NO
  40          /* Output   :   NO
  41          /* Description: 
  42          /*---------------------------------------------------------------------------*/
  43          void OutLoopParameterSet(void)
  44          {
  45   1        memset(&SpeedPICtrl,0, sizeof(OUTLOOP));                                // SpeedControl clear
  46   1      
  47   1        mcFocCtrl.SpeedLoopTime = SPEED_LOOP_TIME;
  48   1        mcFocCtrl.SpeedRampTime = SPEEDRAMPTIME;
  49   1        
  50   1        mcSpeedRamp.IncValue    = SPEEDRAMPSTARTINC;
  51   1        mcSpeedRamp.DecValue    = SPEEDRAMPSTARTDEC;
  52   1      
  53   1        SpeedPICtrl.ExtKP       = SKP;
C51 COMPILER V9.52.0.0   ADDFUNCTION                                                       09/21/2018 09:06:09 PAGE 2   

  54   1        SpeedPICtrl.ExtKI       = SKI;
  55   1        SpeedPICtrl.ExtOutMax   = SOUTMAX;
  56   1        SpeedPICtrl.ExtOutMin   = SOUTMIN;
  57   1        
  58   1        PI_KP     = SpeedPICtrl.ExtKP;
  59   1        PI_KI     = SpeedPICtrl.ExtKI;
  60   1        PI_UKMAX  = SpeedPICtrl.ExtOutMax;
  61   1        PI_UKMIN  = SpeedPICtrl.ExtOutMin;
  62   1        
  63   1        PI_UK     = IQ_RUN_CURRENT;
  64   1      }
  65          /*---------------------------------------------------------------------------*/
  66          /* Name   : int16 KLPF_VALUE(int16 INVlaue, int16 OutLastValue)
  67          /* Input  : INVlaue，OutLastValue
  68          /* Output : int16的变量
  69          /* Description: 滤波函数,用乘法器做的
  70          /*---------------------------------------------------------------------------*/
  71          int16 KLPF_VALUE(int16 INVlaue, int16 OutLastValue)
  72          {
  73   1        int16 Result = 0;
  74   1        MDU_MA = (INVlaue-OutLastValue);
  75   1        MDU_MB = (int16)480;                    /*写被乘数和乘数*/
  76   1      
  77   1        Result = MDU_MB;
  78   1        Result += OutLastValue;
  79   1        return(Result);
  80   1      }
  81          
  82          /*---------------------------------------------------------------------------*/
  83          /* Name   : void FaultProcess(void)
  84          /* Input  : NO
  85          /* Output : NO
  86          /* Description: 保护处理函数，关闭FOC输出，同时将状态变为mcFault
  87          /*---------------------------------------------------------------------------*/
  88          void FaultProcess(void)
  89          {
  90   1        MOE     = 0;
  91   1        ClrBit(DRV_CR, FOCEN);  //关闭FOC
  92   1        mcState = mcFault;
  93   1      
  94   1      }
  95          
  96          /*---------------------------------------------------------------------------*/
  97          /* Name   : int16 Abs_F16(int16 value)
  98          /* Input  : value
  99          /* Output : int16
 100          /* Description: 对变量取16位的绝对值
 101          /*---------------------------------------------------------------------------*/
 102          uint16 Abs_F16(int16 value)
 103          {
 104   1        if(value < 0)
 105   1        {
 106   2          return (- value);
 107   2        }
 108   1        else
 109   1        {
 110   2          return (value);
 111   2        }
 112   1      }
 113          /*---------------------------------------------------------------------------*/
 114          /* Name   : int32 Abs_F32(int32 value)
 115          /* Input  : value
C51 COMPILER V9.52.0.0   ADDFUNCTION                                                       09/21/2018 09:06:09 PAGE 3   

 116          /* Output : int16
 117          /* Description: 对变量取16位的绝对值
 118          /*---------------------------------------------------------------------------*/
 119          uint32 Abs_F32(int32 value)
 120          {
 121   1        if(value < 0)
 122   1        {
 123   2          return (- value);
 124   2        }
 125   1        else
 126   1        {
 127   2          return (value);
 128   2        }
 129   1      }
 130          /*---------------------------------------------------------------------------*/
 131          /* Name   : void APP_DIV(void)
 132          /* Input  : void
 133          /* Output : void
 134          /* Description: 将所有用到除法的地方，放在同一个中断，以避免中断串扰
 135          /*---------------------------------------------------------------------------*/
 136          void APP_DIV(void)
 137          {
 138   1          if( mcPwmInput.PWMDivFlag==1)  //启动除法器，避免与过调值中的除法冲突
 139   1          {
 140   2             mcPwmInput.PWMDuty = MDU_DIV_IDATA_U32(&mcPwmInput.pwm.PWMCompareAMP, &mcPwmInput.PWMARRUpdate);
 141   2             mcPwmInput.PWMDivFlag=0;
 142   2          }
 143   1          if( mcFocCtrl.ESDIVFlag==1)  //启动除法器，避免与过调值中的除法冲突
 144   1          {
 145   2             mcFocCtrl.SQUSpeedDIVEs = MDU_DIV_XDATA_U32(&mcFocCtrl.SQUSysSpeed,&mcFocCtrl.EsValue);
 146   2             mcFocCtrl.ESDIVFlag=0;
 147   2          }
 148   1      }
 149          /*---------------------------------------------------------------------------*/
 150          /* Name   : void PWMInputCapture(void)
 151          /* Input  : NO
 152          /* Output : NO
 153          /* Description: 输入PWM处理
 154          /*---------------------------------------------------------------------------*/
 155          void PWMInputCapture(void)
 156          {
 157   1        uint16 MotorControlVSP;
 158   1      
 159   1         if(mcPwmInput.PWMUpdateFlag==1)  // 有新的duty更新
 160   1         {
 161   2            if((Abs_F32(mcPwmInput.PWMCompare-mcPwmInput.PWMCompareOld)<50)// 两次比较值相近，减少读�
             -��率
 162   2              &&(Abs_F32(mcPwmInput.PWMARROld-mcPwmInput.PWMARR)<50)// 两次周期值相近，减少读错率
 163   2      //        &&((100<mcPwmInput.PWMARR)&&(mcPwmInput.PWMARR<6000))// 周期值在一定范围内才认为有�
             -��，即一定频率范围
 164   2              &&(mcPwmInput.PWMDivFlag==0))
 165   2              {
 166   3                mcPwmInput.PWMFlag=1;                               // PWMFlag置1期间，不读取TIM3__DR和T
             -IM3__ARR;，防止出错
 167   3                mcPwmInput.pwm.PWMCompareUpdate[0]=(mcPwmInput.PWMCompare>>1);// 对其乘以32768
 168   3                mcPwmInput.PWMARRUpdate=mcPwmInput.PWMARR;
 169   3                mcPwmInput.PWMDivFlag=1;                            // 启动除法
 170   3                mcPwmInput.PWMFlag=0;
 171   3              }
 172   2             if(mcPwmInput.PWMDivFlag==0)
 173   2              {
 174   3                if(mcPwmInput.PWMcnt<3)//2次求平均值
C51 COMPILER V9.52.0.0   ADDFUNCTION                                                       09/21/2018 09:06:09 PAGE 4   

 175   3                {
 176   4                  mcPwmInput.PWMcnt++;
 177   4                  mcPwmInput.PWMVSum +=mcPwmInput.PWMDuty;
 178   4                }
 179   3                else
 180   3                {
 181   4                  MotorControlVSP= (mcPwmInput.PWMVSum >>1);//注意其有一个右移与前面的比较值标�
             -�化有关
 182   4                  mcPwmInput.PWMVSum=0;
 183   4                  mcPwmInput.PWMcnt =0;
 184   4                }
 185   3                 MotorControlVSP=mcPwmInput.PWMDuty;
 186   3               if((MotorControlVSP > ONPWMDuty)&&(MotorControlVSP <=(OFFPWMDutyHigh+1)))
 187   3                {
 188   4                  mcSpeedRamp.FlagONOFF = 1;
 189   4                }
 190   3               else if(MotorControlVSP < OFFPWMDuty)//||(MotorControlVSP >= OFFPWMDutyHigh))
 191   3                {
 192   4                  mcSpeedRamp.FlagONOFF = 0;
 193   4                }
 194   3      
 195   3                //转速曲线计算
 196   3                if(mcSpeedRamp.FlagONOFF==1)
 197   3                {
 198   4                  if(MotorControlVSP <= MINPWMDuty)
 199   4                  {
 200   5                    mcSpeedRamp.TargetValue = Motor_Min_Speed;
 201   5                  }
 202   4                  else if(MotorControlVSP < MAXPWMDuty)
 203   4                  {
 204   5                    mcSpeedRamp.TargetValue = Motor_Min_Speed + SPEED_K*(MotorControlVSP-MINPWMDuty);
 205   5                  }
 206   4                  else
 207   4                  {
 208   5                    mcSpeedRamp.TargetValue = Motor_Max_Speed;
 209   5                  }
 210   4                }
 211   3                else
 212   3                {
 213   4                  mcSpeedRamp.TargetValue =0;
 214   4                }
 215   3              }
 216   2      
 217   2            mcPwmInput.PWMUpdateFlag =0;
 218   2            mcPwmInput.PWMCompareOld=mcPwmInput.PWMCompare;//将此次比较值赋值给上次比较值
 219   2            mcPwmInput.PWMARROld=mcPwmInput.PWMARR;//将此次周期值赋值给上次周期值
 220   2          }
 221   1      }
 222          /*****************************************************************************
 223           * Function:     void Fault_OverVoltage(mcFaultVarible *h_Fault)
 224           * Description:  过压欠压保护函数：程序每5ms判断一次，母线电压大于过压保护值�
             -�，计数器加一，计数器值超过20次，判断为过压保护，关闭输出;反之，计数器慢慢减
 225                           同理，欠压保护。
 226                           电机过欠压保护状态下，母线电压恢复到欠压恢复值以上，过压恢复
             -值以下时，计数器加一，超过200次后，恢复。根据档位信息来决定恢复到哪个状态。
 227           * Parameter:    mcFaultVarible *h_Fault
 228           * Return:       no
 229           *****************************************************************************/
 230          void Fault_OverUnderVoltage(FaultVarible *h_Fault)
 231          {
 232   1        //过压保护
 233   1          if(mcFaultSource == FaultNoSource)//程序无其他保护下
C51 COMPILER V9.52.0.0   ADDFUNCTION                                                       09/21/2018 09:06:09 PAGE 5   

 234   1          {
 235   2              if(mcFocCtrl.mcDcbusFlt > OVER_PROTECT_VALUE)  //母线电压大于过压保护值时，计数，超�
             -�20次，判断为过压保护，关闭输出;反之，计数器慢慢减
 236   2              {
 237   3                h_Fault->OverVoltDetecCnt++;
 238   3                if(h_Fault->OverVoltDetecCnt > 20)//检测100ms
 239   3                {
 240   4                  h_Fault->OverVoltDetecCnt = 0;
 241   4                  mcFaultSource=FaultOverVoltage;
 242   4                  FaultProcess();
 243   4                }
 244   3              }
 245   2              else
 246   2              {
 247   3                if(h_Fault->OverVoltDetecCnt>0)
 248   3                {
 249   4                  h_Fault->OverVoltDetecCnt--;
 250   4                }
 251   3              }
 252   2      
 253   2            //欠压保护
 254   2      
 255   2              if(mcFocCtrl.mcDcbusFlt< UNDER_PROTECT_VALUE)
 256   2              {
 257   3                h_Fault->UnderVoltDetecCnt++;
 258   3      
 259   3                if(h_Fault->UnderVoltDetecCnt > 20)//检测100ms
 260   3                {
 261   4                  h_Fault->UnderVoltDetecCnt = 0;
 262   4                  mcFaultSource=FaultUnderVoltage;
 263   4                  FaultProcess();
 264   4                }
 265   3              }
 266   2              else
 267   2              {
 268   3                if(h_Fault->UnderVoltDetecCnt>0)
 269   3                {
 270   4                  h_Fault->UnderVoltDetecCnt--;
 271   4                }
 272   3              }
 273   2          }
 274   1      
 275   1          /*******过压欠压保护恢复*********/
 276   1          if((mcState == mcFault) &&((mcFaultSource==FaultUnderVoltage)||(mcFaultSource==FaultOverVoltage)))
 277   1          {
 278   2            if((mcFocCtrl.mcDcbusFlt< OVER_RECOVER_VALUE)&&(mcFocCtrl.mcDcbusFlt> UNDER_RECOVER_VALUE))
 279   2            {
 280   3              h_Fault->VoltRecoverCnt++;
 281   3              if(h_Fault->VoltRecoverCnt>200)//连续检测1s，若正常则恢复
 282   3              {
 283   4                mcState = mcReady;
 284   4                mcFaultSource=FaultNoSource;
 285   4                h_Fault->VoltRecoverCnt = 0;
 286   4              }
 287   3            }
 288   2            else
 289   2            {
 290   3              h_Fault->VoltRecoverCnt = 0;
 291   3            }
 292   2         }
 293   1      }
 294          
C51 COMPILER V9.52.0.0   ADDFUNCTION                                                       09/21/2018 09:06:09 PAGE 6   

 295          
 296          /*****************************************************************************
 297           * Function:     void Fault_Power(mcFaultVarible *h_Fault)
 298           * Description:  功率保护函数
 299           * Parameter:    mcFaultVarible *h_Fault
 300           * Return:       no
 301           *****************************************************************************/
 302          void Fault_Power(FaultVarible *h_Fault)
 303          {
 304   1      
 305   1          if(mcFaultSource == FaultNoSource)//程序无其他保护下
 306   1          {
 307   2              if(mcFocCtrl.Powerlpf > PowerLimit)  //功率大于保护值时计数，超过20次，判断为过载�
             -��护，关闭输出;反之，计数器慢慢减
 308   2              {
 309   3                h_Fault->OverPowerDetecCnt++;
 310   3                if(h_Fault->OverPowerDetecCnt > 20)
 311   3                {
 312   4                  h_Fault->OverPowerDetecCnt = 0;
 313   4                  mcFaultSource=FaultOverPower;
 314   4                  FaultProcess();
 315   4                }
 316   3              }
 317   2              else
 318   2              {
 319   3                if(h_Fault->OverPowerDetecCnt>0)
 320   3                {
 321   4                  h_Fault->OverPowerDetecCnt--;
 322   4                }
 323   3              }
 324   2          }
 325   1      
 326   1        }
 327          
 328          
 329          /*****************************************************************************
 330           * Function:     void Fault_Overcurrent(CurrentVarible *h_Cur)
 331           * Description:  电机运行或者启动时，当三相中某一相最大值大于OverCurrentValue，则O
             -verCurCnt加1。
 332                           连续累加3次，判断为软件过流保护。执行时间约30.4us。
 333           * Parameter:    mcFaultVarible *h_Fault
 334           * Return:       no
 335           *****************************************************************************/
 336          void Fault_Overcurrent(CurrentVarible *h_Cur)
 337          {
 338   1        if((mcState == mcRun)||(mcState == mcStart))            // check over current in rum and open mode
 339   1        {
 340   2      
 341   2          h_Cur->Abs_ia = Abs_F16(FOC__IA);
 342   2          h_Cur->Abs_ib = Abs_F16(FOC__IB);
 343   2          h_Cur->Abs_ic = Abs_F16(FOC__IC);
 344   2          if(h_Cur->Abs_ia> h_Cur->Max_ia)                      // 此部分既用于软件过流保护，又用�
             -�缺相保护
 345   2          {
 346   3             h_Cur->Max_ia = h_Cur->Abs_ia;
 347   3          }
 348   2          if(h_Cur->Abs_ib > h_Cur->Max_ib)
 349   2          {
 350   3             h_Cur->Max_ib = h_Cur->Abs_ib;
 351   3          }
 352   2          if(h_Cur->Abs_ic > h_Cur->Max_ic)
 353   2          {
C51 COMPILER V9.52.0.0   ADDFUNCTION                                                       09/21/2018 09:06:09 PAGE 7   

 354   3             h_Cur->Max_ic = h_Cur->Abs_ic;
 355   3          }
 356   2      
 357   2          if((h_Cur->Max_ia>=OverSoftCurrentValue)||(h_Cur->Max_ib>=OverSoftCurrentValue)||(h_Cur->Max_ic>=OverSof
             -tCurrentValue))
 358   2          {
 359   3            h_Cur->OverCurCnt++;
 360   3            if(h_Cur->OverCurCnt>=3)
 361   3            {
 362   4              GP36=~GP36;
 363   4              h_Cur->Max_ia=0;
 364   4              h_Cur->Max_ib=0;
 365   4              h_Cur->Max_ic=0;
 366   4              h_Cur->OverCurCnt=0;
 367   4              mcFaultSource=FaultSoftOVCurrent;
 368   4              FaultProcess();
 369   4            }
 370   3          }
 371   2          else
 372   2          {
 373   3            if(h_Cur->OverCurCnt>0)
 374   3            {
 375   4              h_Cur->OverCurCnt--;
 376   4            }
 377   3          }
 378   2        }
 379   1      }
 380          
 381          
 382          /*****************************************************************************
 383           * Function:     void Fault_OverCurrentRecover(mcFaultVarible *h_Fault)
 384           * Description:  软硬件过流保护恢复
 385           * Parameter:    mcFaultVarible *h_Fault
 386           * Return:       no
 387           *****************************************************************************/
 388          void Fault_OverCurrentRecover(FaultVarible *h_Fault)
 389          {
 390   1        if((mcState == mcFault)&&((mcFaultSource==FaultSoftOVCurrent)||(mcFaultSource==FaultHardOVCurrent))&&(mcP
             -rotectTime.CurrentPretectTimes<5))
 391   1        {
 392   2          h_Fault->CurrentRecoverCnt++;
 393   2          if(h_Fault->CurrentRecoverCnt>=OverCurrentRecoverTime)//1000*5=5s
 394   2          {
 395   3            h_Fault->CurrentRecoverCnt=0;
 396   3            mcProtectTime.CurrentPretectTimes++;
 397   3            mcState = mcReady;
 398   3            mcFaultSource=FaultNoSource;
 399   3          }
 400   2        }
 401   1      }
 402          
 403          /*****************************************************************************
 404           * Function:     void Fault_OverPowerRecover(mcFaultVarible *h_Fault)
 405           * Description:  功率保护恢复函数
 406           * Parameter:    mcFaultVarible *h_Fault
 407           * Return:       no
 408           *****************************************************************************/
 409          void Fault_OverPowerRecover(FaultVarible *h_Fault)
 410          {
 411   1        if((mcState == mcFault)&&(mcFaultSource==FaultOverPower)&&(mcProtectTime.PowerPretectTimes<5))
 412   1        {
 413   2          h_Fault->OverPowerDetecCnt++;
C51 COMPILER V9.52.0.0   ADDFUNCTION                                                       09/21/2018 09:06:09 PAGE 8   

 414   2          if(h_Fault->OverPowerDetecCnt>=OverPowerRecoverTime)
 415   2          {
 416   3            h_Fault->OverPowerDetecCnt=0;
 417   3            mcProtectTime.PowerPretectTimes++;
 418   3            mcState = mcReady;
 419   3            mcFaultSource=FaultNoSource;
 420   3          }
 421   2        }
 422   1      }
 423          
 424          
 425          
 426          /*****************************************************************************
 427           * Function:     void Fault_Start(mcFaultVarible *h_Fault)
 428           * Description:  启动保护函数，电机运行状态下，电机在前5s估算转速达到堵转保护
             -值或者5s后反电动势值太低(此方法未验证)
 429                            或4s内还在CtrlMode状态，即速度低于MOTOR_LOOP_RPM，程序判断为启动失�
             -��，电机停机。
 430                            当程序判断为启动失败后，若重启次数少于或等于5次，程序立即进
             -入校准状态，等待重启。
 431           * Parameter:    mcFaultVarible *h_Fault
 432           * Return:       no
 433           *****************************************************************************/
 434            void Fault_Start(FaultVarible *h_Fault)
 435            {
 436   1          /*******启动保护恢复*********/
 437   1      //    h_Fault->mcEsValue = FOC__ESQU;
 438   1      
 439   1          if(mcState == mcRun)
 440   1          {
 441   2            //方法一，5s内速度大于最大速度，同时反电动势值低于一定值
 442   2            if(h_Fault->StartSpeedCnt<=1000)
 443   2            {
 444   3              h_Fault->StartSpeedCnt++;
 445   3              if((mcFocCtrl.SpeedFlt > Motor_Max_Speed)&&(mcFocCtrl.EsValue<50))
 446   3              {
 447   4                h_Fault->StartSpeedCnt = 0;
 448   4                mcFaultSource=FaultStart;
 449   4                FaultProcess();
 450   4                mcProtectTime.SecondStartTimes++;
 451   4                mcProtectTime.StartFlag  =  1;
 452   4              }
 453   3            }
 454   2           //方法二
 455   2            if(h_Fault->StartEsCnt<=1200)//前6s，等待1.5s后，开始判断Es，如果超过一定次数，则�
             -��败
 456   2            {
 457   3              h_Fault->StartEsCnt++;
 458   3              h_Fault->StartDelay++;
 459   3              if(h_Fault->StartDelay>=300)        // 1.5s
 460   3              {
 461   4                 h_Fault->StartDelay=300;
 462   4                 if((mcFocCtrl.EsValue <20))//&&(mcFocCtrl.CtrlMode==0))
 463   4                  {
 464   5                    h_Fault->StartESCount++;
 465   5                    if(h_Fault->StartESCount>=50)     //50次Es小于设定值，启动失败
 466   5                    {
 467   6                      mcFaultSource=FaultStart;
 468   6                      FaultProcess();
 469   6                      mcProtectTime.SecondStartTimes++;
 470   6                      h_Fault->StartDelay=0;
 471   6                      h_Fault->StartESCount=0;
C51 COMPILER V9.52.0.0   ADDFUNCTION                                                       09/21/2018 09:06:09 PAGE 9   

 472   6                      mcProtectTime.StartFlag  =  2;
 473   6                    }
 474   5                  }
 475   4                  else
 476   4                  {
 477   5                    if(h_Fault->StartESCount>0)
 478   5                      h_Fault->StartESCount--;
 479   5                  }
 480   4             }
 481   3           }
 482   2           else
 483   2           {
 484   3             h_Fault->StartESCount=0;
 485   3           }
 486   2           
 487   2           
 488   2            //方法三，长时间在CtrlMode=0状态
 489   2            if(mcFocCtrl.CtrlMode==0)         
 490   2            {
 491   3              h_Fault->StartFocmode++;
 492   3              if(h_Fault->StartFocmode>=8000)   //在MODE0模式超过8s起动失败
 493   3              {
 494   4                h_Fault->StartFocmode=0;
 495   4                mcFaultSource=FaultStart;
 496   4                FaultProcess();
 497   4                mcProtectTime.SecondStartTimes++;
 498   4                mcProtectTime.StartFlag  =  3;
 499   4              }
 500   3            }
 501   2            
 502   2          }
 503   1          
 504   1          //起动保护恢复
 505   1       #if (!StartONOFF_Enable)    //起停测试时屏蔽起动保护恢复
 506   1          {
 507   2           if((mcFaultSource==FaultStart)&&(mcState == mcFault)&&(mcProtectTime.SecondStartTimes<=StartProtectRest
             -artTimes))
 508   2           {
 509   3             
 510   3             h_Fault->StartRecoverDelayCnt++;
 511   3             if(h_Fault->StartRecoverDelayCnt > StartRecoverDelayTimes)   //启动保护延时恢复时间
 512   3             {
 513   4                h_Fault->StartRecoverDelayCnt = 0;
 514   4                mcFaultSource=FaultNoSource;
 515   4                mcState = mcInit;
 516   4             }
 517   3           }
 518   2          }
 519   1        #endif
 520   1          
 521   1        }
 522          
 523           
 524          
 525          
 526           /*****************************************************************************
 527           * Function:     void Fault_Stall(mcFaultVarible *h_Fault)
 528           * Description:  堵转保护函数，有三种保护方式，
 529                           第一种，
 530                           第二种，电机运行状态下，延迟4s判断，估算速度绝对值超过堵转速�
             -��连续5次；
 531                           第三种，电机运行状态下，当U,V两相电流绝对值大于堵转电流保护值
C51 COMPILER V9.52.0.0   ADDFUNCTION                                                       09/21/2018 09:06:09 PAGE 10  

             -连续6次；
 532                           当以上三种的任何一种保护触发时，电机停机，程序判断为堵转保护
             -；
 533                           当堵转保护状态下，U相采集值低于堵转恢复值时，若堵转次数小于�
             -�等于堵转重启次数8次，
 534                           程序延迟mcStallRecover重新启动，进行校准状态。
 535           * Parameter:    mcFaultVarible *h_Fault
 536           * Return:       no
 537           *****************************************************************************/
 538          //堵转保护
 539          void Fault_Stall(FaultVarible *h_Fault,CurrentVarible *h_Cur)
 540          {
 541   1      //  h_Fault->mcEsValue = FOC__ESQU;
 542   1        if(mcState == mcRun)
 543   1        {
 544   2          if(h_Fault->StallDelayCnt <=1000)// 启动后延时5s判断是否堵转保护
 545   2          {
 546   3            h_Fault->StallDelayCnt ++;
 547   3          }
 548   2          else
 549   2          {
 550   3            //method 1，当反电动势太小 或当 转速太大但反电动势却很小时进入保护
 551   3            if((&mcFocCtrl.EsValue< 20)||((FOC__EOME > _Q15(1200.0/MOTOR_SPEED_BASE))&&(mcFocCtrl.EsValue< 50)))
 552   3            {
 553   4              h_Fault->StallDectEs++;
 554   4              if(h_Fault->StallDectEs >= 50)   //判断满足条件达到设置次数，保护动作
 555   4              {
 556   5                h_Fault->StallDectEs=0;
 557   5                mcFaultSource=FaultStall;
 558   5                mcProtectTime.StallTimes++;        //堵转次数+1
 559   5                FaultProcess();
 560   5                h_Fault->StallDelayCnt = 0;        //保护后重启开始重新计数，保持5S延迟
 561   5                mcProtectTime.StallFlag  =  1;
 562   5              }
 563   4            }
 564   3            else
 565   3            {
 566   4              if( h_Fault->StallDectEs>0)
 567   4                h_Fault->StallDectEs--;
 568   4            }
 569   3            
 570   3            //method 2，判断速度低于堵转最小值或者超过堵转最大值
 571   3            if((mcFocCtrl.SpeedFlt<Motor_Stall_Min_Speed)||(mcFocCtrl.SpeedFlt > Motor_Stall_Max_Speed))
 572   3            {
 573   4              h_Fault->StallDectSpeed++;
 574   4              if(h_Fault->StallDectSpeed >= 20)   //判断满足条件达到设置次数，保护动作
 575   4              {
 576   5                h_Fault->StallDectSpeed=0;
 577   5                mcFaultSource=FaultStall;
 578   5                mcProtectTime.StallTimes++;       //堵转次数+1
 579   5                FaultProcess();
 580   5                h_Fault->StallDelayCnt = 0;       //保护后重启开始重新计数，保持5S延迟
 581   5                mcProtectTime.StallFlag =2;
 582   5              }
 583   4            }
 584   3            else
 585   3            {
 586   4              if(h_Fault->StallDectSpeed>0)
 587   4              h_Fault->StallDectSpeed--;
 588   4            }
 589   3            
 590   3            //method 3      A/B/C三相任意一相相电流大于堵转电流，堵转保护
C51 COMPILER V9.52.0.0   ADDFUNCTION                                                       09/21/2018 09:06:09 PAGE 11  

 591   3          if((h_Cur->Max_ia >= StallCurrentValue)||
 592   3             (h_Cur->Max_ib >= StallCurrentValue)||
 593   3             (h_Cur->Max_ic >= StallCurrentValue))
 594   3          {      
 595   4            h_Fault->mcStallDeCurrent++;              
 596   4            if(h_Fault->mcStallDeCurrent >= 20)         //判断满足条件达到设置次数，保护动作
 597   4            {
 598   5              h_Fault->mcStallDeCurrent=0;
 599   5              mcFaultSource=FaultStall; 
 600   5              mcProtectTime.StallTimes++;
 601   5              FaultProcess();   
 602   5              h_Fault->StallDelayCnt = 0;       //保护后重启开始重新计数，保持5S延迟
 603   5              mcProtectTime.StallFlag  =  3;
 604   5            }            
 605   4          } 
 606   3          else
 607   3            {
 608   4              if(h_Fault->mcStallDeCurrent>0)
 609   4              h_Fault->mcStallDeCurrent--;
 610   4            }
 611   3        }
 612   2      }
 613   1         
 614   1       #if (!StartONOFF_Enable)
 615   1      {
 616   2           /*******堵转保护恢复*********/
 617   2              if((mcFaultSource==FaultStall)&&(mcState == mcFault)&&(mcProtectTime.StallTimes<=4))  //堵转重�
             -��次数
 618   2              {
 619   3                h_Fault->StallReCount++;
 620   3                if(h_Fault->StallReCount>=StallRecoverTime)
 621   3                {
 622   4                  h_Fault->StallReCount=0;        //16000
 623   4                  mcFaultSource=FaultNoSource;
 624   4                  mcState =   mcInit;
 625   4                }
 626   3              }
 627   2              else
 628   2              {
 629   3                h_Fault->StallReCount=0;
 630   3              }
 631   2      }
 632   1      #endif
 633   1          
 634   1      }
 635           /*****************************************************************************
 636           * Function:     void Fault_phaseloss(mcFaultVarible *h_Fault)
 637           * Description:  缺相保护函数，当电机运行状态下，10ms取三相电流的最大值，
 638                           1.5s判断各相电流最大值，若存在两相电流值大于一定值，而第三相�
             -�流值却非常小，则判断为缺相保护，电机停机；
 639           * Parameter:    mcFaultVarible *h_Fault
 640           * Return:       no
 641           *****************************************************************************/
 642            void Fault_phaseloss(FaultVarible *h_Fault)
 643            {
 644   1            if(mcState == mcRun)
 645   1           {
 646   2              h_Fault->Lphasecnt++;
 647   2              if(h_Fault->Lphasecnt>100)//100*5=500ms  500ms判断一次
 648   2              {
 649   3                 h_Fault->Lphasecnt=0;
 650   3                 if(((mcCurVarible.Max_ia>(mcCurVarible.Max_ib*3))||(mcCurVarible.Max_ia>(mcCurVarible.Max_ic*3)))&&(
C51 COMPILER V9.52.0.0   ADDFUNCTION                                                       09/21/2018 09:06:09 PAGE 12  

             -mcCurVarible.Max_ia>PhaseLossCurrentValue))
 651   3                 {
 652   4                    h_Fault->AOpencnt++;
 653   4                 }
 654   3                 else
 655   3                 {
 656   4                  if(h_Fault->AOpencnt>0)
 657   4                    h_Fault->AOpencnt --;
 658   4                 }
 659   3                 if(((mcCurVarible.Max_ib >(mcCurVarible.Max_ia*3))||(mcCurVarible.Max_ib >(mcCurVarible.Max_ic*3)))&
             -&(mcCurVarible.Max_ib >PhaseLossCurrentValue))
 660   3                 {
 661   4                   h_Fault->BOpencnt++;
 662   4                 }
 663   3                else
 664   3                 {
 665   4                   if(h_Fault->BOpencnt>0)
 666   4                    h_Fault->BOpencnt --;
 667   4                 }
 668   3                 if(((mcCurVarible.Max_ic >(mcCurVarible.Max_ia*3))||(mcCurVarible.Max_ic >(mcCurVarible.Max_ib*3)))&
             -&(mcCurVarible.Max_ic >PhaseLossCurrentValue))
 669   3                 {
 670   4                   h_Fault->COpencnt++;
 671   4                 }
 672   3                else
 673   3                 {
 674   4                   if(h_Fault->COpencnt>0)
 675   4                    h_Fault->COpencnt --;
 676   4                 }
 677   3                  mcCurVarible.Max_ia = 0;
 678   3                  mcCurVarible.Max_ib = 0;
 679   3                  mcCurVarible.Max_ic = 0;
 680   3                if(h_Fault->AOpencnt > 1|| h_Fault->BOpencnt > 1 || h_Fault->COpencnt > 1)
 681   3                 {
 682   4                    mcProtectTime.LossPHTimes++;
 683   4                    mcFaultSource=FaultLossPhase;
 684   4                    FaultProcess();
 685   4                 }
 686   3              }
 687   2            }
 688   1      
 689   1           /*******缺相保护恢复*********/
 690   1      #if (!StartONOFF_Enable)           //起停测试时屏蔽缺相保护恢复
 691   1      {
 692   2            if((mcFaultSource==FaultLossPhase)&&(mcState == mcFault)&&(mcProtectTime.LossPHTimes<5))//可重启5
             -次
 693   2             {
 694   3                 h_Fault->mcLossPHRecCount++;
 695   3                 if(h_Fault->mcLossPHRecCount>=PhaseLossRecoverTime)
 696   3                 {
 697   4                   h_Fault->AOpencnt=0;
 698   4                   h_Fault->BOpencnt=0;
 699   4                   h_Fault->COpencnt=0;
 700   4                   mcState = mcReady;
 701   4                   mcFaultSource=FaultNoSource;
 702   4                 }
 703   3             }
 704   2          else
 705   2             {
 706   3                 h_Fault->mcLossPHRecCount=0;
 707   3             }
 708   2      }
C51 COMPILER V9.52.0.0   ADDFUNCTION                                                       09/21/2018 09:06:09 PAGE 13  

 709   1      #endif
 710   1                 
 711   1          }
 712          
 713          
 714          
 715          /*---------------------------------------------------------------------------*/
 716          /* Name   : void Fault_Detection(void)
 717          /* Input  : NO
 718          /* Output : NO
 719          /* Description: 保护函数，因保护的时间响应不会很高，采用分段处理，每5个定时�
             -�中断执行一次对应的保护
 720                          常见保护有过欠压、过温、堵转、启动、缺相等保护，调试时，可根�
             -��需求，一个个的调试加入。
 721          /*---------------------------------------------------------------------------*/
 722          void Fault_Detection(void)
 723          {
 724   1          mcFaultDect.segment++;
 725   1          if(mcFaultDect.segment>=5)
 726   1          {
 727   2            mcFaultDect.segment=0;
 728   2          }
 729   1          if(mcFaultDect.segment==0)
 730   1          {
 731   2            if(CurrentRecoverEnable)//过流保护恢复使能
 732   2            {
 733   3              Fault_OverCurrentRecover(&mcFaultDect);
 734   3            }
 735   2            if(PowerRecoverEnable) //功率保护恢复使能
 736   2            {
 737   3              Fault_OverPowerRecover(&mcFaultDect);
 738   3            }
 739   2            
 740   2          }
 741   1          else if(mcFaultDect.segment==1)
 742   1          {
 743   2            if(VoltageProtectEnable==1)//过压保护使能
 744   2            {
 745   3              Fault_OverUnderVoltage(&mcFaultDect);
 746   3            }
 747   2            
 748   2            if(OverPowerProtectEnable==1) //功率保护使能
 749   2            {
 750   3              Fault_Power(&mcFaultDect);
 751   3            }       
 752   2          }
 753   1          else if(mcFaultDect.segment==2)
 754   1          {
 755   2            if(StartProtectEnable==1)//启动保护使能
 756   2            {
 757   3              Fault_Start(&mcFaultDect);
 758   3            }
 759   2          }
 760   1          else if(mcFaultDect.segment==3)
 761   1          {
 762   2            if(StallProtectEnable==1)//堵转保护使能
 763   2            {
 764   3              Fault_Stall(&mcFaultDect,&mcCurVarible);
 765   3            }
 766   2          }
 767   1          else if(mcFaultDect.segment==4)
 768   1          {
C51 COMPILER V9.52.0.0   ADDFUNCTION                                                       09/21/2018 09:06:09 PAGE 14  

 769   2            if(PhaseLossProtectEnable==1)//缺相保护使能
 770   2            {
 771   3              Fault_phaseloss(&mcFaultDect);
 772   3            }
 773   2            
 774   2          }
 775   1          else
 776   1          {
 777   2      
 778   2          }
 779   1      }
 780          
 781          /*---------------------------------------------------------------------------*/
 782          /* Name   : void ONOFF_Starttest(void)
 783          /* Input  : NO
 784          /* Output : NO
 785          /* Description: 启动测试
 786          /*---------------------------------------------------------------------------*/
 787          void ONOFF_Starttest(ONVarible  *h_test)
 788          {
 789   1        if(h_test->ONOFF_Flag==1)
 790   1        {
 791   2           h_test->ON_Count++;
 792   2          if(h_test->ON_Count>StartON_Time)
 793   2          {
 794   3            h_test->ON_Count=0;
 795   3            h_test->ONOFF_Times++;
 796   3            h_test->ONOFF_Flag=0;
 797   3            mcSpeedRamp.FlagONOFF = 0;
 798   3            mcSpeedRamp.TargetValue = 0;
 799   3      
 800   3          }
 801   2        }
 802   1        else
 803   1        {
 804   2          if(mcState!=mcFault)
 805   2          {
 806   3            h_test->OFF_Count++;
 807   3            if(h_test->OFF_Count>StartOFF_Time)
 808   3            {
 809   4              h_test->OFF_Count=0;
 810   4              h_test->ONOFF_Flag=1;
 811   4               mcSpeedRamp.FlagONOFF = 1;
 812   4               mcSpeedRamp.TargetValue = Motor_Min_Speed;
 813   4      
 814   4            }
 815   3          }
 816   2      
 817   2        }
 818   1      }
 819          
 820          /*---------------------------------------------------------------------------*/
 821          /* Name   : void Speed_response(void)
 822          /* Input  : NO
 823          /* Output : NO
 824          /* Description: 速度响应函数，可根据需求加入控制环，如恒转矩控制、恒转速控制
             -、恒功率控制
 825          /*---------------------------------------------------------------------------*/
 826          
 827          
 828          void Speed_response(void)
 829          {
C51 COMPILER V9.52.0.0   ADDFUNCTION                                                       09/21/2018 09:06:09 PAGE 15  

 830   1          uint16  UqMinTemp = 0;     //UQ限幅最小值缓存变量
 831   1        
 832   1          if((mcState ==mcRun)||(mcState ==mcStop))
 833   1          {
 834   2              if(mcFocCtrl.RunStateCnt < MotorStartHoldTime)                          // MotorStartHoldTime目标转速为启动�
             -��速
 835   2              {
 836   3                  mcFocCtrl.RunStateCnt++;
 837   3                  if(mcSpeedRamp.FlagONOFF == 1)                                      // PWM开机
 838   3                    mcSpeedRamp.TargetValue = Motor_Start_Hold_Speed;              //上油时间内维持上油转速
 839   3              }
 840   2              else                                                        //如果达到上油转速后，将起动速度环增量变为运行�
             -�度环增量
 841   2              {
 842   3                  if(mcSpeedRamp.IncValue == SpeedRampStartInc)       
 843   3                  {
 844   4                      mcSpeedRamp.IncValue  = SPEEDRAMPINC;             //运行速度环增量
 845   4                      mcSpeedRamp.DecValue  = SPEEDRAMPDEC;
 846   4                  }
 847   3                  #if (FRDetectMethod!=FOCMethod)                                     //延迟MotorStartHoldTime，用于清RSDCCWFlag,B
             -EMFCCWFlag
                          {
                              BEMFDetect.BEMFCCWFlag= 0;
                              RSDDetect.RSDCCWFlag  = 0;
                          }
                          #endif
 853   3              }
 854   2      
 855   2                
 856   2            switch(mcFocCtrl.CtrlMode)
 857   2            {
 858   3              case 0:
 859   3                {
 860   4                   if(Time.Start_CNT < 200)                       //前200ms维持参考电流
 861   4                    {
 862   5                      Time.Start_CNT ++;
 863   5                    }
 864   4                  else          
 865   4                  {
 866   5                    if(FOC__EOME > Motor_Loop_Speed)//>=MOTOR_LOOP_RPM
 867   5                      {
 868   6                        mcFocCtrl.CtrlMode = 1;
 869   6                        FOC_DQKP = DQKP;
 870   6                        FOC_DQKI = DQKI;
 871   6                        # if (Motor_Speed_Control_Mode == SPEED_LOOP_CONTROL)
 872   6                        {
 873   7      //                      mcSpeedRamp.ActualValue = Motor_Start_Hold_Speed;
 874   7                          if(FOC__EOME < _Q15(1800 / MOTOR_SPEED_BASE))
 875   7                          {
 876   8                              mcSpeedRamp.ActualValue = _Q15(1800 / MOTOR_SPEED_BASE);    //
 877   8                          }
 878   7                          else if(FOC__EOME > mcSpeedRamp.TargetValue)
 879   7                          {
 880   8                              mcSpeedRamp.ActualValue = mcSpeedRamp.TargetValue;
 881   8                          }
 882   7                          else
 883   7                          {
 884   8                              mcSpeedRamp.ActualValue = FOC__EOME;
 885   8                          }
 886   7                        }
 887   6                        #elif (Motor_Speed_Control_Mode == POWER_LOOP_CONTROL)
                                {
C51 COMPILER V9.52.0.0   ADDFUNCTION                                                       09/21/2018 09:06:09 PAGE 16  

                                  mcSpeedRamp.ActualValue = mcFocCtrl.Powerlpf;
                                }
                                #endif
 892   6      
 893   6                        OutLoopParameterSet();
 894   6        //              SpeedPICtrl.ExtOutL = mcFocCtrl.mcIqref;
 895   6        //              SpeedPICtrl.ExtErrL = mcSpeedRamp.ActualValue- mcFocCtrl.SpeedFlt;
 896   6                      }
 897   5                    else
 898   5                      {
 899   6                          UqMinTemp = FOC_QMIN;
 900   6                          if(UqMinTemp < QOUTMINSTEND)
 901   6                            {
 902   7                              UqMinTemp += 10 ;
 903   7                              FOC_QMIN = UqMinTemp;
 904   7                            }
 905   6                      }
 906   5                    }
 907   4                  }
 908   3                break;
 909   3                case 1:
 910   3                {
 911   4                              
 912   4                  #if (OUTLoop_Mode== OUTLoop_Disable)
                          {
                              mcFocCtrl.mcIqref = FOC_IQREF;
                              if (FOC_IQREF < QOUTVALUE)
                              {
                                  mcFocCtrl.mcIqref += QOUTINC;
                                  if (mcFocCtrl.mcIqref > QOUTVALUE) mcFocCtrl.mcIqref = QOUTVALUE;
                                  FOC_IQREF = mcFocCtrl.mcIqref;
                              }
                              else if (FOC_IQREF > QOUTVALUE)
                              {
                                  mcFocCtrl.mcIqref -= QOUTINC;
                                  if (mcFocCtrl.mcIqref < QOUTVALUE) mcFocCtrl.mcIqref = QOUTVALUE;
                                  FOC_IQREF = mcFocCtrl.mcIqref;
                              }
                          }
              
                          #elif (OUTLoop_Mode== OUTLoop_Enable)
 930   4                  {
 931   5                      mcFocCtrl.SpeedRampTime++;
 932   5                      if(mcFocCtrl.SpeedRampTime > SPEEDRAMPTIME)
 933   5                      {
 934   6                          mcFocCtrl.SpeedRampTime = 0;
 935   6                          mc_ramp(&mcSpeedRamp);       //外环速度爬坡或减速。每SpeedRampTime执行一次
 936   6                      }
 937   5                      
 938   5                      mcFocCtrl.SpeedLoopTime++;
 939   5                      if(mcFocCtrl.SpeedLoopTime > SPEED_LOOP_TIME)  //外环调节周期
 940   5                      {
 941   6                          mcFocCtrl.SpeedLoopTime=0;
 942   6      
 943   6                          #if (Motor_Speed_Control_Mode == SPEED_LOOP_CONTROL)
 944   6                          {             
 945   7                             SpeedPICtrl.ExtRef = mcSpeedRamp.ActualValue;
 946   7                             SpeedPICtrl.ExtFed = mcFocCtrl.SpeedFlt;
 947   7                             HW_PI_Control(&SpeedPICtrl);             //
 948   7                             FOC_IQREF = SpeedPICtrl.ExtOut;
 949   7                          }
 950   6                          #elif (Motor_Speed_Control_Mode == POWER_LOOP_CONTROL)
C51 COMPILER V9.52.0.0   ADDFUNCTION                                                       09/21/2018 09:06:09 PAGE 17  

                                  {
                                     FOC_IQREF= HW_One_PI(mcSpeedRamp.ActualValue- mcFocCtrl.Powerlpf);
                                  }
                                  #endif
 955   6                      }
 956   5                      /*******弱磁函数**********/
 957   5                      #if (FiledWeakenCompEnable == 1)
 958   5                      {
 959   6                        mcFieldWeakenComp();
 960   6                      }
 961   5                      #endif
 962   5                      
 963   5      
 964   5                // 判断FOC_QMIN值恢复到正常值
 965   5                UqMinTemp = FOC_QMIN;
 966   5                if(UqMinTemp > QOUTMIN)
 967   5                {
 968   6                  UqMinTemp -- ;
 969   6                  FOC_QMIN = UqMinTemp;
 970   6                } 
 971   5                      
 972   5                 }
 973   4                 #endif //END OUTLoop_Mode
 974   4      
 975   4      
 976   4                 if(FOC_EK2 < OBS_K2T_Actual-10)
 977   4                 {
 978   5                   mcFocCtrl.Smo_EK2=FOC_EK2;
 979   5                   mcFocCtrl.Smo_EK2+=10;
 980   5                   FOC_EK2=mcFocCtrl.Smo_EK2;
 981   5                 }
 982   4                 else if(FOC_EK2 > OBS_K2T_Actual+10)
 983   4                 {
 984   5                   mcFocCtrl.Smo_EK2=FOC_EK2;
 985   5                   mcFocCtrl.Smo_EK2-=10;
 986   5                   FOC_EK2=mcFocCtrl.Smo_EK2;
 987   5                 }
 988   4                 else
 989   4                 {
 990   5                   FOC_EK2=OBS_K2T_Actual;
 991   5                 }
 992   4                  
 993   4              }
 994   3              break;
 995   3            }
 996   2          }
 997   1      
 998   1      }
 999          
1000          
1001          /*---------------------------------------------------------------------------*/
1002          /* Name   : void FGOutput(void)
1003          /* Input  : NO
1004          /* Output : NO
1005          /* Description: FG信号输出
1006          /*---------------------------------------------------------------------------*/
1007          void FGOutput(void)
1008          {//FG可用中断FGIF进行判断
1009   1        if(mcState==mcRun)
1010   1        {
1011   2          if((FOC__THETA>=0)&&(FOC__THETA<32768))//0-180
1012   2          {
C51 COMPILER V9.52.0.0   ADDFUNCTION                                                       09/21/2018 09:06:09 PAGE 18  

1013   3            ResetFGPin;
1014   3          }
1015   2          else if((FOC__THETA>=32768)&&(FOC__THETA<65536))//180-360
1016   2          {
1017   3            SetFGPin;
1018   3          }
1019   2        }
1020   1        else if(mcState == mcFault)
1021   1        {
1022   2            SetFGPin;
1023   2        }
1024   1        else
1025   1        {
1026   2            ResetFGPin;
1027   2        }
1028   1      }
1029          /*---------------------------------------------------------------------------*/
1030          /* Name   :  uint16 SoftLPF(uint16 Xn1, uint16 Xn0, uint16 K)
1031          /* Input  : uint16 Xn1, uint16 Xn0, uint16 K
1032          /* Output : uint16
1033          /* Description: 软件低通滤波
1034          /*---------------------------------------------------------------------------*/
1035           int16 SoftLPF(int16 Xn1, int16 Xn0, int16 K)
1036           {
1037   1         int16 Temp16 = 0;
1038   1         int32 Temp32 = 0;
1039   1      
1040   1         Temp32 = (((int32)Xn1 - (int32)Xn0) * (int32)K) >> 15;
1041   1         Temp16 = Xn0 + (int16)Temp32;
1042   1         return Temp16;
1043   1       }
1044          
1045          
1046          /*---------------------------------------------------------------------------*/
1047          /* Name   : void LED_Display(void)
1048          /* Input  : NO
1049          /* Output : NO
1050          /* Description: LED灯显示
1051          /*---------------------------------------------------------------------------*/
1052          void LED_Display(void)
1053          {
1054   1        switch(mcFaultSource)
1055   1        {
1056   2          case FaultNoSource:
1057   2          SetLEDPin;                          //低电平点亮，高电平灭
1058   2            break;
1059   2          case FaultHardOVCurrent:             //硬件过流保护闪烁10次           
1060   2              Led_OnOff(&mcLedDisplay,10);
1061   2            break;
1062   2          case FaultSoftOVCurrent:            //软件过流保护闪烁4次           
1063   2              Led_OnOff(&mcLedDisplay,4);
1064   2            break;
1065   2          case FaultUnderVoltage:              //欠压保护闪烁2次
1066   2              Led_OnOff(&mcLedDisplay,2);
1067   2            break;
1068   2          case FaultOverVoltage:                //过压保护闪烁3次
1069   2              Led_OnOff(&mcLedDisplay,3);
1070   2            break;
1071   2          case FaultLossPhase:                  //缺相保护闪烁8次
1072   2              Led_OnOff(&mcLedDisplay,5);
1073   2            break;
1074   2          case FaultStall:                     //堵转保护闪烁6次
C51 COMPILER V9.52.0.0   ADDFUNCTION                                                       09/21/2018 09:06:09 PAGE 19  

1075   2              Led_OnOff(&mcLedDisplay,6);
1076   2            break;
1077   2          case  FaultOverPower:               //功率保护闪烁7次
1078   2              Led_OnOff(&mcLedDisplay,7);
1079   2            break;
1080   2      //    case  FaultStart:
1081   2      //        Led_OnOff(&mcLedDisplay,7);
1082   2      //      break;
1083   2      //    case  FaultOverwind:
1084   2      //        Led_OnOff(&mcLedDisplay,8);
1085   2      //      break;
1086   2          default:
1087   2            break;
1088   2        }
1089   1      
1090   1      }
1091          
1092            //LED灯的闪烁
1093          void Led_OnOff(MCLedDisplay *hLedDisplay,uint8 htime)
1094          {
1095   1        hLedDisplay->LedCount++;
1096   1        if(hLedDisplay->LedCount<hLedDisplay->Counttime)
1097   1         {
1098   2            if(hLedDisplay->Count<200)
1099   2            {
1100   3             hLedDisplay->Count++;
1101   3      
1102   3            }
1103   2            else if((hLedDisplay->Count>=200)&&(hLedDisplay->Count<201))
1104   2            {
1105   3              hLedDisplay->Count=0;
1106   3              LEDPinONOFF;
1107   3              hLedDisplay->LedTimCot++;
1108   3            }
1109   2            else
1110   2              ;
1111   2            if(hLedDisplay->LedTimCot>=2*htime)
1112   2            {
1113   3              hLedDisplay->Count=202;
1114   3              SetLEDPin;
1115   3            }
1116   2          }
1117   1          else if(hLedDisplay->LedCount>=hLedDisplay->Counttime)
1118   1          {
1119   2           hLedDisplay->LedCount=0;
1120   2           hLedDisplay->LedTimCot=0;
1121   2           hLedDisplay->Count=0;
1122   2          }
1123   1      }
1124          /*---------------------------------------------------------------------------*/
1125          /* Name   : void mc_ramp(void)
1126          /* Input  : hTarget,MC_RAMP *hSpeedramp
1127          /* Output : NO
1128          /* Description:
1129          /*---------------------------------------------------------------------------*/
1130          
1131          void mc_ramp(MCRAMP *hSpeedramp)
1132          {
1133   1        if( --hSpeedramp->DelayCount < 0)
1134   1        {
1135   2          hSpeedramp->DelayCount = hSpeedramp->DelayPeriod;
1136   2      
C51 COMPILER V9.52.0.0   ADDFUNCTION                                                       09/21/2018 09:06:09 PAGE 20  

1137   2          if (hSpeedramp->ActualValue < hSpeedramp->TargetValue)
1138   2          {
1139   3              if(hSpeedramp->ActualValue + hSpeedramp->IncValue < hSpeedramp->TargetValue)
1140   3              {
1141   4                  hSpeedramp->ActualValue += hSpeedramp->IncValue;
1142   4              }
1143   3              else
1144   3              {
1145   4                  hSpeedramp->ActualValue = hSpeedramp->TargetValue;
1146   4              }
1147   3          }
1148   2          else
1149   2          {
1150   3              if(hSpeedramp->ActualValue - hSpeedramp->DecValue > hSpeedramp->TargetValue)
1151   3              {
1152   4      
1153   4                  hSpeedramp->ActualValue -= hSpeedramp->DecValue;
1154   4              }
1155   3              else
1156   3              {
1157   4                  hSpeedramp->ActualValue = hSpeedramp->TargetValue;
1158   4              }
1159   3          }
1160   2        }
1161   1      }
1162          
1163          /*-------------------------------------------------------------------------------------------------
1164            Function Name : int16 HW_One_PI(int16 Xn1, int16 Yn0, int16 Xn2)
1165            Description   : PI控制
1166            Input         : Xn1--E(K)
1167            Output      : PI_UK--当前PI输出值,执行时间us
1168          -------------------------------------------------------------------------------------------------*/
1169          int16 HW_One_PI(int16 Xn1)
1170          {
1171   1          PI_EK =  Xn1;                               //填入EK
1172   1          SetBit(PI_LPF_CR,PISTA);                    // Start PI
1173   1      //    PI_LPF_CR |= 0x02;                          // Start PI
1174   1          _nop_();  _nop_();  _nop_();  _nop_();  _nop_();
1175   1          PI_UK += ((SKP>>12) +1);
1176   1          return PI_UK;
1177   1      }
1178          
1179          void HW_PI_Control(OUTLOOP *PIPara)
1180          {
1181   1          PIPara->ExtErr = PIPara->ExtRef - PIPara->ExtFed;
1182   1          PI_EK =  PIPara->ExtErr;                                                    //填入本次EK 误差值
1183   1      //    PI_UK =  PIPara->ExtOutL;                                                   //填入上一次UL
1184   1          SetBit(PI_LPF_CR,PISTA);                                                    // Start PI
1185   1          _nop_();  _nop_();  _nop_();  _nop_();  _nop_();
1186   1          
1187   1          PIPara->ExtOut = PI_UK + 1;                                                 // 补偿PI输出误差
1188   1      }
1189          
1190          /*-------------------------------------------------------------------------------------------------
1191            Function Name : int16 HW_TWO_PI(int16 Err1, int16 ErrL, int16 Out1, int16 OutL)
1192            Description   : 程序中连续调用PI控制
1193            Input         : Xn1--E(K)
1194            Output        : 反馈计算值
1195          -------------------------------------------------------------------------------------------------*/
1196          //void HW_TWO_PI(OUTLOOP *PIPara)
1197          //{
1198          //    // 写入上一次ERR值
C51 COMPILER V9.52.0.0   ADDFUNCTION                                                       09/21/2018 09:06:09 PAGE 21  

1199          //    PI_EK =  PIPara->ExtErrL;                                                   //填入上次EKL
1200          //    SetBit(PI_LPF_CR,PISTA);                                                    // Start PI
1201          //    _nop_();  _nop_();  _nop_();  _nop_();  _nop_();
1202          //  
1203          //    //
1204          //    PIPara->ExtErr = PIPara->ExtRef - PIPara->ExtFed;
1205          //    PI_EK =  PIPara->ExtErr;                                                    //填入本次EK
1206          //    PI_UK =  PIPara->ExtOutL;                                                   //填入上一次UL
1207          //    SetBit(PI_LPF_CR,PISTA);                                                    // Start PI
1208          //    _nop_();  _nop_();  _nop_();  _nop_();  _nop_();
1209          //    
1210          ////    PIPara->ExtOut = PI_UK + ((SKP>>12) +1);                                    // 补偿PI输出误差
1211          //    PIPara->ExtOut = PI_UK + 1;                                                 // 补偿PI输出误差
1212          //  
1213          //    PIPara->ExtErrL = PIPara->ExtErr;                                           // 保存误差值
1214          //    PIPara->ExtOutL = PIPara->ExtOut;                                           // 保存输出值
1215          //}
1216          /*-------------------------------------------------------------------------------------------------
1217            Function Name : int16 LPF(int16 Xn1, int16 Xn0, int8 K)
1218            Description   : LFP控制
1219            Input         : Xn1--当前输入值
1220                            Xn0--上一次滤波输出值
1221                            K--LPF滤波系数
1222            Output        : LPF_Y--当前滤波输出值，执行时间为4us。
1223          -------------------------------------------------------------------------------------------------*/
1224          int16 LPFFunction(int16 Xn1, int16 Xn0, int8 K)
1225          {
1226   1        LPF_K = K;
1227   1        LPF_X = Xn1;
1228   1        LPF_Y = Xn0;
1229   1        SetBit(PI_LPF_CR, LPFSTA);
1230   1        _nop_();_nop_();_nop_();_nop_();_nop_();
1231   1        return LPF_Y;
1232   1      }
1233          
1234          /*---------------------------------------------------------------------------*/
1235          /* Name   : void VSPSample(void)
1236          /* Input  : NO
1237          /* Output : NO
1238          /* Description: VSP采样
1239          /*---------------------------------------------------------------------------*/
1240          void VSPSample(void)
1241          {
1242   1        /*****VREF的采样获取值并滤波******/
1243   1        AdcSampleValue.ADCVref = (ADC7_DR<<3);//采集对应端口
1244   1        VSP=LPFFunction(AdcSampleValue.ADCVref,VSP,30);//低通滤波
1245   1      
1246   1          if((VSP > ONPWMDuty)&&(VSP <= OFFPWMDutyHigh))//在ONPWMDuty-OFFPWMDutyHigh之间，电机有转速运�
             -��
1247   1          {
1248   2            mcSpeedRamp.FlagONOFF = 1;
1249   2          }
1250   1          else if((VSP < OFFPWMDuty)||(VSP > OFFPWMDutyHigh))//电机停机
1251   1          {
1252   2            mcSpeedRamp.FlagONOFF = 0;
1253   2          }
1254   1          //转速曲线计算
1255   1          if(mcSpeedRamp.FlagONOFF==1)//
1256   1          {
1257   2            #if (Motor_Speed_Control_Mode == SPEED_LOOP_CONTROL)
1258   2            {
1259   3              if(VSP <= MINPWMDuty)    //最小转速运行
C51 COMPILER V9.52.0.0   ADDFUNCTION                                                       09/21/2018 09:06:09 PAGE 22  

1260   3              {
1261   4                mcSpeedRamp.TargetValue = Motor_Min_Speed;
1262   4              }
1263   3              else if(VSP < MAXPWMDuty)//调速
1264   3              {
1265   4                mcSpeedRamp.TargetValue = Motor_Min_Speed + SPEED_K*(VSP-MINPWMDuty);
1266   4              }
1267   3              else         //最大转速运行
1268   3              {
1269   4                mcSpeedRamp.TargetValue = Motor_Max_Speed;
1270   4              }
1271   3            }
1272   2            #elif (Motor_Speed_Control_Mode == POWER_LOOP_CONTROL)
                    {
                      if(VSP <= MINPWMDuty)    //最小转速运行
                      {
                        mcSpeedRamp.TargetValue = Motor_Min_Power;
                      }
                      else if(VSP < MAXPWMDuty)//调速
                      {
                        mcSpeedRamp.TargetValue = Motor_Min_Power + POWER_K*(VSP-MINPWMDuty);
                      }
                      else         //最大转速运行
                      {
                        mcSpeedRamp.TargetValue = Motor_Max_Power;
                      }
                    }
                    #endif
1288   2      
1289   2          }
1290   1          else
1291   1          {
1292   2            mcSpeedRamp.TargetValue =0;
1293   2          }
1294   1      }
1295          /*---------------------------------------------------------------------------*/
1296          /* Name   : void Sleepmode(void)
1297          /* Input  : NO
1298          /* Output : NO
1299          /* Description: 睡眠模式测试
1300          /*---------------------------------------------------------------------------*/
1301           void Sleepmode(void)
1302           {
1303   1          SleepSet.SleepDelayCout++;
1304   1          if(SleepSet.SleepDelayCout>=20000)//最大65530，若要再大，需改数据类型
1305   1          {
1306   2      //        FOC_EFREQMIN  = -Motor_Omega_Ramp_Min;
1307   2      //        FOC_EFREQHOLD = -Motor_Omega_Ramp_End;
1308   2            mcSpeedRamp.TargetValue=0;
1309   2            MOE     = 0;
1310   2            ClrBit(DRV_CR, FOCEN);  //关闭FOC
1311   2            SleepSet.SleepDelayCout=0;
1312   2            SleepSet.SleepFlag=1;
1313   2            SetBit(P1_IE, P11);   // config P11 as the source of EXTI1
1314   2            SetBit(PCON, STOP);
1315   2          }
1316   1      
1317   1       }
1318          /*---------------------------------------------------------------------------*/
1319          /* Name   : void StarRampDealwith(void)
1320          /* Input  : NO
1321          /* Output : NO
C51 COMPILER V9.52.0.0   ADDFUNCTION                                                       09/21/2018 09:06:09 PAGE 23  

1322          /* Description:
1323          /*---------------------------------------------------------------------------*/
1324          void StarRampDealwith(void)
1325          {
1326   1          if((mcState == mcRun))
1327   1          {
1328   2            if(mcFocCtrl.State_Count == 2400)//2300
1329   2            {
1330   3              FOC_EKP = OBSW_KP_GAIN_RUN;                           // 估算器里的PI的KP
1331   3              FOC_EKI = OBSW_KI_GAIN_RUN;                           // 估算器里的PI的KI
1332   3            }
1333   2            else if(mcFocCtrl.State_Count == 2200)//2000
1334   2            {
1335   3              FOC_EKP = OBSW_KP_GAIN_RUN1;                          // 估算器里的PI的KP
1336   3              FOC_EKI = OBSW_KI_GAIN_RUN1;                          // 估算器里的PI的KI
1337   3            }
1338   2            else if(mcFocCtrl.State_Count == 1950)//1600
1339   2            {
1340   3              FOC_EKP = OBSW_KP_GAIN_RUN2;                          // 估算器里的PI的KP
1341   3              FOC_EKI = OBSW_KI_GAIN_RUN2;                          // 估算器里的PI的KI
1342   3            }
1343   2            else if(mcFocCtrl.State_Count ==1600)//1200
1344   2            {
1345   3              FOC_EKP = OBSW_KP_GAIN_RUN3;                          // 估算器里的PI的KP
1346   3              FOC_EKI = OBSW_KI_GAIN_RUN3;                          // 估算器里的PI的KI
1347   3            }
1348   2            else if(mcFocCtrl.State_Count == 1000)
1349   2            {
1350   3              FOC_EKP = OBSW_KP_GAIN_RUN4;                          // 估算器里的PI的KP
1351   3              FOC_EKI = OBSW_KI_GAIN_RUN4;                          // 估算器里的PI的KI
1352   3            }
1353   2            else;
1354   2          }
1355   1      }


MODULE INFORMATION:   STATIC OVERLAYABLE
   CODE SIZE        =   5789    ----
   CONSTANT SIZE    =   ----    ----
   XDATA SIZE       =    141      39
   PDATA SIZE       =   ----    ----
   DATA SIZE        =   ----    ----
   IDATA SIZE       =    128    ----
   BIT SIZE         =   ----    ----
   EDATA SIZE       =   ----    ----
   HDATA SIZE       =   ----    ----
   XDATA CONST SIZE =   ----    ----
   FAR CONST SIZE   =   ----    ----
END OF MODULE INFORMATION.


C51 COMPILATION COMPLETE.  0 WARNING(S),  0 ERROR(S)
