motor_ctl-2/User/Src/motor_control.c

#include "motor_control.h"
#include "MC_FOC_driver.h"
#include "MC_PID_regulators.h"
#include "MC_Globals.h"
#include "stm32f10x_svpwm_3shunt.h"
#include "hall_sensor.h"
#include "cadence_sensor.h"
#include "speed_sensor.h"
#include "torque_sensor.h"
#include "gas_sensor.h"
#include "key_driver.h"
#include "pwm_driver.h"
#include "math_tools.h"
#include "power12V_driver.h"
#include "ICM20600.h"

/************************全局变量定义************************/
//工作模式
MC_WorkMode_Struct_t MC_WorkMode = MC_WorkMode_Run; 
MC_WorkMode_Struct_t MC_WorkMode_Back = ~MC_WorkMode_Run; 
//MC_CTL控制指令
MC_ControlCode_Struct_t MC_ControlCode= {MC_GearSt_OFF, MC_LightSwitch_OFF}; 
MC_ControlCode_Struct_t MC_ControlCode_Back = {(MC_GearSt_Struct_t)~MC_GearSt_OFF, (MC_LightSwitch_Struct_t)~MC_LightSwitch_OFF};
//电机控制计算参数
MC_CalParam_Struct_t MC_CalParam = {MC_AssistRunMode_INVALID, 0,	0, RESET}; 
MC_CalParam_Struct_t MC_CalParam_Back = {(MC_AssistRunMode_Struct_t)~MC_AssistRunMode_INVALID, ~0, ~0, (FlagStatus)~RESET}; 
//踏频限流系数
uint8_t MC_CadenceLimit_K = 100;
//力矩助力控制参数
MC_TorqueProcess_Param_Struct_t MC_TorqueProcess_Param = {SET, 0, 0, 0};
//推行助力控制参数
MC_WalkProcess_Param_Struct_t MC_WalkProcess_Param = {FALSE, 0};  
//电机启动标志
FlagStatus MC_StarFlag = RESET;  

/*************************局部函数定义***********************/
//设定值线性变化处理
uint16_t MC_DataSet_Linear_Process(uint16_t SetData, uint16_t PresentData, uint16_t AddCnt, uint16_t DecCnt)
{
  int16_t ErrorData;
	uint16_t Result;
	
	ErrorData = SetData - PresentData;
	if(ErrorData > 0)  //升速
	{
	  if(ErrorData >= AddCnt)
		{
		  Result = PresentData + AddCnt;
		}
		else
		{
		  Result = SetData;
		}
	}
	else if(ErrorData < 0)  //降速
	{
	  if((-ErrorData) >= DecCnt)
		{
		  Result = PresentData - DecCnt;
		}
		else
		{
		  Result = SetData;
		}
	}
	else
	{
	  Result = SetData;
	}
	
	return Result;
}

//随电压计算助力衰减系数
uint16_t MC_Cal_K_ByVoltage(uint16_t Voltage, uint16_t DesignVol, uint16_t K_Voltage_Old)
{
 	uint32_t Cal_Temp;
	uint16_t SetVol_Th = 0;
	uint16_t ResetVol_Th = 0;
	uint16_t Result = 1024;
	
	//根据马达额定电压设定衰减点、衰减系数和恢复点
	switch(DesignVol)
	{
		case 24:
		{
			SetVol_Th = 33 * 7;
			ResetVol_Th = 36 * 7;
			Cal_Temp = (Voltage > (SetVol_Th * 100)) ? 1024 : (uint16_t)((uint32_t)(Voltage) / SetVol_Th);//Voltage单位0.001V, SetVol_Th单位0.1V, 系数放大100倍 
		  break;
		}
		case 36:
		{
			SetVol_Th = 33 * 10;
			ResetVol_Th = 36 * 10;
			Cal_Temp = (Voltage > SetVol_Th * 100) ? 1024 : (uint16_t)((uint32_t)(Voltage) / SetVol_Th);//Voltage单位0.001V, SetVol_Th单位0.1V, 系数放大100倍
		  break;
		}
		case 48:
		{
			SetVol_Th = 33 * 13;
			ResetVol_Th = 36 * 13;
			Cal_Temp = (Voltage > SetVol_Th * 100) ? 1024 : (uint16_t)((uint32_t)(Voltage) / SetVol_Th);//Voltage单位0.001V, SetVol_Th单位0.1V, 系数放大100倍
		  break;
		}
		default:
		{
		  Cal_Temp = 100;
			break;
		}
	}
	Cal_Temp = Cal_Temp * Cal_Temp / 100 * Cal_Temp / 100 * Cal_Temp * 1024 / 10000; //f(x) = x^4
	Cal_Temp = (Cal_Temp < 820) ? 820 : Cal_Temp;
	
	//系数仅衰减
	if(Cal_Temp < K_Voltage_Old)
	{
	  Result = Cal_Temp;
	}
	else
	{
	  Result = K_Voltage_Old;
	}

	//高于设定恢复电压后，恢复系数
	if(Voltage > (ResetVol_Th * 100))
	{
	  Result = 1024;
	}

	return(Result);
}

//随温度计算助力衰减系数
uint16_t MC_Cal_K_ByTemperature(uint16_t CoilTemp, uint16_t AlarmTempTH)
{
  uint32_t CalTemp;
	uint16_t Result = 1024;
	
	if(CoilTemp > AlarmTempTH)
	{
	  CalTemp = (uint32_t)AlarmTempTH * AlarmTempTH * 1024;
		Result = (uint16_t)(CalTemp / CoilTemp / CoilTemp);
	}
	else
	{
	  Result = 1024;
	}
	
	return(Result);
}

//助力模式判断处理
MC_AssistRunMode_Struct_t MC_JudgeAsistRunMode_Process(MC_SupportFlag_Struct_t GasCtrlMode, uint16_t GasSensorData, MC_GearSt_Struct_t GearSt, TrueOrFalse_Flag_Struct_t StopFlag)
{
  MC_AssistRunMode_Struct_t MC_AssistRunMode_Result;
	
	if(MC_ErrorCode.Code == 0)  // 无故障
	{
	  if((GearSt != MC_GearSt_OFF) && (StopFlag == FALSE))
		{
		  //进入指拨模式
			if((GasSensorData > 100) && (GasCtrlMode == MC_SUPPORT_ENABLE)) 
			{
				MC_AssistRunMode_Result = MC_AssistRunMode_GAS;
			}
			//退出指拨模式
			else if(GasSensorData < 50)  
			{
				//进入推行模式
				if(GearSt == MC_GearSt_WALK)
				{
					MC_AssistRunMode_Result = MC_AssistRunMode_WALK;
				}
				else 
				{
					//进入踏频模式
					if(((GearSt & 0xF0) != 0) && (GearSt != MC_GearSt_SMART))
					{
						MC_AssistRunMode_Result = MC_AssistRunMode_CADENCE;
					}
					//进入力矩模式
					else
					{
						MC_AssistRunMode_Result = MC_AssistRunMode_TORQUE;
					}
				}
		  }
		}
		else
		{
	  	MC_AssistRunMode_Result = MC_AssistRunMode_INVALID;
		}
		Power12V_Driver_Process(SET);
	}
	else  //存在故障
	{
	  MC_AssistRunMode_Result = MC_AssistRunMode_INVALID;
		MC_ControlCode.GearSt = MC_GearSt_OFF;
		MC_ControlCode_Back.GearSt = (MC_GearSt_Struct_t)~MC_ControlCode.GearSt;
		#if 0
		Power12V_Driver_Process(RESET);
		#endif
	}
	
	return MC_AssistRunMode_Result;
}

/*指拨模式相关变量*/

static int32_t SpdMotorDivWheelFlt=0;
int16_t SpdProportion=490; //车轮电机速度比
static uint16_t SpdProportion_buff_CNT=0;
uint8_t  SpdProportion_CAL_flag=0;
static uint16_t SpdProportion_Save_CNT=0;
uint16_t SpdProportion_buff[100]={0};
float SpdProportion_StandardDeviation=0;
int32_t test_StandardDeviation=0;
uint16_t test_SpdProportionAver=0;
int32_t SpeedSetMiddle=0;
int16_t dbSpdWheelSet=0;  //调试用

int16_t wheelSpeed=0;
static int16_t DbSpdMotorPre=0;
static int16_t wheelSpeedPre=0;
int16_t SpdMotorDivWheel=0;
int16_t SpdMotorDivWheelFlted=0;
int16_t SpeedMax = 0;	// 最高时速
int16_t SpeedSet = 0;	// 速度设定值
uint32_t accStep = 0;	// 加速时间步进
uint32_t decStep = 0;	// 减速时间步进
int16_t SpeedSetReal = 0;	// 速度设定真实值

/*指拨模式相关变量*/

//指拨模式处理
MC_CalParam_Struct_t MC_AssistRunMode_Gas_Process(uint16_t SensorData, MC_GearSt_Struct_t GearSt)
{
	int32_t Tmp;
	int16_t TorQueBySpd = 0;
	int32_t Ref_Speed_Temp;
	int16_t SpdMotorByIdc = 0;
	
	MC_CalParam_Struct_t p_MC_CalParam = {MC_AssistRunMode_INVALID, 0,	0, RESET};
	
  //...插入指拨处理

	/*车轮速度使用原始数据，滤波后的数据有滞后，影响控制回路*/
	wheelSpeed = (int16_t)MC_SpeedSensorData.Speed_Data;
	/*实时计算电机转速与车轮速的比值*/
	SpdMotorDivWheel = 	(uint32_t)(MC_RunInfo.MotorSpeed * 100) / wheelSpeed ;
	
	Tmp = SpdMotorDivWheel;
	SpdMotorDivWheelFlt += ((Tmp<<8) - SpdMotorDivWheelFlt) >> 6;
	SpdMotorDivWheelFlted = SpdMotorDivWheelFlt>>8;
	
	
	/*在电机转速与车轮速比值，与实际速比一致时，更新速比*/
	if( ( MC_RunInfo.MotorSpeed > 100 ) && ( wheelSpeed > 0 ))
	{
		/*加速时，更新速比，比较法*/
		if((wheelSpeed - wheelSpeedPre ) > 5)
		{
			if(( MC_RunInfo.MotorSpeed - DbSpdMotorPre )>0)
			{
				SpdProportion = SpdMotorDivWheel ;
			}
			DbSpdMotorPre = MC_RunInfo.MotorSpeed;
		}
		wheelSpeedPre = wheelSpeed;
		
		
		/*求标准差,速比稳定后，更新速比*/
		/*此处将数据保存到数组中，标准差计算，时间较长，放在主循环进行*/
		if((SpdProportion_CAL_flag==0) && (MC_CalParam.Ref_Speed > 25))  //电机力矩控制量低于25时，认为是空载，此时不更新速比
		{
			SpdProportion_Save_CNT++;
			/*40ms保存一次数据到数组*/
			if(SpdProportion_Save_CNT >= 40 )
			{
				SpdProportion_Save_CNT = 0;
				
				SpdProportion_buff[SpdProportion_buff_CNT] = SpdMotorDivWheelFlted;
				SpdProportion_buff_CNT++;
				if( SpdProportion_buff_CNT >=50 )
				{
					SpdProportion_buff_CNT = 0;
					
					/*标志位置1，主循环里求标准差*/
					SpdProportion_CAL_flag = 1;
				}
			}
		}
	}

	/*电机最高速度,上位机配置参数*/
	SpeedMax = MC_MotorParam.Rate_Speed;
	Tmp = SensorData + 50 ;  //加50偏移量，确保能达到最大值2048
	Tmp = Tmp > 2048 ? 2048 : Tmp; 
	
//			/*调试用，根据车速限速值，换算指拨对应的设定车速*/
		  dbSpdWheelSet = (Tmp * MC_ConfigParam1.SpeedLimit * 10 )>> 11;	
	//if(((int16_t)wheelSpeed - (int16_t)dbSpdWheelSet) < 100 )
		//|| (dbSpdWheelSet < 50  )  )
//			{
	
	/*电机转速设定，根据指拨大小、车轮限速值和速比，换算*/
	SpeedSet = ((Tmp * MC_ConfigParam1.SpeedLimit  * SpdProportion) / 10  >> 11);			//(Tmp >> 11) * cd_Speedlimit * ( SpdProportion / 10)
	//SpeedSet = ((Tmp * cd_MotorSpeed)  >> 11);	//调试
	
	//超过限速值，设定电机转速为0
	if(wheelSpeed > (MC_ConfigParam1.SpeedLimit * 10 + 10))
	{
		SpeedSet = 0;
	}
	
	SpeedSet = (SpeedSet > 0) ? SpeedSet : 0;
	SpeedSet = (SpeedSet < SpeedMax) ? SpeedSet : SpeedMax;
	
	
	switch (GearSt & 0x0F)//Help_mode:bit4: 0-力矩模式，1-踏频模式;低四位表示助力档位
	{
		case 0x01:
			accStep = StepCalc(SpeedMax,1,6500);
			break;
		case 0x02:
			accStep = StepCalc(SpeedMax,1,6000);
			break;
		case 0x03:
			accStep = StepCalc(SpeedMax,1,5500);
			break;
		case 0x04:
			accStep = StepCalc(SpeedMax,1,5000);
			break;
		default:
			/*计算周期1ms, 加减速时间为 5.00s  加减速步进计算*/
			accStep = StepCalc(SpeedMax,1,5000);
			break;
	}
	
	/*减速步进*/
	decStep = StepCalc(SpeedMax,1,1000);

	/* 跟踪启动 */
	if(MC_CalParam.Foc_Flag  == RESET)			
	{
		//MotorStartFlg = 1;
		if(MC_RunInfo.MotorSpeed > 100)
		{
			SpeedSetReal = MC_RunInfo.MotorSpeed;
			SpeedSetMiddle = SpeedSetReal << 16;
		}
	}
		
	/*速度指令的加减速处理*/
	SpeedSetReal = accDecProcess(SpeedSet,accStep,decStep,&SpeedSetMiddle);  

	/*母线电流限流*/
	PID_IMax.hLower_Limit_Output= -(MC_AssisParam.Gear_TURBO.Upper_Iq / 2);   //Lower Limit for Output limitation
	PID_IMax.hUpper_Limit_Output= 0;   //Upper Limit for Output limitation
	PID_IMax.wLower_Limit_Integral = -((MC_AssisParam.Gear_TURBO.Upper_Iq / 2) << 10); // 放大1024
	PID_IMax.wUpper_Limit_Integral = 0; // 放大1024
	SpdMotorByIdc = PID_Regulator((MC_ConfigParam1.CurrentLimit * 1000) >> 7, MC_RunInfo.BusCurrent >> 7, &PID_ConstantPower); // 母线电流闭环
	
	/* 电机速度闭环 */
	/*最大力矩为4档的力矩参数*/
	PID_MotorSpd.hLower_Limit_Output= -(MC_AssisParam.Gear_TURBO.Upper_Iq / 2);
	PID_MotorSpd.hUpper_Limit_Output= (MC_AssisParam.Gear_TURBO.Upper_Iq / 2);
//			PID_MotorSpd.wLower_Limit_Integral = -((ContrlParam.Gear_4st.Upper_Iq / 2) << 10); // 放大1024
//			PID_MotorSpd.wUpper_Limit_Integral = ((ContrlParam.Gear_4st.Upper_Iq / 2) << 10); // 放大1024
	TorQueBySpd = PID_Regulator(SpeedSetReal,	\
															MC_RunInfo.MotorSpeed ,   \
															&PID_MotorSpd); // 电机速度闭环输出

	TorQueBySpd += SpdMotorByIdc;
	
	#if 1
	static uint16_t K_ByVoltage_Set_Old = 1024;
	uint16_t K_ByVoltage_Set;
	static uint16_t K_ByVoltage_Result;
	uint16_t K_ByTemperature_Set;
	static uint16_t K_ByTemperature_Result;

	//根据电压调节输出
  K_ByVoltage_Set = MC_Cal_K_ByVoltage(MC_RunInfo.BusVoltage, MC_MotorParam.Rate_Voltage, K_ByVoltage_Set_Old);//根据母线电压计算衰减比例，递减
	K_ByVoltage_Set_Old = K_ByVoltage_Set;
  K_ByVoltage_Result = MC_DataSet_Linear_Process(K_ByVoltage_Set, K_ByVoltage_Result, 1, 1); //设定值与给定值线性处理
	
	//根据温度调节输出
	K_ByTemperature_Set = MC_Cal_K_ByTemperature(MC_RunInfo.T_Coil, MC_ConfigParam1.TempTH_Alarm); //根据温度计算衰减比例
	K_ByTemperature_Result = MC_DataSet_Linear_Process(K_ByTemperature_Set, K_ByTemperature_Result, 1, 1); //设定值与给定值线性处理
	
	#else
	
	uint16_t K_ByVoltage_Result = 1024;
	uint16_t K_ByTemperature_Result = 1024;
	#endif
	
	//速度环控制量为0时停机，防止电机出现异响
	if(SpeedSetReal == 0)
	{
		MC_MotorStop(&MC_StarFlag);
	}
	else
	{
		//电机启动
		MC_MotorStar(&MC_StarFlag);
	}
	
	Ref_Speed_Temp = ((int32_t)TorQueBySpd * K_ByVoltage_Result) >> 10;
	Ref_Speed_Temp = ((int32_t)Ref_Speed_Temp * K_ByTemperature_Result) >> 10;
	
	p_MC_CalParam.Ref_Speed = (int16_t)(Ref_Speed_Temp);
	p_MC_CalParam.Foc_Flag = SET;
	p_MC_CalParam.AssistRunMode = MC_AssistRunMode_GAS;
	
	return (p_MC_CalParam);
}

//推行模式处理
MC_CalParam_Struct_t MC_AssistRunMode_Walk_Process(MC_WorkMode_Struct_t p_MC_WorkMode)
{
	MC_CalParam_Struct_t p_MC_CalParam = {MC_AssistRunMode_INVALID, 0,	0, RESET};
	int16_t TorQueBySpd = 0;
	int32_t Ref_Speed_Temp;
	int16_t SpdMotorByIdc = 0;
	uint8_t StepData = 0;
	
	//配置模式，设定转速 = 最高转速
	if(p_MC_WorkMode == MC_WorkMode_Config)
	{
	  StepData = (MC_MotorParam.Rate_Speed << 5) / 5000;//设计5s加速到最大值
		StepData = (StepData < 1) ? 1 : StepData;
		if(MC_WalkProcess_Param.MotorSpeedSetBigin < (MC_MotorParam.Rate_Speed << 5) * MC_WalkMode_Persent / 100 - 10)
		{
		  MC_WalkProcess_Param.MotorSpeedSetBigin += StepData;
		}
		else if(MC_WalkProcess_Param.MotorSpeedSetBigin > (MC_MotorParam.Rate_Speed << 5) * MC_WalkMode_Persent / 100 + 10)
		{
		  if(MC_WalkProcess_Param.MotorSpeedSetBigin > StepData)
			{
			  MC_WalkProcess_Param.MotorSpeedSetBigin -= StepData;
			}
			else
			{
			  MC_WalkProcess_Param.MotorSpeedSetBigin = 0;
			}
		}
		else
		{
		  MC_WalkProcess_Param.MotorSpeedSetBigin = (MC_MotorParam.Rate_Speed << 5 ) * MC_WalkMode_Persent / 100;
		}
		SpdMotorByIdc = PID_Regulator((MC_ConfigParam1.CurrentLimit * 1000) >> 7, MC_RunInfo.BusCurrent >> 7, &PID_ConstantPower); // 母线电流闭环
	}
	
	//运行模式，设定转速 = 135rpm
	else
	{
		if(MC_WalkProcess_Param.MotorSpeedSetBigin < (135 << 5) - 10)
		{
		  MC_WalkProcess_Param.MotorSpeedSetBigin += 1;
		}
		else if(MC_WalkProcess_Param.MotorSpeedSetBigin > (135 << 5) + 10)
		{
		  MC_WalkProcess_Param.MotorSpeedSetBigin -= 1;
		}
		else
		{
		  MC_WalkProcess_Param.MotorSpeedSetBigin = 135 << 5;
		}
		SpdMotorByIdc = PID_Regulator((MC_ConfigParam1.CurrentLimit * 500) >> 7, MC_RunInfo.BusCurrent >> 7, &PID_ConstantPower); // 母线电流闭环
	}
	
	//速度环
	TorQueBySpd = PID_Regulator((MC_WalkProcess_Param.MotorSpeedSetBigin >> 5), MC_RunInfo.MotorSpeed, &PID_MotorSpd); // 电机速度闭环输出
	TorQueBySpd += SpdMotorByIdc;
	
	//限制车速低于6km/h
	if(p_MC_WorkMode != MC_WorkMode_Config) //运行模式,推行限速6km/h
	{
	  TorQueBySpd = (uint16_t)((uint32_t)(TorQueBySpd * Function_Linear_3Stage(55, 0, 55, 128, MC_RunInfo.BikeSpeed)) >> 10);
		if(MC_RunInfo.BikeSpeed > 60)
		{
			MC_WalkProcess_Param.MotorSpeedSetBigin = 0;
			MC_MotorStop(&MC_StarFlag);
		}
		else
		{
			//电机启动
			MC_MotorStar(&MC_StarFlag);
		}
	}
	else //配置模式不限速
	{
	  //电机启动
	  MC_MotorStar(&MC_StarFlag);
	}
	
	#if 1
	static uint16_t K_ByVoltage_Set_Old = 1024;
	uint16_t K_ByVoltage_Set;
	static uint16_t K_ByVoltage_Result;
	uint16_t K_ByTemperature_Set;
	static uint16_t K_ByTemperature_Result;

	//根据电压调节输出
  K_ByVoltage_Set = MC_Cal_K_ByVoltage(MC_RunInfo.BusVoltage, MC_MotorParam.Rate_Voltage, K_ByVoltage_Set_Old);//根据母线电压计算衰减比例，递减
	K_ByVoltage_Set_Old = K_ByVoltage_Set;
  K_ByVoltage_Result = MC_DataSet_Linear_Process(K_ByVoltage_Set, K_ByVoltage_Result, 1, 1); //设定值与给定值线性处理
	
	//根据温度调节输出
	K_ByTemperature_Set = MC_Cal_K_ByTemperature(MC_RunInfo.T_Coil, MC_ConfigParam1.TempTH_Alarm); //根据温度计算衰减比例
	K_ByTemperature_Result = MC_DataSet_Linear_Process(K_ByTemperature_Set, K_ByTemperature_Result, 1, 1); //设定值与给定值线性处理
	
	#else
	
	uint16_t K_ByVoltage_Result = 1024;
	uint16_t K_ByTemperature_Result = 1024;
	#endif
	
	#if 0
	//限制最大输出功率为250W
	static uint16_t IqsMax;
	if(MC_RunInfo.MotorSpeed < 10)
	{
	  IqsMax = 1050;
	}
	else
	{
	  IqsMax = 235000 / MC_RunInfo.MotorSpeed;
	}
	IqsMax = (IqsMax > 1050) ? 1050 : IqsMax;
	if(TorQueBySpd > IqsMax)
	{
	  TorQueBySpd = IqsMax;
	}
	#elif 0
	if(TorQueBySpd > 450)
	{
	  TorQueBySpd = 450;
	}
	#endif
	
	Ref_Speed_Temp = ((int32_t)TorQueBySpd * K_ByVoltage_Result) >> 10;
	Ref_Speed_Temp = ((int32_t)Ref_Speed_Temp * K_ByTemperature_Result) >> 10;
	
	p_MC_CalParam.Ref_Speed = (int16_t)(Ref_Speed_Temp);
	p_MC_CalParam.Foc_Flag = SET;
	p_MC_CalParam.AssistRunMode = MC_AssistRunMode_WALK;
	
	return (p_MC_CalParam);
}

//踏频模式处理
MC_CalParam_Struct_t MC_AssistRunMode_Cadence_Process(MC_GearSt_Struct_t GearSt)
{
  MC_CalParam_Struct_t p_MC_CalParam = {MC_AssistRunMode_INVALID, 0,	0, RESET};
	
  //...插入踏频处理
	
	//电机启动
	MC_MotorStar(&MC_StarFlag);
	
	p_MC_CalParam.Foc_Flag = SET;
	p_MC_CalParam.AssistRunMode = MC_AssistRunMode_CADENCE;
	
	return (p_MC_CalParam);
}

//力矩模式处理
MC_CalParam_Struct_t MC_AssistRunMode_Torque_Process(uint16_t SenorData, MC_GearSt_Struct_t GearSt)
{
  MC_CalParam_Struct_t p_MC_CalParam = {MC_AssistRunMode_INVALID, 0,	0, RESET};
  uint8_t TorqueAccStep = 0;//力矩上升斜率
	uint8_t TorqueDecStep = 0;//力矩下降斜率
	uint16_t TorqueStartData, TorqueStopData;//力矩启动值，力矩停机值
	int16_t Torque_Temp;
	int32_t Torque_Ref_Temp;
	static uint32_t TorqueStopDelayTimeCnt = 0; //低力矩停机计时
	uint16_t TorqueStopDelayTime;
	static int16_t IqRefByInPower;           //限流计算结果
  static uint16_t CurrentLimitPresent;     //限流实际值，做升降速处理
	uint16_t CurrentLimitSet;                //限流设置值，不同助力档位更新
	
	static uint8_t TorqueRefEndUpdateCount = 0;

	#if 1
	
	//踩踏力矩输入
  MC_TorqueProcess_Param.TorqueApp = SenorData;
	
	#elif 1
	
	//输入阶跃
	MC_TorqueProcess_Param.TorqueApp  = 1000;
	
	//踏频设为启动
	MC_CadenceResult.Cadence_Dir = MC_Cadence_Forward;
	MC_CadenceResult.IsStopFlag = FALSE;
	
	#elif 1
	
	//输入斜坡
	static uint32_t WaveTime_Zero = 0;
	static uint32_t Time_Enter = 0;
	if((HAL_GetTick() - Time_Enter) > 10) // 超时10ms未进入，波形发生初始时刻清零
	{
	  WaveTime_Zero = HAL_GetTick();
	}
	Time_Enter = HAL_GetTick();
	MC_TorqueProcess_Param.TorqueApp = RampWaveGenerate(WaveTime_Zero, 6000, 2100);
	
	//踏频设为启动
	MC_CadenceResult.Cadence_Dir = MC_Cadence_Forward;
	MC_CadenceResult.IsStopFlag = FALSE;
	
	#elif 1
	
	//输入三角波，测试输出响应
	static uint32_t WaveTime_Zero = 0;
	static uint32_t Time_Enter = 0;
	if((HAL_GetTick() - Time_Enter) > 10) // 超时10ms未进入，波形发生初始时刻清零
	{
	  WaveTime_Zero = HAL_GetTick();
	}
	Time_Enter = HAL_GetTick();
	MC_TorqueProcess_Param.TorqueApp  = TriangleWaveGenerate(WaveTime_Zero, 500, 1000 ,1500);
	
	//踏频设为启动
	MC_CadenceResult.Cadence_Dir = MC_Cadence_Forward;
	MC_CadenceResult.IsStopFlag = FALSE;
	
	#elif 1
	
	//输入方波，测试输出响应
	static uint32_t WaveTime_Zero = 0;
	static uint32_t Time_Enter = 0;
	if((HAL_GetTick() - Time_Enter) > 10) // 超时10ms未进入，波形发生初始时刻清零
	{
	  WaveTime_Zero = HAL_GetTick();
	}
	Time_Enter = HAL_GetTick();
	MC_TorqueProcess_Param.TorqueApp  = SquareWaveGenerate(WaveTime_Zero, 5000, 8000, 1500);
	
	//踏频设为启动
	MC_CadenceResult.Cadence_Dir = MC_Cadence_Forward;
	MC_CadenceResult.IsStopFlag = FALSE;
	
	#endif
		
	//低力矩停机
	TorqueStopData = (TorqueStartData < 200) ? 100 : (TorqueStartData >> 1);
	if(MC_TorqueProcess_Param.TorqueApp >= (TorqueStopData))
	{
	  TorqueStopDelayTimeCnt = HAL_GetTick();
	}
	else
	{
		if(MC_RunInfo.MotorSpeed > 200)
		{
		  TorqueStopDelayTime = 218400 / MC_RunInfo.MotorSpeed; //60s / (电机转速 / 4.55 / 2.4) / 3，曲柄1/3圈
		}
		else
		{
		  TorqueStopDelayTime = 1200;
		}
		TorqueStopDelayTime= (TorqueStopDelayTime < 500) ? 500 : TorqueStopDelayTime;
		if((HAL_GetTick() - TorqueStopDelayTimeCnt) > TorqueStopDelayTime)//超时1200ms
		{
			MC_TorqueProcess_Param.MotorStopLock_Flag = SET;
		}
	}
	
	//启动值判断
	if(MC_RunInfo.BikeSpeed > 60) 
	{
		TorqueStartData =  (TorqueStartData < 200 ? 150 : (TorqueStartData > 700 ? 525 : ((TorqueStartData * 3) >> 2)));
	}
	else
	{
		TorqueStartData =  (TorqueStartData < 200 ? 150 : (TorqueStartData > 700 ? 525 : TorqueStartData));
	}
	if(MC_TorqueProcess_Param.TorqueApp >= TorqueStartData)
	{
		 MC_TorqueProcess_Param.MotorStopLock_Flag = RESET;
	}
	
	//踏频反向或踏频停止停机
	if((MC_CadenceResult.Cadence_Dir == MC_Cadence_Backward) ||
		 (MC_CadenceResult.IsStopFlag == TRUE)		
		)
	{
		MC_TorqueProcess_Param.MotorStopLock_Flag = SET;
	}
	
	//停机状态，延时处理
	if(MC_TorqueProcess_Param.MotorStopLock_Flag == SET)
	{
	  if(MC_TorqueProcess_Param.TorqueRefEnd <= 7)
		{
		  MC_TorqueProcess_Param.TorqueRefEnd = 0;
			//停机处理
			MC_MotorStop(&MC_StarFlag);
		}
		else
		{
		  MC_TorqueProcess_Param.TorqueRefEnd -= 7;  //这里影响到停止踩踏后的断电时间
			MC_MotorStar(&MC_StarFlag);
		}
	}
	//力矩给定升降速处理
	else
	{
		//按照助力档位调节力矩输入值
		switch(GearSt)
		{
			case MC_GearSt_Torque_ECO:
			{
				//控制输入给定加速斜率
				if(MC_ConfigParam1.UserAdjParam_ECO.StarModel_GAIN <= 80)
				{
				  TorqueAccStep = MC_AssisParam.Gear_ECO.AccCnt - 1;
				}
				else if(MC_ConfigParam1.UserAdjParam_ECO.StarModel_GAIN >= 120)
				{
				  TorqueAccStep = MC_AssisParam.Gear_ECO.AccCnt + 1;
				}
				else
				{
				  TorqueAccStep = MC_AssisParam.Gear_ECO.AccCnt;
				}
				TorqueAccStep = (TorqueAccStep <= 0) ? 1 : TorqueAccStep;
				//控制输入给定减速斜率
				TorqueDecStep = MC_AssisParam.Gear_ECO.DecCnt;
				//随力矩输入调节助力比
				Torque_Temp = (uint16_t)((uint32_t)(MC_TorqueProcess_Param.TorqueApp * Coefficient_GainCal(MC_AssisParam.Gear_ECO.Gain_K >> 1, MC_AssisParam.Gear_ECO.Gain_K, MC_AssisParam.Gear_ECO.TorqueApp_TH, MC_TorqueProcess_Param.TorqueApp)) >> 10);
				//根据助力增益调节助力比
				Torque_Temp = Torque_Temp * MC_ConfigParam1.UserAdjParam_ECO.Assist_K_GAIN / 100;
				//给定下限
				Torque_Temp = (Torque_Temp < MC_AssisParam.Gear_ECO.Lower_Iq) ? MC_AssisParam.Gear_ECO.Lower_Iq : Torque_Temp;
				//给定上限
				Torque_Temp = (Torque_Temp > MC_AssisParam.Gear_ECO.Upper_Iq) ? MC_AssisParam.Gear_ECO.Upper_Iq : Torque_Temp;
				//限流参数设置
				CurrentLimitSet = (uint32_t)(MC_AssisParam.Gear_ECO.CurrentMax_K * MC_ConfigParam1.CurrentLimit * 1000 >> 17) * 100;
				CurrentLimitPresent = MC_DataSet_Linear_Process(CurrentLimitSet, CurrentLimitPresent, 5 ,1);
				PID_IMax.hLower_Limit_Output = -(MC_AssisParam.Gear_ECO.Upper_Iq);   //Lower Limit for Output limitation
				PID_IMax.hUpper_Limit_Output = 0; //Upper Limit for Output limitation
				PID_IMax.wLower_Limit_Integral = -(MC_AssisParam.Gear_ECO.Upper_Iq << 10); // 放大1024
				PID_IMax.wUpper_Limit_Integral = 0; // 放大1024
				break;
			}
			case MC_GearSt_Torque_NORM:
			{
				//控制输入给定加速斜率
				if(MC_ConfigParam1.UserAdjParam_NORM.StarModel_GAIN <= 80)
				{
				  TorqueAccStep = MC_AssisParam.Gear_NORM.AccCnt - 1;
				}
				else if(MC_ConfigParam1.UserAdjParam_NORM.StarModel_GAIN >= 120)
				{
				  TorqueAccStep = MC_AssisParam.Gear_NORM.AccCnt + 1;
				}
				else
				{
				  TorqueAccStep = MC_AssisParam.Gear_NORM.AccCnt;
				}
				TorqueAccStep = (TorqueAccStep <= 0) ? 1 : TorqueAccStep;
				//控制输入给定减速斜率
				TorqueDecStep = MC_AssisParam.Gear_NORM.DecCnt;
				//随力矩输入调节助力比
				Torque_Temp = (uint16_t)((uint32_t)(MC_TorqueProcess_Param.TorqueApp * Coefficient_GainCal(MC_AssisParam.Gear_NORM.Gain_K >> 1, MC_AssisParam.Gear_NORM.Gain_K, MC_AssisParam.Gear_NORM.TorqueApp_TH, MC_TorqueProcess_Param.TorqueApp)) >> 10);
				//根据助力增益调节助力比
				Torque_Temp = Torque_Temp * MC_ConfigParam1.UserAdjParam_NORM.Assist_K_GAIN / 100;
				//给定下限
				Torque_Temp = (Torque_Temp < MC_AssisParam.Gear_NORM.Lower_Iq) ? MC_AssisParam.Gear_NORM.Lower_Iq : Torque_Temp;
				//给定上限
				Torque_Temp = (Torque_Temp > MC_AssisParam.Gear_NORM.Upper_Iq) ? MC_AssisParam.Gear_NORM.Upper_Iq : Torque_Temp;
				
				//限流参数设置
				CurrentLimitSet = (uint32_t)(MC_AssisParam.Gear_NORM.CurrentMax_K * MC_ConfigParam1.CurrentLimit * 1000 >> 17) * 100;
				CurrentLimitPresent = MC_DataSet_Linear_Process(CurrentLimitSet, CurrentLimitPresent, 5 ,1);
				PID_IMax.hLower_Limit_Output = -(MC_AssisParam.Gear_NORM.Upper_Iq);   //Lower Limit for Output limitation
				PID_IMax.hUpper_Limit_Output = 0; //Upper Limit for Output limitation
				PID_IMax.wLower_Limit_Integral = -(MC_AssisParam.Gear_NORM.Upper_Iq << 10); // 放大1024
				PID_IMax.wUpper_Limit_Integral = 0; // 放大1024
				break;
			}
			case MC_GearSt_Torque_SPORT:
			{
				//控制输入给定加速斜率
				if(MC_ConfigParam1.UserAdjParam_SPORT.StarModel_GAIN <= 80)
				{
				  TorqueAccStep = MC_AssisParam.Gear_SPORT.AccCnt - 1;
				}
				else if(MC_ConfigParam1.UserAdjParam_SPORT.StarModel_GAIN >= 120)
				{
				  TorqueAccStep = MC_AssisParam.Gear_SPORT.AccCnt + 1;
				}
				else
				{
				  TorqueAccStep = MC_AssisParam.Gear_SPORT.AccCnt;
				}
				TorqueAccStep = (TorqueAccStep <= 0) ? 1 : TorqueAccStep;
				//控制输入给定减速斜率
				TorqueDecStep = MC_AssisParam.Gear_SPORT.DecCnt;
				//随力矩输入调节助力比
				Torque_Temp = (uint16_t)((uint32_t)(MC_TorqueProcess_Param.TorqueApp * Coefficient_GainCal(MC_AssisParam.Gear_SPORT.Gain_K >> 1, MC_AssisParam.Gear_SPORT.Gain_K, MC_AssisParam.Gear_SPORT.TorqueApp_TH, MC_TorqueProcess_Param.TorqueApp)) >> 10);
				//根据助力增益调节助力比
				Torque_Temp = Torque_Temp * MC_ConfigParam1.UserAdjParam_SPORT.Assist_K_GAIN / 100;
				//给定下限
				Torque_Temp = (Torque_Temp < MC_AssisParam.Gear_SPORT.Lower_Iq) ? MC_AssisParam.Gear_SPORT.Lower_Iq : Torque_Temp;
				//给定上限
				Torque_Temp = (Torque_Temp > MC_AssisParam.Gear_SPORT.Upper_Iq) ? MC_AssisParam.Gear_SPORT.Upper_Iq : Torque_Temp;
				//限流参数设置
				CurrentLimitSet = (uint32_t)(MC_AssisParam.Gear_SPORT.CurrentMax_K * MC_ConfigParam1.CurrentLimit * 1000 >> 17) * MC_CadenceLimit_K;
				CurrentLimitPresent = MC_DataSet_Linear_Process(CurrentLimitSet, CurrentLimitPresent, 5 ,1);
				PID_IMax.hLower_Limit_Output = -(MC_AssisParam.Gear_SPORT.Upper_Iq);   //Lower Limit for Output limitation
				PID_IMax.hUpper_Limit_Output = 0; //Upper Limit for Output limitation
				PID_IMax.wLower_Limit_Integral = -(MC_AssisParam.Gear_SPORT.Upper_Iq << 10); // 放大1024
				PID_IMax.wUpper_Limit_Integral = 0; // 放大1024
				break;
			}
			case MC_GearSt_Torque_TURBO:
			{
				//控制输入给定加速斜率
				if(MC_ConfigParam1.UserAdjParam_TURBO.StarModel_GAIN <= 80)
				{
				  TorqueAccStep = MC_AssisParam.Gear_TURBO.AccCnt - 1;
				}
				else if(MC_ConfigParam1.UserAdjParam_TURBO.StarModel_GAIN >= 120)
				{
				  TorqueAccStep = MC_AssisParam.Gear_TURBO.AccCnt + 1;
				}
				else
				{
				  TorqueAccStep = MC_AssisParam.Gear_TURBO.AccCnt;
				}
				TorqueAccStep = (TorqueAccStep <= 0) ? 1 : TorqueAccStep;
				//控制输入给定减速斜率
				TorqueDecStep = MC_AssisParam.Gear_TURBO.DecCnt;
				//随力矩输入调节助力比
				Torque_Temp = (uint16_t)((uint32_t)(MC_TorqueProcess_Param.TorqueApp * Coefficient_GainCal(MC_AssisParam.Gear_TURBO.Gain_K >> 1, MC_AssisParam.Gear_TURBO.Gain_K, MC_AssisParam.Gear_TURBO.TorqueApp_TH, MC_TorqueProcess_Param.TorqueApp)) >> 10);
				//根据助力增益调节助力比
				Torque_Temp = Torque_Temp * MC_ConfigParam1.UserAdjParam_TURBO.Assist_K_GAIN / 100;
				//给定下限
				Torque_Temp = (Torque_Temp < MC_AssisParam.Gear_TURBO.Lower_Iq) ? MC_AssisParam.Gear_TURBO.Lower_Iq : Torque_Temp;
				//给定上限
				Torque_Temp = (Torque_Temp > MC_AssisParam.Gear_TURBO.Upper_Iq) ? MC_AssisParam.Gear_TURBO.Upper_Iq : Torque_Temp;
				//限流参数设置
				CurrentLimitSet = (uint32_t)(MC_AssisParam.Gear_TURBO.CurrentMax_K * MC_ConfigParam1.CurrentLimit * 1000 >> 17) * MC_CadenceLimit_K;
				CurrentLimitPresent = MC_DataSet_Linear_Process(CurrentLimitSet, CurrentLimitPresent, 5 ,1);
				PID_IMax.hLower_Limit_Output = -(MC_AssisParam.Gear_TURBO.Upper_Iq);   //Lower Limit for Output limitation
				PID_IMax.hUpper_Limit_Output = 0; //Upper Limit for Output limitation
				PID_IMax.wLower_Limit_Integral = -(MC_AssisParam.Gear_TURBO.Upper_Iq << 10); // 放大1024
				PID_IMax.wUpper_Limit_Integral = 0; // 放大1024
				break;
			}
			case MC_GearSt_SMART:
			{
				//控制输入给定加速斜率
				if(MC_ConfigParam1.UserAdjParam_SMART.StarModel_GAIN <= 80)
				{
				  TorqueAccStep = MC_AssisParam.Gear_SMART.AccCnt - 1;
				}
				else if(MC_ConfigParam1.UserAdjParam_SMART.StarModel_GAIN >= 120)
				{
				  TorqueAccStep = MC_AssisParam.Gear_SMART.AccCnt + 1;
				}
				else
				{
				  TorqueAccStep = MC_AssisParam.Gear_SMART.AccCnt;
				}
				TorqueAccStep = (TorqueAccStep <= 0) ? 1 : TorqueAccStep;
				//控制输入给定减速斜率
				TorqueDecStep = MC_AssisParam.Gear_SMART.DecCnt;
				//助力比控制系数
				Torque_Temp = (uint32_t)(MC_TorqueProcess_Param.TorqueApp * MC_TorqueProcess_Param.TorqueApp) / (MC_AssisParam.Gear_SMART.TorqueApp_TH);
				//根据助力增益调节助力比
				Torque_Temp = Torque_Temp * MC_ConfigParam1.UserAdjParam_SMART.Assist_K_GAIN / 100;
				//给定下限
				Torque_Temp = (Torque_Temp < MC_AssisParam.Gear_SMART.Lower_Iq) ? MC_AssisParam.Gear_SMART.Lower_Iq : Torque_Temp;
				//给定上限
				Torque_Temp = (Torque_Temp > MC_AssisParam.Gear_SMART.Upper_Iq) ? MC_AssisParam.Gear_SMART.Upper_Iq : Torque_Temp;
				//限流参数设置
				CurrentLimitSet = (uint32_t)(MC_AssisParam.Gear_SMART.CurrentMax_K * MC_ConfigParam1.CurrentLimit * 1000 >> 17) * MC_CadenceLimit_K;
				CurrentLimitPresent = MC_DataSet_Linear_Process(CurrentLimitSet, CurrentLimitPresent, 5 ,1);
				PID_IMax.hLower_Limit_Output = -(MC_AssisParam.Gear_SMART.Upper_Iq);   //Lower Limit for Output limitation
				PID_IMax.hUpper_Limit_Output = 0; //Upper Limit for Output limitation
				PID_IMax.wLower_Limit_Integral = -(MC_AssisParam.Gear_SMART.Upper_Iq << 10); // 放大1024
				PID_IMax.wUpper_Limit_Integral = 0; // 放大1024
				break;
			}
			default: 
			{
				TorqueAccStep = 0;
				TorqueDecStep = 0;
				Torque_Temp = 0;
				break;
			}
		}
		
		//随车速调节助力比
		Torque_Temp = (uint16_t)((uint32_t)(Torque_Temp * Function_Linear_3Stage(MC_ConfigParam1.SpeedLimit * 10, 0, MC_ConfigParam1.SpeedLimit * 10, 52, MC_SpeedSensorData.Speed_Data)) >> 10);
			
    //助力输出		
		MC_TorqueProcess_Param.TorqueRef = Torque_Temp;
		if(MC_TorqueProcess_Param.TorqueRef <= 0)
		{
			MC_TorqueProcess_Param.TorqueRef = 0;
		}
				
		//升降速曲线计算
		if( MC_SpeedSensorData.Speed_Data > (MC_ConfigParam1.SpeedLimit * 10) ) //限速处理
		{
			if((MC_TorqueProcess_Param.TorqueRef - MC_TorqueProcess_Param.TorqueRefEnd) > 2)
			{
				MC_TorqueProcess_Param.TorqueRefEnd += 1;
			}
			else if((MC_TorqueProcess_Param.TorqueRef - MC_TorqueProcess_Param.TorqueRefEnd) < (- 1))
			{
				MC_TorqueProcess_Param.TorqueRefEnd -= 1; 
			}
		}
		else if( (Bike_Attitude.UpWardSlope_flag == TRUE)&&(MC_SpeedSensorData.Speed_Data < 100))  //上坡处理			  
		{
			if((MC_TorqueProcess_Param.TorqueRef - MC_TorqueProcess_Param.TorqueRefEnd) > 2)
			{
				MC_TorqueProcess_Param.TorqueRefEnd += TorqueAccStep;
			}
			else if((MC_TorqueProcess_Param.TorqueRef - MC_TorqueProcess_Param.TorqueRefEnd) < (- 1))
			{
				TorqueRefEndUpdateCount++;			
				if(TorqueRefEndUpdateCount >=3)
				{
					TorqueRefEndUpdateCount = 0;
					MC_TorqueProcess_Param.TorqueRefEnd -= TorqueDecStep;
				}
			}		
		}		
		else //正常骑行
		{
			if((MC_TorqueProcess_Param.TorqueRef - MC_TorqueProcess_Param.TorqueRefEnd) > 2)
			{
				MC_TorqueProcess_Param.TorqueRefEnd += TorqueAccStep;
			}
			else if((MC_TorqueProcess_Param.TorqueRef - MC_TorqueProcess_Param.TorqueRefEnd) < (- 1))
			{
				MC_TorqueProcess_Param.TorqueRefEnd -= TorqueDecStep;
			}
		}
		MC_TorqueProcess_Param.TorqueRefEnd = (MC_TorqueProcess_Param.TorqueRefEnd < 6) ? 6 : MC_TorqueProcess_Param.TorqueRefEnd;
		
		//限速点处理
		if( MC_SpeedSensorData.Speed_Data > (MC_ConfigParam1.SpeedLimit * 10 + 20) ) //限速值+2
		{
			MC_TorqueProcess_Param.MotorStopLock_Flag = SET;
			MC_TorqueProcess_Param.TorqueRefEnd = 0;
			//停机处理
			MC_MotorStop(&MC_StarFlag);
		}
		#if 0  //低于限速点启动电机
		else if(MC_RunInfo.BikeSpeed < ((MC_ConfigParam1.SpeedLimit) * 10))
		{
		  MC_MotorStar(&MC_StarFlag);
		}
		#elif 1  //低于断电点即启动电机
		else
		{
		  MC_MotorStar(&MC_StarFlag);
		}
		#endif
	}
	
	#if 1
	static uint16_t K_ByVoltage_Set_Old = 1024;
	uint16_t K_ByVoltage_Set;
	static uint16_t K_ByVoltage_Result;
	uint16_t K_ByTemperature_Set;
	static uint16_t K_ByTemperature_Result;

	//根据电压调节输出
  K_ByVoltage_Set = MC_Cal_K_ByVoltage(MC_RunInfo.BusVoltage, MC_MotorParam.Rate_Voltage, K_ByVoltage_Set_Old);//根据母线电压计算衰减比例，递减
	K_ByVoltage_Set_Old = K_ByVoltage_Set;
  K_ByVoltage_Result = MC_DataSet_Linear_Process(K_ByVoltage_Set, K_ByVoltage_Result, 1, 1); //设定值与给定值线性处理
	
	//根据温度调节输出
	K_ByTemperature_Set = MC_Cal_K_ByTemperature(MC_RunInfo.T_Coil, MC_ConfigParam1.TempTH_Alarm); //根据温度计算衰减比例
	K_ByTemperature_Result = MC_DataSet_Linear_Process(K_ByTemperature_Set, K_ByTemperature_Result, 1, 1); //设定值与给定值线性处理
	
	#else
	
	uint16_t K_ByVoltage_Result = 1024;
	uint16_t K_ByTemperature_Result = 1024;
	#endif
	
  //限流计算
  IqRefByInPower =  PID_Regulator(CurrentLimitPresent / 100, (MC_RunInfo.BusCurrent >> 7), &PID_IMax);
	
	Torque_Ref_Temp = ((int32_t)MC_TorqueProcess_Param.TorqueRefEnd * K_ByVoltage_Result) >> 10;
	Torque_Ref_Temp = (Torque_Ref_Temp * K_ByTemperature_Result) >> 10;
	Torque_Ref_Temp = (Torque_Ref_Temp + IqRefByInPower) >> 1;
	
	p_MC_CalParam.Ref_Torque = (int16_t)Torque_Ref_Temp;
	p_MC_CalParam.Foc_Flag = SET;
	p_MC_CalParam.AssistRunMode = MC_AssistRunMode_TORQUE;
	
	return (p_MC_CalParam);
}


/******************************全局函数定义*****************************/
//传感器初始化
void MC_SensorInit(void)
{
  //霍尔传感器IO设置
	HallSensor_GPIO_Init();
	
	//霍尔电角度初始化
	HallSensorAngle_Init();
	
	//踏频传感器IO设置
	CadenceSensor_GPIO_Init();
	
	//速度传感器IO设置
	SpeedSensor_GPIO_Init();
	
	//刹车信号和Gear信号检测IO设置
	KeyInitial();
	
	//力矩传感器参数还原
	TorqueSensor_ParamSetDefaultData_Init(&TorqueSensor_1_Param, ADC1_Result[ADC1_RANK_TORQUE_SENSOR_1]);
	TorqueSensor_ParamSetDefaultData_Init(&TorqueSensor_2_Param, ADC1_Result[ADC1_RANK_TORQUE_SENSOR_2]);
	TorqueSensor_ParamSetDefaultData_Init(&TorqueSensor_3_Param, ADC1_Result[ADC1_RANK_TORQUE_SENSOR_3]);
	
	//指拨零点初值
	GasSensorOffSet_Init(&GasSensor_OffSet, ADC1_Result[ADC1_RANK_GAS]);
	
	//ICM20600初始化	
	ICM20600_initialize();
	if(ICM20600_OK_Flag == TRUE)
	{
		ICM20600_coefficientinitialize(RANGE_250_DPS, RANGE_2G, &ICM20600Sensor);
	}
}

//MC控制初始化
void MC_Init(void)
{	
	//PID参数初始化
	PID_Init(MC_ConfigParam1.SerialNum);	
	
	//助力参数初始化
	UpdateGearParam(MC_ConfigParam1.SerialNum);
	
	//三相电流零点校准
	SVPWM_3ShuntCurrentReadingCalibration(&MC_ErrorCode);
	
	//母线电流零点校准
	CurrentReadingCalibration(&MC_ErrorCode);
	
	//力矩传感器零点值处理
	TorqueOffSetData_Process(&TorqueSensor_1_Param.Torque_OffSetData, ADC1_Result[ADC1_RANK_TORQUE_SENSOR_1]);//112ms
	TorqueOffSetData_Process(&TorqueSensor_2_Param.Torque_OffSetData, ADC1_Result[ADC1_RANK_TORQUE_SENSOR_2]);//112ms
	TorqueOffSetData_Process(&TorqueSensor_3_Param.Torque_OffSetData, ADC1_Result[ADC1_RANK_TORQUE_SENSOR_3]);//112ms
	
	//智能档位初始化处理
	if(MC_ConfigParam1.NoPBU_Flag == MC_SUPPORT_ENABLE)
	{
	  MC_ControlCode.GearSt = MC_GearSt_SMART;
		Update_MC_ControlCode_Back();
	}
	
	//12V驱动电源初始化
	Power12V_Driver_Init();
	
	//打开12V驱动电源
  Power12V_Driver_Process(SET);
}

//MC控制参数初始化
void MC_ControlParam_Init(void)
{
  //清除推行模式初始变量
	MC_WalkProcess_Param.IsEnterFlag = FALSE;
	MC_WalkProcess_Param.MotorSpeedSetBigin = 0;
	
	//清除力矩模式初始变量
	MC_TorqueProcess_Param.MotorStopLock_Flag  = SET;
	MC_TorqueProcess_Param.TorqueApp = 0;
	MC_TorqueProcess_Param.TorqueRef = 0;
	MC_TorqueProcess_Param.TorqueRefEnd = 0;
	
	//全局运算变量归零
	IqFdbFlt =0;
	IdFdbFlt = 0;
	VoltSquareFlt = 0;
	UqVoltFlt = 0;
	UdVoltFlt = 0;
		
	//PDI积分清零
	PID_Flux_InitStructure.wIntegral = 0;
 	PID_Torque_InitStructure.wIntegral = 0;
 	PID_Weak_InitStructure.wIntegral = 0;
 	PID_IMax.wIntegral = 0;
  PID_MotorSpd.wIntegral = 0;
 	PID_ConstantPower.wIntegral = 0;
}

//控制参数输入值计算
void MC_CalParam_Cal(MC_WorkMode_Struct_t p_MC_WorkMode, \
	                   ADC_SensorData_Struct_t p_ADC_SensorData, \
                     MC_GearSt_Struct_t GearSt, \
                     TrueOrFalse_Flag_Struct_t Break_Flag, \
                     TrueOrFalse_Flag_Struct_t GearSensor_Flag, \
                     MC_CalParam_Struct_t* p_MC_CalParam)
{
	MC_AssistRunMode_Struct_t MC_AssistRunMode_Temp;
	static FlagStatus MC_AssistRunMode_ShiftFlag = RESET;  //电机助力模式切换标志
	
	//根据指拨信号、助力档位指令、刹车信号判断助力模式
	MC_AssistRunMode_Temp = MC_JudgeAsistRunMode_Process(MC_ConfigParam1.GasCtrlMode_Flag, p_ADC_SensorData.GasSensor, GearSt, (TrueOrFalse_Flag_Struct_t)(Break_Flag & GearSensor_Flag & Bike_Attitude.FellDown_flag));//TRUE 0, FALSE 1
	
	//发生助力模式切换时，清空变量
	if(MC_AssistRunMode_Temp != p_MC_CalParam->AssistRunMode)
	{
		if(MC_AssistRunMode_ShiftFlag == RESET)
		{
	  	MC_AssistRunMode_Temp = MC_AssistRunMode_INVALID;
			MC_AssistRunMode_ShiftFlag = SET;
			
			SpeedSetMiddle = 0;  //指拨模式，清零速度中间量
			SpdMotorDivWheelFlt = 0;
		}
	}
	//助力模式处理
	switch(MC_AssistRunMode_Temp)
	{
	  //指拨模式
		case MC_AssistRunMode_GAS:
		{
			//计算FOC控制输入
			*p_MC_CalParam = MC_AssistRunMode_Gas_Process(p_ADC_SensorData.GasSensor, (MC_GearSt_Struct_t)(GearSt & 0x0F));
			//助力模式切换标志复位
			MC_AssistRunMode_ShiftFlag = RESET;
			break;
		}
		//推行模式
		case MC_AssistRunMode_WALK:
		{
			//计算FOC控制输入
			if(MC_WalkProcess_Param.IsEnterFlag == FALSE)
			{
				MC_WalkProcess_Param.MotorSpeedSetBigin = (uint32_t)MC_RunInfo.MotorSpeed << 5;
				MC_WalkProcess_Param.IsEnterFlag = TRUE;
			}
			*p_MC_CalParam = MC_AssistRunMode_Walk_Process(p_MC_WorkMode);
			//助力模式切换标志复位
			MC_AssistRunMode_ShiftFlag = RESET;
			break;
		}
		//踏频模式
		case MC_AssistRunMode_CADENCE:
		{
			//计算FOC控制输入
			*p_MC_CalParam = MC_AssistRunMode_Cadence_Process(GearSt);
			//助力模式切换标志复位
			MC_AssistRunMode_ShiftFlag = RESET;
			break;
		}
		//力矩模式
		case MC_AssistRunMode_TORQUE:
		{
			//计算FOC控制输入
			*p_MC_CalParam = MC_AssistRunMode_Torque_Process(p_ADC_SensorData.TorqueSensor, GearSt);
			//助力模式切换标志复位
			MC_AssistRunMode_ShiftFlag = RESET;
			break;
		}
		//空闲模式或存在故障
		case MC_AssistRunMode_INVALID: default:
		{
			//停机处理
			MC_MotorStop(&MC_StarFlag);
			//更新母线电流零点值
			CurrentReadingCalibration(&MC_ErrorCode);
			//控制计算值初始化为默认值
			p_MC_CalParam->AssistRunMode = MC_AssistRunMode_INVALID;
			p_MC_CalParam->Foc_Flag = RESET;
			p_MC_CalParam->Ref_Torque = 0;
			p_MC_CalParam->Ref_Speed = 0;
			break;
		}
	}	
}

void MC_MotorStop(FlagStatus* StarFlag)
{			
	//关闭PWM输出
	Pwm_Timer_Stop();
	
	//FOC运算停止
  FOC_Disable();
	
	//控制参数归零
	MC_ControlParam_Init();
	
	//电机启动标志复位
	*StarFlag = RESET;
}

void MC_MotorStar(FlagStatus* StarFlag)
{	
	if(*StarFlag == RESET)
	{
		//开启PWM输出
		Enable_Pwm_Output();
		
		//霍尔电角度初始化
	  HallSensorAngle_Init();
		
		//FOC运算启动
    FOC_Enable();
		
		//电机启动标志置位
		*StarFlag = SET;
	}
}	

/*
指拨模式计算速比，计算费时，在主循环调用
*/
void SpdProportion_calculate(void)
{
	if(SpdProportion_CAL_flag==1)
	{
		SpdProportion_StandardDeviation = Standard_deviation_aver(SpdProportion_buff, 50, &test_SpdProportionAver);
		test_StandardDeviation = (int32_t)(SpdProportion_StandardDeviation );
		SpdProportion_CAL_flag = 0;
		/*更新速比*/
		if(test_StandardDeviation < 20)
		{
			SpdProportion = test_SpdProportionAver;
		}
	}
}