#include "speed_sensor.h"


//局部变量定义

//全局变量定义
MC_SpeedSensorData_Struct_t MC_SpeedSensorData = {0, TRUE, FALSE, 0, 0xFFFFFFFF, 0};
uint16_t MC_Speed_Array[10] = {0};

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


/**************************全局函数定义*************************/
//速度传感器IO初始化
void SpeedSensor_GPIO_Init(void)
{
   GPIO_InitTypeDef GPIO_InitStruct;

	__HAL_RCC_GPIOB_CLK_ENABLE();
	
  GPIO_InitStruct.Pin = SPEED_SENSOR_Pin;
  GPIO_InitStruct.Mode = GPIO_MODE_IT_RISING;
  GPIO_InitStruct.Pull = GPIO_PULLUP;
  HAL_GPIO_Init(SPEED_SENSOR_GPIO_Port, &GPIO_InitStruct);
	
	HAL_NVIC_SetPriority(SPEED_SENSOR_EXTI_IRQn, 2, 0);
  HAL_NVIC_EnableIRQ(SPEED_SENSOR_EXTI_IRQn);
	
}

//车速计算和停止判断处理
void SpeedSensor_Process(MC_SpeedSensorData_Struct_t* p_MC_SpeedSensorData, uint16_t* AvgResult, uint8_t WheelSize)
{
	//车速计算
	static uint32_t PeriodTimeCnt = 0;
	static uint32_t SpeedFltSum = 0;
	static uint8_t Count = 0;
	
	if((HAL_GetTick() - PeriodTimeCnt) >= 50)
	{
		//停止判断
		static uint32_t StopDelayTimeCnt = 0;
		
		if(p_MC_SpeedSensorData->IsTrigFlag == TRUE)
		{
			StopDelayTimeCnt = HAL_GetTick();
			p_MC_SpeedSensorData->IsTrigFlag = FALSE;
		}
		if((HAL_GetTick() - StopDelayTimeCnt) > 4000) // 超时4s信号触发标志未更新，认为停车
		{
			p_MC_SpeedSensorData->IsStopFlag = TRUE;
		}
		
		//车速计算，计算周期50ms
		if(p_MC_SpeedSensorData->IsStopFlag == FALSE)
		{
			if(p_MC_SpeedSensorData->DiffTime_ms == 0)
			{
				p_MC_SpeedSensorData->DiffTime_ms = 0xFFFFFFFF;
			}
			p_MC_SpeedSensorData->Speed_Data = (uint32_t)WheelSize * 360 / p_MC_SpeedSensorData->DiffTime_ms;//单位0.1km/h
			
			//在确定停止前进行衰减
			p_MC_SpeedSensorData->DiffTime_ms = p_MC_SpeedSensorData->DiffTime_ms * 1030 >> 10;
		}
		else
		{
			p_MC_SpeedSensorData->Speed_Data = 0;
			*AvgResult = 0;
			SpeedFltSum = 0;
			Count = 0;
			p_MC_SpeedSensorData->DiffTime_ms = 0xFFFFFFFF;
			return;
		}
		//计算400ms平均值
		SpeedFltSum += p_MC_SpeedSensorData->Speed_Data;
		Count++;
		if(Count >= 8)
		{
			*AvgResult = SpeedFltSum >> 3;
			
			SpeedFltSum = 0;
			Count = 0;
		}
		PeriodTimeCnt = HAL_GetTick();
	}
	
}