競(jìng)賽無(wú)人機(jī)搭積木式編程（三）---用戶自定義航點(diǎn)自動(dòng)飛行功能（全局定位，指哪打哪）

無(wú)名小哥 2023年6月10日

用戶通過(guò)對(duì)前面兩講中全國(guó)大學(xué)生電子設(shè)計(jì)競(jìng)賽真題植保無(wú)人機(jī)（2021）、送貨無(wú)人機(jī)（2022）完整方案的學(xué)習(xí)。細(xì)心一點(diǎn)的客戶可以發(fā)現(xiàn)：在激光雷達(dá)SLAM/T265雙目相機(jī)提供全局定位數(shù)據(jù)的情況下，無(wú)人機(jī)的自主飛行部分的程序設(shè)計(jì)，基本都是通過(guò)飛控代碼二次開發(fā)模式中已有的飛行控制API函數(shù)，即自動(dòng)飛行支持函數(shù)和導(dǎo)航控制函數(shù)實(shí)現(xiàn)。

同時(shí)需要結(jié)合底部/前向的機(jī)器視覺(jué)傳感器以及激光雷達(dá)傳感器對(duì)目標(biāo)特征進(jìn)行視覺(jué)/距離定位。比如送貨無(wú)人機(jī)賽題中，達(dá)到目標(biāo)航點(diǎn)上方后，通過(guò)視覺(jué)實(shí)現(xiàn)無(wú)人機(jī)的精準(zhǔn)目標(biāo)定位。通過(guò)視覺(jué)實(shí)現(xiàn)無(wú)人機(jī)位置的二次對(duì)準(zhǔn)，其實(shí)就是用到的前些年賽題中的追蹤移動(dòng)色塊的功能；

另外在植保無(wú)人機(jī)發(fā)揮部分要求中，需要識(shí)別到塔桿和條形碼信息。通過(guò)激光雷達(dá)傳感器識(shí)別道塔桿的水平位置、相對(duì)無(wú)人機(jī)機(jī)頭方向的角度，對(duì)無(wú)人機(jī)偏航方向和機(jī)頭與桿之間的距離進(jìn)行控制，進(jìn)而實(shí)現(xiàn)機(jī)頭對(duì)準(zhǔn)塔桿并調(diào)整與桿之間的間距并通過(guò)視覺(jué)識(shí)別特征這一復(fù)合的運(yùn)動(dòng)，這里用的就是基礎(chǔ)飛行控制函數(shù)中的偏航控制API和速度控制API函數(shù)予以實(shí)現(xiàn)。

為了方便萌新用戶針對(duì)具體競(jìng)賽內(nèi)容實(shí)現(xiàn)快速的二次開發(fā)，我們新增加了用戶自定義航點(diǎn)飛行功能，用戶可以不需要自己編程去改飛控代碼，就可以實(shí)現(xiàn)航點(diǎn)參數(shù)的錄入，無(wú)人機(jī)能按照用戶錄入的航點(diǎn)動(dòng)作進(jìn)行自主飛行。這里需要注意的是默認(rèn)提供的自定義航點(diǎn)自動(dòng)飛行函數(shù)中飛行動(dòng)作只是對(duì)航點(diǎn)的依次遍歷，尚不涉及中間動(dòng)作，比如需要結(jié)合視覺(jué)/激光雷達(dá)進(jìn)行視覺(jué)定位、距離定位。

用戶自定義航點(diǎn)自動(dòng)飛行功能提供的是一個(gè)精簡(jiǎn)化的自主飛行框架，用戶可以快速實(shí)現(xiàn)飛行動(dòng)作的修改和編排。針對(duì)某一比賽任務(wù)設(shè)計(jì)中航點(diǎn)飛行任務(wù)部分用戶可以無(wú)需重復(fù)設(shè)計(jì)，直接參考本框架就可以輕松實(shí)現(xiàn)多航點(diǎn)目標(biāo)自動(dòng)飛行任務(wù)，留足更多的時(shí)間去著手機(jī)器視覺(jué)部分和更為精細(xì)化的任務(wù)設(shè)計(jì)。另外電賽國(guó)賽測(cè)評(píng)中要求參賽者去現(xiàn)場(chǎng)編程，快速實(shí)現(xiàn)某一飛行動(dòng)作要求變得不再有任何壓力。

12_用戶通過(guò)ADC按鍵錄入自定義航點(diǎn)飛行功能——支持現(xiàn)場(chǎng)設(shè)置坐標(biāo)參數(shù)（全局定位，指哪打哪）

演示視頻：https://www.bilibili.com/video/BV1wP411z7jo/

1 五向按鍵的檢測(cè)設(shè)計(jì)與航點(diǎn)參數(shù)調(diào)節(jié)界面設(shè)計(jì)

根據(jù)用戶需要現(xiàn)場(chǎng)高效率的錄入航點(diǎn)參數(shù)的這一具體要求，我們將上一版本ADC安全繩按鍵進(jìn)行了升級(jí)，在保留原有解鎖、開發(fā)、上鎖、降落四個(gè)獨(dú)立按鍵的功能的同時(shí)，新增加了一路獨(dú)立的五向按鍵采集ADC通道，一個(gè)IO口實(shí)現(xiàn)了5個(gè)方向按鍵的檢測(cè)，并可配備有短按、長(zhǎng)按、持續(xù)按、多次點(diǎn)動(dòng)觸發(fā)等，進(jìn)一步豐富了按鍵鍵值。五向按鍵配合擴(kuò)展版上OLED顯示屏可方便實(shí)現(xiàn)多個(gè)參數(shù)的修改與保存。

1.1ADC按鍵檢測(cè)原理

ADC按鍵的檢測(cè)原理為每個(gè)按鍵串入不同的電阻值，按鍵按下后偏置電壓經(jīng)過(guò)電阻分壓作用后，由單片機(jī)的ADC端口采集，飛控程序可以通過(guò)采集到的ADC值在某個(gè)區(qū)間范圍來(lái)判斷是哪個(gè)按鍵按下，并對(duì)按下的持續(xù)時(shí)間、次數(shù)燈進(jìn)行邏輯處理，實(shí)現(xiàn)按鍵事件的響應(yīng)。這部分原理以往教程有詳細(xì)的講解和分析，不屬于本文重點(diǎn)，這里不再展開，用戶可以參照以下鏈接自行學(xué)習(xí)。

電賽飛行器安全繩+無(wú)遙控器按鍵控制方案?https://www.bilibili.com/read/CV11399668

電賽禁止“無(wú)線通訊及遙控”解決方案?https://www.bilibili.com/read/CV7897817

電賽神器ADC安全繩按鍵無(wú)遙控器控制方案https://www.bilibili.com/video/BV1Gq4y1E7XM

1.2 航點(diǎn)參數(shù)界面設(shè)計(jì)

OLED顯示屏參數(shù)顯示分為8行，第1行顯示內(nèi)容為頁(yè)面和頁(yè)碼提示，第2行至第8行數(shù)據(jù)為1-7個(gè)航點(diǎn)的ENU（等效東北天）方向的坐標(biāo)信息，其中EN方向的輸入?yún)?shù)表示相對(duì)初始基準(zhǔn)點(diǎn)的位置增/減量。它是一個(gè)相對(duì)位置坐標(biāo)參數(shù)，并非實(shí)際無(wú)人機(jī)內(nèi)部實(shí)時(shí)位置，二者在水平位置上相差一個(gè)初始基準(zhǔn)點(diǎn)的坐標(biāo)(base_position.x,base_position.y)。U方向輸入的是絕對(duì)坐標(biāo)信息，即無(wú)人機(jī)離地面的高度值。

五向按鍵的中的上下按鍵短按可以實(shí)現(xiàn)某一參數(shù)的選中，*提示光標(biāo)會(huì)移動(dòng)到待調(diào)整參數(shù)的前面。

五向按鍵的中的左右按鍵短按/長(zhǎng)按可以實(shí)現(xiàn)某一參數(shù)的選中的自加、自減，其中短按是自加/減1，長(zhǎng)按是自加/減50。

五向按鍵里面的中按鍵長(zhǎng)按可以實(shí)現(xiàn)當(dāng)前頁(yè)面所有參數(shù)寫入到EEPROM實(shí)現(xiàn)掉電存儲(chǔ)。

2 航點(diǎn)自動(dòng)飛行功能軟件實(shí)現(xiàn)

用戶錄入的航點(diǎn)參數(shù)存儲(chǔ)在飛控EEPROM內(nèi)，飛控每次上電時(shí)會(huì)從EEPROM中讀取航點(diǎn)參數(shù)并保存在param_value參數(shù)數(shù)組內(nèi)，第一個(gè)航點(diǎn)的參數(shù)存儲(chǔ)在該變量的第51維開始的三個(gè)變量?jī)?nèi)，航點(diǎn)生成函數(shù)的作用是判斷param_value數(shù)組航點(diǎn)字節(jié)段區(qū)間的數(shù)據(jù)是否全為0，來(lái)判斷航點(diǎn)數(shù)據(jù)是否有效。當(dāng)前能錄入的最大航點(diǎn)數(shù)量為28個(gè)，針對(duì)不同的賽題任務(wù)，客戶可以自己靈活調(diào)整。

2.1第一階段——自動(dòng)起飛到航巡高度方法uint8_t Auto_Takeoff(float target)

函數(shù)輸入?yún)?shù)target為目標(biāo)高度，自動(dòng)起飛任務(wù)分為兩個(gè)線程，第一步為記錄當(dāng)前3維位置信息，作為導(dǎo)航初始原點(diǎn)位置。并且通過(guò)導(dǎo)航控制函數(shù)設(shè)置期望目標(biāo)高度位置。第二步為實(shí)時(shí)檢測(cè)高度偏差值，連續(xù)2S滿足位置偏差在10cm以內(nèi)后，函數(shù)返回值置1后，自動(dòng)起飛到目標(biāo)高度任務(wù)完成，用法參照Developer_Mode.c開發(fā)者模式中case 18用法，自主起飛任務(wù)完成后會(huì)進(jìn)入case 19執(zhí)行航點(diǎn)自動(dòng)飛行功能。

2.2第二階段——自定義航點(diǎn)飛行Navigation_User_Setpoint(void)

第一步將高度期望設(shè)置成第一作業(yè)高度150cm，水平位置期望為初始起飛時(shí)候的水平位置，在起飛點(diǎn)上方懸停時(shí)間設(shè)置為1S，懸停時(shí)間可以根據(jù)實(shí)際需要調(diào)整。

起飛點(diǎn)上方懸停1S后，會(huì)根據(jù)按鍵錄入航點(diǎn)數(shù)據(jù)進(jìn)行判斷，如果航點(diǎn)數(shù)據(jù)有效則將此航點(diǎn)設(shè)置成目標(biāo)航點(diǎn)，并且線程計(jì)數(shù)器flight_subtask_cnt[n]會(huì)賦值為2，隨后進(jìn)入航點(diǎn)任務(wù)執(zhí)行線程。

在線程2中執(zhí)行航點(diǎn)飛行任務(wù)，并實(shí)時(shí)判斷無(wú)人機(jī)的三維位置偏差，連續(xù)N次滿足偏差小于某一閾值時(shí)，可以認(rèn)為當(dāng)前航點(diǎn)已抵達(dá)，繼續(xù)刷新下一航點(diǎn)信息。當(dāng)所有航點(diǎn)遍歷完畢后，跳到第3 線程中，結(jié)束整個(gè)航點(diǎn)遍歷過(guò)程。

最后執(zhí)行線程3中的原地下降任務(wù)，無(wú)人機(jī)降落到地面后會(huì)觸發(fā)地面檢測(cè)條件無(wú)人機(jī)會(huì)自動(dòng)上鎖，結(jié)束整個(gè)飛行過(guò)程。