
/************************ (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 <fu68xx_mdu.h>
#include <ESC_PPM.h>
#include <ESC_Beep.h>
#include <MC_UserInterface_layer.h>
#include <MC_MotorCotrol_layer.h>  
#include <MC_MotorDriver_layer.h>                                     


/* Private typedef ------------------------------------------------------------------------------*/
/* Private define -------------------------------------------------------------------------------*/
//------------------------------------------------------------------------------
//PWMIN  调速输入   禁止修改
#define PWMIN              GP11
#define PWMIN_PIN          P11
#define PWMIN_GPIO_PORT    P1_OE

#define PWMIN_ON           {PWMIN = 0;}
#define PWMIN_OFF          {PWMIN = 1;}
#define PWMIN_ONOFF        {PWMIN = ~PWMIN;}
/* Private macro --------------------------------------------------------------------------------*/
/* Private variables ----------------------------------------------------------------------------*/
PWMINCtl_TypeDef xdata PWMINCtl;

/* Private function prototypes ------------------------------------------------------------------*/
void ESC_PPMSwitch(void);
void ESC_PPMCheckErr(void);
/* Private functions ----------------------------------------------------------------------------*/

/*-------------------------------------------------------------------------------------------------
Function Name :	void PWMIN_Init(void)
Description   :	PWM检测参数初始化
Input         :	无
Output				:	无
-------------------------------------------------------------------------------------------------*/
void ESC_PPMInit(void)
{
  float tfAlineA;
  #if(ESC_PPMSREFEN)
  PWMINCtl.FlagPWMAline = 0x7F;
  PWMINCtl.FlagPwmIn = 0xF1;
  
  PWMINCtl.PWMINBreakCnt = 0;
  PWMINCtl.PWMINHigh = 0;
  PWMINCtl.PWMINHighMin = 0;
  PWMINCtl.PWMINHighMax = 0;
  
  MDUControl.SpeedSrefErr = 32767;
  tfAlineA= ((float)(SREF_OUT_MAX-SREF_OUT_MIN)/(32767.0));
  MDUControl.AlineA = (u16)(tfAlineA*32767);                       //系数 a
  MDUControl.AlineB = (SREF_OUT_MAX - 32767*tfAlineA);             //系数 b  
#endif
}

/*-------------------------------------------------------------------------------------------------
Function Name :	void EscPpmGet(void)
Description   : //670 ~ 1559
Input         :	无
Output				:	无
-------------------------------------------------------------------------------------------------*/
void ESC_PPMGet(void)
{
  #if(ESC_PPMSREFEN)  
  u32 Temp32A;
  u16 Temp16B;
  u16 tPWMINHigh;
  u16 tValue,trefTar;
  
  if(ReadBit(TIM3_CR1, T3IR))
  {    
    PWMINCtl.PWMINTim3ITconter = 0;
    //速度给定标幺化_IQ(15) 0~32767

    if(PWMINCtl.PWMINSelfCheck == 3)
    {
      tValue = TIM3__DR<<3;
    }
    else if(PWMINCtl.PWMINSelfCheck == -3)
    {
      tValue = TIM3__DR;
    }
    else    //油门宽度识别125us or 1000us 
    {
      if(Ctl.State == MOTOR_STOP)
      {
        if((TIM3__DR > TempPWMINHighMinFiltA)&&(TIM3__DR < TempPWMINHighMaxFiltA))
        {
          PWMINCtl.PWMINSelfCheck--;
        }
        else if((TIM3__DR > TempPWMINHighMinFiltB)&&(TIM3__DR < TempPWMINHighMaxFiltB))
        {
          PWMINCtl.PWMINSelfCheck++;
        }
      }        
    }
    
    if((tValue > TempPWMINHighMinFilt)&&(tValue < TempPWMINHighMaxFilt))  //PPM 宽度在允许范围内
    {
      PWMINCtl.PWMINOutRangeCounter = 0;
      PWMINCtl.PWMINErrCotnter = 0;                       //油门恢复正常重启
      
      PWMINCtl.PWMINHigh = tValue;
      if(PWMINCtl.PWMINHigh > PWMINCtl.PWMINHighErr)      //PPM 宽度大于最小宽度
      {            
        if(PWMINCtl.PWMINHigh <= PWMINCtl.PWMINHighMax)   //PPM 宽度小于最大宽度
        {

          if(Ctl.State == MOTOR_NORMAL)
          {
            if(PWMINCtl.PWMINHigh < (PWMINCtl.PWMINHighMin+TempMotorLinearTimeErr))
            {
              tPWMINHigh = 1;
            }
            else
            {
              tPWMINHigh = PWMINCtl.PWMINHigh - PWMINCtl.PWMINHighErr;
            }
            
            Temp32A = (u32)tPWMINHigh<<15;
            Temp16B = PWMINCtl.PWMINHighMax - PWMINCtl.PWMINHighErr;
            EA = 0;
            MDUControl.SpeedSref = MDU_DIV_XDATA_U32(&Temp32A,&Temp16B); 
            trefTar = MDU_MULA_U16(MDUControl.SpeedSref,MDUControl.AlineA,15) + MDUControl.AlineB;
            EA = 1; 
            if( trefTar > 0x7FFE)
            {
              trefTar = 0x7FFE;              
            }    
            
            if(Ui.flg.FR == CCW)
            {
              Ctl.Spd.refTar = -trefTar; 
            }
            else
            {
              Ctl.Spd.refTar = trefTar; 
            }
//            Drv_SpeedRampCale();
//            Drv.PWM.DutyCur = Ctl.Spd.refCur;
//            Drv_PWM_Update(Drv.PWM.DutyCur);  //占空比给定
//            DRV_DR = MDU_MULA_U16(MDUControl.DutyCur,Drv.PWM.DutyArr+2,15);
          }
        }
        else                                              //PPM 宽度大于最大宽度
        {
          if(Ui.flg.FR == CW)
          {
            MDUControl.SpeedSref = _IQ(1.0);             
            Ctl.Spd.refTar = _IQ(1.0);
          }
          else
          {
            MDUControl.SpeedSref = _IQ(-1.0);             
            Ctl.Spd.refTar = _IQ(-1.0);           
          }
        }
      }
    }
    else
    {
      PWMINCtl.PWMINOutRangeCounter++;    
      if((PWMINCtl.PWMINOutRangeCounter >= 100)&&(PWMINCtl.PWMINErrCotnter >= 1000)) //50*2.73 = 136
      {
//        Ctl.SysError = E_ERR3;
//        PWMINCtl.PWMINOutRangeCounter = 100;
//        PWMINCtl.PWMINHigh = 0;
      }
    }
    ClrBit(TIM3_CR1,T3IR);    //CLRFlag_T3IR;
  }
  if(ReadBit(TIM3_CR1, T3IP))
  {
    ClrBit(TIM3_CR1,T3IP);    //CLRFlag_T3IP;
  }
  if(ReadBit(TIM3_CR1, T3IF))
  {
    PWMINCtl.PWMINTim3ITconter++;
    if((PWMINCtl.PWMINTim3ITconter >= 100)&&(PWMINCtl.PWMINErrCotnter >= 1000))  //50*2.73 = 136
    {
      Ctl.SysError = E_ERR2;
      PWMINCtl.PWMINTim3ITconter = 100;
      PWMINCtl.PWMINHigh = 0;
    }
    ClrBit(TIM3_CR1,T3IF); //CLRFlag_T3IF;
  }
  #endif  
}

/*-------------------------------------------------------------------------------------------------
Function Name :	void ESC_PPMSwitch(void)
Description   :  小于900us 刹车  小于920us 停机 大于920us ~2000us 调速
Input         :	无
Output				:	无
-------------------------------------------------------------------------------------------------*/

void ESC_PPMSwitch(void)
{
  #if(ESC_PPMSREFEN)  
  float tfAlineA;

  if(PWMINCtl.FlagPwmIn == 0xF1)  //上电油门报警或者油门校准 油门信息读取
  {
    if(PWMINCtl.PWMINHigh == 0)   //油门报警
    { 
      BeepRequestFun(PWMINError);
      PWMINCtl.FlagPwmIn = 0xF1;
    }
    else if(PWMINCtl.PWMINHigh > TempPWMHighMaxAline) //油门校准动作A
    {
      PWMINCtl.FlagPwmIn = 0xF1;
      PWMINCtl.PWMINCnt ++;
      if(PWMINCtl.PWMINCnt > 100)
      {
        PWMINCtl.PWMINCnt = 100;
        if(PWMINCtl.FlagPWMAline == 0x7F)
        {
          PWMINCtl.PWMINHighMax = PWMINCtl.PWMINHigh - 20; //修正最大值减去 30
          BeepRequestFun(PWMINAline);
        }
      }
    }
    else if(PWMINCtl.PWMINHigh < TempPWMHighMinAline) //油门校准动作B
    {
      PWMINCtl.FlagPwmIn = 0xF1;
      PWMINCtl.PWMINCnt --;
      
      if(Beep.BeepType != PWMINReady)
      {
        if(PWMINCtl.PWMINCnt < -30)
        {
          PWMINCtl.PWMINCnt = -30;
          #if(PWMINALINE_EN)
          if(PWMINCtl.FlagPWMAline == 0xFF)
          {
            PWMINCtl.PWMINHighMin = PWMINCtl.PWMINHigh;
            while(Flash_Sector_Erase((uint8 xdata *)0x3F00)); // 擦写第126扇区（16128~16255）
            while(Flash_Sector_Write((uint8 xdata *)0x3F00, PWMINCtl.PWMINHighMin)); // 写最小油门低字节
            while(Flash_Sector_Write((uint8 xdata *)0x3F01, (PWMINCtl.PWMINHighMin >> 8))); // 写最低油门高字节
            while(Flash_Sector_Write((uint8 xdata *)0x3F02, PWMINCtl.PWMINHighMax)); // 写最高油门低字节
            while(Flash_Sector_Write((uint8 xdata *)0x3F03, (PWMINCtl.PWMINHighMax >> 8))); // 写最高油门高字节
          }
          PWMINCtl.PWMINHighMin = ((*(unsigned char code *)16129)<<8) + (*(unsigned char code *)0x3F00);
          PWMINCtl.PWMINHighMax = ((*(unsigned char code *)16131)<<8) + (*(unsigned char code *)0x3F02);
          #endif 

          if(!(PWMINCtl.PWMINHighMin && PWMINCtl.PWMINHighMax))
          {
            PWMINCtl.PWMINHighMin = TempPWMHighMinDefault;
            PWMINCtl.PWMINHighMax = TempPWMHighMaxDefault; 
          }
        
          PWMINCtl.PWMINHighErr = PWMINCtl.PWMINHighMin + TempMotorONTimeErr;

          MDUControl.SpeedSrefErr = 32767;

          tfAlineA = ((float)(SREF_OUT_MAX-SREF_OUT_MIN)/(MDUControl.SpeedSrefErr));
          if(tfAlineA > 2.0)
          {
            tfAlineA = 1.9999;
          }
          MDUControl.AlineA = (u16)(tfAlineA*32767);
          MDUControl.AlineB = (SREF_OUT_MAX - 32767*tfAlineA);

          BeepRequestFun(PWMINReady);
        }
      }
    }
    else                        //油门报警
    {
      PWMINCtl.PWMINCnt = 0;
      PWMINCtl.FlagPwmIn = 0xF1;
      if(PWMINCtl.FlagPWMAline == 0x7F)
      {
        BeepRequestFun(PWMINError);
      }
    }
  }
  else                           //PWMIN上电校准完成
  {
    if(PWMINCtl.PWMINHigh == 0) //
    {
      Ui.flg.START = FALSE;
      //BeepRequestFun(PWMINError);
    }
    else if(PWMINCtl.PWMINHigh <= (PWMINCtl.PWMINHighMin + TempMotorOFFTimeErr))
    {
      PWMINCtl.PWMINCnt --;
      if(PWMINCtl.PWMINCnt < -5)
      {
        PWMINCtl.PWMINCnt = -5;
        
        Ui.flg.START = FALSE;
        
        if(Ctl.SysError != NONE)
        {
          Ctl.SysError = NONE;        //清除故障
          Ctl.State = MOTOR_STOP; 
          DRV_OE_ON;
        }        
      }
//      if(PWMINCtl.PWMINHigh <= (PWMINCtl.PWMINHighMin + TempMotorBreakTime))    //刹车判断
//      {
//        PWMINCtl.PWMINBreakCnt++;
//        if(PWMINCtl.PWMINBreakCnt > 100)
//        {
//          PWMINCtl.PWMINBreakCnt = 100;
//          //Ctl.FnStopmodeC = BREAK_DOWN;
//        }
//      }
//      else
//      {
//        PWMINCtl.PWMINBreakCnt--;
//        if(PWMINCtl.PWMINBreakCnt < -100)
//        {
//          PWMINCtl.PWMINBreakCnt = -100;
//          //Ctl.FnStopmodeC = FREE_DOWN;
//        }
//      }
    }
    else if(PWMINCtl.PWMINHigh > (PWMINCtl.PWMINHighMin + TempMotorONTimeErr))
    {
      PWMINCtl.PWMINCnt++;
      if(PWMINCtl.PWMINCnt > 100)
      {
        PWMINCtl.PWMINCnt = 100;
        if(Ui.flg.START == FALSE) 
        {
          Ui.flg.START = TRUE;
        }
      }
    }
  }
  #endif
}

void ESC_PPMCheckErr(void)
{
  //电调油门
  #if(ESC_PPMSREFEN)    
  if(PWMINCtl.PWMINErrCotnter >= 5000)  //油门故障信号复位
  {
    PWMINCtl.PWMINErrCotnter = 5000;
    PWMINCtl.PWMINHigh = 0;
    PWMINCtl.FlagPwmIn = 0xF1;
    if((Ctl.SysError == E_ERR2)||(Ctl.SysError == E_ERR3))
    {
      Ctl.SysError = NONE;        //清除故障
      Ctl.State = MOTOR_STOP; 
      DRV_OE_ON;
    }
  }

  if((PWMINCtl.PWMINTim3ITconter >= 50)||(PWMINCtl.PWMINOutRangeCounter >= 50))
  {
    PWMINCtl.PWMINErrCotnter++;
  }  
  #endif 
}