标题: 基于STM32的四轴无人机源代码(PID算法、姿态解算、卡尔曼滤波与OLED显示) [打印本页]
作者: gaohan130    时间: 2018-5-11 15:08
标题: 基于STM32的四轴无人机源代码(PID算法、姿态解算、卡尔曼滤波与OLED显示)
此代码支持STM32所有系列，实现的功能有：OLED显示、PID算法、姿态解算、卡尔曼滤波算法对数据整合、MPU6050、HMC磁力计、MS5611气压计、NRF24L01、GPS定位、超声波定高，亲测可以用，喜欢研究无人机的朋友可以参考一下此代码
使用keli5编程

本代码纯属作者周寒宇个人编写，虽然有参考开源的代码，但是移植也很辛苦
希望参考学习的同学能够学到内容和代码风格
本代码支持STM32所有系列
参考来源：匿名科创、智能车部分代码、正点原子以及一些其他的开源代码
功能：OLED显示、PID算法、姿态解算、卡尔曼滤波算法对数据整合、MPU6050、HMC磁力计、MS5611气压计、
NRF24L01、GPS定位、超声波定高

后期更新部分：光流传感器视觉定位（加强室内无外部信号情况下的悬停功能）、摄像头图像采集。

有同学优化了代码的请发我一份


单片机源程序如下:
  1. /**************************
  2. @作者：周寒宇
  3. @大学：中南大学
  4. @目的：自制飞控板飞控代码
  5. @效果：稳定
  6. ************************/
  7. #include "sys.h"
  8. #include "delay.h"
  9. #include "usart.h"
  10. #include "mpu6050.h"
  11. #include "hmc5883l.h"
  12. #include "timer.h"
  13. #include "led.h"
  14. #include "i2c.h"
  15. #include "Moto.h"
  16. #include "Control.h"
  17. #include "PWM_Capture.h"
  18. #include "ms5611.h"
  19. #include "nrf24l01.h"
  20. #include "hcsr04.h"
  21. #include "beep.h"
  22. #include "GPS.h"
  23. #include "oled.h"
  24. extern uint32_t time3_cnt;
  25. extern u8 FlyMode;
  26. extern uint8_t Attitude;


  29. int main(void)
  30. {       

  32.         u8 i;
  33.         delay_init();                     //延时函数初始化          
  34.         NVIC_Configuration();          //设置NVIC中断分组3:3位抢占优先级，3位响应优先级
  35.         Control_Init();                                                                                                                                                                                                                   
  36.         IIC_Init();
  37.         LED_Init();
  38.         PWM_Capture_Init(0xFFFF,71);  //1MHZ计数
  39.         delay_ms(100);           //上电延时
  40.         MPU6050_Init();
  41.         HMC5883L_Init();
  42.         HCSR04_Init();
  43.         // GPS_Init();
  44.          //UART3_Init(9600);
  45.          //OLED_Init();
  46.         TIM2_PWM_Init(1999,71); //周期2MS频率500HZ 计数频率1MHZ         计数到20000为20MS
  47.         TIM5_Int_Init(499,71);//1Mhz的计数频率，计数到500为0.5ms
  48.         TIM7_Int_Init(0xFFFF,71);//1Mhz的计数频率,1us时间度量
  49.         TIM3_Int_Init(499,71);
  50.         for(i=0;i<6;i++)        delay_ms(1000);                 //等待姿态稳定
  51.         Moto_Init();
  52.         delay_ms(1000);       
  53.         delay_ms(1000);       
  54.         delay_ms(1000);       
  55.         delay_ms(1000);       
  56.         delay_ms(1000);       
  57.         while(1)
  58.         {
  59.                 if(time3_cnt<=2000)//10sec
  60.                 {                 HCSR04_Run(); //更新频率30HZ
  61.                                 Get_Attitude();                //姿态计算
  62.                                 FlyMode=1;
  63.                                 Fly_Control();//阻尼爬升                               
  64.                
  65.                 }
  66.                 if(2000<time3_cnt&&time3_cnt<=6000)//10sec-30sec
  67.                 {                   HCSR04_Run(); //更新频率30HZ
  68.                                 Get_Attitude();                //姿态计算
  69.                                 FlyMode=2;//飞行                       
  70.                                 Fly_Control();                               
  71.                 }
  72.                 if(6000<time3_cnt&&time3_cnt<=8000)//30sec-40sec
  73.                 {                HCSR04_Run(); //更新频率30HZ
  74.                                 Get_Attitude();                //姿态计算
  75.                                 FlyMode=3;//飞行                       
  76.                                 Fly_Control();                               
  77.                 }
  78.                   break;
  79.        
  80.         }
  81. }


复制代码

所有资料51hei提供下载:
四轴飞控代码(周寒宇).rar (438.35 KB, 下载次数: 1322)


作者: 冬天的野狼    时间: 2018-5-13 17:09
很牛了，有没有电路图等资料呢？如果能开源那可以分享共同提高
作者: P小四哥    时间: 2018-8-15 21:34
感谢楼主
作者: DylanMingan    时间: 2018-8-19 23:13
太感谢lge
作者: 123DUAN    时间: 2018-8-20 15:51
新人拜访~可惜黑币不够啊
作者: NPC-1024    时间: 2018-8-20 18:01
感谢，对飞控有兴趣，下载来看看。
作者: xode    时间: 2018-8-21 17:47
谢谢分享
作者: zxgpy    时间: 2018-8-22 16:14
谢谢大神分享！！！
作者: kanwoe    时间: 2018-8-27 12:44
感谢楼主的分享！
作者: JIANGJING520    时间: 2018-11-30 17:11
感谢感谢，最近在研究这个
作者: feiyi26    时间: 2018-12-4 19:53
感谢感谢，我也在研究这个
作者: feiyi26    时间: 2018-12-4 20:16
有没有电路图等资料呢
作者: 1113634577    时间: 2018-12-27 18:02
这莫大程序工程，是很繁琐辛苦
作者: 610930444    时间: 2019-1-3 09:22
谢谢分享
作者: Ye_tian    时间: 2019-1-7 19:43
感觉很牛
作者: zhlip    时间: 2019-1-7 21:53
不错
作者: 1539703473    时间: 2019-3-13 10:44
多谢，6666
作者: 丛林狼    时间: 2019-4-24 00:27
谢谢分享
作者: sjh7366    时间: 2019-4-24 18:55
感谢楼主
作者: Gerald-C    时间: 2019-4-25 07:14
感谢楼主，好人一生平安
作者: tarchen    时间: 2019-4-27 10:05
高手，大大的支持。
作者: private80    时间: 2019-6-11 00:50
感谢版主的无私，本人学习了
作者: get    时间: 2019-6-12 20:14
真的非常感谢
作者: 初出茅庐111    时间: 2019-6-14 09:36
非常感谢分享
作者: 田裕中    时间: 2019-6-14 19:09
有没有PCB呢？大神
作者: cat9975    时间: 2019-6-15 11:06
感谢楼主的分享！
作者: QSM980405    时间: 2019-6-16 15:32
感谢楼主的分享。
作者: 班班班    时间: 2019-6-17 12:25
冬天的野狼 发表于 2018-5-13 17:09
很牛了，有没有电路图等资料呢？如果能开源那可以分享共同提高

你好，这篇帖子里的程序可以分享一下吗
作者: mcu_mpu    时间: 2019-6-24 16:05
好东西，想完下无人机。
作者: Flyyyy    时间: 2019-7-11 20:06
感谢楼主分享
作者: ailinyi    时间: 2019-7-12 17:15
很好的例子
作者: snf    时间: 2019-7-14 16:48
谢谢分享
作者: 小苏ka    时间: 2019-7-20 19:12
感谢分享！！
作者: 鹏博士PBs    时间: 2019-7-21 10:15
可以分享一下电路图吗 或者PCB板图之类的也行啊
作者: wyman188    时间: 2019-7-25 14:45
谢谢分享！！！
作者: qq646926493    时间: 2019-8-6 22:19
谢谢分享~~~
作者: cl813814519    时间: 2019-8-9 10:48
感谢作者分享
作者: FMCH    时间: 2019-10-19 09:26
很全面，感谢分享
作者: iiia    时间: 2019-11-11 16:53
很想看一看楼主的理解
作者: 384448010    时间: 2019-11-29 18:54
有没有遥控的啊
作者: incendiary    时间: 2019-12-6 16:55
谢谢分享
作者: F-8    时间: 2020-3-21 19:49
谢谢楼主的分享
作者: wenzedong    时间: 2020-3-26 10:36
这无人机真是够难学的，我就简单分析试着了解一下就用了一天半的时间，同样是从工作原理，系统结构，程序的设计思想，具体的程序设计思路四个方面试图了解一下。首先说明一下，只是我的理解，和楼主的代码实现流程是不一样的。

飞行原理：转速差会推动其向一个方向飞行，有四种飞行方式：上下飞行（垂直运动，如悬停状态时，4个电机同时提高转速，就会升高），前后飞行（俯仰运动，如后方电机转速大于前方，就会往前飞），左右飞行（滚转运动，如左侧电机转速大于右侧，就会往右飞），左右转向（偏航运动，如逆时针转动的M1,M3转速大于顺时针转动的M2,M4，就会右转向）。
四轴飞行为了抵消螺旋桨的自旋，相隔的浆旋转方向是不一样的，适合顺时针转的叫正桨，适合逆时针转的是反桨，正反桨的风都是往下吹的。
有两种飞行姿态，+型和X型，区别是：①电机摆放角度不同（将+型四个电机顺时针旋转45°就是X型），②“前后/左右”飞行时需控制的电机数量不同（+型前后左右每个方位相当于只有一个电机，X型前后左右每个方位相当于都是两个电机）X型对于飞行姿态控制更好。

可能表述不够清楚，可以结合下面这篇文章理解
https://blog.csdn.net/qq_27270029/article/details/79877022
没玩过四旋翼，看看这个文章多少能理解些概念
https://blog.csdn.net/msdnwolaile/article/details/51525245

系统结构：遥控器，飞行器。
遥控器：采集（按键，油门杆，方向杆的动作）控制（数据传输 和OLED显示）
飞行器：采集（MPU5060加速度，陀螺仪，HMC磁力定方位（飞行姿态感测），超声波定高，MS5611气压定高（两者互补），GPS定位点（返回点定位），NRF24L01遥控数据通信）处理（PID算法，姿态解算，卡尔曼滤波算法对数据整合）控制（PID控制算法，控制电调来控制电机转速）
https://v.youku.com/v_show/id_XM ... t.13141534.2_1.d1_2

程序设计思想：
从结果往过程去分析。使用角度：当按顺序打开遥控器和飞行器的电源后，先进行对频（NRF24L01的2.4GHz数据通信（遥控器的指令，飞行器的数据），遥控和飞行器的配对）然后地磁校准（定方位，为了前后左右飞行），再GPS收星（定位点，让飞行器自动返航），再解锁电机，缓推油门（否则好像可能会炸机），使它起飞，然后就是各种操作了。
在飞行过程中，需要融合三轴加速度计和三轴陀螺仪（MPU5060）及磁力计(HMC5883L)的数据进行姿态解算（此时会用到卡尔曼滤波算法），陀螺仪知道“我们转了个身”，加速计知道“我们又向前走了几米”，而磁力计知道“我们是向西方向”的。 然后通过PID控制算法（PID飞行控制算法能有效对飞行器的姿态和位置跟踪进行有效控制），PWM波输出，通过‘电调（电机驱动模块）’来控制电机转速。
高度在3米内，使用超声波(HCSR04)测高，否则用气压计（MS5611）测高，OLED在遥控器上，可能会显示飞行高度。

具体程序设计思路：
首先，关于NRF24L01, OLED,BEEP都没有用到（通过查看初始化函数），也就表示没有遥控器的功能，GPS没有用到，说明就是没有返航功能。没有遥控器控制，就是能飞起来，然后应该对飞行器的飞行姿态来说好控制了很多。功能就是围绕姿态解算和PID飞行控制为核心的，飞个几十秒后落下。关于楼主的代码，没有功能说明注释，没有实现步骤注释，没有使用单片机某些资源的作用注释，总的来说，注释太少，再加上本人水平不高，看得一头雾水。
关于为什么楼主介绍的和代码不一致，我的理解是这样的：楼主完成此项目用时较长，且没有使用版本管理，每过一段时间就保存一个版本，最后都不知道哪个是最新版本，每个版本都实现了哪些细节了，因为疫情，再次接触的时候都过了快两个月了，虽然自己写的但都快忘差不多了，不过好在主要实现的函数都在main里，查看main文件修改日期确定哪个是最新版本，再加上注释还算详尽，补救了回来。

两点注释：
卡尔曼滤波：需要对陀螺仪和加速度传感器所测得的数据进行综合和校正，而均值滤波，很难满足精度和实时性上的要求，通过卡尔曼滤波将加速度传感器和陀螺仪的数据进行融合，在计算实时姿态时很好地抑制了噪声的干扰作用，以提高测量精度。

PID飞行控制算法：对四旋翼的控制包括姿态控制（内环控制）和位置控制（外环控制），外环控制，解算出为了到达预定的位置期望的姿态角，该期望的姿态角作为内环控制器的输入。PID飞行控制算法能有效对飞行器的姿态和位置跟踪进行有效控制。
作者: weskyfly    时间: 2020-4-2 17:54
代码很不错，多谢楼主分享。
作者: QEQWE123    时间: 2020-5-21 14:26
有原理图PCB吗大佬
作者: gameguy    时间: 2020-5-28 15:36
感謝分享
作者: 1804889557    时间: 2020-9-18 12:33
有个毛的卡尔曼，别一天到晚扯淡好吧
作者: 流浪深圳    时间: 2020-9-22 10:02
能全部整合，牛逼啊
作者: kiccleaf    时间: 2020-11-22 11:40
分享快乐，学习一下源码
作者: ＩＩＴＳＵＫＩ    时间: 2020-12-4 20:04
刚刚看了六轴的，不懂来看一下四轴的
作者: iYoutudou    时间: 2021-2-4 12:29
同求电路原理图，我准备自己再做一版stm32
作者: gmlipv123    时间: 2021-10-16 10:48
最近在研究无人机控制算法优化，希望能和各位前辈学习学习。



欢迎光临 (http://www.51hei.com/bbs/) Powered by Discuz! X3.1