
/************************ (C) COPYRIGHT 2015 FT *******************************
* File Name          : 
* Author             : Application Team  Tom.wang 
* Version            : V2.0.0
* Date               : 06/15/2015
* Description        : 电机驱动层
********************************************************************************
/*******************************************************************************
* All Rights Reserved
*******************************************************************************/

/* Includes ------------------------------------------------------------------*/
#include <FU6812_MCU.h>
#include <sys_conf.h>
#include <fu681x_Device.h>
#include <interrupt.h>
#include <Ftech_BLDC.h>
#include <MC_Led.h>
#include <MC_key.h>
#include <MC_PwmIn.h>
#include <MC_Beep.h>
#include <MC_init.h>
#include <MC_UserInterface_layer.h>
#include <MC_MotorCotrol_layer.h>  
#include <MC_MotorFunction_layer.h> 
#include <MC_MotorDriver_layer.h>   

/* Private typedef -----------------------------------------------------------*/
/* Private define ------------------------------------------------------------*/

/* Private macro -------------------------------------------------------------*/
/* Private variables ---------------------------------------------------------*/
DRV_TypeDef xdata Drv; 
u16 idata giEventCounter;

/* Private function prototypes -----------------------------------------------*/
void Drv_DutyLimit(void);
void Drv_StallRestart(void);               //堵转重启
void Drv_SpeedRampCale(void);              //闭环加减速曲线
void Drv_DutyRampCale(void);               //开环加减速曲线   准备废除 此函数 TOM20170831

void Drv_PWM_Update(u16 tduty);            //PWM更新
void Drv_PosTrack(void);                   //转速跟踪
void DRV_HallEventCalc(void);              
void DRV_EemfEventCalc(void);
void Drv_BemfTim1PDapp(void) ;             //bemf位置检测中断服务函数
/* Private functions ---------------------------------------------------------*/

/*******************************************************************************
* Function Name  : Drv_StallRestart    
* Description    : 堵转重启
* Input          : 
* Output         : 
* Return         : 
*******************************************************************************/
void Drv_DutyLimit(void)           
{
  #if (CURRENT_CLOSE_EN == 1)
  if(Drv.AdcMeas.ImeasBus >  Drv.PWM.DutyLimitMaxRef)
  {
    Drv.PWM.DutyLimitFlag = 0x01;
  }
  else
  {
    Drv.PWM.DutyLimitFlag &= (~0x1);
  }
  #endif
  
  #if(SPEED_LIMIT_EN)
  if((Drv.speed.EventPeriod < ESC_SPEEDLIMITCNT)&&(Drv.speed.EventPeriod != 0))
  {
    Drv.PWM.DutyLimitFlag |= 0x02;
  }
  else
  {
    Drv.PWM.DutyLimitFlag &= (~0x2);
  }
  #endif
}
/*******************************************************************************
* Function Name  : Drv_StallRestart    
* Description    : 堵转重启
* Input          : 
* Output         : 
* Return         : 
*******************************************************************************/
void Drv_StallRestart(void)             //堵转重启
{
  #if (STALLRESTARTNUM != 255)
  if(Ctl.Stall.u8Num < STALLRESTARTNUM)
  #endif
  {
    Ctl.Stall.u8Num++;
    Ctl.Tim.STAnms = 0;
    Ctl.Tim.STB1nms = 0;
    Ctl.Tim.STB2nms = 0;
    Ctl.Stall.u8FL = 0;
    DRV_OE_ON;
    Ctl.SysError = NONE;
    if(Ctl.Stall.u16NormalRunms < STANMS+500)
    {
      Ctl.State = MOTOR_STOP;
    }
    else
    {
      Ctl.State = MOTOR_READY;
    }
  }
}


/*******************************************************************************
* Function Name  : Drv_SpeedRampCale 闭环加减速曲线
* Description    : 速度闭环加减速曲线 开环无效
* Input          : 目标速度 refTar _IQ15格式
* Output         : 给定速度 refCur _IQ15格式
* Return         : 给定速度 refCur _IQ15格式
问题：TOM++ u16 Q15格式 无法计算30000-(-2000) 会溢出 20170830 18：43 
*******************************************************************************/
void Drv_SpeedRampCale(void)
{
  if((Ctl.Spd.refTar > 0)&&(Ctl.Spd.refCur < 0))
  {
    Ctl.Spd.refTartemp = 0;
  }
  else if((Ctl.Spd.refTar < 0)&&(Ctl.Spd.refCur > 0))
  {
    Ctl.Spd.refTartemp = 0;
  }
  else
  {
    Ctl.Spd.refTartemp = Ctl.Spd.refTar; 
  }

  if((Ctl.Spd.refTartemp - Ctl.Spd.refCur) > Ctl.Spd.IncValue)
  {
    Ctl.Spd.refCur += Ctl.Spd.IncValue;
  }
  else if((Ctl.Spd.refCur - Ctl.Spd.refTartemp) > Ctl.Spd.DecValue)
  {
    Ctl.Spd.refCur -= Ctl.Spd.DecValue;
  }
  else
  {
    Ctl.Spd.refCur = Ctl.Spd.refTartemp;
  }
}

/*******************************************************************************
* Function Name  : Drv_DutyRampCale 
* Description    : 计算频率，每次斩波计算一次 
* Input          : 目标DUTY  格式_IQ15 即0x7FFF
* Output         : 当前DUTY  格式_IQ15 即0x7FFF
* Return         : 当前DUTY  格式_IQ15 即0x7FFF       
*******************************************************************************/
void Drv_DutyRampCale(void)
{
  if((Drv.PWM.DutyDecValue != 0x7FFF)&&(Drv.PWM.DutyIncValue != 0x7FFF))
  {
    if((Drv.PWM.DutyCur - Drv.PWM.DutyTar) > Drv.PWM.DutyDecValue)
    {
      Drv.PWM.DutyCur -= Drv.PWM.DutyDecValue;
    }
    else if((Drv.PWM.DutyTar - Drv.PWM.DutyCur) > Drv.PWM.DutyIncValue) 
    {
      Drv.PWM.DutyCur += Drv.PWM.DutyIncValue;  
    }
    else
    {
      Drv.PWM.DutyCur = Drv.PWM.DutyTar;
    }
  }
  else
  {
    Drv.PWM.DutyCur = Drv.PWM.DutyTar;
  }
}

/*******************************************************************************
* Function Name  : Drv_PWM_Update
* Description    : PWM更新模块，每次斩波更新，实时更新
* Input          : _IQ15 格式duty
* Output         : DRV_DR          
* Return         : DRV_DR
*******************************************************************************/
void Drv_PWM_Update(u16 tduty)
{
  if(Drv.PWM.DutyLimitFlag != 0)                 //输出限制
  {
    Drv.PWM.DutyMax -= Drv.PWM.DutyLimitValue;
    if(Drv.PWM.DutyMax < 3276)
    {
      Drv.PWM.DutyMax = 3276;
    }
  }
  else
  {
    Drv.PWM.DutyMax += Drv.PWM.DutyLimitValue;
    if(Drv.PWM.DutyMax > MOTOR_VS_MAX)
    {
      Drv.PWM.DutyMax = MOTOR_VS_MAX;
    }
  }    
  //#endif
  tduty +=10;
  if(tduty > Drv.PWM.DutyMax)
  {
    tduty = Drv.PWM.DutyMax;
  }
  else if(tduty < Drv.PWM.DutyMin)
  {
    tduty = Drv.PWM.DutyMin;
  }
  
  MDUControl.DutyCur = tduty;
  MDUFlag.PWMDuty = 1;
}


/*******************************************************************************
* Function Name  : Drv_PosTrack
* Description    : 电机转子位置跟踪
* Input          : None
* Output         : None
* Return         : 1
*******************************************************************************/
void Drv_PosTrack(void)
{
#if (POS_FB_MODE == SensorLess)

  #if (WAIT_STEP == WAIT_STRACK)          //调试模式
  Drv.Stk.Calcnms = 0;
  #endif
  
  if(Drv.Stk.Calcnms < STKNMS) 
  { 
    if(Drv.Stk.BemfFR != 0xFF)
    {
      if(Ctl.gDirectionC == Drv.Stk.BemfFR)
      {
        if(Ctl.gDirectionC == CW)
        {
          Ctl.gStepCur = Drv.Stk.BemfTabA; 
        }
        else
        {
          Ctl.gStepCur = Drv.Stk.BemfTabB;
        }

        TIM1__RCNTR = 0;
        ANGLE_MASK(10);
        ANGLE_DELAY(10);         
  
        //TIM1__RARR = Ctl.Ramp.MaskTime>>2;
        ClrBit(TIM1_CR0, T1OPS1);
        ClrBit(TIM1_CR0, T1OPS0);                          //CMP检测反电势
        
        TIM1_CR4 = Ctl.gStepCur;                            //选择检测沿 
        
        ClrBit(TIM1_CR0, T1OPS1);
        SetBit(TIM1_CR0, T1OPS0);                          //CMP检测反电势

        SetBit(CMP_CR3, SAMSEL1);                        // 使能ON 采样
        ClrBit(CMP_CR3, SAMSEL0);  
        
        SetBit(TIM1_IER, T1PDIE);                          //使能位置检测中断
        SetBit(TIM1_CR2, T1BRS);     //++                  //1位置检测复位 0写入复位TIM

        Drv.PWM.DutyTar = Ctl.Ramp.DutyEnd ;            
        Drv.PWM.DutyCur = Drv.PWM.DutyTar;  
        DRV_DR = MDU_MULA_U16(MDUControl.DutyCur,Drv.PWM.DutyArr+2,15);
        giEventCounter = 0;
        Ctl.State = MOTOR_NORMAL;

      }
      else if(Drv.Stk.BemfFR == 0x7F) 
      {
        #if (STRACK_EN == 2)
        Ctl.FnStopmodeC = FREE_DOWN;
        #endif
        ClrBit(TIM1_CR2, T1BRS);   //TOM++ TIM1_CR2?
        Ctl.State = MOTOR_PRECHARGE;
      }  
      else
      {     
        #if (STRACK_EN == 2)
        Ctl.State = MOTOR_STOP;
        Ctl.FnStopmodeC = BREAK_DOWN;
        #endif
      }
    } 
  }
  else
  {
    #if (STRACK_EN == 2)
    Ctl.FnStopmodeC = FREE_DOWN;
    #endif
    ClrBit(TIM1_CR2, T1BRS);       // 0写入复位TIM1   1位置检测复位 
    Ctl.State = MOTOR_PRECHARGE;
  }

  if(Ctl.gStartC == FALSE) 
  {
    Ctl.State = MOTOR_STOP;
  }
#endif
//  ClrBit(TIM1_CR2, T1BRS);      // 0写入复位TIM1   1位置检测复位 
//  Ctl.State = MOTOR_PRECHARGE;

}


/*******************************************************************************
* Function Name  : Drv_BemfTim1PDapp    
* Description    : 
* Input          : None
* Output         : None
* Return         : None
*******************************************************************************/
void Drv_BemfTim1PDapp(void) 
{
  if(Ctl.State == MOTOR_NORMAL)
  {
    DRV_EemfEventCalc();                    //运行策略
  }
  else if(Ctl.State == MOTOR_OPENLOOP)
  {
    //Drv_SmartStartCalc();                   //启动算法
  }
  else if(Ctl.State == MOTOR_READY)      
  {
    Drv_SectionCheak();                   //转速跟踪
  }
}  


/*******************************************************************************
* Function Name  : DRV_HallEventCalc    
* Description    : 
速度计算模块，根据HALL信号计算速度
1、软件换相
2、硬件换相
3、软件换相，设置超前角（滞后角） 需要软件干预，防止超前换相时，hall信号先来。
* Input          : TIM1_BCCR
* Output         : 速度单位 RPM
* Return         : 无

*******************************************************************************/

#define HALL_ADVANCEEANGLE (20) //超前角度  
#define HALL_RUNMODE       (2)  //1->软件换相 2->硬件换相 3->超前换相(软件参与)

void DRV_HallEventCalc(void)
{
  giEventCounter++;
 
  if(giEventCounter >= 200)
  {
    giEventCounter = 200;
  }
  else if(giEventCounter == 30)
  {
    #if (HALL_RUNMODE == 3)
    ANGLE_DELAY((60-HALL_ADVANCEEANGLE));    
    ClrBit(TIM1_CR0, T1OPS1);         //00->软件触发 01->上溢 10->位置检测|ADC计算 11->重载+上溢
    SetBit(TIM1_CR0, T1OPS0);         //10  01
    #endif
  }
  else if(giEventCounter == 12)
  {   
    #if (HALL_RUNMODE == 2)
    TIM1_CR4 = Ctl.gStepCur;   
    SetBit(TIM1_CR0, T1OPS1);         //00->软件触发 01->上溢 10->位置检测|ADC计算 11->重载+上溢
    ClrBit(TIM1_CR0, T1OPS0);         //10  01 
    #endif
  }
  
  else     //软件换相
  {
    #if (HALL_RUNMODE == 1)
    giEventCounter = 6;
    #endif
    if(Ctl.gStepPre != Ctl.gStepCur )
    {
      if((Ctl.State == MOTOR_NORMAL)||(Ctl.State == MOTOR_PRECHARGE))
      {
        TIM1_CR4 = Ctl.gStepCur;
        Ctl.gStepPre = Ctl.gStepCur ;
      }
    }  
  }
}

/*******************************************************************************
* Function Name  : DRV_EemfEventCalc    
* Description    : 速度计算模块，根据HALL信号计算速度
* Input          : TIM1_BCCR
* Output         : 速度单位 RPM
* Return         : 无
*******************************************************************************/
#define BEMF_CMP          1 //CMP换相
#define BEMF_ADC          2 //ADC换相
#define BEMF_CMP_ADC0     3 //CMP+ADC上溢换相
#define BEMF_CMP_ADC1     4 //CMP+ADC立刻换相
#define BEMF_MODE         BEMF_CMP     
     

void DRV_EemfEventCalc(void)
{  
  #if (POS_FB_MODE == SensorLess) 
  giEventCounter++; 

  if(giEventCounter >= 6)
  {
//    if((Drv.speed.EventPeriod > STB1C))
//    {
//      Ctl.SysError = E_STB1;
//    } 
//    if(Drv.speed.EventPeriod < STB2C)
//    {
//      Ctl.SysError = E_STB2;
//    }

    #if(STB3_EN)
    if(((Drv.speed.EventPeriod >>2) > Drv.speed.EventPeriodPer)||(Drv.speed.EventPeriod < (Drv.speed.EventPeriodPer>>2)))
    {
//      Ctl.SysError = E_STB3;
    }

    #endif
  }
   
  Drv.speed.EventPeriodPer = Drv.speed.EventPeriod;
  
  if(giEventCounter >= 200)
  {
    giEventCounter = 200;
  }
  else if(giEventCounter >= 150)
  {
    #if (BEMF_MODE == BEMF_CMP_ADC0 )
    ANGLE_DELAY(30);                      //ADC模式 只是你ON采样
    SetBit(CMP_CR3, SAMSEL1);       // 00->ON+OFF 01->OFF  10->ON 11->ONOFF加MASK
    ClrBit(CMP_CR3, SAMSEL0);       // 
    
    ClrBit(TIM1_CR1, T1BRS);          // 1位置检测复位 0写入复位TIM
    SetBit(TIM1_CR3, T1TIS1);         // 00->GPIO 01->CMP 10->ADC 11->CMP+ADC
    SetBit(TIM1_CR3, T1TIS0);
    SetBit(TIM1_CR0, T1OPS1);         // 00->软件触发 01->上溢 10->位置检测|ADC计算 11->重载+上溢
    SetBit(TIM1_CR0, T1OPS0);         //注意 10测试没问题 11测试没问题
    #elif (BEMF_MODE == BEMF_CMP_ADC0 )
    ANGLE_DELAY(30);                      //ADC模式 只是你ON采样
    SetBit(CMP_CR3, SAMSEL1);       // 00->ON+OFF 01->OFF  10->ON 11->ONOFF加MASK
    ClrBit(CMP_CR3, SAMSEL0);       // 
    
    ClrBit(TIM1_CR1, T1BRS);          // 1位置检测复位 0写入复位TIM
    SetBit(TIM1_CR3, T1TIS1);         // 00->GPIO 01->CMP 10->ADC 11->CMP+ADC
    SetBit(TIM1_CR3, T1TIS0);
    SetBit(TIM1_CR0, T1OPS1);         // 00->软件触发 01->上溢 10->位置检测|ADC计算 11->重载+上溢
    ClrBit(TIM1_CR0, T1OPS0);         //注意 10测试没问题 11测试没问题
    #elif (BEMF_MODE == BEMF_ADC )
    ANGLE_DELAY(30);                      //ADC模式 只是你ON采样
    SetBit(CMP_CR3, SAMSEL1);       // 00->ON+OFF 01->OFF  10->ON 11->ONOFF加MASK
    ClrBit(CMP_CR3, SAMSEL0);       // 
    
    ClrBit(TIM1_CR1, T1BRS);          // 1位置检测复位 0写入复位TIM
    SetBit(TIM1_CR3, T1TIS1);         // 00->GPIO 01->CMP 10->ADC 11->CMP+ADC
    ClrBit(TIM1_CR3, T1TIS0);
    SetBit(TIM1_CR0, T1OPS1);         // 00->软件触发 01->上溢 10->位置检测|ADC计算 11->重载+上溢
    ClrBit(TIM1_CR0, T1OPS0);      
    #elif (BEMF_MODE == BEMF_CMP ) 

    ClrBit(TIM1_CR2, T1BRS);      // 1位置检测复位 0写入复位TIM
//    SetBit(TIM1_CR1, BAPE);       
//    SetBit(TIM1_CR0, FORC);      // 强制换相60度电角度

    ANGLE_MASK(RUN_ANGLE_MASK);
    ANGLE_DELAY(RUN_ANGLE_DELAY);                //
    SetBit(CMP_CR3, SAMSEL1);    // 使能ON 采样
    SetBit(CMP_CR3, SAMSEL0);    // 使能OFF采样
    #endif   
  }    
  else if(giEventCounter >= 100)
  {
    ANGLE_MASK(35);
    ANGLE_DELAY(1);  
  }
  else if(giEventCounter == 30)
  {

    Drv.PWM.DutyArr = RUN_PWMARR;
    DRV_ARR = Drv.PWM.DutyArr;
    DRV_COMR = PWM_FLOW ;    

    Drv.PWM.DutyIncValue = SPEED_INCVALUE ;   //loop INC DEC
    Drv.PWM.DutyDecValue = SPEED_DECVALUE ;
  }
    
  #endif
}

