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:
@@ -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:
|
||||
|
||||
Reference in New Issue
Block a user