C51 COMPILER V9.52.0.0   FOCTAILDECT                                                       09/25/2018 17:39:39 PAGE 1   


C51 COMPILER V9.52.0.0, COMPILATION OF MODULE FOCTAILDECT
OBJECT MODULE PLACED IN .\Output\FOCTailDect.obj
COMPILER INVOKED BY: F:\keil\C51\BIN\C51.EXE ..\User\Source\Function\FOCTailDect.c LARGE OMF2 WARNINGLEVEL(0) BROWSE INC
                    -DIR(..\Library\FU68xx_Hardware_Driver\Include;..\User\Include) DEBUG PRINT(.\Listing\FOCTailDect.lst) OBJECT(.\Output\FO
                    -CTailDect.obj)

line level    source

   1          /**************************** (C) COPYRIGHT 2017 Fortiortech shenzhen *****************************
   2          * File Name          : FOCTailDect.c
   3          * Author             : Vina Peng,Fortiortech  Appliction Team
   4          * Version            : V1.0
   5          * Date               : 2017-12-26
   6          * Description        : This file contains foc tailwind detection used for Motor Control.
   7          ***************************************************************************************************
   8          * All Rights Reserved
   9          **************************************************************************************************/
  10          
  11          
  12          /* Includes -------------------------------------------------------------------------------------*/
  13          // #include <FOCTailDect.h>
  14          #include <FU68xx_2.h>
  15          #include <Myproject.h>
  16          
  17          /* Private typedef ------------------------------------------------------------------------------*/
  18          /* Private define -------------------------------------------------------------------------------*/
  19          /* Private macro --------------------------------------------------------------------------------*/
  20          /* Private variables ----------------------------------------------------------------------------*/
  21          
  22          /* Private function prototypes ------------------------------------------------------------------*/
  23          /* Private functions ----------------------------------------------------------------------------*/
  24          MotorTailWindTypeDef xdata  TailWindDetect;
  25          /*---------------------------------------------------------------------------*/
  26          /* Name     :   void TailWindDetectInit(void)
  27          /* Input    :   NO
  28          /* Output   :   NO
  29          /* Description: 顺逆风参数初始化
  30          /*---------------------------------------------------------------------------*/
  31          void TailWindDetectInit(void)
  32          {
  33   1          TailWindDetect.MotorTailWindState     = NormalTailWind;                     //  初始状态为正常�
             -��顺逆风状态
  34   1          TailWindDetect.TempThailWindSpeedBase = ThailWindSpeedBase;                 //  顺逆风计算的spee
             -dbase
  35   1      
  36   1          FOC_Init();                                                                 // FOC的初始化
  37   1          #if (EstimateAlgorithm == PLL)
                  {
                      FOC_KSLIDE    = OBSE_PLLKP_GAIN_WIND;
                      FOC_EKLPFMIN  = OBSE_PLLKI_GAIN_WIND;
                  }
                  #endif //end SVPMW_Mode
  43   1          FOC_DQKP    = DQKP_TailWind;                                                // 顺逆风的电流环KP
  44   1          FOC_DQKI    = DQKI_TailWind;                                                // 顺逆风的电流环KI
  45   1          FOC_EKP     = OBSW_KP_GAIN_WIND;                                            // 顺逆风速度估算�
             -�KP
  46   1          FOC_EKI     = OBSW_KI_GAIN_WIND;                                            // 顺逆风速度估算�
             -�KI
  47   1          FOC_OMEKLPF = SPEED_KLPF_WIND;                                              // 顺逆风下的速度�
             -�波系数
  48   1          SetBit(FOC_CR1, ANGM);                                                      // 估算模式
C51 COMPILER V9.52.0.0   FOCTAILDECT                                                       09/25/2018 17:39:39 PAGE 2   

  49   1          // SetBit(DRV_CR, DRVEN);                                                   // Driver 输出计数器�
             -��能，0-禁止，1-使能
  50   1          DRV_CMR   |= 0x3F;                                                          // U、V、W相输出
  51   1          MOE       = 1;                                                              // 打开MOE
  52   1          FOC_IDREF = 0;                                                              // D轴给定电流
  53   1          FOC_IQREF = 0;
  54   1      }
  55          /*---------------------------------------------------------------------------*/
  56          /* Name     :   void TailWindSpeedDetect(void)
  57          /* Input    :   NO
  58          /* Output   :   NO
  59          /* Description: 顺逆风速度检测，根据角度的变化趋势来判断方向
  60          /*---------------------------------------------------------------------------*/
  61          void TailWindSpeedDetect(void)
  62          {
  63   1          static int16 LatestTheta;
  64   1      //    static uint8 ThetaStep=0;
  65   1      
  66   1          if(mcState == mcTailWind)
  67   1          {
  68   2              //顺风检测过程由于FOC内部根据电流估算角度，故直接对FOC_ETHETA进行处理
  69   2              if(TailWindDetect.MotorTailWindState == NormalTailWind)
  70   2              {
  71   3                  LatestTheta = FOC__ETHETA;
  72   3      
  73   3                  //旋转方向判断在 <-170度   <10 >-10  >170度三个状态切换的时间
  74   3                  if(LatestTheta < -30946)
  75   3                  {
  76   4                      //计数器未清零或从状态3跳到状态1时清零
  77   4                      if((TailWindDetect.SpeedTimerClearStatus == 0) || (TailWindDetect.AngleState == 3))
  78   4                      {
  79   5                          TailWindDetect.SpeedCountTimer       = 0;
  80   5                          TailWindDetect.SpeedTimerClearStatus = 1;
  81   5                          if(TailWindDetect.AngleState == 3) TailWindDetect.ShakeTimes++;//来回1和3之间抖�
             -��，抖动次数加1
  82   5                          switch(TailWindDetect.ThetaStep)
  83   5                          {
  84   6                              case 0:     TailWindDetect.ThetaStep+=1;  break;
  85   6                              case 1:     TailWindDetect.ThetaStep+=1;  break;
  86   6                              case 4:
  87   6                              case 5:     
  88   6                                  TailWindDetect.SpeedStoreNum++;
  89   6                                  TailWindDetect.ThetaStep=0;
  90   6                              break;
  91   6                              default:    TailWindDetect.ThetaStep =0;  break;
  92   6                          }
  93   5                      }
  94   4                      //<-170度  时设置状态为1，并清零SpeedCountTimer在TIM5中计时
  95   4                      TailWindDetect.AngleState = 1;
  96   4                  }
  97   3                  else if((LatestTheta>-1820)&&(LatestTheta<1820)) //<10 >-10
  98   3                  {
  99   4                      //状态1或状态3切换到状态2时保存当前转速时间至TailWindDetect.SpeedCount
             -[SpeedStoreNum]
 100   4                      if((TailWindDetect.AngleState==1)||(TailWindDetect.AngleState==3))
 101   4                      {
 102   5                          //计算当前转速，RPM
 103   5                          TailWindDetect.SpeedCountTimer += 1;//防止为0
 104   5                          TailWindDetect.TailWindSpeed   = MDU_DIV_XDATA_U32(&TailWindDetect.TempThailWindSpeedB
             -ase, &TailWindDetect.SpeedCountTimer);//测出速度
 105   5                          switch(TailWindDetect.ThetaStep)
 106   5                          {
C51 COMPILER V9.52.0.0   FOCTAILDECT                                                       09/25/2018 17:39:39 PAGE 3   

 107   6                              case 0:     break;
 108   6                              case 1:
 109   6                              case 2:     TailWindDetect.ThetaStep+=3;  break;
 110   6                              default:    TailWindDetect.ThetaStep =0;  break;
 111   6                          }
 112   5      //                    TailWindDetect.SpeedStoreNum++;
 113   5      
 114   5                          //用于SpeedCountTimer清零
 115   5                          if(TailWindDetect.SpeedTimerClearStatus == 1) TailWindDetect.SpeedTimerClearStatus = 0
             -;
 116   5      
 117   5                          //有1状态切换到2状态说明角度递增旋转方向为CW，3->2则为CCW
 118   5                          if(TailWindDetect.AngleState == 1)      TailWindDetect.MotorDir = CW;
 119   5                          else if(TailWindDetect.AngleState == 3) TailWindDetect.MotorDir = CCW;
 120   5                          TailWindDetect.ShakeTimes = 0;//清除来回抖动的计数
 121   5                      }
 122   4                      TailWindDetect.AngleState = 2;
 123   4      
 124   4                  }
 125   3                  //>170度时
 126   3                  else if(LatestTheta>30946)
 127   3                  {
 128   4                      //计数器未清零或从状态1跳到状态3时清零
 129   4                      if((TailWindDetect.SpeedTimerClearStatus == 0) || (TailWindDetect.AngleState == 1))
 130   4                      {
 131   5                          TailWindDetect.SpeedCountTimer       = 0;
 132   5                          TailWindDetect.SpeedTimerClearStatus = 1;
 133   5                          if(TailWindDetect.AngleState==1) TailWindDetect.ShakeTimes++;//来回1和3之间抖动
 134   5                          switch(TailWindDetect.ThetaStep)
 135   5                          {
 136   6                              case 0:     TailWindDetect.ThetaStep+=1;  break;
 137   6                              case 1:     TailWindDetect.ThetaStep+=1;  break;
 138   6                              case 4:
 139   6                              case 5:     
 140   6                                  TailWindDetect.SpeedStoreNum++;
 141   6                                  TailWindDetect.ThetaStep=0;
 142   6                              break;
 143   6                              default:    TailWindDetect.ThetaStep =0;  break;
 144   6                          }
 145   5                      }
 146   4                      TailWindDetect.AngleState = 3;
 147   4                  }
 148   3              }
 149   2          }
 150   1      }
 151          /*---------------------------------------------------------------------------*/
 152          /* Name     :   void FOCCloseLoopStart(void)
 153          /* Input    :   NO
 154          /* Output   :   NO
 155          /* Description: 顺风启动
 156          /*---------------------------------------------------------------------------*/
 157          void FOCCloseLoopStart(void)
 158          {
 159   1          //配置启动的参数，Omega模式
 160   1          FOC_EFREQACC    = Motor_Omega_Ramp_ACC;
 161   1          FOC_EFREQMIN    = Motor_Omega_Ramp_Min;
 162   1          FOC_EFREQHOLD = Motor_Omega_Ramp_End;
 163   1      
 164   1          SetBit(FOC_CR1,EFAE);                                                       // 估算器强制输出
 165   1          ClrBit(FOC_CR1,RFAE);                                                       // 禁止强拉
 166   1          SetBit(FOC_CR1,ANGM);                                                       // 估算模式
 167   1      
C51 COMPILER V9.52.0.0   FOCTAILDECT                                                       09/25/2018 17:39:39 PAGE 4   

 168   1          //电流环的PI和输出限赋值
 169   1          FOC_DQKP = DQKP;
 170   1          FOC_DQKI = DQKI;
 171   1      //    FOC_DMAX = DOUTMAX;
 172   1      //    FOC_DMIN = DOUTMIN;
 173   1      //    FOC_QMAX = QOUTMAX;
 174   1      //    FOC_QMIN = QOUTMIN;
 175   1      
 176   1          /*********PLL或SMO**********/
 177   1          #if (EstimateAlgorithm == SMO)
 178   1              //根据不同转速确启动的ATO_BW值
 179   1              if(TailWindDetect.TailWindSpeed > 300)//300rpm/min
 180   1              {
 181   2                  FOC_EKP               = OBSW_KP_GAIN_RUN4;
 182   2                  FOC_EKI               = OBSW_KI_GAIN_RUN4;
 183   2                  mcFocCtrl.mcIqref     = IQ_RUN_CURRENT;
 184   2                  mcFocCtrl.State_Count = 100;
 185   2              }
 186   1              else if(TailWindDetect.TailWindSpeed > 100)//300rpm/min
 187   1              {
 188   2                  FOC_EKP               = OBSW_KP_GAIN_RUN3;
 189   2                  FOC_EKI               = OBSW_KI_GAIN_RUN3;
 190   2                  mcFocCtrl.mcIqref     = IQ_RUN_CURRENT;
 191   2                  mcFocCtrl.State_Count = 100;
 192   2              }
 193   1              else
 194   1              {
 195   2                  FOC_EKP               = OBSW_KP_GAIN_RUN;
 196   2                  FOC_EKI               = OBSW_KI_GAIN_RUN;
 197   2                  mcFocCtrl.mcIqref     = IQ_RUN_CURRENT;
 198   2                  mcFocCtrl.State_Count = 2300;
 199   2              }
 200   1      
 201   1          #elif (EstimateAlgorithm == PLL)
                       mcFocCtrl.mcIqref = IQ_RUN_CURRENT;
              
                  #endif //end    EstimateAlgorithm
 205   1      
 206   1      
 207   1          /*estimate parameter set*/
 208   1          FOC_OMEKLPF = SPEED_KLPF;
 209   1          FOC_IDREF   = ID_RUN_CURRENT;     // D轴启动电流
 210   1          FOC_IQREF   = mcFocCtrl.mcIqref ;         // Q轴启动电流
 211   1          PI_UK       =   mcFocCtrl.mcIqref   ;
 212   1      
 213   1      
 214   1          mcState            = mcRun;
 215   1          mcFocCtrl.CtrlMode = 0;
 216   1      }
 217          
 218          /*---------------------------------------------------------------------------*/
 219          /* Name     :   void FOCTailWindTimeLimit(void)
 220          /* Input    :   NO
 221          /* Output   :   NO
 222          /* Description: FOC顺逆风检测的时间处理
 223          /*---------------------------------------------------------------------------*/
 224          //void FOCTailWindTimeLimit(void)
 225          //{
 226          //    if(mcState == mcTailWind)
 227          //    {
 228          //        //1Hz Min
 229          //        if(TailWindDetect.SpeedCountTimer < TailWind_Time)                      // SpeedCountTimer-计�
C51 COMPILER V9.52.0.0   FOCTAILDECT                                                       09/25/2018 17:39:39 PAGE 5   

             -�时间间隔
 230          //        {
 231          //            TailWindDetect.SpeedCountTimer++;
 232          //        }
 233          //        else
 234          //        {
 235          //            TailWindDetect.TailWindSpeed       = 0;                             //速度为0
 236          //            TailWindDetect.SpeedCountTimer     = 0;
 237          //            TailWindDetect.SpeedOverFlowStatus = 1;                             // 转速计数溢出标
             -志
 238          //        }
 239          //    }
 240          //}
 241          
 242           /*---------------------------------------------------------------------------*/
 243          /* Name     :   void TailWindDealwith(void)
 244          /* Input    :   NO
 245          /* Output   :   NO
 246          /* Description: 顺风逆风参数配置函数
 247          /*---------------------------------------------------------------------------*/
 248          void FOCTailWindDealwith(void)
 249          {
 250   1          /*********PLL或SMO**********/
 251   1          #if (EstimateAlgorithm == SMO)
 252   1               SMO_TailWindDealwith();
 253   1      
 254   1          #elif (EstimateAlgorithm == PLL)
                       PLL_TailWindDealwith();
              
                  #endif //end    EstimateAlgorithm
 258   1      }
 259          
 260          void SMO_TailWindDealwith(void)
 261          {
 262   1          //电机状态为正转且转速计数溢出时；或1-3状态抖动次数大于2次时，认为静止
 263   1          if(((TailWindDetect.MotorDir == CW) && (TailWindDetect.SpeedOverFlowStatus))||(TailWindDetect.ShakeTim
             -es > 2))
 264   1          {
 265   2              MOE = 0;
 266   2              SetBit(DRV_CR, FOCEN, 0);   //关闭FOC
 267   2              mcState                           = mcPosiCheck;
 268   2              McStaSet.SetFlag.PosiCheckSetFlag = 0;
 269   2              mcFocCtrl.mcPosCheckAngle         = 0xffff;         // 角度赋初值
 270   2          }
 271   1          else if((TailWindDetect.MotorDir == CCW) && (TailWindDetect.SpeedStoreNum > 2))//||((TailWindDetect.Mo
             -torDir == CW)&&(TailWindDetect.TailWindSpeed>TailWindStartMinSpeed)))
 272   1          {
 273   2              if(TailWindDetect.TailWindSpeed < 130)
 274   2              {
 275   3                  mcFocCtrl.mcIqref       = IQ_Start_CURRENT + I_Value(0.0);       // Q轴启动电流
 276   3                  TailWindDetect.AntiFlag = 1;
 277   3              }
 278   2              else if(TailWindDetect.TailWindSpeed < 300)
 279   2              {
 280   3                  mcFocCtrl.mcIqref       = IQ_Start_CURRENT + I_Value(0.0);       // Q轴启动电流
 281   3                  TailWindDetect.AntiFlag = 2;
 282   3              }
 283   2              else if(TailWindDetect.TailWindSpeed < 500)
 284   2              {
 285   3                  mcFocCtrl.mcIqref       = IQ_Start_CURRENT + I_Value(0.0);       // Q轴启动电流
 286   3                  TailWindDetect.AntiFlag = 3;
 287   3              }
C51 COMPILER V9.52.0.0   FOCTAILDECT                                                       09/25/2018 17:39:39 PAGE 6   

 288   2              else
 289   2              {
 290   3                  mcFocCtrl.mcIqref       = IQ_Start_CURRENT + I_Value(0.0);       // Q轴启动电流
 291   3                  TailWindDetect.AntiFlag = 4;
 292   3              }
 293   2      
 294   2              FOC_DQKP              = DQKP;
 295   2              FOC_DQKI              = DQKI;
 296   2      
 297   2              FOC_DMAX              = DOUTMAX;
 298   2              //FOC_DMIN              = DOUTMIN;
 299   2              //FOC_QMAX              = QOUTMAX;
 300   2              FOC_QMIN              = QOUTMIN;
 301   2              FOC_EK2               = OBS_K2T_SMO;
 302   2      
 303   2              FOC_IQREF             = mcFocCtrl.mcIqref;
 304   2              mcFocCtrl.State_Count = 1600;
 305   2              FOC_EKP               = OBSW_KP_GAIN;
 306   2              FOC_EKI               = OBSW_KI_GAIN;
 307   2      
 308   2              FOC_IDREF             = 0;
 309   2              mcState               = mcRun;
 310   2              mcFocCtrl.CtrlMode    = 0;
 311   2      
 312   2          }
 313   1          else if(TailWindDetect.MotorDir == CW)
 314   1          {
 315   2              //超过检测次数TailWindDetect.SpeedStoreNum切转速TailWindStartMinSpeed以上时直接顺�
             -�启动
 316   2              if((TailWindDetect.TailWindSpeed > TailWindStartMinSpeed) && (TailWindDetect.SpeedStoreNum >= 2))
 317   2              {
 318   3                  FOCCloseLoopStart();
 319   3              }
 320   2          }
 321   1      }
 322          
 323          void PLL_TailWindDealwith(void)
 324          {
 325   1          //电机状态为正转且转速计数溢出时；或1-3状态抖动次数大于2次时，认为静止
 326   1          if(((TailWindDetect.MotorDir == CW) && (TailWindDetect.SpeedOverFlowStatus)) || (TailWindDetect.ShakeT
             -imes > 2))
 327   1          {
 328   2              MOE = 0;
 329   2              SetBit(DRV_CR, FOCEN, 0);   //关闭FOC
 330   2              mcState                           = mcPosiCheck;
 331   2              McStaSet.SetFlag.PosiCheckSetFlag = 0;
 332   2              mcFocCtrl.mcPosCheckAngle         = 0xffff;         // 角度赋初值
 333   2      
 334   2          }
 335   1          else if((TailWindDetect.MotorDir == CCW) && (TailWindDetect.TailWindSpeed > 50) && (TailWindDetect.Spe
             -edStoreNum >= 3))// 当电机反转时，根据不同转速设置电流，并跳入mcPllTect状态。
 336   1          {
 337   2              if(TailWindDetect.TailWindSpeed < 130)
 338   2              {
 339   3                  FOC_IQREF               = I_Value(-0.2);
 340   3                  mcFocCtrl.mcIqref       = IQ_Start_CURRENT + I_Value(0.7);         // Q轴启动电流
 341   3                  TailWindDetect.AntiFlag = 1;
 342   3              }
 343   2              else if(TailWindDetect.TailWindSpeed < 380)
 344   2              {
 345   3                  FOC_IQREF               = I_Value(-0.8);
 346   3                  mcFocCtrl.mcIqref       = IQ_Start_CURRENT + I_Value(1.0);         // Q轴启动电流
C51 COMPILER V9.52.0.0   FOCTAILDECT                                                       09/25/2018 17:39:39 PAGE 7   

 347   3                  TailWindDetect.AntiFlag = 2;
 348   3              }
 349   2              else if(TailWindDetect.TailWindSpeed < 440)
 350   2              {
 351   3                  FOC_IQREF               = I_Value(-1.0);
 352   3                  mcFocCtrl.mcIqref       = IQ_Start_CURRENT + I_Value(1.2);         // Q轴启动电流
 353   3                  TailWindDetect.AntiFlag = 3;
 354   3              }
 355   2              else if(TailWindDetect.TailWindSpeed < 500)
 356   2              {
 357   3                  FOC_IQREF               = I_Value(-1.07);
 358   3                  mcFocCtrl.mcIqref       = IQ_Start_CURRENT + I_Value(1.3);         // Q轴启动电流
 359   3                  TailWindDetect.AntiFlag = 4;
 360   3              }
 361   2              else
 362   2              {
 363   3                      FOC_IQREF = I_Value(-1.2);
 364   3                      mcFocCtrl.mcIqref= IQ_Start_CURRENT + I_Value(1.3);         // Q轴启动电流
 365   3                      TailWindDetect.AntiFlag             = 5;
 366   3              }
 367   2              TailWindDetect.PLLFlag = 1;
 368   2      //        FOC_DQKP = _Q12(2.0);
 369   2      //        FOC_DQKI = _Q15(0.2);
 370   2              mcState                = mcPllTect;      
 371   2          }
 372   1          else if(TailWindDetect.MotorDir == CW)//
 373   1          {
 374   2              //超过检测次数TailWindDetect.SpeedStoreNum,且转速大于TailWindStartMinSpeed，则直接�
             -��风启动
 375   2              if((TailWindDetect.TailWindSpeed > TailWindStartMinSpeed) && (TailWindDetect.SpeedStoreNum >= 2))
 376   2              {
 377   3                  FOCCloseLoopStart();
 378   3              }
 379   2          }
 380   1      
 381   1      }


MODULE INFORMATION:   STATIC OVERLAYABLE
   CODE SIZE        =   1338    ----
   CONSTANT SIZE    =   ----    ----
   XDATA SIZE       =     26    ----
   PDATA SIZE       =   ----    ----
   DATA SIZE        =   ----    ----
   IDATA SIZE       =   ----    ----
   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)
