四旋翼无人机从0到1的实现（二十四）无人机工程中系统控制

2023-12-15 12:34:21

Author：家有仙妻谢掌柜
Date：2021/2/18

今年会更新一个系列，小四轴无人机从功能设计→思维导图→原理图设计→PCBLayout→焊接PCB→程序代码的编写→整机调试一系列，以此记录自己的成长历程！
这个小四轴无人机是大学时期学习制作的，加上现在工作学习对嵌入式的理解更加深入，因此想要重新梳理一下小四轴，之后在此基础上实现大四轴的飞控设计，这些都将在工作之余完成！

//小四轴无人机设计

#include "system_control.h"

/*******************************************************************************
 * fuction	atititude_Outer_loop_control
 * brief	姿态外环的控制
 * param	无
 * return	无
 *******************************************************************************/ 
void atititude_Outer_loop_control(void)
{
	/*pitch 俯仰角PID的控制  */
	pitch_out = pid_angle_control(&Pitch,Expect.pitch,Eur.pitch);
	/*roll 翻滚角PID的控制  */
	roll_out = pid_angle_control(&Roll,Expect.roll,Eur.roll);
}
/*******************************************************************************
 * fuction	atititude_inner_loop_control
 * brief	姿态内环的控制
 * param	无
 * return	无
 *******************************************************************************/ 
void atititude_inner_loop_control(void)
{
	/*  有翻跟头指令*/
	if(filp_flag){
		/*判断哪个方向*/
		switch(filp_dir){
			/*往前翻*/
		case 1:
		
		/*pitch 俯仰角PID的控制  */
	pitchRate_out = pid_gyro_control(&PitchRate,900*(3.8f/bat_voltage),Gry_FeedBack_F.Y);
	/*roll 翻滚角PID的控制  */
	rollRate_out = pid_gyro_control(&RollRate,0,Gry_FeedBack_F.X);
	/*yaw 航向角PID的控制  */
	YawRate_out = pid_gyro_control(&YawRate,Expect.yaw,Gry_FeedBack_F.Z);
		
		break;
		
		/*往后翻*/
		case 2:
			
		/*pitch 俯仰角PID的控制  */
	pitchRate_out = pid_gyro_control(&PitchRate,-900*(3.8f/bat_voltage),Gry_FeedBack_F.Y);
	/*roll 翻滚角PID的控制  */
	rollRate_out = pid_gyro_control(&RollRate,0,Gry_FeedBack_F.X);
	/*yaw 航向角PID的控制  */
	YawRate_out = pid_gyro_control(&YawRate,Expect.yaw,Gry_FeedBack_F.Z);
		break;
		
		/*往右翻*/
		case 3:
		
		/*pitch 俯仰角PID的控制  */
	pitchRate_out = pid_gyro_control(&PitchRate,0,Gry_FeedBack_F.Y);
	/*roll 翻滚角PID的控制  */
	rollRate_out = pid_gyro_control(&RollRate,800*(3.8f/bat_voltage),Gry_FeedBack_F.X);
	/*yaw 航向角PID的控制  */
	YawRate_out = pid_gyro_control(&YawRate,Expect.yaw,Gry_FeedBack_F.Z);
		break;
		
		/*往左翻*/
		case 4:
		
		/*pitch 俯仰角PID的控制  */
	pitchRate_out = pid_gyro_control(&PitchRate,0,Gry_FeedBack_F.Y);
	/*roll 翻滚角PID的控制  */
	rollRate_out = pid_gyro_control(&RollRate,-800*(3.8f/bat_voltage),Gry_FeedBack_F.X);
	/*yaw 航向角PID的控制  */
	YawRate_out = pid_gyro_control(&YawRate,Expect.yaw,Gry_FeedBack_F.Z);
		break;
		/*   没有任何方向翻跟头指令*/
		default: 
				/*pitch 俯仰角PID的控制  */
			pitchRate_out = pid_gyro_control(&PitchRate,pitch_out,Gry_FeedBack_F.Y);
			/*roll 翻滚角PID的控制  */
			rollRate_out = pid_gyro_control(&RollRate,roll_out,Gry_FeedBack_F.X);
			/*yaw 航向角PID的控制  */
			YawRate_out = pid_gyro_control(&YawRate,Expect.yaw,Gry_FeedBack_F.Z);
			break;
		
	}	
	}
	else {
			/*pitch 俯仰角PID的控制  */
			pitchRate_out = pid_gyro_control(&PitchRate,pitch_out,Gry_FeedBack_F.Y);
			/*roll 翻滚角PID的控制  */
			rollRate_out = pid_gyro_control(&RollRate,roll_out,Gry_FeedBack_F.X);
			/*yaw 航向角PID的控制  */
			YawRate_out = pid_gyro_control(&YawRate,Expect.yaw,Gry_FeedBack_F.Z);	
		
	}
	/*起飞前清除积分项 */
	if(Fly_State != FlyStart){
		RollRate.Integral = 0.0f;
		PitchRate.Integral = 0.0f;
		YawRate.Integral = 0.0f;
	}
}
/*******************************************************************************
 * fuction	motor_control_output
 * brief	电机控制函数
 * param	无
 * return	无
 *******************************************************************************/ 
void motor_control_output(void)
{
	P_motoOut = pitchRate_out;
	R_motoOut = rollRate_out;
	Y_motoOut = YawRate_out;
	
	/*为了让电机转速不变，需要进行电压补偿 */
	motor_throttle = throttle*(4.2f/bat_voltage); 
	/*计算PWM的输出 */
	PWM1=motor_throttle+P_motoOut+R_motoOut+Y_motoOut;
	PWM2=motor_throttle-P_motoOut-R_motoOut+Y_motoOut;
	PWM3=motor_throttle-P_motoOut+R_motoOut-Y_motoOut;
	PWM4=motor_throttle+P_motoOut-R_motoOut-Y_motoOut;
	/*怠机时给的PWM */
	if(Fly_State == FlyIdle)
	{
		motor_pwm_set(PWM_ldle,PWM_ldle,PWM_ldle,PWM_ldle );
		IdleAccMod = get_accmod();
	}
	/*起飞机时给的PWM */
	else if(Fly_State == FlyStart)
	{
		motor_pwm_set(PWM3,PWM1,PWM4,PWM2 );
	}
	else  
		motor_pwm_set(PWM_MIN,PWM_MIN,PWM_MIN,PWM_MIN );
}
/*******************************************************************************
 * fuction	safe_control
 * brief	安全控制
 * param	无
 * return	无
 *******************************************************************************/ 
void safe_control(void)
{
	//  做保护需要借助标志位   需要借助计时器
	/* 电池低于3V保护 */
	if(bat_voltage<3.2f)
	 bat_low_cnt++;
	/* 计数100次之后置位低电压标志位 */
	if( bat_low_cnt>100)
	bat_low_flag = 1;
	
	/* 遥控器丢信号  判断：如果一段之间之后，数据未发生改变，则判断为丢信号 */
	if(bind_count == pre_bind_count){
		if(lost_signal_cnt==100){
			lost_signal_cnt = 100;
		}
		else 
		  lost_signal_cnt++;	
		
	}
	else 
	   lost_signal_cnt = 0;
	
	if(lost_signal_cnt>25)
		lost_signal_flag = 1;
	else 
		lost_signal_flag = 0;
	pre_bind_count = bind_count;
	
	
	/* 落地检测  在静止的时候求加速度值的模，标记一下，当飞机缓慢下降的时候加速度的模也接近*/
	/* 静止时候的模，求加速度计的模   1. 起飞的时候油门值大于30 下降时<30 则认为降落*/
	//get_accmod(void)
	if(fabs(IdleAccMod-get_accmod())<1.8f&&throttle<30.0f&&Fly_State == FlyStart)
		land_check_cnt++;
	else 
		land_check_cnt = 0;
	//ACC_F.Z
	if(land_check_cnt>25||(ACC_F.Z>20.0f&&Fly_State == FlyStart))
	  Fly_State = FlyIdle;
	
	/* 低电压保护 减油门降落 缓冲 软着落*/
	if(bat_low_flag==1){
		if(throttle>380)
			throttle=380;
		if(ACC_F.Z>15.0f)
			Fly_State = FlyStop;
		
	}
	/* 丢信号保护 */
	else if(lost_signal_flag==1){
		start_input_once_flag = false;
		Expect.pitch = 0.0f;
		Expect.roll = 0.0f;
		Expect.yaw = 0.0f;
		
		/* 减油门降落 缓冲 软着落 */
		if(throttle>380)
			throttle=380;
		if(ACC_F.Z>15.0f)
			Fly_State = FlyStop;		
	}
}
/*******************************************************************************
 * fuction	flip_control
 * brief	翻跟头控制
 * param	无
 * return	无
 *******************************************************************************/ 
void flip_control(void)
{
	if(filp_state==0){
		if(filp_dir!=0){
			
			filp_time++;
			if(filp_time>=210){       //  翻越成功之后的过度阶段 
				IMU_kp = 6.6f;
				filp_flag = false; 
				if(filp_time>=280){			//  切回正常飞行模式
					filp_state=1;
					filp_time = 0;
					RollRate.Integral = 0.0f;
					PitchRate.Integral = 0.0f;
					YawRate.Integral = 0.0f;
					IMU_kp = 0.6f;
					Fly_Mode = FLYMODE_6AXIE;	

				}
				else 
				throttle = 950;
			}
			else{   //  630ms
			
				if(filp_time<85)    //255ms
				throttle = 950;     //弹射255ms
				else if(filp_time>85&&filp_time<100)
				throttle = 0;				//惯性上升45ms
			  else {							//翻跟头
					throttle = 0;
					filp_flag = true; 
					IMU_kp = 6.6f;
					
				}			
			}			
		}	
	}	
}
/*******************************************************************************
 * fuction	system_task
 * brief	系统任务函数
 * param	无
 * return	无
 *******************************************************************************/ 
void system_task(void)
{
	if(init_success==true)
	{
		Systime_cnt++;
		/* 5毫秒获取一次遥控器数据 */
		if(Systime_cnt%5 ==0)
		{
			nrf51822_get_data();		
		}
		/* 1毫秒获取一次MPU6500数据 */
		mpu6500_read();
		Cal_Mpu_Data();
		/* 5毫秒获取更新四元数获取欧拉角 */
		if(Systime_cnt%5 ==0)
		{		
			if(Fly_State!=FlyStart&&GRYStable==true)
			{
				IMU_kp = 10.0f;			
			}
			else  
				IMU_kp = 0.6f;	
			UpdateQ_GetEurAngle(&Gry_F,&ACC_F,0.005f);
		}
		//atititude_Outer_loop_control();
		if(Systime_cnt%5 ==0)
		{
			atititude_Outer_loop_control();
		}
		if(Systime_cnt%3 ==0)
		{
			flip_control();
		}
		atititude_inner_loop_control();
		motor_control_output();
		if(Systime_cnt%5 ==0)
		{
			led_bling();
		}
		/* 安全保护 */
		if(Systime_cnt == 20)
		{
			Systime_cnt = 0;
			safe_control();
		}
	}
}

#ifndef _SYSTEM_CONTROL_H__
#define _SYSTEM_CONTROL_H__

#include "board_define.h"
#include "var_global.h"
void system_task(void);

#endif
上一篇：Proj THUDBFuzz Paper Reading: NAUTILUS


下一篇：【DB笔试面试642】在Oracle中，什么是基数反馈（Cardinality Feedback）？