#include "hall_sensor.h"
#include "fault_check.h"

//全局变量定义
const uint8_t HallSensorGroup_Encoder_Forward[8] = {1,3,6,2,5,1,4,1};//霍尔正转编码表，正转时信号为：1，3，2，6，4，5
const uint8_t HallSensorGroup_Encoder_Backward[8] = {1,5,3,1,6,4,2,1};//霍尔反转编码表，反转时信号为：1，5，4，6，2，3

//电角度值
const uint16_t HallAngle_Data[8] = 
{
	0, 
  HALL_ORIGIN * 65536 / 360 + ANGLE_60D,
	HALL_ORIGIN * 65536 / 360 + ANGLE_120D + ANGLE_60D,
  HALL_ORIGIN * 65536 / 360 + ANGLE_120D,
	HALL_ORIGIN * 65536 / 360 - ANGLE_60D + 65535,
  HALL_ORIGIN * 65536 / 360 + 65535, 
  HALL_ORIGIN * 65536 / 360 - ANGLE_120D + 65535,
	0
};

//全局变量定义
MC_HallSensorData_Struct_t MC_HallSensorData = {0, 0, 0, 0, TRUE, TRUE, 0};

/**************************局部函数定义*************************/



/**************************全局函数定义*************************/
//读取霍尔传感器IO状态
uint8_t Hall_ReadState(void)
{
  uint8_t ReadValue;
  if(MC_HallSensorData.InverterExistFlag == TRUE)  //存在反相器
	{
		ReadValue  = (uint8_t)(HAL_GPIO_ReadPin(HALL_C_GPIO_Port, HALL_C_Pin));        //HALL C  
		ReadValue |= (uint8_t)(HAL_GPIO_ReadPin(HALL_B_GPIO_Port, HALL_B_Pin)) << 1;  //HALL B
		ReadValue |= (uint8_t)(HAL_GPIO_ReadPin(HALL_A_GPIO_Port, HALL_A_Pin)) << 2;  //HALL A   			
	}
	else                                            //不存在反相器，软件反相
	{
		ReadValue  = (uint8_t)((HAL_GPIO_ReadPin(HALL_C_GPIO_Port, HALL_C_Pin) == GPIO_PIN_RESET)?1:0);                //HALL C  
		ReadValue |= (uint8_t)((HAL_GPIO_ReadPin(HALL_B_GPIO_Port, HALL_B_Pin) == GPIO_PIN_RESET)?1:0) << 1;          //HALL B
		ReadValue |= (uint8_t)((HAL_GPIO_ReadPin(HALL_A_GPIO_Port, HALL_A_Pin) == GPIO_PIN_RESET)?1:0) <<  2;        //HALL A
	}
  return(ReadValue & 0x07);
}

//霍尔传感器IO初始化
void HallSensor_GPIO_Init(void)
{
  GPIO_InitTypeDef GPIO_InitStruct;
	
	__HAL_RCC_GPIOC_CLK_ENABLE();
  
  GPIO_InitStruct.Pin = HALL_C_Pin|HALL_A_Pin|HALL_B_Pin;
  GPIO_InitStruct.Mode = GPIO_MODE_INPUT;
  GPIO_InitStruct.Pull = GPIO_PULLUP;
  HAL_GPIO_Init(GPIOC, &GPIO_InitStruct);
}

void HallSensorAngle_Init(void)
{
	//初始化电角度值
	MC_HallSensorData.SVM_Angle = HallAngle_Data[Hall_ReadState()] + ANGLE_60D / 2;	
	
	//PWM计数值清零
	MC_HallSensorData.PWM_NumCnt = 0;
}

void HallSensor_Process(void)
{		
	static MC_HallSensorStatus_Struct_t MC_HallSensorStatus;
	static TrueOrFalse_Flag_Struct_t IsFirstEnterFalg = TRUE;
	uint8_t HallStatus1, HallStatus2, HallStatus3;
	HallStatus1 = Hall_ReadState();
	HallStatus2 = Hall_ReadState();
	HallStatus3 = Hall_ReadState();
	//连续采集三次霍尔，三次状态都一致才更新霍尔状态，防us级干扰
	if( (HallStatus1==HallStatus2)&&(HallStatus2==HallStatus3) )
	{
		MC_HallSensorStatus.HallGropuStatus = HallStatus3;
	}
	if(IsFirstEnterFalg == TRUE)
	{
	  MC_HallSensorStatus.HallGropuStatus_Old = MC_HallSensorStatus.HallGropuStatus;
		IsFirstEnterFalg = FALSE;
	}
	
	//启动和停止状态更新
	static uint8_t HallSensorTrigCnt = 0;
	static uint32_t StopDelayTimeCnt = 0;
	static uint8_t backwardcount = 0;
	if(MC_HallSensorStatus.HallGropuStatus != MC_HallSensorStatus.HallGropuStatus_Old)
	{
	  StopDelayTimeCnt = HAL_GetTick();
		if(HallSensorTrigCnt != 0)
		{
		  HallSensorTrigCnt++;
		}
		else
		{
		  MC_HallSensorData.IsStopFlag = FALSE;
		}
	}
	else
	{
	  if((HAL_GetTick() - StopDelayTimeCnt) > 200)//超时200ms霍尔信号不变化，认为静止
		{
		  MC_HallSensorData.IsStopFlag = TRUE;
			HallSensorTrigCnt = 0;
			MC_HallSensorData.motorspeed = 0;
			MC_HallSensorData.motorspeed_RCFlt = 0;
			backwardcount = 0;
			MC_HallSensorData.BackwardFlag = FALSE;
		}
	}
	
	//步进角和电角度计算
	static uint8_t HallSensorAngleCnt = 0;
	
	if(MC_HallSensorData.IsStopFlag == TRUE)
	{
	  HallSensorAngleCnt = 0;//停止时清零，防止下次启动异常
	}
	
	if(MC_HallSensorStatus.HallGropuStatus == HallSensorGroup_Encoder_Forward[MC_HallSensorStatus.HallGropuStatus_Old])
	{
		//步进角计算
		if(MC_HallSensorData.PWM_NumCnt != 0)
		{
		  MC_HallSensorData.Delta_Angle = ANGLE_60D / MC_HallSensorData.PWM_NumCnt;
		}
		//电角度计算
		else
		{
		  MC_HallSensorData.Delta_Angle = 1;
		}
		HallSensorAngleCnt++;
		if(HallSensorAngleCnt>2)
		{
			MC_HallSensorData.motorspeed = 18750/MC_HallSensorData.PWM_NumCnt;
			backwardcount = 0;
			MC_HallSensorData.BackwardFlag = FALSE;
		}
		if(HallSensorAngleCnt < 15)
		{
		  MC_HallSensorData.SVM_Angle = HallAngle_Data[MC_HallSensorStatus.HallGropuStatus] + (ANGLE_60D >> 1);
			MC_HallSensorData.Delta_Angle = 0;
		}
		else //15个霍尔变化后，在FOC中计算电角度
		{
		  HallSensorAngleCnt = 15;
			MC_HallSensorData.SVM_Angle = HallAngle_Data[MC_HallSensorStatus.HallGropuStatus];
		}
		MC_HallSensorData.PWM_NumCnt = 0;
		MC_HallSensorData.Delta_AngleSum = 0;
	}
	else if(MC_HallSensorStatus.HallGropuStatus_Old == HallSensorGroup_Encoder_Forward[MC_HallSensorStatus.HallGropuStatus])
	{
		HallSensorAngleCnt = 0;
		MC_HallSensorData.Delta_Angle = 0;
		MC_HallSensorData.SVM_Angle = HallAngle_Data[MC_HallSensorStatus.HallGropuStatus] + (ANGLE_60D >> 1);
		MC_HallSensorData.PWM_NumCnt = 0xFFFF;
		if(++backwardcount>5) 
		{
			backwardcount = 6;
			MC_HallSensorData.BackwardFlag = TRUE;
		}
	}
	
	//霍尔传感器故障检测
	MC_Fault_HallSensor_Process(MC_HallSensorStatus, &MC_ErrorCode);
	
	MC_HallSensorStatus.HallGropuStatus_Old = MC_HallSensorStatus.HallGropuStatus;
}

//电机转速计算
int16_t MotorSpeedCal(uint16_t SVM_Angle, TrueOrFalse_Flag_Struct_t IsStopFlag)
{
  static uint8_t PreCnt = 0;
	static uint16_t SVM_Angle_Old = 0;
	int32_t AngleStep = 0;
	static int32_t FreqMotorFlt = 0, motrospeedFltSum=0;
	int16_t FreqMotor = 0;
	static int16_t Result = 0;
	
	PreCnt++;
	if(PreCnt >=15 )
	{
	  PreCnt = 0;
		AngleStep = (int32_t)(MC_HallSensorData.SVM_Angle - SVM_Angle_Old);
		if(AngleStep < 0)
		{
		  AngleStep = (int32_t)(MC_HallSensorData.SVM_Angle + 65535 - SVM_Angle_Old);
		}
		if(AngleStep > 20000)
		{
		  AngleStep = 0;
		}
		SVM_Angle_Old = MC_HallSensorData.SVM_Angle;
		AngleStep = AngleStep * 1000;//f = [Angle(k-1)-Angle(k)]/Tc
		FreqMotorFlt += (AngleStep - FreqMotorFlt) >> 8;
		FreqMotor = FreqMotorFlt >> 16;
		Result = 60 * FreqMotor >> 3;// 8n=60*f/p
		
		motrospeedFltSum += ((MC_HallSensorData.motorspeed<<10) - motrospeedFltSum)>>5;
		MC_HallSensorData.motorspeed_RCFlt = motrospeedFltSum>>10; 
	}
	
	if(IsStopFlag == TRUE)
	{
	  Result = 0;
	}
	return Result;
}