四旋翼无人机从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.Zif(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{   //  630msif(filp_time<85)    //255msthrottle = 950;     //弹射255mselse if(filp_time>85&&filp_time<100)throttle = 0;                //惯性上升45mselse {                            //翻跟头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

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

Android9.0 setStereoVolume左右声道流程(二十四)
栗子:setStereoVolume()控制左右声道流程. private AudioTrack mAudioTrack; int bufferSize = AudioTrack.getMinBuff ...
家乡二十四桥的介绍
二十四桥 "青山隐隐水迢迢,秋尽江南草未凋.二十四桥明月夜,玉人何处教吹箫".这首诗已流传了一千多年,可谓妇孺皆知.诗因桥而咏出,桥因诗而闻名.单项式桥名就引动多少文人学者打 ...
二十四孝，图文并茂，古今必读!
知道"二十四孝"的人,已经不多了:知道"二十四孝"的年轻人,更是少之又少."孝"在今日社会,似乎已是一个"过气"的词,人 ...
噢~已二十四了。。。
鱼跃龙门恨难敌,潜龙困兽终是迷.又是一篇章-- 二十四,已二十四了. 二十四,就这么二十四了. 二十四,我记住了很多. 二十四,我忘记了很多. 二十四,算是什么概念呢? ...
【Vue2.0】—默认插槽、具名插槽、作用域插槽（二十四）
[Vue2.0]-默认插槽.具名插槽.作用域插槽(二十四) 默认插槽.具名插槽 Cateory.vue文件 <template><div class="cateory&qu ...
【Vue2.0学习】—插槽（六十四）
[Vue2.0学习]-插槽(六十四) 作用:让父组件可以向子组件指定的位置插入html结构,也是一种组件间通信的方式,适用于父组件=>子组件分类:默认插槽.具名插槽.作用域插槽默认插槽使 ...
python 分数序列求和公式_Python分数序列求和，编程练习题实例二十四
本文是关于Python分数序列求和的应用练习,适合菜鸟练习使用,python大牛绕行哦. Python练习题问题如下: 问题简述:有一分数序列:2/1,3/2,5/3,8/5,13/8,21/13 要 ...
异常处理程序和软件异常——Windows核心编程学习手札之二十四
异常处理程序和软件异常 --Windows核心编程学习手札之二十四 CPU负责捕捉无效内存访问和用0除一个数值这种错误,并相应引发一个异常作为对错误的反应,CPU引发的异常称为硬件异常(hardwar ...
数字图像处理领域的二十四个典型算法及vc实现、第一章
数字图像处理领域的二十四个典型算法及vc实现.第一章作者:July 二零一一年二月二十六日. 参考:百度百科.维基百科.vc数字图像处理. --------------------------- ...
[系统安全] 二十四.逆向分析之OllyDbg调试INT3断点、反调试、硬件断点与内存断点
您可能之前看到过我写的类似文章,为什么还要重复撰写呢?只是想更好地帮助初学者了解病毒逆向分析和系统安全,更加成体系且不破坏之前的系列.因此,我重新开设了这个专栏,准备系统整理和深入学习系统安全.逆向分 ...

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

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

最新文章

热门文章