extern void CalWheelSpeed(GearControl_Handle GearControlHandle,HAL_Handle halhandle)
{
	GearControl_Obj *gsobj = (GearControl_Obj *)GearControlHandle;
	uint32_t Cur_GPIOWheelSpeed;
	Cur_GPIOWheelSpeed = HAL_readGpio(halhandle,GPIO_WheelSpeed);
	int i;
	if(Cur_GPIOWheelSpeed == 1 && gsobj->Pre_GPIO_Switch == 0 && gFlagWheelSwitchFirstTime == true)
	{
		gFlagWheelSwitchFirstTime = false;
		gsobj->WHeelSpeed_Krpm = _IQ(0.0); //gWheelSpd
		gsobj->WheelSpeed_CNT = 0;	//gWheelTimeCnt
		gsobj->WheelAccel =_IQ(0.0);
		//gsobj->WHeelAcc_Krpmps=_IQ(0.0);
		//gsobj->Pre_WHeelAcc_Krpmps = _IQ(0.0);
		//gsobj->WHeelJerk_Krpmpsps=_IQ(0.0);
	}
	else if(Cur_GPIOWheelSpeed == 1 && gsobj->Pre_GPIO_Switch == 0) //
		//if(gWheelSwitch == 1 		&& gWheelSwitch_L == 0)
	{
		gsobj->WHeelSpeed_Krpm = _IQ( gsobj->SpeedScaler_krpm_f/(float_t)gsobj->WheelSpeed_CNT); //gWheelSpd
		//_iq tmpKrpm = _IQ( gsobj->SpeedScaler_krpm_f/(float_t)gsobj->WheelSpeed_CNT);
		//gsobj->WHeelSpeed_Krpm = WindowFilter(tmpKrpm);

		gsobj->WheelAccel = _IQ(_IQtoF(gsobj->WHeelSpeed_Krpm - gsobj->Pre_WHeelSpeed_Krpm)/(float_t)gsobj->WheelSpeed_CNT);
		gsobj->WHeelAcc_Krpmps = _IQdiv((gsobj->WHeelSpeed_Krpm - gsobj->Pre_WHeelSpeed_Krpm),
				                     _IQ((float_t)gsobj->WheelSpeed_CNT/(float_t)(15000)));
		gsobj->WHeelJerk_Krpmpsps=_IQdiv((gsobj->Pre_WHeelAcc_Krpmps-gsobj->WHeelAcc_Krpmps),
				                        _IQ((float_t)gsobj->WheelSpeed_CNT/(float_t)(15000)));
		//gsobj->WheelSpeed_CNT = 0;
		//gWheelSpd = _IQ( gWheelSpeedScaler_krpm_f/(float_t)gWheelTimeCnt);
		gsobj->gWheelTimeOut = (uint32_t)(gsobj->SpeedScaler_krpm_f / _IQtoF(gsobj->WHeelSpeed_Krpm + gsobj->WheelAccel))*2;
		gsobj->WheelSpeed_CNT = 0;
		gsobj->WheelAccels[gAccelsCount++] = gsobj->WheelAccel;

		for(i=0; i < gAccelsMaxCount; i++)
		{
			gsobj->SumWheelAccels = gsobj->SumWheelAccels + gsobj->WheelAccels[i];
		}
		if(gAccelsCount > gAccelsMaxCount)
			gAccelsCount = 0;
	}
	else
	{
		if (gsobj->WheelSpeed_CNT >= gsobj->gWheelTimeOut)
		{
			gFlagWheelSwitchFirstTime = true;
			gsobj->WHeelSpeed_Krpm = _IQ(0.0);
			gsobj->Pre_WHeelSpeed_Krpm = _IQ(0.0);
			gsobj->WheelAccel = _IQ(-0.001);
			gsobj->WHeelAcc_Krpmps=_IQ(0.0);
			gsobj->Pre_WHeelAcc_Krpmps = _IQ(0.0);
			gsobj->WHeelJerk_Krpmpsps=_IQ(0.0);
			gsobj->gWheelTimeOut = 90000000;
			for(gHallSpeedRPMCnt=0; gHallSpeedRPMCnt<10; gHallSpeedRPMCnt++)
			{	gHallSpeedRPMArray[gHallSpeedRPMCnt] = _IQ(0.0);		}
			gHallSpeedRPMCnt=0;
			//gWheelSpd = _IQ(0.0);
			//gsobj->WheelSpeed_CNT = 9000000;
			//gFlagWheelSwitchFirstTime = true;
			gsobj->SumWheelAccels = _IQ(0.0);
		}
		else
		{
			gsobj->WheelSpeed_CNT++;
		}
		//gsobj->WheelSpeed_CNT++;
	}
	gsobj->Pre_WHeelSpeed_Krpm = gsobj->WHeelSpeed_Krpm;
	gsobj->Pre_GPIO_Switch = Cur_GPIOWheelSpeed;
	gsobj->Pre_WHeelAcc_Krpmps=gsobj->WHeelAcc_Krpmps;

	return;
}