diff --git a/src/GUI/command_sender.py b/src/GUI/command_sender.py index 0136bfe..eab6159 100644 --- a/src/GUI/command_sender.py +++ b/src/GUI/command_sender.py @@ -106,7 +106,11 @@ class Ros2CommandSender(QObject): # (drone_id, sysid, success, message) send_result = pyqtSignal(str, int, bool, str) - DEFAULT_TIMEOUT_SEC = 2.0 + # 閉環設計：goto setpoint 由 mission_executor 週期性重送、換點靠 GPS 到達判定， + # 所以這裡的 timeout 只影響「echo 驗證要等多久」，不影響指令是否送達飛控 + # （adapter 是先送 setpoint 再等 echo）。故意設短，讓 adapter 序列化的 service + # 在 echo miss 時快速放棄、換下一台，避免多機時排隊塞爆 client poll。 + DEFAULT_TIMEOUT_SEC = 0.5 def __init__(self, monitor, timeout_sec: float = DEFAULT_TIMEOUT_SEC): super().__init__() diff --git a/src/GUI/gui.py b/src/GUI/gui.py index 8841847..aa4ef15 100644 --- a/src/GUI/gui.py +++ b/src/GUI/gui.py @@ -1475,6 +1475,7 @@ class ControlStationUI(QMainWindow): arrival_radius=exec_params.get('arrival_radius', 4.0), hover_stable_sec=exec_params.get('hover_stable_sec', 2.0), tick_rate_hz=2.0, + resend_interval_sec=exec_params.get('resend_interval_sec', 2.5), ) executor.drone_waypoint_reached.connect(self.on_drone_waypoint_reached) executor.task_status_changed.connect(self._on_task_status_changed) @@ -2294,14 +2295,40 @@ class ControlStationUI(QMainWindow): # 任務執行回呼 # ================================================================================ + # 任務狀態列的通訊狀態短標籤（對應 MissionExecutor 的 TaskStatus.value） + _MISSION_COMM_LABELS = { + "normal": "送出OK", + "retrying": "送出未驗證", + "waiting_at_barrier": "等待同伴", + "fallback_loiter": "LOITER", + } + + def _set_overview_mission(self, drone_id, wp=None, comm=None): + """組合 WP 進度 + 通訊狀態，寫進總覽表『任務狀態』列（每台一格，實飛一眼看全部）。""" + cache = getattr(self, '_mission_ui', None) + if cache is None: + cache = self._mission_ui = {} + slot = cache.setdefault(drone_id, {'wp': '', 'comm': ''}) + if wp is not None: + slot['wp'] = wp + if comm is not None: + slot['comm'] = comm + text = ' · '.join(p for p in (slot['wp'], slot['comm']) if p) or '--' + self.update_overview_table(drone_id, 'mission', text) + def on_drone_waypoint_reached(self, drone_id, wp_index, total): if wp_index >= total: self.statusBar().showMessage(f"{drone_id} 已完成所有航點", 3000) + self._set_overview_mission(drone_id, wp=f"完成 {total}/{total}") else: self.statusBar().showMessage(f"{drone_id} 到達 WP {wp_index}/{total}", 2000) + self._set_overview_mission(drone_id, wp=f"WP {wp_index}/{total}") def _on_task_status_changed(self, drone_id, status, message): - """MissionExecutor.task_status_changed slot：把 goto 失敗/重試/fallback/barrier 丟到 status bar""" + """MissionExecutor.task_status_changed slot：狀態列提示 + 更新總覽表『任務狀態』列""" + comm = self._MISSION_COMM_LABELS.get(status) + if comm is not None: + self._set_overview_mission(drone_id, comm=comm) if status == "retrying": self.statusBar().showMessage(f"{drone_id} {message}", 4000) elif status == "fallback_loiter": diff --git a/src/GUI/mission_executor.py b/src/GUI/mission_executor.py index 2104925..3ebdb2d 100644 --- a/src/GUI/mission_executor.py +++ b/src/GUI/mission_executor.py @@ -3,14 +3,19 @@ 任務執行模組 管理多架無人機的 GUIDED 模式飛行控制迴圈 -設計: +設計 (閉環 / closed-loop): - 每架無人機持有一個航點序列，逐點推進 - 各自到達就各自切換到下一個航點 - - 事件驅動發送：航點切換時送一次，收到 send_result 才決定重送/前進 - - 失敗策略 (b)：單機重試 MAX_RETRY 次仍失敗 → 該機 fallback LOITER，其他機繼續 + - 閉環發送：以固定週期「持續重送」當前目標 setpoint（GUIDED 位置目標冪等）， + 掉包會在下個週期自癒；換點完全靠 GPS 到達判定驅動，不依賴通訊 ACK + - send_result（echo 驗證）僅作觀測/UI 顯示，不影響飛行控制： + 驗證失敗不會 fallback LOITER，指令其實已送達飛控，交給 GPS 續飛 - Rendezvous barrier：在指定 wp index 等所有活著的機到齊才一起推進，有 timeout 保護 - 用 QTimer 驅動到達判定，在 Qt 主線程執行 - 暫停 = 停止到達判定 + 停止新指令 + +背景：adapter 的 pos_global_int service 被序列化 + 阻塞等 msg87 echo，實飛慢速鏈路下 +多機會只有一台在 client timeout 內拿到回應。閉環設計繞過此瓶頸（見專案記憶）。 """ import asyncio import math @@ -42,7 +47,7 @@ class DroneTask: """單架無人機的任務資料""" __slots__ = ( 'drone_id', 'sysid', 'waypoints', 'wp_index', 'done', - 'sent_current_wp', 'fail_count', 'status', 'waiting_since', + 'last_sent_at', 'fail_count', 'status', 'waiting_since', 'entered_radius_at', 'last_log_at', ) @@ -52,7 +57,8 @@ class DroneTask: self.waypoints = waypoints self.wp_index = 0 self.done = len(waypoints) == 0 - self.sent_current_wp = False + # 上次送出當前目標的 monotonic 時間；0.0 = 尚未送過（下個 tick 立即送） + self.last_sent_at = 0.0 self.fail_count = 0 self.status = TaskStatus.NORMAL self.waiting_since = 0.0 # monotonic time 進入 WAITING_AT_BARRIER 的瞬間 @@ -85,7 +91,7 @@ class MissionExecutor(QObject): } """ - MAX_RETRY = 3 + MAX_RETRY = 3 # 已停用：閉環下 echo 驗證失敗不再累計切 LOITER（保留常數以防外部引用） drone_waypoint_reached = pyqtSignal(str, int, int) # (drone_id, wp_index, total) task_status_changed = pyqtSignal(str, str, str) # (drone_id, status, message) @@ -95,12 +101,16 @@ class MissionExecutor(QObject): arrival_radius=4.0, tick_rate_hz=2.0, barrier_timeout_sec=20.0, hover_stable_sec=2.0, - progress_log_interval_sec=3.0): + progress_log_interval_sec=3.0, + resend_interval_sec=2.5): super().__init__() self.sender = sender self.drone_gps = drone_gps - self.monitor = monitor # 用於失敗 fallback 到 LOITER + self.monitor = monitor # 保留：僅供外部手動 fallback 使用，閉環下不自動切 LOITER self.arrival_radius = arrival_radius + # 閉環重送週期：每隔這麼久重送一次當前目標 setpoint（掉包自癒）。 + # 注意：需 > N架 × client_timeout，否則多機序列化會塞不完（見 command_sender）。 + self.resend_interval_sec = resend_interval_sec self.barrier_timeout_sec = barrier_timeout_sec # hover-stable 判定：進入 radius 後須穩定停留 hover_stable_sec 秒才算到達， # 容忍 GPS 抖動跨越邊界（用 radius * 1.5 作 hysteresis） @@ -152,6 +162,7 @@ class MissionExecutor(QObject): f"共 {total_wps} 個航點, " f"到達半徑={self.arrival_radius}m (hover-stable {self.hover_stable_sec}s), " f"tick 週期={self._interval_ms}ms, " + f"重送週期={self.resend_interval_sec}s (閉環，GPS 驅動換點), " f"barrier timeout={self.barrier_timeout_sec}s, " f"{rv_info}", ) @@ -249,19 +260,21 @@ class MissionExecutor(QObject): # ---- Phase 2: barrier 釋放檢查 ---- self._check_barriers(now) - # ---- Phase 3: 發送未送過的目標 ---- + # ---- Phase 3: 週期性重送當前目標（閉環）---- + # 每 resend_interval_sec 重送一次目前 wp 的 setpoint，不靠 send_result 決定前進。 + # GUIDED 位置目標冪等，重送安全；掉包會在下個週期自癒。first send: last_sent_at=0 → 立即送。 for task in self.tasks.values(): if task.done or task.status in ( TaskStatus.FALLBACK_LOITER, TaskStatus.WAITING_AT_BARRIER ): continue - if task.sent_current_wp: - continue target = task.current_target if target is None: continue + if now - task.last_sent_at < self.resend_interval_sec: + continue tgt_lat, tgt_lon, tgt_alt = target - task.sent_current_wp = True + task.last_sent_at = now self.sender.send_position_global( task.drone_id, task.sysid, tgt_lat, tgt_lon, tgt_alt ) @@ -293,12 +306,14 @@ class MissionExecutor(QObject): ) def _advance_waypoint(self, task, arrived_distance): - """把 task 推進一個航點，重置發送旗標。不處理 barrier 邏輯。""" + """把 task 推進一個航點，重置發送計時。不處理 barrier 邏輯。""" task.wp_index += 1 - task.sent_current_wp = False + task.last_sent_at = 0.0 # 新 wp 下個 tick 立即送 task.fail_count = 0 task.entered_radius_at = 0.0 task.last_log_at = 0.0 + if task.status == TaskStatus.RETRYING: + task.status = TaskStatus.NORMAL # 清掉上個 wp 殘留的「送出未驗證」狀態 if task.wp_index >= task.total_waypoints: task.done = True self.drone_waypoint_reached.emit( @@ -369,42 +384,46 @@ class MissionExecutor(QObject): # ------------------------------------------------------------------ 結果回呼 def _on_send_result(self, drone_id, sysid, success, message): - """Ros2CommandSender.send_result 的 slot""" + """ + Ros2CommandSender.send_result 的 slot（閉環：僅觀測，不影響飛行控制）。 + + 指令在 adapter 端等 echo 前就已送達飛控，echo 驗證失敗不代表沒送到； + 因此這裡不再累計失敗切 LOITER，只更新 UI 狀態。真正的前進由 GPS 到達判定驅動， + 重送由 Phase 3 的週期性重送負責。 + """ task = self.tasks.get(drone_id) if task is None or task.done: return - # 若已在 barrier 等待，舊指令的遲到回應不要再觸發重試/fallback + # 若已在 barrier 等待，舊指令的遲到回應不要再干擾狀態 if task.status == TaskStatus.WAITING_AT_BARRIER: return if success: if task.fail_count > 0 or task.status == TaskStatus.RETRYING: task.status = TaskStatus.NORMAL - self.task_status_changed.emit(drone_id, task.status.value, "recovered") + self.task_status_changed.emit(drone_id, task.status.value, "send verified") task.fail_count = 0 return + # echo 驗證失敗：僅記錄 + UI 提示，不改變飛行（交給 GPS 續飛、下個週期重送） task.fail_count += 1 - _log("WARN", f"{drone_id} 發送失敗 {task.fail_count}/{self.MAX_RETRY}: {message}") - - if task.fail_count < self.MAX_RETRY: - task.status = TaskStatus.RETRYING - task.sent_current_wp = False # 下個 tick 會重送 - self.task_status_changed.emit( - drone_id, task.status.value, - f"retry {task.fail_count}/{self.MAX_RETRY}: {message}" - ) - else: - task.status = TaskStatus.FALLBACK_LOITER - self.task_status_changed.emit( - drone_id, task.status.value, - f"fallback LOITER after {self.MAX_RETRY} fails: {message}" - ) - _log("ERROR", f"{drone_id} 連續失敗 {self.MAX_RETRY} 次，切換至 LOITER") - self._fallback_to_loiter(drone_id) + task.status = TaskStatus.RETRYING # 純觀測用途，Phase 1/3 不會因此停飛 + _log( + "WARN", + f"{drone_id} 送出未驗證 x{task.fail_count}（指令已送達飛控，由 GPS 續飛）: {message}", + ) + self.task_status_changed.emit( + drone_id, task.status.value, + f"send unverified x{task.fail_count} (GPS 續飛): {message}" + ) def _fallback_to_loiter(self, drone_id): - """用 monitor.set_mode 切 LOITER。set_mode 是 coroutine，透過 event loop 派送。""" + """ + 用 monitor.set_mode 切 LOITER。set_mode 是 coroutine，透過 event loop 派送。 + + 閉環改版後不再由 _on_send_result 自動呼叫（送指令失敗不代表沒送到）； + 保留此方法供外部（例如 GUI 手動安全按鈕）需要時使用。 + """ if self.monitor is None: _log("WARN", f"無 monitor，無法將 {drone_id} 切換至 LOITER") return diff --git a/src/GUI/overview_table.py b/src/GUI/overview_table.py index 5549530..4ee925e 100644 --- a/src/GUI/overview_table.py +++ b/src/GUI/overview_table.py @@ -6,34 +6,38 @@ class OverviewTable(QTableWidget): """總覽表格，顯示所有無人機的狀態資訊""" # 默認的資訊類型和映射 - DEFAULT_INFO_TYPES = ["模式", "ARM", "電壓", "經度", "緯度", "FixType", "衛星數", "EPH", "EPV", + # 「任務狀態」放最上面：閉環跑點時一眼看每台的 WP 進度 + 通訊(送出)狀態。 + # 此列不來自遙測 QLabel，由 MissionExecutor 訊號經 update_table(field='mission') 餵入。 + DEFAULT_INFO_TYPES = ["任務狀態", + "模式", "ARM", "電壓", "經度", "緯度", "FixType", "衛星數", "EPH", "EPV", "高度", "XY位置", "XY速度", "地速", "航向", "空速", "油門", "海拔高度", "爬升率", "Roll", "Pitch", "Yaw", "丟包", "延遲"] - + DEFAULT_INFO_TYPE_MAP = { - "mode": 0, - "armed": 1, - "battery": 2, - "longitude": 3, - "latitude": 4, - "fix_type": 5, - "satellites_visible": 6, - "eph": 7, - "epv": 8, - "altitude": 9, - "local": 10, - "velocity": 11, - "groundspeed": 12, - "heading": 13, - "airspeed": 14, - "throttle": 15, - "hud_alt": 16, - "climb": 17, - "roll": 18, - "pitch": 19, - "yaw": 20, - "loss_rate": 21, - "ping": 22 + "mission": 0, + "mode": 1, + "armed": 2, + "battery": 3, + "longitude": 4, + "latitude": 5, + "fix_type": 6, + "satellites_visible": 7, + "eph": 8, + "epv": 9, + "altitude": 10, + "local": 11, + "velocity": 12, + "groundspeed": 13, + "heading": 14, + "airspeed": 15, + "throttle": 16, + "hud_alt": 17, + "climb": 18, + "roll": 19, + "pitch": 20, + "yaw": 21, + "loss_rate": 22, + "ping": 23 } def __init__(self, info_types=None, info_type_map=None, parent=None): @@ -43,6 +47,8 @@ class OverviewTable(QTableWidget): self.info_types = info_types if info_types is not None else self.DEFAULT_INFO_TYPES self.info_type_map = info_type_map if info_type_map is not None else self.DEFAULT_INFO_TYPE_MAP self.drones = {} # 存儲無人機面板的引用 + # 任務狀態不是遙測，不在 panel QLabel 內；獨立保存，避免 refresh_all 用遙測把它蓋掉 + self.mission_status = {} # drone_id -> 任務狀態文字 # 初始化表格 self.setColumnCount(1) @@ -75,7 +81,11 @@ class OverviewTable(QTableWidget): if drone_id and field and value: if drone_id not in self.drones: return - + + # 任務狀態另存一份，讓 refresh_all（用遙測重繪）不會把它清掉 + if field == 'mission': + self.mission_status[drone_id] = value + col = 1 + list(self.drones.keys()).index(drone_id) row = self.info_type_map.get(field, -1) @@ -104,8 +114,12 @@ class OverviewTable(QTableWidget): for col, did in enumerate(self.drones, start=1): panel = self.drones[did] for field, row in self.info_type_map.items(): - lbl = panel.findChild(QLabel, f"{did}_{field}") - val = lbl.text() if lbl else "--" + if field == 'mission': + # 任務狀態非遙測，取自獨立保存，避免被 "--" 蓋掉 + val = self.mission_status.get(did, "--") + else: + lbl = panel.findChild(QLabel, f"{did}_{field}") + val = lbl.text() if lbl else "--" val_item = QTableWidgetItem(val) val_item.setTextAlignment(Qt.AlignmentFlag.AlignCenter) self.setItem(row, col, val_item)