C51 COMPILER V9.52.0.0   FOCTAILDECT                                                       09/21/2018 10:43:22 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>
*** ERROR C129 IN LINE 27 OF ..\User\Include\FocControlFunction.h: missing ';' before 'idata'
  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              TailWindDetect.MotorTailWindState     = NormalTailWind;                     //  初始状态为正常�
             -��顺逆风状态
  34              TailWindDetect.TempThailWindSpeedBase = ThailWindSpeedBase;                 //  顺逆风计算的spee
             -dbase
  35          
  36              FOC_Init();                                                                 // FOC的初始化
  37              #if (EstimateAlgorithm == PLL)
                  {
                      FOC_KSLIDE    = OBSE_PLLKP_GAIN_WIND;
                      FOC_EKLPFMIN  = OBSE_PLLKI_GAIN_WIND;
                  }
                  #endif //end SVPMW_Mode
  43              FOC_DQKP    = DQKP_TailWind;                                                // 顺逆风的电流环KP
  44              FOC_DQKI    = DQKI_TailWind;                                                // 顺逆风的电流环KI
  45              FOC_EKP     = OBSW_KP_GAIN_WIND;                                            // 顺逆风速度估算�
             -�KP
  46              FOC_EKI     = OBSW_KI_GAIN_WIND;                                            // 顺逆风速度估算�
             -�KI
  47              FOC_OMEKLPF = SPEED_KLPF_WIND;                                              // 顺逆风下的速度�
             -�波系数
C51 COMPILER V9.52.0.0   FOCTAILDECT                                                       09/21/2018 10:43:22 PAGE 2   

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

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

 167          
 168              //电流环的PI和输出限赋值
 169              FOC_DQKP = DQKP;
 170              FOC_DQKI = DQKI;
 171          //    FOC_DMAX = DOUTMAX;
 172          //    FOC_DMIN = DOUTMIN;
 173          //    FOC_QMAX = QOUTMAX;
 174          //    FOC_QMIN = QOUTMIN;
 175          
 176              /*********PLL或SMO**********/
 177              #if (EstimateAlgorithm == SMO)
 178                  //根据不同转速确启动的ATO_BW值
 179                  if(TailWindDetect.TailWindSpeed > 300)//300rpm/min
 180                  {
 181                      FOC_EKP               = OBSW_KP_GAIN_RUN4;
 182                      FOC_EKI               = OBSW_KI_GAIN_RUN4;
 183                      mcFocCtrl.mcIqref     = IQ_RUN_CURRENT;
 184                      mcFocCtrl.State_Count = 100;
 185                  }
 186                  else if(TailWindDetect.TailWindSpeed > 100)//300rpm/min
 187                  {
 188                      FOC_EKP               = OBSW_KP_GAIN_RUN3;
 189                      FOC_EKI               = OBSW_KI_GAIN_RUN3;
 190                      mcFocCtrl.mcIqref     = IQ_RUN_CURRENT;
 191                      mcFocCtrl.State_Count = 100;
 192                  }
 193                  else
 194                  {
 195                      FOC_EKP               = OBSW_KP_GAIN_RUN;
 196                      FOC_EKI               = OBSW_KI_GAIN_RUN;
 197                      mcFocCtrl.mcIqref     = IQ_RUN_CURRENT;
 198                      mcFocCtrl.State_Count = 2300;
 199                  }
 200          
 201              #elif (EstimateAlgorithm == PLL)
                       mcFocCtrl.mcIqref = IQ_RUN_CURRENT;
              
                  #endif //end    EstimateAlgorithm
 205          
 206          
 207              /*estimate parameter set*/
 208              FOC_OMEKLPF = SPEED_KLPF;
 209              FOC_IDREF   = ID_RUN_CURRENT;     // D轴启动电流
 210              FOC_IQREF   = mcFocCtrl.mcIqref ;         // Q轴启动电流
 211              PI_UK       =   mcFocCtrl.mcIqref   ;
 212          
 213          
 214              mcState            = mcRun;
 215              mcFocCtrl.CtrlMode = 0;
 216          }
 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
C51 COMPILER V9.52.0.0   FOCTAILDECT                                                       09/21/2018 10:43:22 PAGE 5   

 229          //        if(TailWindDetect.SpeedCountTimer < TailWind_Time)                      // SpeedCountTimer-计�
             -�时间间隔
 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              /*********PLL或SMO**********/
 251              #if (EstimateAlgorithm == SMO)
 252                   SMO_TailWindDealwith();
 253          
 254              #elif (EstimateAlgorithm == PLL)
                       PLL_TailWindDealwith();
              
                  #endif //end    EstimateAlgorithm
 258          }
 259          
 260          void SMO_TailWindDealwith(void)
 261          {
 262              //电机状态为正转且转速计数溢出时；或1-3状态抖动次数大于2次时，认为静止
 263              if(((TailWindDetect.MotorDir == CW) && (TailWindDetect.SpeedOverFlowStatus))||(TailWindDetect.ShakeTim
             -es > 2))
 264              {
 265                  MOE = 0;
 266                  SetBit(DRV_CR, FOCEN, 0);   //关闭FOC
 267                  mcState                           = mcPosiCheck;
 268                  McStaSet.SetFlag.PosiCheckSetFlag = 0;
 269                  mcFocCtrl.mcPosCheckAngle         = 0xffff;         // 角度赋初值
 270              }
 271              else if((TailWindDetect.MotorDir == CCW) && (TailWindDetect.SpeedStoreNum > 2))//||((TailWindDetect.Mo
             -torDir == CW)&&(TailWindDetect.TailWindSpeed>TailWindStartMinSpeed)))
 272              {
 273                  if(TailWindDetect.TailWindSpeed < 130)
 274                  {
 275                      mcFocCtrl.mcIqref       = IQ_Start_CURRENT + I_Value(0.0);       // Q轴启动电流
 276                      TailWindDetect.AntiFlag = 1;
 277                  }
 278                  else if(TailWindDetect.TailWindSpeed < 300)
 279                  {
 280                      mcFocCtrl.mcIqref       = IQ_Start_CURRENT + I_Value(0.0);       // Q轴启动电流
 281                      TailWindDetect.AntiFlag = 2;
 282                  }
 283                  else if(TailWindDetect.TailWindSpeed < 500)
 284                  {
 285                      mcFocCtrl.mcIqref       = IQ_Start_CURRENT + I_Value(0.0);       // Q轴启动电流
 286                      TailWindDetect.AntiFlag = 3;
C51 COMPILER V9.52.0.0   FOCTAILDECT                                                       09/21/2018 10:43:22 PAGE 6   

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

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

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