匿名四轴【任务六（20Hz）】

匿名四轴【任务六（20Hz）】

任务六

static void Loop_20Hz(void)	//50ms执行一次
{	
	/*电压相关任务*/
	Power_UpdateTask(50);
	//恒温控制
	Thermostatic_Ctrl_Task(50);
}

电压相关任务

//运用，传入参数50，没有接受返回值
Power_UpdateTask(50);
//函数
void Power_UpdateTask(u8 dT_ms)
{
	static s16 voltage_s16;
    //s16为int16，voltage为电压的意思，这边定义16位的电压
	float cut_off_freq;
	voltage_s16 = AdcValue *8.8653f;//1.128f
    //AdcValue为通过Adc采取的ADC的值
	if(voltage_init_ok == 0)
	{
		cut_off_freq = 2.0f;
		if(voltage_f >2000 && ABS(voltage_s16 - voltage_f) <200)
		{
			voltage_init_ok = 1;
		}
	}	
	else
	{
		cut_off_freq = 0.02f;
	}
	
	//#define LPF_1_(hz,t,in,out) ((out) += ( 1 / ( 1 + 1 / ( (hz) *6.28f *(t) ) ) ) *( (in) - (out) ))	
	LPF_1_(cut_off_freq,dT_ms*1e-3f,voltage_s16,voltage_f);
	Plane_Votage = voltage_f *0.001f;
//	Plane_Votage = 15;

	if(Plane_Votage<Ano_Parame.set.lowest_power_voltage)
	{
		flag.power_state = 3;//将禁止解锁		
	}
	else
	{
		flag.power_state = 1;
	}

	if(Plane_Votage<Ano_Parame.set.warn_power_voltage)
	{
		LED_STA.lowVt = 1;此时相应的灯光就会亮起
	}
	else if(Plane_Votage>Ano_Parame.set.warn_power_voltage+0.2f)
	{
		LED_STA.lowVt = 0;
	}
		
	if(Plane_Votage<Ano_Parame.set.return_home_power_voltage)
	{
		
	}
}
//这段代码大致就是通过ADC测电压，但是单片机测电压并不能测这么大的电压，所以PCB上采取分压的方式来测电压，如果电压过低，则LED_STA.lowVt = 1;此时相应的灯光就会亮起，起到报警的作用

恒温控制

//恒温控制
	Thermostatic_Ctrl_Task(50);
//没必要 不看了 用到的时候再看
void Thermostatic_Ctrl_Task(u8 dT_ms)
{
	//解锁前才允许操作
	if(flag.unlock_sta == 0)
	{
		if(Ano_Parame.set.heatSwitch == 1)//开启恒温功能
		{
			if(thermostatic_en == 0)
			{
				//
				thermostatic_en = 1;
				//复位完成标记
				flag.mems_temperature_ok = 0;
				//
				sensor.acc_z_auto_CALIBRATE = 1; //重新对准Z轴
				sensor.gyr_CALIBRATE = 2;//重新校准陀螺仪
				//
				ANO_DT_SendString("Thermostatic ON......");	
			}
		}
		else
		{
			if(thermostatic_en)
			{
				//
				thermostatic_en = 0;
				//
				ANO_DT_SendString("Thermostatic OFF,Please Restart ANO_Pioneer_pro!");	
			}
		}
	}
	
	//
	if(thermostatic_en)
	{
		//上次反馈
		temperature_fb[1] = temperature_fb[0];
		//本次反馈
		temperature_fb[0] = sensor.Tempreature_C;
		//微分
		temperature_diff = (temperature_fb[0] - temperature_fb[1]) *1000/dT_ms;
		//微分先行（用微分预测反馈）
		temperature_fb[2] = temperature_fb[0] + temperature_diff *TEMPERATURE_KD ;//*test_temperature_ctrl_arg[2];
		//计算偏差
		temperature_err = EXP_TEMPERATURE - temperature_fb[2];
		//-----
		if(1)//((temperature_ctrl_val)<100)
		{
			//积分偏差限幅
			temperature_err_i += LIMIT(temperature_err,-10,10) *dT_ms *0.001f;
			//积分限幅
			temperature_err_i = LIMIT(temperature_err_i,-20,20);
		}
		//计算控制输出量
		temperature_ctrl_val = 
		TEMPERATURE_KP *temperature_err // *test_temperature_ctrl_arg[0]
		+ TEMPERATURE_KI *temperature_err_i;// *test_temperature_ctrl_arg[1];
		//
		temperature_ctrl_val = LIMIT(temperature_ctrl_val ,0,100);
		//恒温控制量输出
		Drv_HeatingSet((u8)temperature_ctrl_val); 
		//判断是否完成恒温的升温过程
		if(flag.mems_temperature_ok == 0)
		{		
			if(temperature_fb[0] > (EXP_TEMPERATURE-0.3f) && temperature_fb[0] < (EXP_TEMPERATURE+0.3f))
			{
				//偏差小鱼0.3摄氏度，且持续1500ms，判定升温完成
				if(temperature_cnt<1500)
				{
					temperature_cnt += dT_ms;
				}
				else
				{

						//
						flag.mems_temperature_ok = 1;
						//
						ANO_DT_SendString("Thermostatic OK!");	
					
				}
			}
			else
			{
				temperature_cnt = 0;
			}
		}
	}
	else //不使用恒温功能
	{
		flag.mems_temperature_ok = 1;
		Drv_HeatingSet((u8)0); 
	}

}