ICode9
精准搜索请尝试： 精确搜索
首页
编程语言>
数据库
系统相关
互联网
其他分享
Python
Java
JavaScript
android
PHP
首页 > 其他分享> 文章详细

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

2021-02-18 23:57:04  阅读：216  来源： 互联网

标签：control FeedBack pid PID Gry 系统控制 无人机 out 旋翼


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

标签：control,FeedBack,pid,PID,Gry,系统控制,无人机,out,旋翼
来源： https://blog.csdn.net/FutureStudio1994/article/details/113855436

本站声明： 1. iCode9 技术分享网（下文简称本站）提供的所有内容，仅供技术学习、探讨和分享；
2. 关于本站的所有留言、评论、转载及引用，纯属内容发起人的个人观点，与本站观点和立场无关；
3. 关于本站的所有言论和文字，纯属内容发起人的个人观点，与本站观点和立场无关；
4. 本站文章均是网友提供，不完全保证技术分享内容的完整性、准确性、时效性、风险性和版权归属；如您发现该文章侵犯了您的权益，可联系我们第一时间进行删除；
5. 本站为非盈利性的个人网站，所有内容不会用来进行牟利，也不会利用任何形式的广告来间接获益，纯粹是为了广大技术爱好者提供技术内容和技术思想的分享性交流网站。

关于我们 | 联系我们 | 留言反馈

专注分享技术，共同学习，共同进步。侵权联系[81616952@qq.com]

Copyright (C)ICode9.com, All Rights Reserved.

ICode9版权所有