
/************************ (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 <ESC_PPM.h>
#include <ESC_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 ---------------------------------------------------------*/
//全关，UH->VL,UH->WL,VH->WL,VH->UL,WH->UL,WH->VL,刹车
/* Private function prototypes -----------------------------------------------*/

void SoftwareInit(void);          //软件初始化
static void GPIO_Config(void);    //静态函数
static void Interrupt_Config(void);
static void EXTI_Config(void);
void MCL_ModuleInit(void);        //初始化
void MCL_ModuleDefault(void);     //默认值
void gDelayus(u16 timer);
void gDelayms(u16 timer);

static void PWMinit(void);
static void Bemfinit(void);
static void HallDefault(void);
static void BemfDefault(void);
static void TimNmsDefault(void);

/* Private functions ---------------------------------------------------------*/

/*******************************************************************************
* Function Name  : ApplicationInit   Peripheral
* Description    :外设初始化
* Input          : None
* Output         : None
* Return         : None
*******************************************************************************/
void SoftwareInit(void)
{   
  // GPIO Configuration 
  GPIO_Config(); 
  // EXTI Configuration 
  //EXTI_Config();
  // Time1 Configuration 
  TIM1_Config();
  // TIM2 Configuration 
  //TIM2_Config();
  // TIM3 Configuration 
  #if(ESC_PPMSREFEN)
  TIM3_Config();
  ESC_PPMInit();
  #endif
  // TIM4 Configuration 
  TIM4_Config();
  // TIM5 Configuration 
  //TIM5_Config();  
  // AD Configuration 
  ADC_Config();
  // CMP Configuration
  CMP_Config();
  // AMP Configuration
  AMP_Config();
  // SPI Configuration
//  SPI_Config();
 
  //KEY Configuration	
  Key_Config();
 
  //Hall Configuration
  #if (POS_FB_MODE == HallSensor)
  Hall_Init();
  #endif
  //USRT Configuration
  #if(UARTSREF_EN)
  USRT_Config();
  #endif
  //DMA Configuration
//  DMA_Config();
  Set_DBG_DMA(0x4082);
  //Pid Configuration
  Pid_Config();
  //mdu Configuration
  MDU_Config();
  
  //INT Configuration
  Interrupt_Config();  
  //上电等待模块，等待电源稳定后启动
  Ctl.Tim.PowerOnNms = 0;                //
  Ctl.gPowerOnF == 0XFF;

//  while(Ctl.Tim.PowerOnNms <= POWER_ON_NMS){}; 

  EA = 0;
  gDelayms(POWER_ON_NMS);
  EA = 1; 
  
  PDELAY_OFF;
  GP42 = 1;
    
  Ctl.gPowerOnF = 0x7F;//                              //上电完成
  Ctl.SysError = NONE;   

}

/*******************************************************************************
* Function Name  : Interrupt_Config   
* Description    : 中断优先级判断
* Input          : None
* Output         : None
* Return         : None
*******************************************************************************/
void Interrupt_Config(void)
{
  EA = 0;                                                    // 禁止全局中断
  #if (EFAL == FO_CMP)  
    PCMP1 = 1;                                               // 中断优先级别为3
    PCMP0 = 1;                                               
  #elif (EFAL == FO_INT)
    ClrBit(P0_OE, P0_OE);                                   // config P00 as input
    SetBit(P0_PU, P0_OE,);                                   // enable P00 pull up	
    IF0 =0;
    PX01 = 1;                                            
    PX00 = 1; 
  #elif (EFAL == FO_CMPINT)
    PCMP1 = 1;                                               // 中断优先级别为3
    PCMP0 = 1;   
   /*EXTI中断配置*/                                          // 中断优先级别为3
    ClrBit(P0_OE, P0_OE);                                   // config P00 as input
    SetBit(P0_PU, P0_OE);                                   // enable P00 pull up	
    IF0 =0;
    PX01 = 1;                                            
    PX00 = 1;  
  #endif

  //drv中断
  PDRV1 = 1;                                              // 中断优先级设置为2，优先级低于FO硬件过流
  PDRV0 = 0;
    
  //TIM1 有HALL/CMP位置捕获用
  PTIM11 = 1;
  PTIM10 = 1;                                              // 中断优先级别为2

  //SYS_TICK&TIM4 100us定时器
  PTIM4S1 = 0;
  PTIM4S0 = 1;                                           // 中断优先级别为1

  //PWMIN 输入
  PTIM31 = 0;
  PTIM30 = 0;                                             // 中断优先级别为1

  #if(UARTSREF_EN)                                        //DMA
  PUART1 = 0;
  PUART0 = 0;                                             // 中断优先级别为1
  #endif                                                     

  //ADC中断配置 电流采样自动采样 PWMON
  PADC1 = 0;
  PADC0 = 0;                                              // 中断优先级别为0

  EA = 1;                                                 // 使能全局中断
}

/*******************************************************************************
* Function Name  : EXTI_Config   
* Description    : 中断优先级判断
* Input          : None
* Output         : None
* Return         : None
*******************************************************************************/

void EXTI_Config(void)
{
  ClrBit(P0_OE, P00);  // config P00 as input
  ClrBit(P0_PU, P00);  // diasable P00 Pull Up
  //  ClrBit(P0_AN, P00_AN);  // diasable P00 analog function
  IF0 = 0;               // clear P00 interrupt flag	
  //  SetBit(P1IE, P10);   // config P00 as the source of EXTI1

  IT11 = 0;
  IT10 = 0;               // 00: posedge mode interrupt，01: negedge mode interrupt，1x: edge-change mode interrupt

  PX11 = 1;
  PX10 = 0;               // 中断优先级别2，中断优先级最高

  EX0 = 1;                // 使能外部中断0
}
/*******************************************************************************
* Function Name  : GPIO_Config
* Description    : GPIO配置
* Input          : None
* Output         : None
* Return         : None
*******************************************************************************/
void GPIO_Config(void)
{    
  //-------------------------------------  
  //FD6536 使能 
//  #if(FD6536PIN_EN)  
//  SetBit(P0_OE,P01);  //输入使能
////  SetBit(X0_PU,X0_PIN);         //上拉电阻
//  GP01 = 1;
//  #endif
  SetBit(P4_OE,P42);  //输入使能
  
  
  //------------------------------------  
  SetBit(PDELAY_GPIO_PORT,PDELAY_PIN); 
  PDELAY_ON;
  
  //-------------------------------------  
  //FG
  SetBit(FG_GPIO_PORT,FG_PIN); 
  //TIM2
  SetBit(P1_OE,P10);
  GP10 = 1;
//  //-------------------------------------  
//  //故障输出
  SetBit(FAULT_GPIO_PORT,FAULT_PIN);
  FAULT_OFF;

  SetBit(NSS_GPIO_PORT,NSS_PIN); 
  SetBit(MOSI_GPIO_PORT,MOSI_PIN); 
  SetBit(SCLK_GPIO_PORT,SCLK_PIN); 
  SetBit(MISO_GPIO_PORT,MISO_PIN);   
    
  //LED显示设置       电量显示
  SetBit(LED1_GPIO_PORT,LED1_PIN); 
  SetBit(LED2_GPIO_PORT,LED2_PIN); 
  SetBit(LED3_GPIO_PORT,LED3_PIN); 
//  LED1_OFF;  
  LED2_OFF; 
  LED3_OFF; 
  gDelayms(200);
  LED1_ON;  
  LED2_ON; 
  LED3_ON; 
  gDelayms(200);
  LED1_OFF;  
  LED2_OFF; 
  LED3_OFF; 
  gDelayms(200);
  LED1_ON;  
  LED2_ON; 
  LED3_ON; 
}

/*******************************************************************************
* Function Name  : LED_show
* Description    : 
* Input          : 
* Output         : None
* Return         : None
*******************************************************************************/
void MCL_ModuleInit(void)
{
  //----------------------------------
  Ctl.SysError = NONE; 
  
  #if (CURRENT_CLOSE_EN==3)
  Power.ref = CURRENT_REF_TAR; 
  #endif
  #if(LAMP_EN) //扳手螺丝批，延时掉电程序
  Rom.ReadValue = Flash_GetAddress();    //4.88ms 
  Rom.WriteValue = Rom.ReadValue;  
  if(Rom.WriteValue == 0x7F)
  {
    Ctl.Spd.SpeedShift = 3;
  }
  else
  {
    Ctl.Spd.SpeedShift = Rom.WriteValue;
  }
  #endif

  //----------------------------------
  //控制参数初始化  
  Ctl.Tim.numTicksPerCurrent = 2;                   //2
  Ctl.Tim.numTicksPerSpeed = SPEED_CTL_CNT;         //6
  
  //----------------------------------
  // 电机运行方向设置 端子控制
  Ui.flg.FR = DIRECTION ;  

  Ctl.gStopmodeC = STOPMODE;

  DRV_CMR = PWMOUT_OFF;

  //----------------------------------
  // 母线电压增益  电流增益   BEMF增益
  ADCInit();
  //----------------------------------
  //PWM初始化
  PWMinit();
  //----------------------------------
  //速度闭环 加减速时间  
  #if(SPEED_CLOSE_EN)
  Ctl.Spd.refCur = 0;  
  Ctl.Spd.refTar = SPEED_REF_TAR;
  Ctl.Spd.IncValue = SPEED_INCVALUE;
  Ctl.Spd.DecValue = SPEED_DECVALUE;
  pid_spd.Kp = SPEED_PTERM; //Q15格式
  pid_spd.Ki = SPEED_ITERM; //Q15格式
  Pid_spdInit(); 
  #endif
  //----------------------------------  
  #if (CURRENT_CLOSE_EN >1)
  //电流环初始化
  Ctl.Is.refCur = _IQ(0.01);  
  Ctl.Is.refTar = _IQ(0.01);
  pid_is.Kp = CURRENT_PTERM; //Q15格式
  pid_is.Ki = CURRENT_ITERM; //Q15格式
  Pid_isInit(); 
  #endif
  //----------------------------------  
  #if(MOTORCIRCLECALEEN)     
  Ctl.Spd.ComNum  = 0;
  Ctl.Spd.MechCircleNum = 0;
  #endif
  //----------------------------------
  //速度计算模块初始化
  Drv.speed.EventPeriod = 0;    
  Drv.speed.InputSelect = 1;
  Drv.speed.BaseRpm = BASE_SPEED; 
  Drv.speed.SpeedScaler = SPEEDSCALER ;
  
  //----------------------------------
  #if (POS_FB_MODE == SensorLess) 
  Bemfinit();
  #else
  HallDefault();
  #endif
  //==================================
  #if(OCRESTART_EN)
  Ctl.OC.u16Runms = 0;
  Ctl.OC.u16nmsCount = 0;
  Ctl.OC.u8ReNum = 0;
  #endif
  //----------------------------------
  #if(OVERLOAD_EN)
  MCL_OverLoadInit(MECASEOVERLOAD0,MECASEOVERLOAD1,MECASEOVERLOAD2,MECASEOVERLOAD3,OL0NMS,OL1NMS,OL2NMS,OL3NMS);  
  #endif
  
  BeepRequestFun(PWMINinit);
}

/*******************************************************************************
* Function Name  : MCL_ModuleDefault
* Description    : ready状态执行一次
* Input          : 
* Output         : None
* Return         : None
*******************************************************************************/
void MCL_ModuleDefault(void)
{
  //----------------------------------
  //电机运行方向设置 端子控制

  if(Ui.flg.FR == CW)
  {
    Ctl.gDirectionC = Ui.flg.FR;    //方向控制
    ClrBit(DRV_CR, DDIR);
  }
  else if(Ui.flg.FR == CCW)
  {
    Ctl.gDirectionC = Ui.flg.FR;    //方向控制
    SetBit(DRV_CR, DDIR); 
  }
  //----------------------------------
  //Step 清零
  Ctl.gStepCur = 5;
  Ctl.gStepPre = 0;

  #if(STALLRESTARTNUM)     //堵转重启
  Ctl.Stall.WaitReatartNms = 0;              
  #endif
  
  #if (POS_FB_MODE == SensorLess) 
  BemfDefault();           //BEMF清零
  #else
  HallDefault();
  #endif
//----------------------------------
  #if(SPEED_CLOSE_EN)     //速度闭环
  Ctl.Spd.refCur = 0;  
  Pid_spdInit();  
  #else       
  Ctl.Spd.refTar = MOTOR_TARGRT_DUTY;
  Drv.PWM.DutyTar = Ctl.Spd.refTar;
  Drv.PWM.DutyCur    = 0;  
  #endif

  #if(CURRENT_CLOSE_EN>1)
  Pid_isInit();          //电流闭环
  #endif
//----------------------------------
  TimNmsDefault();       //计时器清零
  Key_init();            //端子清零

//==================================
  #if(MOTORCIRCLECALEEN)  //运行圈数计算
  Ctl.Spd.ComNum  = 0;
  Ctl.Spd.MechCircleNum = 0;
  #endif
  
  #if(FAILSTOREEN) //故障记录
  Ctl.E_message.State = MOTOR_NONE;
  Ctl.E_message.ErrorF = 0;
  #endif
  
  #if(OVERLOAD_EN) //过载
  MCL_OverLoadInit(MECASEOVERLOAD0,MECASEOVERLOAD1,MECASEOVERLOAD2,MECASEOVERLOAD3,OL0NMS,OL1NMS,OL2NMS,OL3NMS);  
  #endif
  
  #if(FO_EN)   //逐波限流
  Ctl.FO.FoCounter = 0;
  Ctl.FO.ReCounter = 0;
  #endif
  
  #if(BEEPSCAN_EN) //电机发声
  Beep.BeepRequest = 0xFF;
  Beep.BeepType = 0;
  #endif
}


void Bemfinit(void)
{
  Ctl.Bemf.TestOut = BEMF_DEBUG;
  
  Ctl.Ramp.cpmode = RAMP_MODE;
  //定位，拖动参数设定
  #if (MOTORROTORCALC == 2)
  Ctl.Alig.duty   = 0; 
  Ctl.Alig.timNms = 0;
  #else
  Ctl.Alig.duty   = ALIGNMENTDUTY; 
  Ctl.Alig.timNms = ALIGNMENTNMS;  
  #endif

  //启动参数调整区
  Ctl.Ramp.ComCnt = 0;
  Ctl.Ramp.cpNms = RAMP_TIM_STA;
  Ctl.Ramp.cpNmsCount = 0;
  Ctl.Ramp.TimSta   = RAMP_TIM_STA;
  Ctl.Ramp.TimEnd   = RAMP_TIM_END;
  Ctl.Ramp.TimStep  = RAMP_TIM_STEP;       
  Ctl.Ramp.DutySta  = RAMP_DUTY_STA;
  Ctl.Ramp.DutyEnd  = RAMP_DUTY_END;
  Ctl.Ramp.Dutystep = RAMP_DUTY_INC;
  Ctl.Ramp.ComNum   = RAMP_COM_CNT;
  Ctl.Ramp.BemfErrCnt = 0;
  Ctl.Ramp.FailCnt = 0;
  Ctl.Ramp.BemfTureNum = RAMP_TURE_NUM ;
  Ctl.Ramp.BemfTureCnt = 0;
  Ctl.Ramp.BemfErrNum = RAMP_ERR_NUM;
  Ctl.Ramp.MaskTime =  TempRampMaskTime;
  Ctl.Bemf.CTAngle =   RUN_ANGLE_MASK;
  Ctl.Bemf.MaskAngle = RUN_ANGLE_DELAY;

  Drv.Stk.Calcnms = 0x7F;  
  Drv.Stk.Calcnum = SRKBNUM;

  MDUControl.DelayAngle = 0;
}

void PWMinit(void)
{

  Ctl.Spd.IncValue = SPEED_INCVALUE;
  Ctl.Spd.DecValue = SPEED_DECVALUE;
  Drv.PWM.DutyCur    = 0;
  Drv.PWM.DutyTar     = 0;  
  Drv.PWM.DutyIncValue  = SPEED_INCVALUE;
  Drv.PWM.DutyDecValue  = SPEED_DECVALUE;
  Drv.PWM.DutyMax = MOTOR_VS_MAX;
  Drv.PWM.DutyMin = MOTOR_VS_MIN;
  Drv.PWM.DutyLimitMaxRef = MECASECURRENTLIMITMAXREF;
  Drv.PWM.DutyLimitValue = LIMIT_DUTYINCDEC;
}
void HallDefault(void)
{
  ANGLE_MASK(0);
  ClrBit(TIM1_CR0, T1OPS0);    //00 软件触发 01重载触发 10位置检测触发 11重载/基本上溢触发
  ClrBit(TIM1_CR0, T1OPS1);
  Drv.Hall.u8ChangePhaseNum = 0;  

  #if ((HALLADVANCEEANGLE_CW)||(HALLADVANCEEANGLE_CCW))
  if(Ctl.gDirectionC == CW)
  {
    Drv.Hall.s16AdvanceEangle = TEMPHALLADVANCEEANGLE_CW; //HALL位置反馈换相超前角度
  }
  else
  {
    Drv.Hall.s16AdvanceEangle = TEMPHALLADVANCEEANGLE_CCW; //HALL位置反馈换相超前角度
  }    
  
  if(Drv.Hall.s16AdvanceEangle < -50.0/60.0*32767)
  {
    Drv.Hall.s16AdvanceEangle = -50.0/60.0*32767;
  }
  else if(Drv.Hall.s16AdvanceEangle == 0)
  {
    Drv.Hall.s16AdvanceEangle = 0;
  }
  else if(Drv.Hall.s16AdvanceEangle > 30/60.0*32767)
  {
    Drv.Hall.s16AdvanceEangle = 30.0/60.0*32767;
  }
  #endif
}

void BemfDefault(void)
{
  
  //----------------------------------
  //电机运行方向设置 端子控制
  if(Ui.flg.FR == CW)
  {
    Ctl.gDirectionC = Ui.flg.FR;    //方向控制
    ClrBit(DRV_CR, DDIR);
  }
  else if(Ui.flg.FR == CCW)
  {
    Ctl.gDirectionC = Ui.flg.FR;    //方向控制
    SetBit(DRV_CR, DDIR); 
  }  

  if(Ctl.gDirectionC == CCW)
  {//正反转 CCW 反转
    TIM1_DBR1 = UH_VLB + CMP_W_DO;
    TIM1_DBR2 = UH_WLB + CMP_V_UP;
    TIM1_DBR3 = VH_WLB + CMP_U_DO;
    TIM1_DBR4 = VH_ULB + CMP_W_UP;
    TIM1_DBR5 = WH_ULB + CMP_V_DO;
    TIM1_DBR6 = WH_VLB + CMP_U_UP;
    TIM1_DBR7 = CMP_UVW_UPDO_PWMOFF;
  }
  else //(Ctl.gDirectionC == CW)
  { //正反转 CW  正转
    TIM1_DBR1 = UH_VLA + CMP_W_DO;
    TIM1_DBR2 = UH_WLA + CMP_V_UP;
    TIM1_DBR3 = VH_WLA + CMP_U_DO;
    TIM1_DBR4 = VH_ULA + CMP_W_UP;
    TIM1_DBR5 = WH_ULA + CMP_V_DO;
    TIM1_DBR6 = WH_VLA + CMP_U_UP;
    
    TIM1_DBR7 = CMP_UVW_UPDO_PWMOFF;
  }  
  
  Drv.Stk.BemfTabA = 0;
  Drv.Stk.BemfTabB = 0;
  Drv.Stk.RefNumX = 0;
  Drv.Stk.RefNumY = 0;
  Drv.Stk.RefNumZ = 0;
  Drv.Stk.BemfFR = 0xFF;

  //启动参数调整区
  Ctl.Ramp.ComCnt = 0;
  Ctl.Ramp.cpNms = Ctl.Ramp.TimSta;
  Ctl.Ramp.cpNmsCount = 0;
  Ctl.Ramp.BemfErrCnt = 0;
  Ctl.Ramp.FailCnt = 0;

  Ctl.Ramp.BemfTureCnt = 0;  
  //Ctl.Bemf.MaskTime = TempRampMaskTime;
  Drv.speed.EventPeriodPer = 0;

  MDUControl.DelayAngle = 0;

  ClrBit(CMP_CR3, SAMSEL1);     // 使能ON OFF全部采样
  ClrBit(CMP_CR3, SAMSEL0);   

  CMP_SAMR = 0;
  ANGLE_MASK(0);
    
  TIM1_DBR7 = CMP_UVW_UPDO_PWMOFF;        //检测UVW反电势
  TIM1_CR4 = 7;                    //选择 TIM1_DBR7

  ClrBit(TIM1_CR0, T1OPS0);       //软件触发UPD
  ClrBit(TIM1_CR0, T1OPS1);
  SetBit(TIM1_IER, T1UPD);       //位置检测中断
  
}
void TimNmsDefault(void)
{
  //----------------------------------
  //过压欠压判断时间，堵转 参数清零
  Ctl.Tim.OVnms = 0;
  Ctl.Tim.UVnms = 0;
  Ctl.Tim.Nonms = 0;
  Ctl.Tim.OCnms = 0;
  Ctl.Tim.OH1nms = 0;
  Ctl.Tim.OH1REnms = 0;
  Ctl.Tim.OH2nms = 0;
  Ctl.Tim.OH2REnms = 0;

  Ctl.Tim.STAnms = 0;           //堵转
  Ctl.Tim.STB1nms = 0;          //失速1
  Ctl.Tim.STB2nms = 0;          //失速2
}


void gDelayus(u16 timer)        //1.042us+n*1us
{
  while(timer--) 
  {
    _nop_();_nop_();_nop_();_nop_();_nop_();
    _nop_();_nop_();_nop_();_nop_();_nop_(); 
  }
}

void gDelayms(u16 timer)       //1ms
{
  while(timer--) gDelayus(999);
}