/**************************** (C) COPYRIGHT 2017 Fortiortech shenzhen *****************************
* File Name          : AddFunction.c
* Author             : Vina Peng,Fortiortech  Appliction Team
* Version            : V1.0
* Date               : 2017-12-26
* Description        : This file contains all the add function used for Motor Control.
***************************************************************************************************
* All Rights Reserved
**************************************************************************************************/


/* Includes -------------------------------------------------------------------------------------*/
// #include <AddFunction.h>
#include <FU68xx_2.h>
#include <Myproject.h>

/* Private variables ---------------------------------------------------------*/
FaultStateType         mcFaultSource;
PWMINPUTCAL        idata   mcPwmInput;
FaultVarible       idata   mcFaultDect;
MotorRSDTypeDef    idata   RSDDetect;
CurrentVarible     idata   mcCurVarible;
ProtectVarible     idata   mcProtectTime;

FOCCTRL            xdata   mcFocCtrl;

uint32             idata   temp32;


/*---------------------------------------------------------------------------*/
/* Name    :  int16 KLPF_VALUE(int16 INVlaue, int16 OutLastValue)
/* Input  :  INVlaue，OutLastValue
/* Output  :  int16的变量
/* Description:  滤波函数,用乘法器做的
/*---------------------------------------------------------------------------*/
int16 KLPF_VALUE(int16 INVlaue, int16 OutLastValue)
{
  int16 Result = 0;
  MDU_MA = (INVlaue-OutLastValue);
  MDU_MB = (int16)480;                     /*写被乘数和乘数*/

  Result = MDU_MB;
  Result += OutLastValue;
  return(Result);
}

/*---------------------------------------------------------------------------*/
/* Name    :  int16 Abs_F16(int16 value)
/* Input  :  value
/* Output  :  int16
/* Description:  对变量取16位的绝对值
/*---------------------------------------------------------------------------*/
uint16 Abs_F16(int16 value)
{
  if(value < 0)
  {
    return (- value);
  }
  else
  {
    return (value);
  }
}
/*---------------------------------------------------------------------------*/
/* Name    :  int32 Abs_F32(int32 value)
/* Input  :  value
/* Output  :  int16
/* Description:  对变量取16位的绝对值
/*---------------------------------------------------------------------------*/
uint32 Abs_F32(int32 value)
{
  if(value < 0)
  {
    return (- value);
  }
  else
  {
    return (value);
  }
}
/*---------------------------------------------------------------------------*/
/* Name    :  void APP_DIV(void)
/* Input  :  void
/* Output  :  void
/* Description:  将所有用到除法的地方，放在同一个中断，以避免中断串扰,
                 计算捕获的PWM的占空比
/*---------------------------------------------------------------------------*/
void APP_DIV(void)
{
//    if( mcPwmInput.PWMDivFlag==1)  //启动除法器，避免与过调值中的除法冲突
//    {
//       mcPwmInput.PWMDuty = MDU_DIV_IDATA_U32(&mcPwmInput.pwm.PWMCompareAMP, &mcPwmInput.PWMARRUpdate);
//       mcPwmInput.PWMDivFlag=0;
//    }
    if( mcFocCtrl.ESDIVFlag==1)  //启动除法器，避免与过调值中的除法冲突
    {
       mcFocCtrl.SQUSpeedDIVEs = MDU_DIV_XDATA_U32(&mcFocCtrl.SQUSysSpeed,&mcFocCtrl.EsValue);
       mcFocCtrl.ESDIVFlag=0;
    }
    if( mcPwmInput.PWMDivFlag==1)  //启动除法器，避免与过调值中的除法冲突
    {
        MDU_MUL_IDATA_U32(32767,mcPwmInput.PWMCompare,&temp32);
        mcPwmInput.PWMDuty = MDU_DIV_IDATA_U32(&temp32,&mcPwmInput.PWMARR);
        mcPwmInput.PWMDivFlag=0;
    }
}
/*---------------------------------------------------------------------------*/
/* Name    :  void PWMInputCapture(void)
/* Input  :  NO
/* Output  :  NO
/* Description:  输入PWM处理
/*---------------------------------------------------------------------------*/
void PWMInputCapture(void)
{
  uint16 MotorControlVSP;

   if(mcPwmInput.PWMUpdateFlag==1)  // 有新的duty更新
   {
      if((Abs_F32(mcPwmInput.PWMCompare-mcPwmInput.PWMCompareOld)<50)// 两次比较值相近，减少读错率
        &&(Abs_F32(mcPwmInput.PWMARROld-mcPwmInput.PWMARR)<50)// 两次周期值相近，减少读错率
//        &&((100<mcPwmInput.PWMARR)&&(mcPwmInput.PWMARR<6000))// 周期值在一定范围内才认为有效，即一定频率范围
        &&(mcPwmInput.PWMDivFlag==0))
        {
            mcPwmInput.PWMFlag=1;                               // PWMFlag置1期间，不读取TIM3__DR和TIM3__ARR;，防止出错
//          mcPwmInput.pwm.PWMCompareUpdate[0]=(mcPwmInput.PWMCompare>>1);// 对其乘以32768  ？
//          mcPwmInput.PWMARRUpdate=mcPwmInput.PWMARR;
            mcPwmInput.PWMDivFlag=1;                            // 启动除法          
            mcPwmInput.PWMFlag=0;
        }
      if(mcPwmInput.PWMDivFlag==0)
      {
          if(mcPwmInput.PWMcnt<4)//4次求平均值
          {
            mcPwmInput.PWMcnt++;
            mcPwmInput.PWMVSum +=mcPwmInput.PWMDuty;
            mcPwmInput.PWMVSum = mcPwmInput.PWMVSum;  
          }
          else
          {
            MotorControlVSP= (mcPwmInput.PWMVSum >>2);//注意其有一个右移与前面的比较值标幺化有关，右移求平均值
            mcPwmInput.PWMVSum=0;
            mcPwmInput.PWMcnt =0;
            mcPwmInput.VSPtest = MotorControlVSP;
          }
          MotorControlVSP=mcPwmInput.PWMDuty;
      }

      mcPwmInput.PWMUpdateFlag =0;
      mcPwmInput.PWMCompareOld=mcPwmInput.PWMCompare;//将此次比较值赋值给上次比较值
      mcPwmInput.PWMARROld=mcPwmInput.PWMARR;//将此次周期值赋值给上次周期值
    }
	
			
}

