fix: 修正實飛 Circle 高度爆衝與 Leader-Follower 卡點

1. mavlinkROS2Nodes: position_gnss topic 改發 relative_altitude 而非 AMSL,
   讓 drone_gps['alt'] 在實飛與 SITL 都代表「相對 home 高度」
2. mission_planner: Circle stage1.z 改用 final_z(不再用 current_z 做中點插值),
   避免實飛時 current_z 是 AMSL 而把過渡高度衝到海拔值的一半
3. mission_executor: 到達判定改為 hover-stable(radius * 1.5 hysteresis +
   穩定計時),預設 arrival_radius 2→4m,解決實飛 GPS 抖動永遠進不了
   2m radius 的卡死問題
4. mission_executor: 新增 progress log,每 3 秒印一行距離與穩定度,
   方便實飛 debug 卡點問題
5. mission_group + gui: 暴露 arrival_radius / hover_stable_sec 給 GUI 任務參數面板,
   每次 start 重讀以支援中途調整

Co-Authored-By: Claude Opus 4.7 <noreply@anthropic.com>
This commit is contained in:
2026-06-08 16:51:19 +08:00
co-authored by Claude Opus 4.7
parent 5231cffcb2
commit 415aed088a
5 changed files with 125 additions and 13 deletions
+47 -3
View File
@@ -32,9 +32,16 @@ DEFAULT_MISSION_PARAM_VALUES = {
'lateral_offset': '3.0',
'longitudinal_spacing': '5.0',
'line_spacing': '5.0',
# 執行參數(所有任務類型共用)
'arrival_radius': '4.0', # 到達半徑 (m),實飛建議 3~5,SITL 可調小到 2
'hover_stable_sec': '2.0', # 在 radius 內穩定停留多久才視為到達 (s)
}
# 執行參數的 key 集合(所有 mission type 共用,不隨類型切換)
EXECUTION_PARAM_KEYS = ('arrival_radius', 'hover_stable_sec')
class MissionGroup:
"""單一任務群組的資料"""
@@ -424,6 +431,12 @@ class GroupPanel(QWidget):
],
}
# 所有 mission type 共用的執行參數(不隨類型切換隱藏)
self._exec_param_defs = [
('arrival_radius', '到達半徑 (m)', self.default_params['arrival_radius']),
('hover_stable_sec', '停留判定 (s)', self.default_params['hover_stable_sec']),
]
# 建立所有參數列的 widget(先全部建好,切換時顯示/隱藏)
self._param_widgets = {} # key → (label_widget, input_widget)
self._param_rows = [] # 所有 row layout 對應的 container widget
@@ -447,6 +460,23 @@ class GroupPanel(QWidget):
self._param_widgets[key] = (row_w, inp)
self._param_rows.append(row_w)
# 執行參數列(所有 mission type 共用,永遠顯示)
for key, label_text, default in self._exec_param_defs:
row_w = QWidget()
row_l = QHBoxLayout(row_w)
row_l.setContentsMargins(0, 0, 0, 0)
row_l.setSpacing(3)
lbl = QLabel(label_text)
lbl.setStyleSheet(LBL)
inp = QLineEdit(default)
inp.setStyleSheet(INPUT)
inp.setFixedWidth(50)
row_l.addWidget(lbl, 1)
row_l.addWidget(inp)
param_col.addWidget(row_w)
self._param_widgets[key] = (row_w, inp)
self._param_rows.append(row_w)
# LEADER_FOLLOWER 專用:領隊下拉選單
self._leader_row = QWidget()
leader_layout = QHBoxLayout(self._leader_row)
@@ -546,15 +576,17 @@ class GroupPanel(QWidget):
self.delete_group_btn.setEnabled(enabled)
def _update_param_visibility(self, _=None):
"""根據當前任務類型,顯示/隱藏對應的參數列"""
"""根據當前任務類型,顯示/隱藏對應的參數列。執行參數永遠顯示。"""
mission_type = self.type_combo.currentText()
visible_keys = {d[0] for d in self._param_defs.get(mission_type, [])}
mission_keys = {d[0] for d in self._param_defs.get(mission_type, [])}
exec_keys = {d[0] for d in self._exec_param_defs}
visible_keys = mission_keys | exec_keys
for key, (row_w, _inp) in self._param_widgets.items():
row_w.setVisible(key in visible_keys)
self._leader_row.setVisible(mission_type == 'LEADER_FOLLOWER')
def get_mission_params(self):
"""讀取當前顯示的參數值,回傳 dict"""
"""讀取當前任務類型的規劃參數,回傳 dict(不含執行參數)"""
mission_type = self.type_combo.currentText()
params = {}
for key, _label, default in self._param_defs.get(mission_type, []):
@@ -566,6 +598,18 @@ class GroupPanel(QWidget):
params[key] = float(default)
return params
def get_execution_params(self):
"""讀取執行參數(arrival_radius, hover_stable_sec 等),回傳 dict"""
params = {}
for key, _label, default in self._exec_param_defs:
if key in self._param_widgets:
_row_w, inp = self._param_widgets[key]
try:
params[key] = float(inp.text())
except ValueError:
params[key] = float(default)
return params
def set_param_value(self, key, value):
"""更新指定參數欄位的文字值。"""
if key not in self._param_widgets: