【系列專題】px4代碼解析(二):航向角到點判據

前言

上次在px4代碼解析（一）：航點位置到點判據中給大家介紹了px4在執行航線任務時是如何進行到點判斷的，航點到點判斷分成兩部分：位置到點判據和航向角到點判據。上篇文章主要是對位置到點判據進行了詳細描述，這篇文章主要對航向角到點判據進行詳細的解讀。

PX4中關於航點到點判斷中航向角到點判斷的內容也主要集中在navigator模塊的mission_block.cpp和mission.cpp兩個文件中。

在開始分析這些判斷邏輯之前，先明確兩個參數以及它的含義：

MIS_YAW_ERR：航點到點判據最大航向角誤差，默認12.0°；

NAV_ACC_RAD：多旋翼飛行器水平位置到點判據半徑，默認10m。

航向角期望計算

首先起飛階段航向角期望就是起飛時刻的航向角保持不變。

在飛航線時，航向角期望是變化的。在飛行器距離當前航點大於設定的閾值（固定翼模式在L1模塊計算所得，多旋翼模式為參數NAV_ACC_RAD）時，航向角期望始終沿當前位置指向航點方向。當該距離小於設定閾值時，航向角期望保持進入此閾值時的航向角期望不變。具體代碼如下：

這裡其實會有個問題，也就是隨著飛行器距離航點越來越近，如果跟蹤航線存在較大誤差就會導致航向角期望變化越來越快，從而使得飛行器無法跟蹤期望航向角的情況出現，最後導致飛行器衝過航點了還沒有判到到點的情況出現。

航向角到點判斷

航向角到點判據計算比較簡單，雖然也分了幾種情況。但其實只要航向角和期望航向角之差小於設定的閾值MIS_YAW_ERR就認為已經到點。另外兩種情況是針對強制航向到角時出現超時未能跟蹤上的時候，一般不會出現。

總結

上次講航點位置到點判據時有個小小的誤解，就是在水平位置小於兩倍盤旋半徑，而高度不小於兩倍設定閾值時會切入盤旋飛行，切回的條件代碼中是水平小於1.2倍盤旋半徑，垂直誤差小於1倍盤旋半徑，其實因為外面嵌套的if else判斷會先保證高度誤差小於兩倍閾值。所以高度條件並沒有放寬。只不過這種寫法會容易讓人誤解。

本節的航向角判據相對邏輯比較簡單，只不過也存在一個小的問題，也就是在接近航點時如果存在較大平面位置誤差會導致航向角期望發生快速變化，容易出現航向角不滿足到點判斷的情況出現。