四旋翼无人机从0到1的实现（二十四）无人机工程中系统控制
作者：互联网
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