基于STM32的四軸無(wú)人機(jī)源代碼(PID算法、姿態(tài)解算、卡爾曼濾波與OLED顯示)

ID:231984 · 發(fā)表于 2018-5-11 15:08

此代碼支持STM32所有系列，實(shí)現(xiàn)的功能有：OLED顯示、PID算法、姿態(tài)解算、卡爾曼濾波算法對(duì)數(shù)據(jù)整合、MPU6050、HMC磁力計(jì)、MS5611氣壓計(jì)、NRF24L01、GPS定位、超聲波定高，親測(cè)可以用，喜歡研究無(wú)人機(jī)的朋友可以參考一下此代碼
使用keli5編程

本代碼純屬作者周寒宇個(gè)人編寫(xiě)，雖然有參考開(kāi)源的代碼，但是移植也很辛苦
希望參考學(xué)習(xí)的同學(xué)能夠?qū)W到內(nèi)容和代碼風(fēng)格
本代碼支持STM32所有系列
參考來(lái)源：匿名科創(chuàng)、智能車部分代碼、正點(diǎn)原子以及一些其他的開(kāi)源代碼
功能：OLED顯示、PID算法、姿態(tài)解算、卡爾曼濾波算法對(duì)數(shù)據(jù)整合、MPU6050、HMC磁力計(jì)、MS5611氣壓計(jì)、
NRF24L01、GPS定位、超聲波定高

后期更新部分：光流傳感器視覺(jué)定位（加強(qiáng)室內(nèi)無(wú)外部信號(hào)情況下的懸停功能）、攝像頭圖像采集。

有同學(xué)優(yōu)化了代碼的請(qǐng)發(fā)我一份

單片機(jī)源程序如下:

/**************************
@作者：周寒宇
@大學(xué)：中南大學(xué)
@目的：自制飛控板飛控代碼
@效果：穩(wěn)定
************************/
#include "sys.h"
#include "delay.h"
#include "usart.h"
#include "mpu6050.h"
#include "hmc5883l.h"
#include "timer.h"
#include "led.h"
#include "i2c.h"
#include "Moto.h"
#include "Control.h"
#include "PWM_Capture.h"
#include "ms5611.h"
#include "nrf24l01.h"
#include "hcsr04.h"
#include "beep.h"
#include "GPS.h"
#include "oled.h"
extern uint32_t time3_cnt;
extern u8 FlyMode;
extern uint8_t Attitude;
int main(void)
{
u8 i;
delay_init(); //延時(shí)函數(shù)初始化
NVIC_Configuration(); //設(shè)置NVIC中斷分組3:3位搶占優(yōu)先級(jí)，3位響應(yīng)優(yōu)先級(jí)
Control_Init();
IIC_Init();
LED_Init();
PWM_Capture_Init(0xFFFF,71); //1MHZ計(jì)數(shù)
delay_ms(100); //上電延時(shí)
MPU6050_Init();
HMC5883L_Init();
HCSR04_Init();
// GPS_Init();
//UART3_Init(9600);
//OLED_Init();
TIM2_PWM_Init(1999,71); //周期2MS頻率500HZ 計(jì)數(shù)頻率1MHZ 計(jì)數(shù)到20000為20MS
TIM5_Int_Init(499,71);//1Mhz的計(jì)數(shù)頻率，計(jì)數(shù)到500為0.5ms
TIM7_Int_Init(0xFFFF,71);//1Mhz的計(jì)數(shù)頻率,1us時(shí)間度量
TIM3_Int_Init(499,71);
for(i=0;i<6;i++) delay_ms(1000); //等待姿態(tài)穩(wěn)定
Moto_Init();
delay_ms(1000);
delay_ms(1000);
delay_ms(1000);
delay_ms(1000);
delay_ms(1000);
while(1)
{
if(time3_cnt<=2000)//10sec
{ HCSR04_Run(); //更新頻率30HZ
Get_Attitude(); //姿態(tài)計(jì)算
FlyMode=1;
Fly_Control();//阻尼爬升
}
if(2000<time3_cnt&&time3_cnt<=6000)//10sec-30sec
{ HCSR04_Run(); //更新頻率30HZ
Get_Attitude(); //姿態(tài)計(jì)算
FlyMode=2;//飛行
Fly_Control();
}
if(6000<time3_cnt&&time3_cnt<=8000)//30sec-40sec
{ HCSR04_Run(); //更新頻率30HZ
Get_Attitude(); //姿態(tài)計(jì)算
FlyMode=3;//飛行
Fly_Control();
}
break;
}
}

復(fù)制代碼

所有資料51hei提供下載:

四軸飛控代碼(周寒宇).rar (438.35 KB, 下載次數(shù): 1322)

ID:319344 · 發(fā)表于 2020-3-26 10:36

這無(wú)人機(jī)真是夠難學(xué)的，我就簡(jiǎn)單分析試著了解一下就用了一天半的時(shí)間，同樣是從工作原理，系統(tǒng)結(jié)構(gòu)，程序的設(shè)計(jì)思想，具體的程序設(shè)計(jì)思路四個(gè)方面試圖了解一下。首先說(shuō)明一下，只是我的理解，和樓主的代碼實(shí)現(xiàn)流程是不一樣的。

飛行原理：轉(zhuǎn)速差會(huì)推動(dòng)其向一個(gè)方向飛行，有四種飛行方式：上下飛行（垂直運(yùn)動(dòng)，如懸停狀態(tài)時(shí)，4個(gè)電機(jī)同時(shí)提高轉(zhuǎn)速，就會(huì)升高），前后飛行（俯仰運(yùn)動(dòng)，如后方電機(jī)轉(zhuǎn)速大于前方，就會(huì)往前飛），左右飛行（滾轉(zhuǎn)運(yùn)動(dòng)，如左側(cè)電機(jī)轉(zhuǎn)速大于右側(cè)，就會(huì)往右飛），左右轉(zhuǎn)向（偏航運(yùn)動(dòng)，如逆時(shí)針轉(zhuǎn)動(dòng)的M1,M3轉(zhuǎn)速大于順時(shí)針轉(zhuǎn)動(dòng)的M2,M4，就會(huì)右轉(zhuǎn)向）。
四軸飛行為了抵消螺旋槳的自旋，相隔的漿旋轉(zhuǎn)方向是不一樣的，適合順時(shí)針轉(zhuǎn)的叫正槳，適合逆時(shí)針轉(zhuǎn)的是反槳，正反槳的風(fēng)都是往下吹的。
有兩種飛行姿態(tài)，+型和X型，區(qū)別是：①電機(jī)擺放角度不同（將+型四個(gè)電機(jī)順時(shí)針旋轉(zhuǎn)45°就是X型），②“前后/左右”飛行時(shí)需控制的電機(jī)數(shù)量不同（+型前后左右每個(gè)方位相當(dāng)于只有一個(gè)電機(jī)，X型前后左右每個(gè)方位相當(dāng)于都是兩個(gè)電機(jī)）X型對(duì)于飛行姿態(tài)控制更好。

可能表述不夠清楚，可以結(jié)合下面這篇文章理解
https://blog.csdn.net/qq_27270029/article/details/79877022
沒(méi)玩過(guò)四旋翼，看看這個(gè)文章多少能理解些概念
https://blog.csdn.net/msdnwolaile/article/details/51525245

系統(tǒng)結(jié)構(gòu)：遙控器，飛行器。
遙控器：采集（按鍵，油門(mén)桿，方向桿的動(dòng)作）控制（數(shù)據(jù)傳輸和OLED顯示）
飛行器：采集（MPU5060加速度，陀螺儀，HMC磁力定方位（飛行姿態(tài)感測(cè)），超聲波定高，MS5611氣壓定高（兩者互補(bǔ)），GPS定位點(diǎn)（返回點(diǎn)定位），NRF24L01遙控?cái)?shù)據(jù)通信）處理（PID算法，姿態(tài)解算，卡爾曼濾波算法對(duì)數(shù)據(jù)整合）控制（PID控制算法，控制電調(diào)來(lái)控制電機(jī)轉(zhuǎn)速）
https://v.youku.com/v_show/id_XM ... t.13141534.2_1.d1_2

程序設(shè)計(jì)思想：
從結(jié)果往過(guò)程去分析。使用角度：當(dāng)按順序打開(kāi)遙控器和飛行器的電源后，先進(jìn)行對(duì)頻（NRF24L01的2.4GHz數(shù)據(jù)通信（遙控器的指令，飛行器的數(shù)據(jù)），遙控和飛行器的配對(duì)）然后地磁校準(zhǔn)（定方位，為了前后左右飛行），再GPS收星（定位點(diǎn)，讓飛行器自動(dòng)返航），再解鎖電機(jī)，緩?fù)朴烷T(mén)（否則好像可能會(huì)炸機(jī)），使它起飛，然后就是各種操作了。
在飛行過(guò)程中，需要融合三軸加速度計(jì)和三軸陀螺儀（MPU5060）及磁力計(jì)(HMC5883L)的數(shù)據(jù)進(jìn)行姿態(tài)解算（此時(shí)會(huì)用到卡爾曼濾波算法），陀螺儀知道“我們轉(zhuǎn)了個(gè)身”，加速計(jì)知道“我們又向前走了幾米”，而磁力計(jì)知道“我們是向西方向”的。然后通過(guò)PID控制算法（PID飛行控制算法能有效對(duì)飛行器的姿態(tài)和位置跟蹤進(jìn)行有效控制），PWM波輸出，通過(guò)‘電調(diào)（電機(jī)驅(qū)動(dòng)模塊）’來(lái)控制電機(jī)轉(zhuǎn)速。
高度在3米內(nèi)，使用超聲波(HCSR04)測(cè)高，否則用氣壓計(jì)（MS5611）測(cè)高，OLED在遙控器上，可能會(huì)顯示飛行高度。

具體程序設(shè)計(jì)思路：
首先，關(guān)于NRF24L01, OLED,BEEP都沒(méi)有用到（通過(guò)查看初始化函數(shù)），也就表示沒(méi)有遙控器的功能，GPS沒(méi)有用到，說(shuō)明就是沒(méi)有返航功能。沒(méi)有遙控器控制，就是能飛起來(lái)，然后應(yīng)該對(duì)飛行器的飛行姿態(tài)來(lái)說(shuō)好控制了很多。功能就是圍繞姿態(tài)解算和PID飛行控制為核心的，飛個(gè)幾十秒后落下。關(guān)于樓主的代碼，沒(méi)有功能說(shuō)明注釋，沒(méi)有實(shí)現(xiàn)步驟注釋，沒(méi)有使用單片機(jī)某些資源的作用注釋，總的來(lái)說(shuō)，注釋太少，再加上本人水平不高，看得一頭霧水。
關(guān)于為什么樓主介紹的和代碼不一致，我的理解是這樣的：樓主完成此項(xiàng)目用時(shí)較長(zhǎng)，且沒(méi)有使用版本管理，每過(guò)一段時(shí)間就保存一個(gè)版本，最后都不知道哪個(gè)是最新版本，每個(gè)版本都實(shí)現(xiàn)了哪些細(xì)節(jié)了，因?yàn)橐咔椋俅谓佑|的時(shí)候都過(guò)了快兩個(gè)月了，雖然自己寫(xiě)的但都快忘差不多了，不過(guò)好在主要實(shí)現(xiàn)的函數(shù)都在main里，查看main文件修改日期確定哪個(gè)是最新版本，再加上注釋還算詳盡，補(bǔ)救了回來(lái)。

兩點(diǎn)注釋：
卡爾曼濾波：需要對(duì)陀螺儀和加速度傳感器所測(cè)得的數(shù)據(jù)進(jìn)行綜合和校正，而均值濾波，很難滿足精度和實(shí)時(shí)性上的要求，通過(guò)卡爾曼濾波將加速度傳感器和陀螺儀的數(shù)據(jù)進(jìn)行融合，在計(jì)算實(shí)時(shí)姿態(tài)時(shí)很好地抑制了噪聲的干擾作用，以提高測(cè)量精度。

PID飛行控制算法：對(duì)四旋翼的控制包括姿態(tài)控制（內(nèi)環(huán)控制）和位置控制（外環(huán)控制），外環(huán)控制，解算出為了到達(dá)預(yù)定的位置期望的姿態(tài)角，該期望的姿態(tài)角作為內(nèi)環(huán)控制器的輸入。PID飛行控制算法能有效對(duì)飛行器的姿態(tài)和位置跟蹤進(jìn)行有效控制。

ID:972494 · 發(fā)表于 2021-10-16 10:48

最近在研究無(wú)人機(jī)控制算法優(yōu)化，希望能和各位前輩學(xué)習(xí)學(xué)習(xí)。

ID:186696 · 發(fā)表于 2021-2-4 12:29

同求電路原理圖，我準(zhǔn)備自己再做一版stm32

ID:843473 · 發(fā)表于 2020-12-4 20:04

剛剛看了六軸的，不懂來(lái)看一下四軸的

ID:846386 · 發(fā)表于 2020-11-22 11:40

分享快樂(lè)，學(xué)習(xí)一下源碼

ID:819112 · 發(fā)表于 2020-9-22 10:02

能全部整合，牛逼啊

ID:555142 · 發(fā)表于 2020-9-18 12:33

有個(gè)毛的卡爾曼，別一天到晚扯淡好吧

ID:763457 · 發(fā)表于 2020-5-28 15:36

感謝分享

ID:316613 · 發(fā)表于 2020-5-21 14:26

有原理圖PCB嗎大佬

ID:538585 · 發(fā)表于 2020-4-2 17:54

代碼很不錯(cuò)，多謝樓主分享。

ID:713039 · 發(fā)表于 2020-3-21 19:49

謝謝樓主的分享

ID:656973 · 發(fā)表于 2019-12-6 16:55

謝謝分享

ID:399931 · 發(fā)表于 2019-11-29 18:54

有沒(méi)有遙控的啊

ID:639831 · 發(fā)表于 2019-11-11 16:53

很想看一看樓主的理解

ID:604530 · 發(fā)表于 2019-10-19 09:26

很全面，感謝分享

ID:513389 · 發(fā)表于 2019-8-9 10:48

感謝作者分享

ID:459450 · 發(fā)表于 2019-8-6 22:19

謝謝分享~~~

ID:590265 · 發(fā)表于 2019-7-25 14:45

謝謝分享

！！！

ID:282095 · 發(fā)表于 2019-7-21 10:15

可以分享一下電路圖嗎或者PCB板圖之類的也行啊

ID:529132 · 發(fā)表于 2019-7-20 19:12

感謝分享！！

ID:543162 · 發(fā)表于 2019-7-14 16:48

謝謝分享

ID:449111 · 發(fā)表于 2019-7-12 17:15

很好的例子

ID:392405 · 發(fā)表于 2019-7-11 20:06

感謝樓主分享

ID:324611 · 發(fā)表于 2019-6-24 16:05

好東西，想完下無(wú)人機(jī)。

ID:564433 · 發(fā)表于 2019-6-17 12:25

冬天的野狼發(fā)表于 2018-5-13 17:09
很牛了，有沒(méi)有電路圖等資料呢？如果能開(kāi)源那可以分享共同提高

你好，這篇帖子里的程序可以分享一下嗎

ID:563554 · 發(fā)表于 2019-6-16 15:32

感謝樓主的分享。

ID:561986 · 發(fā)表于 2019-6-15 11:06

感謝樓主的分享！

ID:394949 · 發(fā)表于 2019-6-14 19:09

有沒(méi)有PCB呢？大神

ID:562340 · 發(fā)表于 2019-6-14 09:36

非常感謝分享

ID:332931 · 發(fā)表于 2019-6-12 20:14

真的非常感謝

ID:108728 · 發(fā)表于 2019-6-11 00:50

感謝版主的無(wú)私，本人學(xué)習(xí)了

ID:228360 · 發(fā)表于 2019-4-27 10:05

高手，大大的支持。

ID:502122 · 發(fā)表于 2019-4-25 07:14

感謝樓主，好人一生平安

ID:349598 · 發(fā)表于 2019-4-24 18:55

感謝樓主

ID:516859 · 發(fā)表于 2019-4-24 00:27

謝謝分享

ID:429276 · 發(fā)表于 2019-3-13 10:44

多謝，6666

ID:318672 · 發(fā)表于 2019-1-7 21:53

不錯(cuò)

ID:464057 · 發(fā)表于 2019-1-7 19:43

感覺(jué)很牛

ID:437716 · 發(fā)表于 2019-1-3 09:22

謝謝分享

ID:371527 · 發(fā)表于 2018-12-27 18:02

這莫大程序工程，是很繁瑣辛苦

帳號(hào)		自動(dòng)登錄	找回密碼
密碼			立即注冊(cè)

亚洲春色中文字幕久久久-三上亚,一吻二脱三床四吻胸,国产真实伦对白视频全集,在线毛片观看,精品成品入口黄网,国产毛aⅴ片久久久,亚洲AV色香蕉一区二区三区老师,萧皇后A级艳片,色情日本视频更新,99久久亚洲精品日本无码

基于STM32的四軸無(wú)人機(jī)源代碼(PID算法、姿態(tài)解算、卡爾曼濾波與OLED顯示)

評(píng)分