C51 COMPILER V9.52.0.0   ADDFUNCTION                                                       09/06/2018 16:05:13 PAGE 1   


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

line level    source

   1          /**************************** (C) COPYRIGHT 2017 Fortiortech shenzhen *****************************
   2          * File Name          : AddFunction.c
   3          * Author             : Vina Peng,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          FOCCTRL            xdata   mcFocCtrl;
  26          ADCSample          xdata   AdcSampleValue;
  27          ONVarible          xdata   ONOFFTest;
  28          MCLedDisplay       xdata   mcLedDisplay;
  29          MCRAMP             xdata   mcSpeedRamp;
  30          SLEEPMODE          xdata   SleepSet;
  31          MotorFRTypeDef     xdata   mcFRState;
  32          int16              xdata   VSP;
  33          int16 Motor_Loop_Speed1;
  34          const int16  PowerGiven[10] ={0,500,1000,2200,2900,4300,5900,7300,7900,8550};
  35          /*---------------------------------------------------------------------------*/
  36          /* Name   : int16 KLPF_VALUE(int16 INVlaue, int16 OutLastValue)
  37          /* Input  : INVlaue，OutLastValue
  38          /* Output : int16的变量
  39          /* Description: 滤波函数,用乘法器做的
  40          /*---------------------------------------------------------------------------*/
  41          int16 KLPF_VALUE(int16 INVlaue, int16 OutLastValue)
  42          {
  43   1        int16 Result = 0;
  44   1        MDU_MA = (INVlaue-OutLastValue);
  45   1        MDU_MB = (int16)480;                    /*写被乘数和乘数*/
  46   1      
  47   1        Result = MDU_MB;
  48   1        Result += OutLastValue;
  49   1        return(Result);
  50   1      }
  51          
  52          /*---------------------------------------------------------------------------*/
  53          /* Name   : void FaultProcess(void)
C51 COMPILER V9.52.0.0   ADDFUNCTION                                                       09/06/2018 16:05:13 PAGE 2   

  54          /* Input  : NO
  55          /* Output : NO
  56          /* Description: 保护处理函数，关闭FOC输出，同时将状态变为mcFault
  57          /*---------------------------------------------------------------------------*/
  58          void FaultProcess(void)
  59          {
  60   1        MOE     = 0;
  61   1        ClrBit(DRV_CR, FOCEN);  //关闭FOC
  62   1        mcState = mcFault;
  63   1      
  64   1      }
  65          
  66          /*---------------------------------------------------------------------------*/
  67          /* Name   : int16 Abs_F16(int16 value)
  68          /* Input  : value
  69          /* Output : int16
  70          /* Description: 对变量取16位的绝对值
  71          /*---------------------------------------------------------------------------*/
  72          uint16 Abs_F16(int16 value)
  73          {
  74   1        if(value < 0)
  75   1        {
  76   2          return (- value);
  77   2        }
  78   1        else
  79   1        {
  80   2          return (value);
  81   2        }
  82   1      }
  83          /*---------------------------------------------------------------------------*/
  84          /* Name   : int32 Abs_F32(int32 value)
  85          /* Input  : value
  86          /* Output : int16
  87          /* Description: 对变量取16位的绝对值
  88          /*---------------------------------------------------------------------------*/
  89          uint32 Abs_F32(int32 value)
  90          {
  91   1        if(value < 0)
  92   1        {
  93   2          return (- value);
  94   2        }
  95   1        else
  96   1        {
  97   2          return (value);
  98   2        }
  99   1      }
 100          /*---------------------------------------------------------------------------*/
 101          /* Name   : void APP_DIV(void)
 102          /* Input  : void
 103          /* Output : void
 104          /* Description: 将所有用到除法的地方，放在同一个中断，以避免中断串扰
 105          /*---------------------------------------------------------------------------*/
 106          void APP_DIV(void)
 107          {
 108   1          if( mcPwmInput.PWMDivFlag==1)  //启动除法器，避免与过调值中的除法冲突
 109   1          {
 110   2             mcPwmInput.PWMDuty = MDU_DIV_IDATA_U32(&mcPwmInput.pwm.PWMCompareAMP, &mcPwmInput.PWMARRUpdate);
 111   2             mcPwmInput.PWMDivFlag=0;
 112   2          }
 113   1          if( mcFocCtrl.ESDIVFlag==1)  //启动除法器，避免与过调值中的除法冲突
 114   1          {
 115   2             mcFocCtrl.SQUSpeedDIVEs = MDU_DIV_XDATA_U32(&mcFocCtrl.SQUSysSpeed,&mcFocCtrl.EsValue);
C51 COMPILER V9.52.0.0   ADDFUNCTION                                                       09/06/2018 16:05:13 PAGE 3   

 116   2             mcFocCtrl.ESDIVFlag=0;
 117   2          }
 118   1      }
 119          /*---------------------------------------------------------------------------*/
 120          /* Name   : void PWMInputCapture(void)
 121          /* Input  : NO
 122          /* Output : NO
 123          /* Description: 输入PWM处理
 124          /*---------------------------------------------------------------------------*/
 125          void PWMInputCapture(void)
 126          {
 127   1        uint16 MotorControlVSP;
 128   1      
 129   1         if(mcPwmInput.PWMUpdateFlag==1)  // 有新的duty更新
 130   1         {
 131   2            if((Abs_F32(mcPwmInput.PWMCompare-mcPwmInput.PWMCompareOld)<50)// 两次比较值相近，减少读�
             -��率
 132   2              &&(Abs_F32(mcPwmInput.PWMARROld-mcPwmInput.PWMARR)<50)// 两次周期值相近，减少读错率
 133   2      //        &&((100<mcPwmInput.PWMARR)&&(mcPwmInput.PWMARR<6000))// 周期值在一定范围内才认为有�
             -��，即一定频率范围
 134   2              &&(mcPwmInput.PWMDivFlag==0))
 135   2              {
 136   3                mcPwmInput.PWMFlag=1;                               // PWMFlag置1期间，不读取TIM3__DR和T
             -IM3__ARR;，防止出错
 137   3                mcPwmInput.pwm.PWMCompareUpdate[0]=(mcPwmInput.PWMCompare>>1);// 对其乘以32768
 138   3                mcPwmInput.PWMARRUpdate=mcPwmInput.PWMARR;
 139   3                mcPwmInput.PWMDivFlag=1;                            // 启动除法
 140   3                mcPwmInput.PWMFlag=0;
 141   3              }
 142   2             if(mcPwmInput.PWMDivFlag==0)
 143   2              {
 144   3                if(mcPwmInput.PWMcnt<3)//2次求平均值
 145   3                {
 146   4                  mcPwmInput.PWMcnt++;
 147   4                  mcPwmInput.PWMVSum +=mcPwmInput.PWMDuty;
 148   4                }
 149   3                else
 150   3                {
 151   4                  MotorControlVSP= (mcPwmInput.PWMVSum >>1);//注意其有一个右移与前面的比较值标�
             -�化有关
 152   4                  mcPwmInput.PWMVSum=0;
 153   4                  mcPwmInput.PWMcnt =0;
 154   4                }
 155   3                 MotorControlVSP=mcPwmInput.PWMDuty;
 156   3               if((MotorControlVSP > ONPWMDuty)&&(MotorControlVSP <=(OFFPWMDutyHigh+1)))
 157   3                {
 158   4                  mcSpeedRamp.FlagONOFF = 1;
 159   4                }
 160   3               else if(MotorControlVSP < OFFPWMDuty)//||(MotorControlVSP >= OFFPWMDutyHigh))
 161   3                {
 162   4                  mcSpeedRamp.FlagONOFF = 0;
 163   4                }
 164   3      
 165   3                //转速曲线计算
 166   3                if(mcSpeedRamp.FlagONOFF==1)
 167   3                {
 168   4                  if(MotorControlVSP <= MINPWMDuty)
 169   4                  {
 170   5                    mcSpeedRamp.TargetValue = Motor_Min_Speed;
 171   5                  }
 172   4                  else if(MotorControlVSP < MAXPWMDuty)
 173   4                  {
C51 COMPILER V9.52.0.0   ADDFUNCTION                                                       09/06/2018 16:05:13 PAGE 4   

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

 233   2              {
 234   3                h_Fault->OverVoltDetecCnt++;
 235   3                if(h_Fault->OverVoltDetecCnt > 20)//检测100ms
 236   3                {
 237   4                  h_Fault->OverVoltDetecCnt = 0;
 238   4                  mcFaultSource=FaultOverVoltage;
 239   4                  FaultProcess();
 240   4                }
 241   3              }
 242   2              else
 243   2              {
 244   3                if(h_Fault->OverVoltDetecCnt>0)
 245   3                {
 246   4                  h_Fault->OverVoltDetecCnt--;
 247   4                }
 248   3              }
 249   2      
 250   2            //欠压保护
 251   2      
 252   2              if(mcFocCtrl.mcDcbusFlt< UNDER_PROTECT_VALUE)
 253   2              {
 254   3                h_Fault->UnderVoltDetecCnt++;
 255   3      
 256   3                if(h_Fault->UnderVoltDetecCnt > 20)//检测100ms
 257   3                {
 258   4                  h_Fault->UnderVoltDetecCnt = 0;
 259   4                  mcFaultSource=FaultUnderVoltage;
 260   4                  FaultProcess();
 261   4                }
 262   3              }
 263   2              else
 264   2              {
 265   3                if(h_Fault->UnderVoltDetecCnt>0)
 266   3                {
 267   4                  h_Fault->UnderVoltDetecCnt--;
 268   4                }
 269   3              }
 270   2          }
 271   1      
 272   1          /*******过压欠压保护恢复*********/
 273   1          if((mcState == mcFault) &&((mcFaultSource==FaultUnderVoltage)||(mcFaultSource==FaultOverVoltage)))
 274   1          {
 275   2            if((mcFocCtrl.mcDcbusFlt< OVER_RECOVER_VALUE)&&(mcFocCtrl.mcDcbusFlt> UNDER_RECOVER_VALUE))
 276   2            {
 277   3              h_Fault->VoltRecoverCnt++;
 278   3              if(h_Fault->VoltRecoverCnt>200)//连续检测1s，若正常则恢复
 279   3              {
 280   4                mcState = mcReady;
 281   4                mcFaultSource=FaultNoSource;
 282   4                h_Fault->VoltRecoverCnt = 0;
 283   4              }
 284   3            }
 285   2            else
 286   2            {
 287   3              h_Fault->VoltRecoverCnt = 0;
 288   3            }
 289   2         }
 290   1      }
 291          
 292          /*****************************************************************************
 293           * Function:     void Fault_Overcurrent(CurrentVarible *h_Cur)
 294           * Description:  电机运行或者启动时，当三相中某一相最大值大于OverCurrentValue，则O
C51 COMPILER V9.52.0.0   ADDFUNCTION                                                       09/06/2018 16:05:13 PAGE 6   

             -verCurCnt加1。
 295                           连续累加3次，判断为软件过流保护。执行时间约30.4us。
 296           * Parameter:    mcFaultVarible *h_Fault
 297           * Return:       no
 298           *****************************************************************************/
 299          void Fault_Overcurrent(CurrentVarible *h_Cur)
 300          {
 301   1        if((mcState == mcRun)||(mcState == mcStart))            // check over current in rum and open mode
 302   1        {
 303   2      
 304   2          h_Cur->Abs_ia = Abs_F16(FOC__IA);
 305   2          h_Cur->Abs_ib = Abs_F16(FOC__IB);
 306   2          h_Cur->Abs_ic = Abs_F16(FOC__IC);
 307   2          if(h_Cur->Abs_ia> h_Cur->Max_ia)                      // 此部分既用于软件过流保护，又用�
             -�缺相保护
 308   2          {
 309   3             h_Cur->Max_ia = h_Cur->Abs_ia;
 310   3          }
 311   2          if(h_Cur->Abs_ib > h_Cur->Max_ib)
 312   2          {
 313   3             h_Cur->Max_ib = h_Cur->Abs_ib;
 314   3          }
 315   2          if(h_Cur->Abs_ic > h_Cur->Max_ic)
 316   2          {
 317   3             h_Cur->Max_ic = h_Cur->Abs_ic;
 318   3          }
 319   2      
 320   2          if((h_Cur->Max_ia>=OverSoftCurrentValue)||(h_Cur->Max_ib>=OverSoftCurrentValue)||(h_Cur->Max_ic>=OverSof
             -tCurrentValue))
 321   2          {
 322   3            h_Cur->OverCurCnt++;
 323   3            if(h_Cur->OverCurCnt>=3)
 324   3            {
 325   4              GP36=~GP36;
 326   4              h_Cur->Max_ia=0;
 327   4              h_Cur->Max_ib=0;
 328   4              h_Cur->Max_ic=0;
 329   4              h_Cur->OverCurCnt=0;
 330   4              mcFaultSource=FaultSoftOVCurrent;
 331   4              FaultProcess();
 332   4            }
 333   3          }
 334   2          else
 335   2          {
 336   3            if(h_Cur->OverCurCnt>0)
 337   3            {
 338   4              h_Cur->OverCurCnt--;
 339   4            }
 340   3          }
 341   2        }
 342   1      }
 343          
 344          /*****************************************************************************
 345           * Function:     void Fault_OverCurrentRecover(mcFaultVarible *h_Fault)
 346           * Description:  软硬件过流保护恢复
 347           * Parameter:    mcFaultVarible *h_Fault
 348           * Return:       no
 349           *****************************************************************************/
 350          void Fault_OverCurrentRecover(FaultVarible *h_Fault)
 351          {
 352   1        if((mcState == mcFault)&&((mcFaultSource==FaultSoftOVCurrent)||(mcFaultSource==FaultHardOVCurrent))&&(mcP
             -rotectTime.CurrentPretectTimes<5))
C51 COMPILER V9.52.0.0   ADDFUNCTION                                                       09/06/2018 16:05:13 PAGE 7   

 353   1        {
 354   2          h_Fault->CurrentRecoverCnt++;
 355   2          if(h_Fault->CurrentRecoverCnt>=OverCurrentRecoverTime)//200*5=1s
 356   2          {
 357   3            h_Fault->CurrentRecoverCnt=0;
 358   3            mcProtectTime.CurrentPretectTimes++;
 359   3            mcState = mcReady;
 360   3            mcFaultSource=FaultNoSource;
 361   3          }
 362   2        }
 363   1      }
 364          
 365          /*****************************************************************************
 366           * Function:     void Fault_Start(mcFaultVarible *h_Fault)
 367           * Description:  启动保护函数，电机运行状态下，电机在前5s估算转速达到堵转保护
             -值或者5s后反电动势值太低(此方法未验证)
 368                            或4s内还在CtrlMode状态，即速度低于MOTOR_LOOP_RPM，程序判断为启动失�
             -��，电机停机。
 369                            当程序判断为启动失败后，若重启次数少于或等于5次，程序立即进
             -入校准状态，等待重启。
 370           * Parameter:    mcFaultVarible *h_Fault
 371           * Return:       no
 372           *****************************************************************************/
 373            void Fault_Start(FaultVarible *h_Fault)
 374            {
 375   1          /*******启动保护恢复*********/
 376   1      //    h_Fault->mcEsValue = FOC__ESQU;
 377   1      
 378   1          if(mcState == mcRun)
 379   1          {
 380   2            //方法一，5s内速度大于最大速度，同时反电动势值低于一定值
 381   2            if(h_Fault->StartSpeedCnt<=1000)
 382   2            {
 383   3              h_Fault->StartSpeedCnt++;
 384   3              if((mcFocCtrl.SpeedFlt > Motor_Max_Speed)&&(mcFocCtrl.EsValue<40))
 385   3              {
 386   4                h_Fault->StartSpeedCnt = 0;
 387   4                mcFaultSource=FaultStart;
 388   4                FaultProcess();
 389   4                mcProtectTime.SecondStartTimes++;
 390   4                mcProtectTime.StartFlag  =  1;
 391   4              }
 392   3            }
 393   2           //方法二
 394   2            if(h_Fault->StartEsCnt<=1200)//前6s，等待1.5s后，开始判断ES，如果超过一定次数，则�
             -��败
 395   2            {
 396   3              h_Fault->StartEsCnt++;
 397   3              h_Fault->StartDelay++;
 398   3              if(h_Fault->StartDelay>=300)        // 1.5s
 399   3              {
 400   4                 h_Fault->StartDelay=300;
 401   4                 if((mcFocCtrl.EsValue <5))//&&(mcFocCtrl.CtrlMode==0))
 402   4                  {
 403   5                    h_Fault->StartESCount++;
 404   5                    if(h_Fault->StartESCount>=20)
 405   5                    {
 406   6                      mcFaultSource=FaultStart;
 407   6                      FaultProcess();
 408   6                      mcProtectTime.SecondStartTimes++;
 409   6                      h_Fault->StartDelay=0;
 410   6                      h_Fault->StartESCount=0;
C51 COMPILER V9.52.0.0   ADDFUNCTION                                                       09/06/2018 16:05:13 PAGE 8   

 411   6                      mcProtectTime.StartFlag  =  2;
 412   6                    }
 413   5                  }
 414   4                  else
 415   4                  {
 416   5                    if(h_Fault->StartESCount>0)
 417   5                      h_Fault->StartESCount--;
 418   5                  }
 419   4             }
 420   3           }
 421   2           else
 422   2           {
 423   3             h_Fault->StartESCount=0;
 424   3           }
 425   2            //方法三，长时间在CtrlMode=0状态
 426   2            if(mcFocCtrl.CtrlMode==0)         //
 427   2            {
 428   3              h_Fault->StartFocmode++;
 429   3              if(h_Fault->StartFocmode>=400)
 430   3              {
 431   4      //          h_Fault->StartFocmode=0;
 432   4      //          mcFaultSource=FaultStart;
 433   4      //          FaultProcess();
 434   4                mcProtectTime.SecondStartTimes++;
 435   4                mcProtectTime.StartFlag  =  3;
 436   4              }
 437   3            }
 438   2          }
 439   1          #if (!StartONOFF_Enable)
 440   1          {
 441   2           if((mcFaultSource==FaultStart)&&(mcState == mcFault)&&(mcProtectTime.SecondStartTimes<=StartProtectRest
             -artTimes))
 442   2           {
 443   3             mcFaultSource=FaultNoSource;
 444   3             mcState = mcInit;
 445   3           }
 446   2          }
 447   1         #endif
 448   1      
 449   1        }
 450           /*****************************************************************************
 451           * Function:     void Fault_Stall(mcFaultVarible *h_Fault)
 452           * Description:  堵转保护函数，有三种保护方式，
 453                           第一种，
 454                           第二种，电机运行状态下，延迟4s判断，估算速度绝对值超过堵转速�
             -��连续5次；
 455                           第三种，电机运行状态下，当U,V两相电流绝对值大于堵转电流保护值
             -连续6次；
 456                           当以上三种的任何一种保护触发时，电机停机，程序判断为堵转保护
             -；
 457                           当堵转保护状态下，U相采集值低于堵转恢复值时，若堵转次数小于�
             -�等于堵转重启次数8次，
 458                           程序延迟mcStallRecover重新启动，进行校准状态。
 459           * Parameter:    mcFaultVarible *h_Fault
 460           * Return:       no
 461           *****************************************************************************/
 462          //堵转保护
 463          void Fault_Stall(FaultVarible *h_Fault)
 464          {
 465   1      //  h_Fault->mcEsValue = FOC__ESQU;
 466   1        if(mcState == mcRun)
 467   1        {
C51 COMPILER V9.52.0.0   ADDFUNCTION                                                       09/06/2018 16:05:13 PAGE 9   

 468   2          if(h_Fault->StallDelayCnt <=1000)//5s
 469   2          {
 470   3            h_Fault->StallDelayCnt ++;
 471   3          }
 472   2          else
 473   2          {
 474   3            //method 1，判断反电动势太小或当反电动势太小，转速太大
 475   3            if((&mcFocCtrl.EsValue< 10)||((FOC__EOME > _Q15(1600.0/MOTOR_SPEED_BASE))&&(mcFocCtrl.EsValue< 80)))
 476   3            {
 477   4              h_Fault->StallDectEs++;
 478   4              if(h_Fault->StallDectEs >= 10)
 479   4              {
 480   5                h_Fault->StallDectEs=0;
 481   5                mcFaultSource=FaultStall;
 482   5                mcProtectTime.StallTimes++;
 483   5                FaultProcess();
 484   5                mcProtectTime.StallFlag  =  1;
 485   5              }
 486   4            }
 487   3            else
 488   3            {
 489   4              if( h_Fault->StallDectEs>0)
 490   4                h_Fault->StallDectEs--;
 491   4            }
 492   3            //method 2，判断速度低于堵转最小值或者超过堵转最大值
 493   3            if((mcFocCtrl.SpeedFlt<Motor_Stall_Min_Speed)||(mcFocCtrl.SpeedFlt > Motor_Stall_Max_Speed))
 494   3            {
 495   4              h_Fault->StallDectSpeed++;
 496   4              if(h_Fault->StallDectSpeed>=8)
 497   4              {
 498   5                h_Fault->StallDectSpeed=0;
 499   5                mcFaultSource=FaultStall;
 500   5                mcProtectTime.StallTimes++;
 501   5                FaultProcess();
 502   5                mcProtectTime.StallFlag =2;
 503   5              }
 504   4            }
 505   3            else
 506   3            {
 507   4              if(h_Fault->StallDectSpeed>0)
 508   4              h_Fault->StallDectSpeed--;
 509   4            }
 510   3          }
 511   2        }
 512   1          #if (!StartONOFF_Enable)
 513   1          {
 514   2           /*******堵转保护恢复*********/
 515   2              if((mcFaultSource==FaultStall)&&(mcState == mcFault)&&(mcProtectTime.StallTimes<=4))
 516   2              {
 517   3                h_Fault->StallReCount++;
 518   3                if(h_Fault->StallReCount>=StallRecoverTime)
 519   3                {
 520   4                  h_Fault->StallReCount=16000;
 521   4                  mcFaultSource=FaultNoSource;
 522   4                  mcState = mcInit;
 523   4                }
 524   3              }
 525   2              else
 526   2              {
 527   3                h_Fault->StallReCount=0;
 528   3              }
 529   2          }
C51 COMPILER V9.52.0.0   ADDFUNCTION                                                       09/06/2018 16:05:13 PAGE 10  

 530   1         #endif
 531   1      
 532   1      }
 533           /*****************************************************************************
 534           * Function:     void Fault_phaseloss(mcFaultVarible *h_Fault)
 535           * Description:  缺相保护函数，当电机运行状态下，10ms取三相电流的最大值，
 536                           1.5s判断各相电流最大值，若存在两相电流值大于一定值，而第三相�
             -�流值却非常小，则判断为缺相保护，电机停机；
 537           * Parameter:    mcFaultVarible *h_Fault
 538           * Return:       no
 539           *****************************************************************************/
 540            void Fault_phaseloss(FaultVarible *h_Fault)
 541            {
 542   1            if(mcState == mcRun)
 543   1           {
 544   2              h_Fault->Lphasecnt++;
 545   2              if(h_Fault->Lphasecnt>100)//100*5=500ms
 546   2              {
 547   3                 h_Fault->Lphasecnt=0;
 548   3                 if(((mcCurVarible.Max_ia>(mcCurVarible.Max_ib*3))||(mcCurVarible.Max_ia>(mcCurVarible.Max_ic*3)))&&(
             -mcCurVarible.Max_ia>PhaseLossCurrentValue))
 549   3                 {
 550   4                    h_Fault->AOpencnt++;
 551   4                 }
 552   3                 else
 553   3                 {
 554   4                  if(h_Fault->AOpencnt>0)
 555   4                    h_Fault->AOpencnt --;
 556   4                 }
 557   3                 if(((mcCurVarible.Max_ib >(mcCurVarible.Max_ia*3))||(mcCurVarible.Max_ib >(mcCurVarible.Max_ic*3)))&
             -&(mcCurVarible.Max_ib >PhaseLossCurrentValue))
 558   3                 {
 559   4                   h_Fault->BOpencnt++;
 560   4                 }
 561   3                else
 562   3                 {
 563   4                   if(h_Fault->BOpencnt>0)
 564   4                    h_Fault->BOpencnt --;
 565   4                 }
 566   3                 if(((mcCurVarible.Max_ic >(mcCurVarible.Max_ia*3))||(mcCurVarible.Max_ic >(mcCurVarible.Max_ib*3)))&
             -&(mcCurVarible.Max_ic >PhaseLossCurrentValue))
 567   3                 {
 568   4                   h_Fault->COpencnt++;
 569   4                 }
 570   3                else
 571   3                 {
 572   4                   if(h_Fault->COpencnt>0)
 573   4                    h_Fault->COpencnt --;
 574   4                 }
 575   3                  mcCurVarible.Max_ia = 0;
 576   3                  mcCurVarible.Max_ib = 0;
 577   3                  mcCurVarible.Max_ic = 0;
 578   3                if(h_Fault->AOpencnt > 1|| h_Fault->BOpencnt > 1 || h_Fault->COpencnt > 1)
 579   3                 {
 580   4                    mcProtectTime.LossPHTimes++;
 581   4                    mcFaultSource=FaultLossPhase;
 582   4                    FaultProcess();
 583   4                 }
 584   3              }
 585   2            }
 586   1      
 587   1          #if (!StartONOFF_Enable)
C51 COMPILER V9.52.0.0   ADDFUNCTION                                                       09/06/2018 16:05:13 PAGE 11  

 588   1          {
 589   2           /*******缺相保护恢复*********/
 590   2            if((mcFaultSource==FaultLossPhase)&&(mcState == mcFault)&&
 591   2            (mcProtectTime.LossPHTimes<5))//可重启5次
 592   2             {
 593   3                 h_Fault->mcLossPHRecCount++;
 594   3                 if(h_Fault->mcLossPHRecCount>=PhaseLossRecoverTime)
 595   3                 {
 596   4                   h_Fault->AOpencnt=0;
 597   4                   h_Fault->BOpencnt=0;
 598   4                   h_Fault->COpencnt=0;
 599   4                   mcState = mcReady;
 600   4                   mcFaultSource=FaultNoSource;
 601   4                 }
 602   3             }
 603   2          else
 604   2             {
 605   3                 h_Fault->mcLossPHRecCount=0;
 606   3             }
 607   2      
 608   2          }
 609   1         #endif
 610   1      
 611   1        }
 612          
 613          /*---------------------------------------------------------------------------*/
 614          /* Name   : void Fault_Detection(void)
 615          /* Input  : NO
 616          /* Output : NO
 617          /* Description: 保护函数，因保护的时间响应不会很高，采用分段处理，每5个定时�
             -�中断执行一次对应的保护
 618                          常见保护有过欠压、过温、堵转、启动、缺相等保护，调试时，可根�
             -��需求，一个个的调试加入。
 619          /*---------------------------------------------------------------------------*/
 620          void Fault_Detection(void)
 621          {
 622   1          mcFaultDect.segment++;
 623   1          if(mcFaultDect.segment>=5)
 624   1          {
 625   2            mcFaultDect.segment=0;
 626   2          }
 627   1          if(mcFaultDect.segment==0)
 628   1            {
 629   2      //         Fault_OverHeat(&mcFaultDect);
 630   2              if(CurrentRecoverEnable)//过流保护恢复使能
 631   2              {
 632   3                Fault_OverCurrentRecover(&mcFaultDect);
 633   3              }
 634   2            }
 635   1          else if(mcFaultDect.segment==1)
 636   1            {
 637   2              if(VoltageProtectEnable==1)//过压保护使能
 638   2              {
 639   3                Fault_OverUnderVoltage(&mcFaultDect);
 640   3              }
 641   2            }
 642   1          else if(mcFaultDect.segment==2)
 643   1            {
 644   2              if(StartProtectEnable==1)//启动保护使能
 645   2              {
 646   3                Fault_Start(&mcFaultDect);
 647   3              }
C51 COMPILER V9.52.0.0   ADDFUNCTION                                                       09/06/2018 16:05:13 PAGE 12  

 648   2            }
 649   1          else if(mcFaultDect.segment==3)
 650   1            {
 651   2              if(StallProtectEnable==1)//堵转保护使能
 652   2              {
 653   3                Fault_Stall(&mcFaultDect);
 654   3              }
 655   2            }
 656   1          else if(mcFaultDect.segment==4)
 657   1            {
 658   2              if(PhaseLossProtectEnable==1)//缺相保护使能
 659   2              {
 660   3                Fault_phaseloss(&mcFaultDect);
 661   3              }
 662   2            }
 663   1          else
 664   1            {
 665   2      
 666   2            }
 667   1      }
 668          
 669          /*---------------------------------------------------------------------------*/
 670          /* Name   : void ONOFF_Starttest(void)
 671          /* Input  : NO
 672          /* Output : NO
 673          /* Description: 启动测试
 674          /*---------------------------------------------------------------------------*/
 675          void ONOFF_Starttest(ONVarible  *h_test)
 676          {
 677   1        if(h_test->ONOFF_Flag==1)
 678   1        {
 679   2           h_test->ON_Count++;
 680   2          if(h_test->ON_Count>StartON_Time)
 681   2          {
 682   3            h_test->ON_Count=0;
 683   3            h_test->ONOFF_Times++;
 684   3            h_test->ONOFF_Flag=0;
 685   3            mcSpeedRamp.FlagONOFF = 0;
 686   3            mcSpeedRamp.TargetValue = 0;
 687   3      
 688   3          }
 689   2        }
 690   1        else
 691   1        {
 692   2          if(mcState!=mcFault)
 693   2          {
 694   3            h_test->OFF_Count++;
 695   3            if(h_test->OFF_Count>StartOFF_Time)
 696   3            {
 697   4              h_test->OFF_Count=0;
 698   4              h_test->ONOFF_Flag=1;
 699   4               mcSpeedRamp.FlagONOFF = 1;
 700   4               mcSpeedRamp.TargetValue = Motor_Min_Speed;
 701   4      
 702   4            }
 703   3          }
 704   2      
 705   2        }
 706   1      }
 707          
 708          /*---------------------------------------------------------------------------*/
 709          /* Name   : void Speed_response(void)
C51 COMPILER V9.52.0.0   ADDFUNCTION                                                       09/06/2018 16:05:13 PAGE 13  

 710          /* Input  : NO
 711          /* Output : NO
 712          /* Description: 速度响应函数，可根据需求加入控制环，如恒转矩控制、恒转速控制
             -、恒功率控制
 713          /*---------------------------------------------------------------------------*/
 714          void Speed_response(void)
 715          {
 716   1      
 717   1        if((mcState ==mcRun)||(mcState ==mcStop))
 718   1        {
 719   2          switch(mcFocCtrl.CtrlMode)
 720   2          {
 721   3            case 0:
 722   3            {
 723   4              if(mcFocCtrl.SpeedFlt >Motor_Loop_Speed)//>=MOTOR_LOOP_RPM
 724   4              {
 725   5                mcFocCtrl.CtrlMode= 1;
 726   5                FOC_DQKP = DQKP;
 727   5                FOC_DQKI = DQKI;
 728   5                #if (Motor_Speed_Control_Mode == SPEED_LOOP_CONTROL)
 729   5                {
 730   6                  mcSpeedRamp.ActualValue = FOC__EOME;
 731   6                }
 732   5                #elif (Motor_Speed_Control_Mode == POWER_LOOP_CONTROL)
                        {
                          mcSpeedRamp.ActualValue = mcFocCtrl.Powerlpf;
                        }
                        #endif
 737   5      
 738   5                mcFocCtrl.TorqueLoopTime= SPEED_LOOP_TIME;
 739   5                PI_UK = mcFocCtrl.mcIqref;
 740   5              }
 741   4            }
 742   3            break;
 743   3            case 1:
 744   3            {
 745   4              mcFocCtrl.TorqueLoopTime++;
 746   4              if(mcFocCtrl.TorqueLoopTime>SPEED_LOOP_TIME)
 747   4              {
 748   5      
 749   5                mcFocCtrl.TorqueLoopTime=0;
 750   5                #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)
 768   5                {
 769   6                  mc_ramp(&mcSpeedRamp);
 770   6      
C51 COMPILER V9.52.0.0   ADDFUNCTION                                                       09/06/2018 16:05:13 PAGE 14  

 771   6                  #if (Motor_Speed_Control_Mode == SPEED_LOOP_CONTROL)
 772   6                  {
 773   7                    FOC_IQREF= HW_One_PI(mcSpeedRamp.ActualValue- mcFocCtrl.SpeedFlt);//COMPESATE是作为补偿
 774   7                  }
 775   6                  #elif (Motor_Speed_Control_Mode == POWER_LOOP_CONTROL)
                          {
                            FOC_IQREF= HW_One_PI(mcSpeedRamp.ActualValue- mcFocCtrl.Powerlpf);
                          }
                          #endif
 780   6                }
 781   5                #endif //END OUTLoop_Mode
 782   5                if(FOC_EK2 < OBS_K2T_Actual-10)
 783   5                {
 784   6                  mcFocCtrl.Smo_EK2=FOC_EK2;
 785   6                  mcFocCtrl.Smo_EK2+=10;
 786   6                  FOC_EK2=mcFocCtrl.Smo_EK2;
 787   6                }
 788   5                else if(FOC_EK2 > OBS_K2T_Actual+10)
 789   5                {
 790   6                  mcFocCtrl.Smo_EK2=FOC_EK2;
 791   6                  mcFocCtrl.Smo_EK2-=10;
 792   6                  FOC_EK2=mcFocCtrl.Smo_EK2;
 793   6                }
 794   5                else
 795   5                {
 796   6                  FOC_EK2=OBS_K2T_Actual;
 797   6                }
 798   5              }
 799   4            }
 800   3            break;
 801   3          }
 802   2        }
 803   1        #if (FRDetectMethod!=FOCMethod)
 804   1        {
 805   2          if(mcState==mcRun)
 806   2          {
 807   3            if(mcFocCtrl.RunStateCnt<15000)//15s的延迟，用于清RSDCCWFlag,BEMFCCWFlag
 808   3            {
 809   4              mcFocCtrl.RunStateCnt++;
 810   4            }
 811   3            else
 812   3            {
 813   4              BEMFDetect.BEMFCCWFlag= 0;
 814   4              RSDDetect.RSDCCWFlag  = 0;
 815   4            }
 816   3          }
 817   2        }
 818   1        #endif
 819   1      
 820   1      
 821   1      }
 822          
 823          /*---------------------------------------------------------------------------*/
 824          /* Name   : void FGOutput(void)
 825          /* Input  : NO
 826          /* Output : NO
 827          /* Description: FG信号输出
 828          /*---------------------------------------------------------------------------*/
 829          void FGOutput(void)
 830          {//FG可用中断FGIF进行判断
 831   1        if(mcState==mcRun)
 832   1        {
C51 COMPILER V9.52.0.0   ADDFUNCTION                                                       09/06/2018 16:05:13 PAGE 15  

 833   2          if((FOC__THETA>=0)&&(FOC__THETA<32768))//0-180
 834   2          {
 835   3            ResetFGPin;
 836   3          }
 837   2          else if((FOC__THETA>=32768)&&(FOC__THETA<65536))//180-360
 838   2          {
 839   3            SetFGPin;
 840   3          }
 841   2        }
 842   1        else if(mcState == mcFault)
 843   1        {
 844   2            SetFGPin;
 845   2        }
 846   1        else
 847   1        {
 848   2            ResetFGPin;
 849   2        }
 850   1      }
 851          /*---------------------------------------------------------------------------*/
 852          /* Name   :  uint16 SoftLPF(uint16 Xn1, uint16 Xn0, uint16 K)
 853          /* Input  : uint16 Xn1, uint16 Xn0, uint16 K
 854          /* Output : uint16
 855          /* Description: 软件低通滤波
 856          /*---------------------------------------------------------------------------*/
 857           int16 SoftLPF(int16 Xn1, int16 Xn0, int16 K)
 858           {
 859   1        int16 Temp16 = 0;
 860   1        int32 Temp32 = 0;
 861   1      
 862   1        Temp32 = (((int32)Xn1 - (int32)Xn0) * (int32)K) >> 15;
 863   1        Temp16 = Xn0 + (int16)Temp32;
 864   1        return Temp16;
 865   1       }
 866          
 867          
 868          /*---------------------------------------------------------------------------*/
 869          /* Name   : void LED_Display(void)
 870          /* Input  : NO
 871          /* Output : NO
 872          /* Description: LED灯显示
 873          /*---------------------------------------------------------------------------*/
 874          void LED_Display(void)
 875          {
 876   1        switch(mcFaultSource)
 877   1          {
 878   2          case FaultNoSource:
 879   2              ResetLEDPin;            //低电平点亮，高电平灭
 880   2              break;
 881   2          case FaultHardOVCurrent:
 882   2              Led_OnOff(&mcLedDisplay, 1);
 883   2              break;
 884   2          case FaultSoftOVCurrent:
 885   2              Led_OnOff(&mcLedDisplay, 2);
 886   2              break;
 887   2          case FaultUnderVoltage:
 888   2              Led_OnOff(&mcLedDisplay, 3);
 889   2              break;
 890   2          case FaultOverVoltage:
 891   2              Led_OnOff(&mcLedDisplay, 4);
 892   2              break;
 893   2          case FaultLossPhase:
 894   2              Led_OnOff(&mcLedDisplay, 5);
C51 COMPILER V9.52.0.0   ADDFUNCTION                                                       09/06/2018 16:05:13 PAGE 16  

 895   2              break;
 896   2          case FaultStall:
 897   2              Led_OnOff(&mcLedDisplay, 6);
 898   2              break;
 899   2          case  FaultStart:
 900   2              Led_OnOff(&mcLedDisplay, 7);
 901   2              break;
 902   2          case  FaultOverwind:
 903   2              Led_OnOff(&mcLedDisplay, 8);
 904   2              break;
 905   2          case FaultOverHeat:
 906   2              Led_OnOff(&mcLedDisplay, 9);
 907   2              break;
 908   2              default:
 909   2                break;
 910   2        }
 911   1      
 912   1      }
 913          
 914           //LED灯的闪烁
 915           void Led_OnOff(MCLedDisplay *hLedDisplay, uint8 htime) {
 916   1           hLedDisplay->LedCount++;
 917   1          //if (hLedDisplay->LedCount < hLedDisplay->Counttime) {
 918   1          hLedDisplay->Count++;
 919   1          if (hLedDisplay->Count < (htime << 10)) {
 920   2              if ((hLedDisplay->Count & 0x03ff) <  0x01ff) {
 921   3                  SetLEDPin;
 922   3              }
 923   2              else{
 924   3                  ResetLEDPin;
 925   3              }
 926   2          } else if (hLedDisplay->Count < ((htime << 10) + 2000)) {
 927   2              SetLEDPin;
 928   2          } else {
 929   2              hLedDisplay->LedCount = 0;
 930   2              hLedDisplay->LedTimCot = 0;
 931   2              hLedDisplay->Count = 0;
 932   2          }
 933   1      }
 934          /*---------------------------------------------------------------------------*/
 935          /* Name   : void mc_ramp(void)
 936          /* Input  : hTarget,MC_RAMP *hSpeedramp
 937          /* Output : NO
 938          /* Description:
 939          /*---------------------------------------------------------------------------*/
 940          
 941          void mc_ramp(MCRAMP *hSpeedramp)
 942          {
 943   1          if( --hSpeedramp->DelayCount < 0)
 944   1          {
 945   2              hSpeedramp->DelayCount = hSpeedramp->DelayPeriod;
 946   2      
 947   2              if (hSpeedramp->ActualValue < hSpeedramp->TargetValue)
 948   2              {
 949   3                  if(hSpeedramp->ActualValue + Motor_Speed_Inc < hSpeedramp->TargetValue)
 950   3                  {
 951   4                      hSpeedramp->ActualValue += Motor_Speed_Inc;
 952   4                  }
 953   3                  else
 954   3                  {
 955   4                      hSpeedramp->ActualValue = hSpeedramp->TargetValue;
 956   4                  }
C51 COMPILER V9.52.0.0   ADDFUNCTION                                                       09/06/2018 16:05:13 PAGE 17  

 957   3              }
 958   2              else
 959   2              {
 960   3                  if(hSpeedramp->ActualValue - Motor_Speed_Dec > hSpeedramp->TargetValue)
 961   3                  {
 962   4      
 963   4                      hSpeedramp->ActualValue -= Motor_Speed_Dec;
 964   4                  }
 965   3                  else
 966   3                  {
 967   4                      hSpeedramp->ActualValue = hSpeedramp->TargetValue;
 968   4                  }
 969   3              }
 970   2          }
 971   1      }
 972          void HW_Speed_PI(void)
 973          {
 974   1      //    PI_EK =  mcSpeedRamp.ActualValue- mcFocCtrl.Powerlpf; //给定转速与实际转速之差，可区分�
             -��功率或恒转速，或恒电压
 975   1          PI_EK =  mcSpeedRamp.ActualValue- FOC__EOME;  //给定转速与实际转速之差，直接减FOC__EOME有
             -风险
 976   1          PI_LPF_CR |= 0x02;                  // Start PI
 977   1          _nop_();  _nop_();  _nop_();  _nop_();  _nop_();
 978   1      //    mcFocCtrl.mcIqref= PI_UK;//可区分限功率与不限功率
 979   1      //    FOC_IQREF =mcFocCtrl.mcIqref;
 980   1          PI_UK+=(SKP/4096 +1);
 981   1          FOC_IQREF= PI_UK;
 982   1      
 983   1      }
 984          /*-------------------------------------------------------------------------------------------------
 985            Function Name : int16 HW_Genal_PI(int16 Xn1, int16 Yn0, int16 Xn2)
 986            U(K) = U(k-1) + Kp*(E(k)-E(k-1)) + Ki*E(k) ----- (Uk_min < U(k) < Uk_max)b
 987            Description   : PI控制
 988            Input         : Xn1--E(K-1)
 989                            Yn0--U(K-1)
 990                    Xn2--E(K)
 991            Output      : PI_UK--当前PI输出值,执行时间为3.8us
 992          -------------------------------------------------------------------------------------------------*/
 993          int16 HW_Genal_PI(int16 Xn1, int16 Yn0, int16 Xn2)
 994          {
 995   1          PI_KP = SKP;
 996   1          PI_KI = SKI;
 997   1          PI_UKMAX = SOUTMAX;
 998   1          PI_UKMIN = SOUTMIN;
 999   1          PI_EK =  Xn1;         //初始化E(K-1)
1000   1          PI_LPF_CR |= 0x02;    // Start PI
1001   1          _nop_();  _nop_();  _nop_();  _nop_();  _nop_();
1002   1          PI_UK =  Yn0;           //初始化U(K-1)
1003   1          PI_EK =  Xn2;       //填入EK
1004   1          PI_LPF_CR |= 0x02;    // Start PI
1005   1          _nop_();  _nop_();  _nop_();  _nop_();  _nop_();
1006   1          PI_UK+=(SKP/4096 +1);
1007   1          return PI_UK;
1008   1      }
1009          /*-------------------------------------------------------------------------------------------------
1010            Function Name : int16 HW_One_PI(int16 Xn1, int16 Yn0, int16 Xn2)
1011            Description   : PI控制
1012            Input         : Xn1--E(K)
1013            Output      : PI_UK--当前PI输出值,执行时间us
1014          -------------------------------------------------------------------------------------------------*/
1015          int16 HW_One_PI(int16 Xn1)
1016          {
C51 COMPILER V9.52.0.0   ADDFUNCTION                                                       09/06/2018 16:05:13 PAGE 18  

1017   1          PI_EK =  Xn1;       //填入EK
1018   1          PI_LPF_CR |= 0x02;    // Start PI
1019   1          _nop_();  _nop_();  _nop_();  _nop_();  _nop_();
1020   1          PI_UK+=(SKP/4096 +1);
1021   1          return PI_UK;
1022   1      }
1023          /*-------------------------------------------------------------------------------------------------
1024            Function Name : int16 LPF(int16 Xn1, int16 Xn0, int8 K)
1025            Description   : LFP控制
1026            Input         : Xn1--当前输入值
1027                            Xn0--上一次滤波输出值
1028                            K--LPF滤波系数
1029            Output        : LPF_Y--当前滤波输出值，执行时间为4us。
1030          -------------------------------------------------------------------------------------------------*/
1031          int16 LPFFunction(int16 Xn1, int16 Xn0, int8 K)
1032          {
1033   1        LPF_K = K;
1034   1        LPF_X = Xn1;
1035   1        LPF_Y = Xn0;
1036   1        SetBit(PI_LPF_CR, LPFSTA);
1037   1        _nop_();_nop_();_nop_();_nop_();_nop_();
1038   1        return LPF_Y;
1039   1      }
1040          
1041          /*---------------------------------------------------------------------------*/
1042          /* Name   : void VSPSample(void)
1043          /* Input  : NO
1044          /* Output : NO
1045          /* Description: VSP采样
1046          /*---------------------------------------------------------------------------*/
1047          void VSPSample(void)
1048          {
1049   1        /*****VREF的采样获取值并滤波******/
1050   1        AdcSampleValue.ADCVref = (ADC7_DR<<3);//采集对应端口
1051   1        VSP=LPFFunction(AdcSampleValue.ADCVref,VSP,30);//低通滤波
1052   1      
1053   1         if((VSP > ONPWMDuty)&&(VSP <= OFFPWMDutyHigh))//在ONPWMDuty-OFFPWMDutyHigh之间，电机有转速运�
             -�
1054   1          {
1055   2            mcSpeedRamp.FlagONOFF = 1;
1056   2          }
1057   1         else if((VSP < OFFPWMDuty)||(VSP > OFFPWMDutyHigh))//电机停机
1058   1          {
1059   2            mcSpeedRamp.FlagONOFF = 0;
1060   2          }
1061   1          //转速曲线计算
1062   1          if(mcSpeedRamp.FlagONOFF==1)//
1063   1          {
1064   2            #if (Motor_Speed_Control_Mode == SPEED_LOOP_CONTROL)
1065   2            {
1066   3              if(VSP <= MINPWMDuty)    //最小转速运行
1067   3              {
1068   4                mcSpeedRamp.TargetValue = Motor_Min_Speed;
1069   4              }
1070   3              else if(VSP < MAXPWMDuty)//调速
1071   3              {
1072   4                mcSpeedRamp.TargetValue = Motor_Min_Speed + SPEED_K*(VSP-MINPWMDuty);
1073   4              }
1074   3              else         //最大转速运行
1075   3              {
1076   4                mcSpeedRamp.TargetValue = Motor_Max_Speed;
1077   4              }
C51 COMPILER V9.52.0.0   ADDFUNCTION                                                       09/06/2018 16:05:13 PAGE 19  

1078   3      
1079   3              if (Uart.uartTimerCtl > 0) {
1080   4                  if (Uart.power == 0x01) {
1081   5                      if (Uart.rpmCtrl > 500) {
1082   6                          mcSpeedRamp.TargetValue = Uart.rpmCtrl;
1083   6                      }
1084   5                      else{
1085   6                          mcSpeedRamp.TargetValue = 0;
1086   6                      }
1087   5                  }
1088   4                  else{
1089   5                      mcSpeedRamp.TargetValue = 0;
1090   5                  }
1091   4              }
1092   3      
1093   3            }
1094   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;
                      }
                    //  if( mcSpeedRamp.TargetValue > _Q15(0.1)) mcSpeedRamp.TargetValue = 400;
                    }
                    #endif
1111   2      
1112   2          }
1113   1          else
1114   1          {
1115   2            mcSpeedRamp.TargetValue =0;
1116   2          }
1117   1      }
1118          /*---------------------------------------------------------------------------*/
1119          /* Name   : void Sleepmode(void)
1120          /* Input  : NO
1121          /* Output : NO
1122          /* Description: 睡眠模式测试
1123          /*---------------------------------------------------------------------------*/
1124           void Sleepmode(void)
1125           {
1126   1            SleepSet.SleepDelayCout++;
1127   1          if(SleepSet.SleepDelayCout>=20000)//最大65530，若要再大，需改数据类型
1128   1          {
1129   2      //        FOC_EFREQMIN  = -Motor_Omega_Ramp_Min;
1130   2      //        FOC_EFREQHOLD = -Motor_Omega_Ramp_End;
1131   2            mcSpeedRamp.TargetValue=0;
1132   2            MOE     = 0;
1133   2            ClrBit(DRV_CR, FOCEN);  //关闭FOC
1134   2            SleepSet.SleepDelayCout=0;
1135   2            SleepSet.SleepFlag=1;
1136   2            SetBit(P1_IE, P11);   // config P11 as the source of EXTI1
1137   2            SetBit(PCON, STOP);
1138   2          }
1139   1      
C51 COMPILER V9.52.0.0   ADDFUNCTION                                                       09/06/2018 16:05:13 PAGE 20  

1140   1       }
1141          /*---------------------------------------------------------------------------*/
1142          /* Name   : void StarRampDealwith(void)
1143          /* Input  : NO
1144          /* Output : NO
1145          /* Description:
1146          /*---------------------------------------------------------------------------*/
1147          void StarRampDealwith(void)
1148          {
1149   1      //    int temp=0;
1150   1          if((mcState == mcRun))
1151   1          {
1152   2      //            if(FOC_IQREF<mcFocCtrl.mcIqref)
1153   2      //            {
1154   2      //                temp=FOC_IQREF;
1155   2      //                temp+=20;
1156   2      //                FOC_IQREF = temp;
1157   2      //            }
1158   2            if(mcFocCtrl.State_Count == 1000)//2300
1159   2            {
1160   3              FOC_EKP = OBSW_KP_GAIN_RUN;                           // 估算器里的PI的KP
1161   3              FOC_EKI = OBSW_KI_GAIN_RUN;                           // 估算器里的PI的KI
1162   3              GP07 = 1;
1163   3            }
1164   2            else if(mcFocCtrl.State_Count == 930)//2000
1165   2            {
1166   3              FOC_EKP = OBSW_KP_GAIN_RUN1;                          // 估算器里的PI的KP
1167   3              FOC_EKI = OBSW_KI_GAIN_RUN1;                          // 估算器里的PI的KI
1168   3              GP07 = 0;
1169   3            }
1170   2            else if(mcFocCtrl.State_Count == 850)//1600
1171   2            {
1172   3              FOC_EKP = OBSW_KP_GAIN_RUN2;                          // 估算器里的PI的KP
1173   3              FOC_EKI = OBSW_KI_GAIN_RUN2;                          // 估算器里的PI的KI
1174   3              GP07 = 1;
1175   3            }
1176   2            else if(mcFocCtrl.State_Count ==700)//1200
1177   2            {
1178   3              FOC_EKP = OBSW_KP_GAIN_RUN3;                          // 估算器里的PI的KP
1179   3              FOC_EKI = OBSW_KI_GAIN_RUN3;                          // 估算器里的PI的KI
1180   3              GP07 = 0;
1181   3            }
1182   2            else if(mcFocCtrl.State_Count == 600)
1183   2            {
1184   3              FOC_EKP = OBSW_KP_GAIN_RUN4;                          // 估算器里的PI的KP
1185   3              FOC_EKI = OBSW_KI_GAIN_RUN4;                          // 估算器里的PI的KI
1186   3              GP07 = 1;
1187   3            }
1188   2            else ;
1189   2          }
1190   1      }


MODULE INFORMATION:   STATIC OVERLAYABLE
   CODE SIZE        =   4587    ----
   CONSTANT SIZE    =   ----    ----
   XDATA SIZE       =    115      16
   PDATA SIZE       =   ----    ----
   DATA SIZE        =   ----    ----
   IDATA SIZE       =    124    ----
   BIT SIZE         =   ----    ----
   EDATA SIZE       =   ----    ----
C51 COMPILER V9.52.0.0   ADDFUNCTION                                                       09/06/2018 16:05:13 PAGE 21  

   HDATA SIZE       =   ----    ----
   XDATA CONST SIZE =   ----    ----
   FAR CONST SIZE   =   ----    ----
END OF MODULE INFORMATION.


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