C51 COMPILER V9.52.0.0   ADDFUNCTION                                                       09/21/2018 10:28:35 PAGE 1   


C51 COMPILER V9.52.0.0, COMPILATION OF MODULE ADDFUNCTION
OBJECT MODULE PLACED IN .\Output\AddFunction.obj
COMPILER INVOKED BY: F:\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) 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          Ramp_TypeDef       idata   SpeedRamp;
  25          
  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          BreakDowmVarible   xdata     mcBreakDowm;
  34          int16                                            xdata   VSP;
  35          
  36          const int16  PowerGiven[10] ={0,500,1000,2200,2900,4300,5900,7300,7900,8550};
  37          
  38          uint8 OppoBrakeFlag = 0;//反角度刹车标志位,0为运行中，1为刹车中
  39          
  40          int16 RealAngleGivenToFoc = 0;
  41          
  42          extern  int Test2;
  43          
  44          extern char KeyBrake;
  45          int16 KeyBrakeRTheStep = 0;
  46          
  47          int16 KeyBrakeAngleCompensate = 0;
  48          
  49          int16 CCCW_Flag = 0;
  50          int tTest3 = 0;
  51          char BackSelfLock = 0;
  52          char OnSelfLock = 0;
  53          int16 RampStepInc = 0;
C51 COMPILER V9.52.0.0   ADDFUNCTION                                                       09/21/2018 10:28:35 PAGE 2   

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

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

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

 233   3                                              if(h_Fault->OverVoltDetecCnt > 20)//检测100ms
 234   3                                              {
 235   4                                                      h_Fault->OverVoltDetecCnt = 0;
 236   4                                                      mcFaultSource=FaultOverVoltage;
 237   4                                                      FaultProcess();
 238   4                                              }
 239   3                                      }
 240   2                                      else
 241   2                                      {
 242   3                                              if(h_Fault->OverVoltDetecCnt>0)
 243   3                                              {
 244   4                                                      h_Fault->OverVoltDetecCnt--;
 245   4                                              }
 246   3                                      }
 247   2      
 248   2                              //欠压保护
 249   2      
 250   2                                      if(mcFocCtrl.mcADCVoltagebus< UNDER_PROTECT_VALUE)
 251   2                                      {
 252   3                                              h_Fault->UnderVoltDetecCnt++;
 253   3      
 254   3                                              if(h_Fault->UnderVoltDetecCnt > 20)//检测100ms
 255   3                                              {
 256   4                                                      h_Fault->UnderVoltDetecCnt = 0;
 257   4                                                      mcFaultSource=FaultUnderVoltage;
 258   4                                                      FaultProcess();
 259   4                                              }
 260   3                                      }
 261   2                                      else
 262   2                                      {
 263   3                                              if(h_Fault->UnderVoltDetecCnt>0)
 264   3                                              {
 265   4                                                      h_Fault->UnderVoltDetecCnt--;
 266   4                                              }
 267   3                                      }
 268   2                      }
 269   1      
 270   1                      /*******过压欠压保护恢复*********/
 271   1                      #if (VoltageProtectRecoverEnble)
                              {
                                      if((mcState == mcFault) &&((mcFaultSource==FaultUnderVoltage)||(mcFaultSource==FaultOverVoltage)))
                                      {
                                              if((mcFocCtrl.mcADCVoltagebus< OVER_RECOVER_VALUE)&&(mcFocCtrl.mcADCVoltagebus> UNDER_RECOVER_VALUE))
                                              {
                                                      h_Fault->VoltRecoverCnt++;
                                                      if(h_Fault->VoltRecoverCnt>200)//连续检测1s，若正常则恢复
                                                      {
                                                              mcState = mcInit;
                                                              mcFaultSource=FaultNoSource;
                                                              h_Fault->VoltRecoverCnt = 0;
                                                      }
                                              }
                                              else
                                              {
                                                      h_Fault->VoltRecoverCnt = 0;
                                              }
                               }
                       }
                       #endif
 292   1      }
 293          
 294          /*****************************************************************************
C51 COMPILER V9.52.0.0   ADDFUNCTION                                                       09/21/2018 10:28:35 PAGE 6   

 295           * Function:             void Fault_Overcurrent(CurrentVarible *h_Cur)
 296           * Description:  电机运行或者启动时，当三相中某一相最大值大于OverCurrentValue，则O
             -verCurCnt加1。
 297                                                                           连续累加3次，判断为软件过流保护。执行时间约30.4us。
 298           * Parameter:            mcFaultVarible *h_Fault
 299           * Return:                       no
 300           *****************************************************************************/
 301          void Fault_Overcurrent(CurrentVarible *h_Cur)
 302          {
 303   1              if(mcState == mcRun)//||(mcState == mcStart))                                           // check over current in rum and open mode
 304   1              {
 305   2                      h_Cur->Abs_ia = Abs_F16(FOC__IA);
 306   2                      h_Cur->Abs_ib = Abs_F16(FOC__IB);
 307   2                      h_Cur->Abs_ic = Abs_F16(-FOC__IA-FOC__IB);
 308   2                      
 309   2      //              if(h_Cur->Abs_ia> h_Cur->Max_ia)                      // 此部分既用于软件过流保护，又用
             -于缺相保护
 310   2      //              {
 311   2      //                       h_Cur->Max_ia = h_Cur->Abs_ia;
 312   2      //              }
 313   2      //              if(h_Cur->Abs_ib > h_Cur->Max_ib)
 314   2      //              {
 315   2      //                       h_Cur->Max_ib = h_Cur->Abs_ib;
 316   2      //              }
 317   2      //              if(h_Cur->Abs_ic > h_Cur->Max_ic)
 318   2      //              {
 319   2      //                       h_Cur->Max_ic = h_Cur->Abs_ic;
 320   2      //              }
 321   2                      
 322   2      //              if((mcFocCtrl.mcADCProtectCurrentbus>=OverSoftCurrentValue)||(h_Cur->Abs_ib>=OverSoftCurrentValue)||(h
             -_Cur->Abs_ic>=OverSoftCurrentValue))
 323   2                      if(mcFocCtrl.mcADCProtectCurrentbus >= OverSoftCurrentValue)
 324   2                      {
 325   3                              h_Cur->OverCurCnt++;
 326   3                              if(h_Cur->OverCurCnt>=3)
 327   3                              {
 328   4      //                              h_Cur->Abs_ia=0;
 329   4      //                              h_Cur->Abs_ib=0;
 330   4      //                              h_Cur->Abs_ic=0;
 331   4                                      h_Cur->OverCurCnt=0;
 332   4                                mcFaultSource=FaultSoftOVCurrent;
 333   4                                FaultProcess();
 334   4                              }
 335   3                      }
 336   2          else
 337   2                      {
 338   3                              if(h_Cur->OverCurCnt>0)
 339   3                              {
 340   4                                      h_Cur->OverCurCnt--;
 341   4                              }
 342   3                      }
 343   2              }
 344   1      }
 345          
 346          /*****************************************************************************
 347           * Function:             void   Fault_OverCurrentRecover(mcFaultVarible *h_Fault)
 348           * Description:  软硬件过流保护恢复
 349           * Parameter:            mcFaultVarible *h_Fault
 350           * Return:                       no
 351           *****************************************************************************/
 352          void Fault_OverCurrentRecover(FaultVarible *h_Fault)
 353          {
C51 COMPILER V9.52.0.0   ADDFUNCTION                                                       09/21/2018 10:28:35 PAGE 7   

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

 411          //                                                              h_Fault->StartDelay=0;
 412          //                                                              h_Fault->StartESCount=0;
 413          //                                                              mcProtectTime.StartFlag  =  2;
 414          //                                                      }
 415          //                                              }
 416          //            else
 417          //                                              {
 418          //                                                      if(h_Fault->StartESCount>0)
 419          //                                                              h_Fault->StartESCount--;
 420          //                                              }
 421          //                       }
 422          //               }
 423          //     else
 424          //     {
 425          //       h_Fault->StartESCount=0;
 426          //     }
 427          //                      //方法三，长时间在CtrlMode=0状态
 428          //                      if(mcFocCtrl.CtrlMode==0)         //
 429          //                      {
 430          //                              h_Fault->StartFocmode++;
 431          //                              if(h_Fault->StartFocmode>=400)
 432          //                              {
 433          //                                      h_Fault->StartFocmode=0;
 434          //                                      mcFaultSource=FaultStart;
 435          //                                      FaultProcess();
 436          //                                      mcProtectTime.SecondStartTimes++;
 437          //          mcProtectTime.StartFlag  =  3;
 438          //        }
 439          //                      }
 440          //        }
 441          
 442          //               if((mcFaultSource==FaultStart)&&(mcState == mcFault)&&(mcProtectTime.SecondStartTimes<=StartProtectRe
             -startTimes))
 443          //               {
 444          //                       mcFaultSource=FaultNoSource;
 445          //                       mcState = mcInit;
 446          //               }
 447          
 448          
 449          //      }
 450           /*****************************************************************************
 451           * Function:             void   Fault_Stall(mcFaultVarible *h_Fault)
 452           * Description:  堵转保护函数
 453           * Parameter:            mcFaultVarible *h_Fault
 454           * Return:                       no
 455           *****************************************************************************/
 456          //堵转保护
 457          void Fault_Stall(FaultVarible *h_Fault)
 458          {       
 459   1              if(!mcFocCtrl.LockedEnbleFlag)  //不锁机
 460   1              {
 461   2                      if((mcState == mcRun)||(mcState == mcStart))
 462   2                      {                                               
 463   3                              if(mcHall.HALLLockStatus)
 464   3                              {
 465   4                                      if(mcHall.mcStallTimeCnt < StallTime)
 466   4                                      {
 467   5                                              if(mcFocCtrl.mcIqref >= StallMINPWMDuty)
 468   5                                              {
 469   6                                                      mcFocCtrl.mcIqref -= 80;
 470   6      //                                              SpeedRamp.RampStep = mcFocCtrl.mcIqref;
 471   6                                                      
C51 COMPILER V9.52.0.0   ADDFUNCTION                                                       09/21/2018 10:28:35 PAGE 9   

 472   6                                                      //FOC__UQ = SpeedRamp.RampStep;
 473   6      //                                              FOC_QMAX                = SpeedRamp.RampStep;
 474   6      //                                              FOC_QMIN                = SpeedRamp.RampStep;
 475   6                                              }       
 476   5                                      }
 477   4                                      else
 478   4                                      {
 479   5                                              mcHall.mcStallTimeCnt = StallTime;
 480   5                                              
 481   5                                              mcFaultSource=FaultStall;
 482   5      
 483   5                                              FaultProcess();         
 484   5                                      }
 485   4                              }                       
 486   3                      }
 487   2              }       
 488   1      
 489   1      //      if(mcState == mcRun)
 490   1      //      {
 491   1      //              if(h_Fault->StallDelayCnt <=1000)//5s
 492   1      //              {
 493   1      //                      h_Fault->StallDelayCnt ++;
 494   1      //              }
 495   1      //              else
 496   1      //              {
 497   1      //          //method 1，判断反电动势太小或当反电动势太小，转速太大
 498   1      //      if((&mcFocCtrl.EsValue< 10)||((FOC__EOME > _Q15(1600.0/MOTOR_SPEED_BASE))&&(mcFocCtrl.EsValue< 80)
             -))
 499   1      //                      {
 500   1      //                              h_Fault->StallDectEs++;
 501   1      //                              if(h_Fault->StallDectEs >= 10)
 502   1      //                              {
 503   1      //                                      h_Fault->StallDectEs=0;
 504   1      //                                      mcFaultSource=FaultStall;
 505   1      //                                      mcProtectTime.StallTimes++;
 506   1      //                                      FaultProcess();
 507   1      //                                      mcProtectTime.StallFlag  =  1;
 508   1      //                              }
 509   1      //                      }
 510   1      //                      else
 511   1      //                      {
 512   1      //        if(   h_Fault->StallDectEs>0)
 513   1      //                                h_Fault->StallDectEs--;
 514   1      //                      }
 515   1      //                      
 516   1      //                      //method 2，判断速度低于堵转最小值或者超过堵转最大值
 517   1      //                      if((mcFocCtrl.SpeedFlt<Motor_Stall_Min_Speed)||(mcFocCtrl.SpeedFlt > Motor_Stall_Max_Speed))
 518   1      //                      {
 519   1      //                              h_Fault->StallDectSpeed++;
 520   1      //                              if(h_Fault->StallDectSpeed>=8)
 521   1      //                              {
 522   1      //                                      h_Fault->StallDectSpeed=0;
 523   1      //                                      mcFaultSource=FaultStall;
 524   1      //                                      mcProtectTime.StallTimes++;
 525   1      //                                      FaultProcess();
 526   1      //                                      mcProtectTime.StallFlag =2;
 527   1      //                              }
 528   1      //                      }
 529   1      //                      else
 530   1      //                      {
 531   1      //        if(h_Fault->StallDectSpeed>0)
 532   1      //                              h_Fault->StallDectSpeed--;
C51 COMPILER V9.52.0.0   ADDFUNCTION                                                       09/21/2018 10:28:35 PAGE 10  

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

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

 651   3                                        Fault_Stall(&mcFaultDect);
 652   3                                      }
 653   2            }
 654   1                      else if(mcFaultDect.segment==4)
 655   1                              {
 656   2                                      if(PhaseLossProtectEnable==1)//缺相保护使能
 657   2                                      {
 658   3                                        Fault_phaseloss(&mcFaultDect);
 659   3                                      }
 660   2            }
 661   1                      else
 662   1                              {
 663   2      
 664   2            }
 665   1      }
 666          
 667          /*---------------------------------------------------------------------------*/
 668          /* Name         :       void ONOFF_Starttest(void)
 669          /* Input        :       NO
 670          /* Output       :       NO
 671          /* Description: 启动测试
 672          /*---------------------------------------------------------------------------*/
 673          //void ONOFF_Starttest(ONVarible  *h_test)
 674          //{
 675          //      if(h_test->ONOFF_Flag==1)
 676          //      {
 677          //               h_test->ON_Count++;
 678          //              if(h_test->ON_Count>StartON_Time)
 679          //              {
 680          //                      h_test->ON_Count=0;
 681          //                      h_test->ONOFF_Times++;
 682          //                      h_test->ONOFF_Flag=0;
 683          //                mcSpeedRamp.FlagONOFF = 0;
 684          //                      mcSpeedRamp.TargetValue = 0;
 685          //              }
 686          //  }
 687          //      else
 688          //      {
 689          //              if(mcState!=mcFault)
 690          //              {
 691          //                h_test->OFF_Count++;
 692          //                      if(h_test->OFF_Count>StartOFF_Time)
 693          //                      {
 694          //                              h_test->OFF_Count=0;
 695          //                              h_test->ONOFF_Flag=1;
 696          //                               mcSpeedRamp.FlagONOFF = 1;
 697          //                               mcSpeedRamp.TargetValue = Motor_Min_Speed;
 698          //                      }
 699          //        }
 700          //  }
 701          //}
 702          
 703          /*---------------------------------------------------------------------------*/
 704          /* Name         :       void Speed_response(void)
 705          /* Input        :       NO
 706          /* Output       :       NO
 707          /* Description: 速度响应函数，可根据需求加入控制环，如恒转矩控制、恒转速控制
             -、恒功率控制
 708          /*---------------------------------------------------------------------------*/
 709          void Speed_response(void)
 710          {
 711   1              static uint16  VOP_K = 0;
C51 COMPILER V9.52.0.0   ADDFUNCTION                                                       09/21/2018 10:28:35 PAGE 13  

 712   1              static uint16 SPEED_MaxPWMDuty = 0;
 713   1      
 714   1              static uint8 SpeedLimit1IOCnt = 0;
 715   1              static uint8 SpeedLimit2IOCnt = 0;
 716   1              static uint8 SpeedLimit3IOCnt = 0;
 717   1      
 718   1              static uint8 SoftStartIOCnt = 0;
 719   1      
 720   1              static uint8 SlowStopDutyFlag = 0;
 721   1              
 722   1                                                      if(mcFocCtrl.mcSpeedVSP <= _Q15(ProtecMinPWMDuty))
 723   1                                                      {
 724   2                                                              mcFocCtrl.mcSpeedVSP = _Q15(ProtecMinPWMDuty);
 725   2                                                      }
 726   1                                                      else if(mcFocCtrl.mcSpeedVSP <= _Q15(ONPWMDuty))
 727   1                                                      {
 728   2                                                              mcFocCtrl.OpenDutyCycle = (mcFocCtrl.mcSpeedVSP - _Q15(ONPWMDuty));//MinPWMDuty;
 729   2                                                              //mcFocCtrl.RMSLimitTargetCurrent = IQ_RUN_MIN_RMS_CURRENT;
 730   2                                                      }
 731   1                                                      else if(mcFocCtrl.mcSpeedVSP <= _Q15(ProtecMaxPWMDuty))
 732   1                                                      {
 733   2                                                              //mcFocCtrl.TempOpenDutyCycle = (mcFocCtrl.mcSpeedVSP - _Q15(ONPWMDuty));
 734   2                                                              
 735   2                                                              //mcFocCtrl.TempOpenDutyCycle = mcFocCtrl.TempOpenDutyCycle >>6;
 736   2                                                                                                              
 737   2                                                              //mcFocCtrl.OpenDutyCycle = MinPWMDuty + VOP_K * mcFocCtrl.TempOpenDutyCycle;
 738   2                                                              mcFocCtrl.OpenDutyCycle = (mcFocCtrl.mcSpeedVSP - _Q15(ONPWMDuty));                                                                                                     
 739   2                                                              //mcFocCtrl.RMSLimitTargetCurrent = IQ_RUN_MIN_RMS_CURRENT+ IQ_RMSlIMIT_K * (mcFocCtrl.mcSpeedVSP-_
             -Q15(ONPWMDuty));
 740   2                                                      }
 741   1                                                      else
 742   1                                                      {
 743   2                                                              mcFocCtrl.mcSpeedVSP = _Q15(ProtecMaxPWMDuty);
 744   2                                                      }
 745   1                                                                              
 746   1                                                      
 747   1      //                                              RampStepInc = 80;
 748   1                                      if(SpeedRamp.RampStep <= mcFocCtrl.OpenDutyCycle)
 749   1                                      {
 750   2                                              if((SpeedRamp.RampStep + RampStepInc) <= mcFocCtrl.OpenDutyCycle)
 751   2                                              {
 752   3                                                      SpeedRamp.RampStep += RampStepInc;
 753   3                                              }
 754   2                                              else
 755   2                                              {
 756   3                                                      SpeedRamp.RampStep = mcFocCtrl.OpenDutyCycle;
 757   3                                              }
 758   2                                      }
 759   1                                      else
 760   1                                      {
 761   2                                              if((SpeedRamp.RampStep - RampStepInc) > mcFocCtrl.OpenDutyCycle)
 762   2                                              {
 763   3                                                      SpeedRamp.RampStep -= RampStepInc;
 764   3                                              }
 765   2                                              else
 766   2                                              {
 767   3                                                      SpeedRamp.RampStep = mcFocCtrl.OpenDutyCycle;
 768   3                                              }
 769   2                                      }
 770   1                                      
 771   1                                      
 772   1                                      //SpeedRamp.RampStep = RampControl(&SpeedRamp,mcFocCtrl.OpenDutyCycle); 
C51 COMPILER V9.52.0.0   ADDFUNCTION                                                       09/21/2018 10:28:35 PAGE 14  

 773   1                                      
 774   1      //                              if(OppoBrakeFlag)
 775   1      //                              {
 776   1      //                                      mcFocCtrl.OpenDutyCycle = -1;
 777   1      //                                      SpeedRamp.RampStep = RampControl(&SpeedRamp,mcFocCtrl.OpenDutyCycle);
 778   1      //                                      
 779   1      //                                      if(mcHall.HallSpeed < 1000)
 780   1      //                                      {
 781   1      //                                              OppoBrakeFlag = 0;
 782   1      //                                              mcState = mcStop;
 783   1      //                                      }
 784   1      //                              }
 785   1      //                              if(KeyBrake)
 786   1      //                              {
 787   1      //                                      SpeedRamp.RampStep += 150;
 788   1      //                                      if(SpeedRamp.RampStep >= 16380)
 789   1      //                                      {
 790   1      //                                              SpeedRamp.RampStep  = 16380;
 791   1      //                                      }
 792   1      //                              }
 793   1      
 794   1      //                                      if(ReverseFlag)
 795   1      //                                      {
 796   1                                                      FOC__UQ = g_fCarAngle;
 797   1                                                      
 798   1      //                                              if(FOC__UQ < 0)
 799   1      //                                              {
 800   1      //                                                      KeyBrakeAngleCompensate = 4450;
 801   1      //                                              }
 802   1      //                                              else
 803   1      //                                              {
 804   1      //                                                      KeyBrakeAngleCompensate = 0;
 805   1      //                                              }
 806   1      //                                      }
 807   1      //                              FOC_QMAX                = SpeedRamp.RampStep;
 808   1      //                              FOC_QMIN                = SpeedRamp.RampStep;
 809   1      //                        FOC_IQREF = SpeedRamp.RampStep/50;                                                                                                            
 810   1      
 811   1              }
 812          
 813          
 814          /*---------------------------------------------------------------------------*/
 815          /* Name         :       void FGOutput(void)
 816          /* Input        :       NO
 817          /* Output       :       NO
 818          /* Description: FG信号输出
 819          /*---------------------------------------------------------------------------*/
 820          void FGOutput(void)
 821          {//FG可用中断FGIF进行判断
 822   1              if(mcState==mcRun)
 823   1              {
 824   2                      if((FOC__THETA>=0)&&(FOC__THETA<32768))//0-180
 825   2                      {
 826   3                              ResetFGPin;
 827   3                      }
 828   2                      else if((FOC__THETA>=32768)&&(FOC__THETA<65536))//180-360
 829   2                      {
 830   3                              SetFGPin;
 831   3                      }
 832   2        }
 833   1              else if(mcState == mcFault)
 834   1              {
C51 COMPILER V9.52.0.0   ADDFUNCTION                                                       09/21/2018 10:28:35 PAGE 15  

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

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

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

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

1080          ////      #elif (Motor_Speed_Control_Mode == POWER_LOOP_CONTROL)
1081          ////      {
1082          ////        if(VSP <= MINPWMDuty)    //最小转速运行
1083          ////        {
1084          ////          mcSpeedRamp.TargetValue = Motor_Min_Power;
1085          ////        }
1086          ////        else if(VSP < MAXPWMDuty)//调速
1087          ////        {
1088          ////          mcSpeedRamp.TargetValue = Motor_Min_Power + POWER_K*(VSP-MINPWMDuty);
1089          ////        }
1090          ////        else                                //最大转速运行
1091          ////        {
1092          ////          mcSpeedRamp.TargetValue   =       Motor_Max_Power;
1093          ////        }
1094          ////      }
1095          //      #endif
1096          //              }
1097          //              else
1098          //              {
1099          //                      mcSpeedRamp.TargetValue =0;
1100          //              }
1101          //}
1102          /*---------------------------------------------------------------------------*/
1103          /* Name         :       void Sleepmode(void)
1104          /* Input        :       NO
1105          /* Output       :       NO
1106          /* Description: 睡眠模式测试
1107          /*---------------------------------------------------------------------------*/
1108           void Sleepmode(void)
1109           {
1110   1                      SleepSet.SleepDelayCout++;
1111   1                      if(SleepSet.SleepDelayCout>=20000)//最大65530，若要再大，需改数据类型
1112   1                      {
1113   2      //                              FOC_EFREQMIN    = -Motor_Omega_Ramp_Min;
1114   2      //                              FOC_EFREQHOLD = -Motor_Omega_Ramp_End;
1115   2                              mcSpeedRamp.TargetValue=0;
1116   2                              MOE     = 0;
1117   2                              ClrBit(DRV_CR, FOCEN);  //关闭FOC
1118   2                              SleepSet.SleepDelayCout=0;
1119   2                              SleepSet.SleepFlag=1;
1120   2                              SetBit(P1_IE, P11);   // config P11 as the source of EXTI1
1121   2                              SetBit(PCON, STOP);
1122   2                      }
1123   1       }
1124          
1125          /*---------------------------------------------------------------------------*/
1126          /* Name         :       void ONOFFControl (void)
1127          /* Input        :       NO
1128          /* Output       :       NO
1129          /* Description:
1130          /*---------------------------------------------------------------------------*/
1131          void ONOFFControl (void)
1132          {
1133   1              static uint8 MotorONFilterTimer=0;
1134   1              static uint8 MotorOFFFilterTimer=0;
1135   1      
1136   1              static uint8  PowerUPRUNProtec = 0;
1137   1              static uint8  PowerUPStatus = 0;
1138   1      
1139   1              static uint8 PWMDutyONOFFCnt1 = 0;
1140   1              static uint8 PWMDutyONOFFCnt2 = 0;
1141   1              
C51 COMPILER V9.52.0.0   ADDFUNCTION                                                       09/21/2018 10:28:35 PAGE 20  

1142   1              static uint8 BrakeDelayTime = 0;
1143   1              
1144   1              static uint8 VSPPrectionCnt = 0;
1145   1              
1146   1      //                      if(((mcFocCtrl.mcSpeedVSP > _Q15(ONPWMDuty))&&(mcFocCtrl.mcSpeedVSP <_Q15(ProtecMaxPWMDuty))) && (mcF
             -ocCtrl.MotorONOFFStatus == 0)) //判断正传
1147   1      //                      {
1148   1      //                              
1149   1      //                                      if(PWMDutyONOFFCnt2<5)
1150   1      //                                      {
1151   1      //                                              PWMDutyONOFFCnt2 ++;
1152   1      //                                      }
1153   1      //                                      if(PWMDutyONOFFCnt2 >= 5)
1154   1      //                                      {
1155   1      //                                              PWMDutyONOFFCnt2 =5;
1156   1      //                                              PWMDutyONOFFCnt1 = 0;
1157   1      //                                              OnSelfLock = 0;
1158   1      //                                              mcFocCtrl.MotorONOFFStatus = 1;    //开机标志位置1                        
1159   1      //                                      }
1160   1      //                      }
1161   1      //                      else if((mcFocCtrl.mcSpeedVSP< _Q15(BackONPWMDuty)) && (mcFocCtrl.MotorONOFFStatus == 1)) //判断反
             -转
1162   1      //                      {
1163   1      //                              
1164   1      //                                      if(PWMDutyONOFFCnt2<5)
1165   1      //                                      {
1166   1      //                                              PWMDutyONOFFCnt2 ++;
1167   1      //                                      }
1168   1      //                                      if(PWMDutyONOFFCnt2 >= 5)
1169   1      //                                      {
1170   1      //                                              PWMDutyONOFFCnt2 =5;
1171   1      //                                              PWMDutyONOFFCnt1 = 0;
1172   1      //                                              BackSelfLock = 0;
1173   1      //                                              mcFocCtrl.MotorONOFFStatus = 0;    //开机标志位置1                        
1174   1      //                                      }
1175   1      //                      }
1176   1      //                      
1177   1      //              
1178   1      //              
1179   1      //      if((mcFocCtrl.MotorONOFFStatus)  && (!KeyBrake) && (!OnSelfLock))//正转处理程序
1180   1      //      {
1181   1      
1182   1      //              MotorONFilterTimer++;
1183   1      
1184   1      //              if(MotorONFilterTimer>10)
1185   1      //              {
1186   1      //                              CCCW_Flag = 2;
1187   1      //                              
1188   1      //                      if(ReverseFlag == 1)
1189   1      //                      {
1190   1      //                              mcHall.GivenFRStatus = CW;
1191   1      //                              ClrBit(DRV_CR,DDIR);
1192   1      //                              mcHall.StartAngleOffset   = StartAngleCW;
1193   1      //                              
1194   1      //                              mcState = mcInit;               
1195   1      //                      }
1196   1      //                      
1197   1      //                      else if(ReverseFlag == 0)
1198   1      //                      {
1199   1      //                              KeyBrake = 1;
1200   1      //                      }
1201   1      //                      
C51 COMPILER V9.52.0.0   ADDFUNCTION                                                       09/21/2018 10:28:35 PAGE 21  

1202   1      //                      OnSelfLock = 1;
1203   1      //                      MotorONFilterTimer=0;
1204   1      //              }
1205   1      //      }
1206   1      
1207   1      //      else if((!mcFocCtrl.MotorONOFFStatus)&&(!KeyBrake) && (!BackSelfLock))//反转处理程序
1208   1      //      {
1209   1      
1210   1      //              MotorONFilterTimer++;
1211   1      
1212   1      //              if(MotorONFilterTimer>10)
1213   1      //              {
1214   1      //                      
1215   1      //                              CCCW_Flag = 3 ;
1216   1      //                      if(ReverseFlag == 1)
1217   1      //                      {
1218   1      
1219   1      //                              mcHall.GivenFRStatus = CCW;
1220   1      //                              SetBit(DRV_CR,DDIR);
1221   1      //                              mcHall.StartAngleOffset   = StartAngleCCW;
1222   1      //                              
1223   1      //                              mcState = mcInit;                               
1224   1      //                      }
1225   1      //                      else if(ReverseFlag ==0)
1226   1      //                      {
1227   1      //                              KeyBrake = 1;
1228   1      //                      }
1229   1      //                      
1230   1      //                      BackSelfLock = 1;
1231   1      //                      MotorONFilterTimer=0;
1232   1      //              }
1233   1      //      }
1234   1      }
1235          
1236          
1237          void Hall_RTHESTEP_Capture(void)
1238          {
1239   1              static uint8 BaseFlag = 0 ;
1240   1      
1241   1              if(mcHall.HallSpeedInitStatus == 0)
1242   1              {
1243   2                      mcHall.StepTimeCount           = 0;     
1244   2                                                      
1245   2                      mcHall.MC_StepTime[0] = 0;
1246   2                      mcHall.MC_StepTime[1] = 0;
1247   2                      mcHall.MC_StepTime[2] = 0;
1248   2                      mcHall.MC_StepTime[3] = 0;
1249   2                      mcHall.MC_StepTime[4] = 0;
1250   2                      mcHall.MC_StepTime[5] = 0;
1251   2                                                              
1252   2                      mcHall.FlagSpeedCal =0;
1253   2                                      
1254   2                      mcHall.RTheStep      = 0;       
1255   2                      
1256   2                      mcHall.HallSpeedInitStatus =1;
1257   2      
1258   2      //              TIM4__CNTR = 0;         
1259   2              }
1260   1              else
1261   1              {
1262   2      //              ClrBit(TIM4_CR1, T4EN);                                            // 0，停止计数；1,使能计数 
1263   2                      
C51 COMPILER V9.52.0.0   ADDFUNCTION                                                       09/21/2018 10:28:35 PAGE 22  

1264   2                      mcHall.MC_StepTime[mcHall.HallStatus_Result-1] = TIM4__CNTR;
1265   2      
1266   2      //              ClrBit(TIM4_CR1, T4EN);                                            // 0，停止计数；1,使能计数         
1267   2                      
1268   2                      TIM4__CNTR = 0;
1269   2                      
1270   2      //              SetBit(TIM4_CR1, T4EN);                                            // 0，停止计数；1,使能计数
1271   2                                                                                      
1272   2                      if(mcHall.HallCommutationCount <= 6)
1273   2                      {
1274   3                              mcHall.HallThetaBase        = TempHallThetaBaseOneStep * mcHall.HallCommutationCount;//TEMPHallThetaBas
             -eOneStep[mcHall.HallCommutationCount-1];
1275   3                              
1276   3                              mcHall.HallSpeedBase        = TempHallSpeedBaseOneStep * mcHall.HallCommutationCount;//TEMPHallSpeedBas
             -eOneStep[mcHall.HallCommutationCount-1];
1277   3                              
1278   3                              BaseFlag =0;
1279   3                              
1280   3                              mcHall.FlagSpeedCal = 1;
1281   3                      }
1282   2                      else if((mcHall.HallCommutationCount > 6)&&(BaseFlag==0))       
1283   2                      {
1284   3                              mcHall.HallThetaBase = TempHallThetaBase;
1285   3                              
1286   3                              mcHall.HallSpeedBase = TempHallSpeedBase;               
1287   3                              
1288   3                              BaseFlag = 1;                                                   
1289   3                      }
1290   2                      
1291   2                                      
1292   2                      mcHall.PeriodTime = mcHall.MC_StepTime[0] + mcHall.MC_StepTime[1] + mcHall.MC_StepTime[2] + mcHall.MC_St
             -epTime[3] + mcHall.MC_StepTime[4] + mcHall.MC_StepTime[5];    //5.4us
1293   2              
1294   2                      if(mcHall.HallStatus_Result==6)
1295   2                      {
1296   3                              mcHall.FlagSpeedCal = 1;
1297   3                      }
1298   2      
1299   2                      MDUAPP1();  //16us
1300   2                      
1301   2      //              FOC__RTHESTEP = 0;
1302   2                      
1303   2      //              if()
1304   2                      if(mcHall.HallFRStatus == 2)
1305   2                      {
1306   3                              FOC__RTHESTEP  = 0- mcHall.RTheStep;            
1307   3                              KeyBrakeAngleCompensate = 4450;
1308   3                      }
1309   2                      else if(mcHall.HallFRStatus == 1)
1310   2                      {
1311   3                              FOC__RTHESTEP  = mcHall.RTheStep;       
1312   3                              KeyBrakeAngleCompensate = 0;                    
1313   3                      }
1314   2      
1315   2                      if(FOC__RTHESTEP>16385)
1316   2                      {
1317   3                              FOC__RTHESTEP = 10;
1318   3                      }
1319   2                              
1320   2              }
1321   1      }
1322          
C51 COMPILER V9.52.0.0   ADDFUNCTION                                                       09/21/2018 10:28:35 PAGE 23  

1323          void Hall_THETA_Capture(void)  
1324          {
1325   1              static int16 TempHall_Angle = 0;
1326   1              
1327   1              mcHall.HallStatus_Next = HallCommutation(mcHall.GivenFRStatus ,mcHall.HallStatus_Result );  //根据当�
             -�的角度和当前方向获得下一步Hall信号
1328   1      
1329   1              
1330   1              // 当赋值为16400（约90度）时，定子磁场方向与转子磁场相同,电机不会转动，说�
             -��在正常工作时，定子磁场方向与转子磁场方向相差90度
1331   1              mcHall.AngleCompensate = KeyBrakeAngleCompensate;
1332   1              
1333   1              TempHall_Angle = ThetaDispose (mcHall.HallStatus_Result,mcHall.GivenFRStatus,TempAngleOffsetCW,TempAngleO
             -ffsetCCW,mcHall.AngleCompensate); //10us
1334   1              
1335   1      
1336   1              FOC__THETA   = TempHall_Angle;//算出角度后给FOC强拉
1337   1              
1338   1              RealAngleGivenToFoc = TempHall_Angle;
1339   1              
1340   1              if(mcHall.HallFRStatus == 1)
1341   1              {
1342   2                      mcHall.NextSectorAngle = TempHall_Angle + 10922;        
1343   2              }
1344   1              if(mcHall.HallFRStatus == 2)
1345   1              {
1346   2                      mcHall.NextSectorAngle = TempHall_Angle - 10922;        
1347   2              }
1348   1      }
1349          
1350          /*-------------------------------------------------------------------------------------------------
1351                  Function Name : int16 RampControl(Ramp_TypeDef *Ramp, int16 Ref)
1352                  Description   : 爬坡目标函数，根据设定爬坡增量把当前值向目标值靠拢
1353                  Input         : Ramp--爬坡对象数组
1354                            Ref-- 爬坡参考值
1355            Output                                :       RampStep--爬坡当前输出
1356          -------------------------------------------------------------------------------------------------*/
1357          int16 RampControl(Ramp_TypeDef *Ramp, int16 Ref)
1358          {
1359   1              Ramp->RampTarget = Ref;
1360   1              Ramp->RampInc = 200;
1361   1              Ramp->RampDec = 200;
1362   1              if(Ramp->RampTarget > Ramp->RampStep)
1363   1              {
1364   2                      if((Ramp->RampStep + Ramp->RampInc) <= Ramp->RampTarget)
1365   2                      {
1366   3                              Ramp->RampStep += Ramp->RampInc;
1367   3                      }
1368   2                      else
1369   2                      {
1370   3                              Ramp->RampStep = Ramp->RampTarget;
1371   3                      }
1372   2              }
1373   1              else if(Ramp->RampTarget < Ramp->RampStep)
1374   1              {
1375   2                      if((Ramp->RampStep - Ramp->RampDec) >= Ramp->RampTarget)
1376   2                      {
1377   3                              Ramp->RampStep -= Ramp->RampDec;
1378   3                      }
1379   2                      else
1380   2                      {
1381   3                              Ramp->RampStep = Ramp->RampTarget;
C51 COMPILER V9.52.0.0   ADDFUNCTION                                                       09/21/2018 10:28:35 PAGE 24  

1382   3                      }
1383   2              }       
1384   1              return  Ramp->RampStep;
1385   1      }
1386          
1387          /*---------------------------------------------------------------------------*/
1388          /* Name         :       void MotorStop(void)
1389          /* Input        :       NO
1390          /* Output       :       NO
1391          /* Description:
1392          /*---------------------------------------------------------------------------*/
1393          void MotorStop(void)
1394          {
1395   1              if(mcFocCtrl.mcStopMode == FreeStop)
1396   1              {
1397   2                      mcFocCtrl.CurrentStopMode = FreeStop;
1398   2              }
1399   1              else if((mcFocCtrl.mcStopMode == QuickBrakeStop)||(mcFocCtrl.mcStopMode == SlowBrakeStop))   
1400   1              {
1401   2                      mcFocCtrl.CurrentStopMode = mcFocCtrl.mcStopMode;
1402   2              }
1403   1              else if(mcFocCtrl.mcStopMode == SlowStop)   
1404   1              {
1405   2                      mcFocCtrl.CurrentStopMode = SlowStop;
1406   2              }
1407   1              else if(mcFocCtrl.mcStopMode == OppoAngleBrakeStop)
1408   1              {
1409   2                      mcFocCtrl.CurrentStopMode = OppoAngleBrakeStop;
1410   2              }
1411   1      
1412   1      
1413   1      
1414   1              if(mcFocCtrl.CurrentStopMode == FreeStop)
1415   1              {
1416   2                              FOC_CR1 = 0x00;
1417   2                              MOE = 0;
1418   2                              /*关闭FOC*/
1419   2                              ClrBit(DRV_CR, FOCEN);                                          
1420   2              }
1421   1              else if((mcFocCtrl.CurrentStopMode == QuickBrakeStop)||(mcFocCtrl.CurrentStopMode == SlowBrakeStop))
1422   1              {
1423   2                      if(mcFocCtrl.CurrentStopMode == QuickBrakeStop)
1424   2                      {
1425   3                              DRV_DR = 1.0*DRV_ARR;                                 // brake PWM DUTY                 
1426   3          }
1427   2                      else if(mcFocCtrl.CurrentStopMode == SlowBrakeStop)
1428   2                      {                       
1429   3                              DRV_DR = mcFocCtrl.BrakeDuty*DRV_ARR;                               // brake PWM DUTY   
1430   3          }   
1431   2                      
1432   2                      mcFocCtrl.mcIqref =0;
1433   2                      SpeedRamp.SPEEDRampCnt = 0;
1434   2      //              SpeedRamp.RampStep =    mcFocCtrl.mcIqref;
1435   2                      
1436   2      //              FOC__UQ = SpeedRamp.RampStep;
1437   2                      //FOC_QMAX              = SpeedRamp.RampStep;
1438   2                      //FOC_QMIN              = SpeedRamp.RampStep;
1439   2                                              
1440   2                      DRV_CMR |= 0x15;
1441   2                      
1442   2                      ClrBit(DRV_CR,OCS);        //OCS = 0, DRV_COMR;OCS = 1, FOC/SVPWM/SPWM
1443   2                                     
C51 COMPILER V9.52.0.0   ADDFUNCTION                                                       09/21/2018 10:28:35 PAGE 25  

1444   2                      SetBit(DRV_CR, FOCEN);                  
1445   2                      MOE     = 1;
1446   2                      
1447   2              }
1448   1              else if (mcFocCtrl.CurrentStopMode == SlowStop) 
1449   1              {
1450   2                      if((SpeedRamp.RampStep<=MinPWMDuty)||(mcFocCtrl.MotorOFFTimes==0)||(mcFocCtrl.McSlowStopCnt >= SlowStopT
             -ime))
1451   2                      {
1452   3                              mcFocCtrl.McRunFlag = 0;
1453   3                              
1454   3      //                      SpeedRamp.RampStep = 0;
1455   3                              
1456   3                              mcFocCtrl.FreeStopDecStatus        = 0;
1457   3                              
1458   3                              FOC_CR1 = 0x00;
1459   3                              MOE = 0;
1460   3                              /*关闭FOC*/
1461   3                              ClrBit(DRV_CR, FOCEN);                                                  
1462   3                      }
1463   2                      else if(SpeedRamp.RampStep > MinPWMDuty)
1464   2                      {
1465   3                              if(mcFocCtrl.McRunFlag)
1466   3                              {                                                                                                                                        
1467   4                                      mcFocCtrl.FreeStopDecStatus        = 1;
1468   4                              }
1469   3          }
1470   2      
1471   2        }
1472   1              else if (mcFocCtrl.CurrentStopMode == OppoAngleBrakeStop)
1473   1              {
1474   2                      if(mcHall.HallSpeed <= 7000)
1475   2                      {
1476   3                              FOC_CR1 = 0x00;
1477   3                              MOE = 0;
1478   3                              /*关闭FOC*/
1479   3                              ClrBit(DRV_CR, FOCEN);                                          
1480   3                      }
1481   2                      else if(mcHall.HallSpeed > 7000)
1482   2                      {
1483   3                              
1484   3                              FOC_CR1 = 0x00;
1485   3                              MOE = 0;
1486   3                              /*关闭FOC*/
1487   3                              ClrBit(DRV_CR, FOCEN);                                          
1488   3      
1489   3      //                      FOC_QMAX                        = QOUTMAX;
1490   3      //                      FOC_QMIN                        = QOUTMIN;
1491   3      
1492   3      //                      
1493   3      //                      FOC_IQREF = -700;
1494   3      //                      
1495   3      //                                              DRV_DR = 0.9*DRV_ARR;                                 // brake PWM DUTY                 
1496   3      //              
1497   3      ////            mcFocCtrl.mcIqref =0;
1498   3      ////            SpeedRamp.SPEEDRampCnt = 0;
1499   3      ////            SpeedRamp.RampStep =    mcFocCtrl.mcIqref;
1500   3      //              
1501   3      //              //FOC__UQ = SpeedRamp.RampStep;
1502   3                      //FOC_QMAX              = SpeedRamp.RampStep;
1503   3                      //FOC_QMIN              = SpeedRamp.RampStep;
1504   3      //                                      
C51 COMPILER V9.52.0.0   ADDFUNCTION                                                       09/21/2018 10:28:35 PAGE 26  

1505   3      ////            DRV_CMR |= 0x15;
1506   3      //              
1507   3      //              ClrBit(DRV_CR,OCS);        //OCS = 0, DRV_COMR;OCS = 1, FOC/SVPWM/SPWM
1508   3      //                             
1509   3      //              ClrBit(DRV_CR, FOCEN);                  
1510   3      //              MOE     = 1;
1511   3      
1512   3      //                      KeyBrakeRTheStep = 0;
1513   3      //                      KeyBrakeAngleCompensate = 17000;
1514   3      //                      
1515   3      //                      //SpeedRamp.RampStep = RampControl(&SpeedRamp,mcFocCtrl.OpenDutyCycle);                                                 
1516   3      //                      
1517   3      //                      mcFocCtrl.OpenDutyCycle = 1000;
1518   3      //                      SpeedRamp.RampStep = RampControl(&SpeedRamp,mcFocCtrl.OpenDutyCycle);
1519   3      //                      FOC__UQ = SpeedRamp.RampStep;
1520   3      
1521   3      ////                    mcHall.GivenFRStatus = 1;
1522   3      ////                    mcHall.StartAngleOffset   = StartAngleCCW;
1523   3      ////                    SetBit(DRV_CR,DDIR);
1524   3      //                      OppoBrakeFlag = 1;
1525   3                      }
1526   2                      
1527   2              }
1528   1      }
1529          
1530          void  mcTailWindDispose (void)
1531          {
1532   1              int16 TempCalCulFOC_QM;
1533   1              
1534   1                      if((mcState== mcRun)||(mcState== mcStart))
1535   1                      {
1536   2                              if(mcHall.HallDirectStartStatus)
1537   2                              {
1538   3                                      if((Time.mcRunStateCount<60000)&&(Time.mcRunStateCount!=1))
1539   3                                      {
1540   4                                              Time.mcRunStateCount++;
1541   4                                      }
1542   3                                      
1543   3                                      if(Time.mcRunStateCount>36)
1544   3                                      {
1545   4                                              MOE = 1;
1546   4                                                                      
1547   4                                              SpeedFollow(983,QOUTMAX,QOUTMIN);  // SRampInc 爬坡增量 327 - 1638
1548   4                                                                                                              
1549   4                                              if(Time.mcRunStateCount>80)
1550   4                                              {
1551   5                                                      mcHall.HallDirectStartStatus =0;                                                                                                                                        
1552   5                                              }
1553   4                                      }
1554   3                                      else if(Time.mcRunStateCount==1)
1555   3                                      {                                                                                                                               
1556   4                                                      mcFocCtrl.SpeedV_k = mcHall.LastHallSpeed/32767.0;
1557   4                                                                                      
1558   4                                                      if(mcHall.LastHallSpeed < 2000)
1559   4                                                      { 
1560   5                                                              TempCalCulFOC_QM =      _Q15(0.1)*mcFocCtrl.SpeedV_k +_Q15(0.10);         
1561   5                                                      }
1562   4                                                      else if(mcHall.LastHallSpeed <= 4000)
1563   4                                                      {
1564   5                                                              TempCalCulFOC_QM =      _Q15(0.1)*mcFocCtrl.SpeedV_k +_Q15(0.11);       
1565   5                                                      } 
1566   4                                                      else if(mcHall.LastHallSpeed <= 6000)
C51 COMPILER V9.52.0.0   ADDFUNCTION                                                       09/21/2018 10:28:35 PAGE 27  

1567   4                                                      {
1568   5                                                              TempCalCulFOC_QM =      _Q15(0.1)*mcFocCtrl.SpeedV_k +_Q15(0.25);       
1569   5                                                      } 
1570   4                                                      else if(mcHall.LastHallSpeed <= 8000)
1571   4                                                      {
1572   5                                                              TempCalCulFOC_QM =      _Q15(0.20)*mcFocCtrl.SpeedV_k +_Q15(0.35);       //0.35
1573   5                                                      } 
1574   4                                                      else if(mcHall.LastHallSpeed <= 9000)
1575   4                                                      {
1576   5                                                              TempCalCulFOC_QM =      _Q15(0.25)*mcFocCtrl.SpeedV_k +_Q15(0.40);       //0.35
1577   5                                                      } 
1578   4                                                      else if(mcHall.LastHallSpeed <= 14000)
1579   4                                                      {
1580   5                                                              TempCalCulFOC_QM =      _Q15(0.40)*mcFocCtrl.SpeedV_k +_Q15(0.45);      
1581   5                                                      } 
1582   4                                                      else 
1583   4                                                      {
1584   5                                                              if(mcHall.LastHallSpeed>=15928)
1585   5                                                              {
1586   6                                                                      mcHall.LastHallSpeed =15928;
1587   6                    }
1588   5                                                              
1589   5      //                                                      if(mcHall.GivenFRStatus == CW)
1590   5      //                                                      {
1591   5                                                                      TempCalCulFOC_QM =      _Q15(0.50)*mcFocCtrl.SpeedV_k +_Q15(0.55);
1592   5      //              }
1593   5      //                                                      else
1594   5      //                                                      {
1595   5      //                                                              TempCalCulFOC_QM =      _Q15(0.50)*mcFocCtrl.SpeedV_k +_Q15(0.55);      
1596   5      //                                                      }
1597   5                                                      }
1598   4                                                                                                      
1599   4                                                      FOCQMAXLimit(TempCalCulFOC_QM);             //输出占空比限制                     
1600   4                                              
1601   4      //                                              SpeedRamp.RampStep = TempCalCulFOC_QM;
1602   4      
1603   4                                                      SpeedRamp.RampTarget = TempCalCulFOC_QM;                                        
1604   4                                              
1605   4                                                      mcHall.HallCommutationCount=1;
1606   4                                              
1607   4                                                      Time.mcRunStateCount =2;                                
1608   4                                      }
1609   3                              }
1610   2                              else
1611   2                              {
1612   3                                      Time.mcRunStateCount=0;
1613   3                              }                       
1614   2                      }
1615   1      }
1616          
1617          
1618          //均值限流处理
1619          void RMSCurrentLimit(void)
1620          {
1621   1              if(mcFocCtrl.mcADCCurrentbus >= mcFocCtrl.RMSLimitTargetCurrent+50)
1622   1              {
1623   2                              if(mcFocCtrl.mcIqref > 150)
1624   2                              {
1625   3                                      mcFocCtrl.mcIqref -= 150;
1626   3      //                              SpeedRamp.RampStep = mcFocCtrl.mcIqref;
1627   3                                      
1628   3      //                              FOC__UQ = SpeedRamp.RampStep;
C51 COMPILER V9.52.0.0   ADDFUNCTION                                                       09/21/2018 10:28:35 PAGE 28  

1629   3      //                              FOC_QMAX                = SpeedRamp.RampStep;
1630   3      //                              FOC_QMIN                = SpeedRamp.RampStep;
1631   3                              }
1632   2              }
1633   1              else if((mcFocCtrl.mcADCCurrentbus > mcFocCtrl.RMSLimitTargetCurrent)&&((mcFocCtrl.mcADCCurrentbus < mcFo
             -cCtrl.RMSLimitTargetCurrent+20)))
1634   1              {
1635   2                              if(mcFocCtrl.mcIqref > 10)
1636   2                              {
1637   3                                      mcFocCtrl.mcIqref -= 10;
1638   3      //                              SpeedRamp.RampStep = mcFocCtrl.mcIqref;
1639   3      
1640   3      //                              FOC__UQ = SpeedRamp.RampStep;
1641   3      //                              FOC_QMAX                = SpeedRamp.RampStep;
1642   3      //                              FOC_QMIN                = SpeedRamp.RampStep;
1643   3                              }
1644   2              }
1645   1      }
1646          
1647          
1648          
1649          /*---------------------------------------------------------------------------*/
1650          /* Name         :       void HallLockDisposeFuntion (void)
1651          /* Input        :       NO
1652          /* Output       :       NO
1653          /* Description:
1654          /*---------------------------------------------------------------------------*/
1655          void HallLockCheckFuntion (void)
1656          {
1657   1              if(mcHall.HALLLockStatus)
1658   1              {
1659   2                      mcHall.mcStallTimeCnt++;
1660   2                      
1661   2                      if(mcHall.mcStallTimeCnt >= StallTime)
1662   2                      {
1663   3                              mcHall.mcStallTimeCnt = StallTime;
1664   3          }
1665   2        }
1666   1              else
1667   1              {
1668   2                      mcHall.mcStallTimeCnt =0;
1669   2        }
1670   1                      
1671   1              if(HALLC)
1672   1              {
1673   2                      mcHall.HALLHCCWCCWTime ++;
1674   2                      
1675   2                      #if (StallProtectEnable)
                              {
                                      if((mcState == mcRun)||(mcState == mcStart))
                                      {                               
                                              mcHall.HALLLockHCTime++;
                                              if(mcHall.HALLLockHCTime>=StallDetectTime)
                                              {
                                                      mcHall.HALLLockHCTime = StallDetectTime;
                                                      mcHall.HALLLockStatus = 1;                                      
                                              }
                                      }
                              }
                              #endif
1688   2              }
1689   1              else
C51 COMPILER V9.52.0.0   ADDFUNCTION                                                       09/21/2018 10:28:35 PAGE 29  

1690   1              {
1691   2                      mcHall.HALLLockHCTime = 0;
1692   2                      mcHall.HALLHCCWCCWTime =0;
1693   2              }
1694   1              
1695   1              
1696   1              if(HALLB)
1697   1              {
1698   2                      mcHall.HALLHBCWCCWTime ++;
1699   2                      
1700   2                      #if (StallProtectEnable)
                              {
                                      if((mcState == mcRun)||(mcState == mcStart))
                                      {                               
                                              mcHall.HALLLockHBTime++;
                                              if(mcHall.HALLLockHBTime>=StallDetectTime)
                                              {
                                                      mcHall.HALLLockHBTime = StallDetectTime;
                                                      mcHall.HALLLockStatus =1;
                                              }
                                      }
                              }
                              #endif
1713   2              }
1714   1              else
1715   1              {
1716   2                      mcHall.HALLLockHBTime = 0;
1717   2                      mcHall.HALLHBCWCCWTime =0;
1718   2              }
1719   1              
1720   1              if(HALLA)
1721   1              {
1722   2                      mcHall.HALLHACWCCWTime++;
1723   2                      
1724   2                      #if (StallProtectEnable)
                              {
                                      if((mcState == mcRun)||(mcState == mcStart))
                                      {                               
                                              mcHall.HALLLockHATime++;
                                              if(mcHall.HALLLockHATime >= StallDetectTime)
                                              {
                                                      mcHall.HALLLockHATime = StallDetectTime;
                                                      mcHall.HALLLockStatus = 1;
                                              }
                                      }
                              }
                              #endif
1737   2              }
1738   1              else
1739   1              {
1740   2                      mcHall.HALLLockHATime = 0;
1741   2                      mcHall.HALLHACWCCWTime =0;
1742   2              }
1743   1      }


MODULE INFORMATION:   STATIC OVERLAYABLE
   CODE SIZE        =   4277    ----
   CONSTANT SIZE    =   ----    ----
   XDATA SIZE       =    211      21
   PDATA SIZE       =   ----    ----
   DATA SIZE        =   ----    ----
C51 COMPILER V9.52.0.0   ADDFUNCTION                                                       09/21/2018 10:28:35 PAGE 30  

   IDATA SIZE       =    136    ----
   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)
