#include "FocSample.h"
#include "FocConfig.h"
#include "FocDataBase.h"
#include "main.h"
#include "arm_math.h"
/*******************************************************************************
 * @brief   计算ADC三个通道偏移值
 * @param   
 * @return  偏移值
 ******************************************************************************/
void CurrentSampleInit(FocSample_t * pHandle)
{
//	__HAL_ADC_DISABLE_IT(&hadc2, ADC_IT_JEOC);
//	HAL_TIM_Base_Start(&htim1);
//	HAL_ADCEx_InjectedStart(&hadc2);
//  
//	HAL_TIM_PWM_Start(&htim1,TIM_CHANNEL_4);
//	pHandle->aCurrentOffset = 0;
//	pHandle->bCurrentOffset = 0;
//	pHandle->cCurrentOffset = 0;
//	for(int i = 0;i < 1024;i++)
//	{
//		HAL_Delay(1);
//		pHandle->aCurrentOffset += ADC2->JDR1;
//		pHandle->bCurrentOffset += ADC2->JDR2;
//		pHandle->cCurrentOffset += ADC2->JDR3;
//	}
//	pHandle->aCurrentOffset /= 1024;
//	pHandle->bCurrentOffset /= 1024;
//	pHandle->cCurrentOffset /= 1024;
//	
//	TIM1->CR1 &=~0x01; 
//	__HAL_ADC_ENABLE_IT(&hadc2, ADC_IT_JEOC);
}
/*******************************************************************************
 * @brief   计算ADC三个电流值
 * @param   
 * @return  
 ******************************************************************************/
void CurrentSampleStep(FocSample_t * pHandle,float sin_cos[2])
{
		#define CURRENT_CONV_RATE    (0.0161F)
	  float id,iq;
		pHandle->aCurrent = pHandle->aCurrent * CURRENT_CONV_RATE;
		pHandle->bCurrent = pHandle->bCurrent * CURRENT_CONV_RATE;
		pHandle->cCurrent = pHandle->cCurrent * CURRENT_CONV_RATE;
		pHandle->iAlpha = (pHandle->aCurrent - 0.5f * ( pHandle->bCurrent + pHandle->cCurrent)) * 0.666666687F;
	  pHandle->iBeta =  0.577350529f * ( pHandle->bCurrent - pHandle->cCurrent);

		id = sin_cos[0] * pHandle->iBeta + pHandle->iAlpha * sin_cos[1];
	  iq = sin_cos[1] * pHandle->iBeta - pHandle->iAlpha * sin_cos[0];  
	
	  pHandle->iD += 0.3f * (id - pHandle->iD);
	  pHandle->iQ += 0.3f * (iq - pHandle->iQ);
}