temp

src/GUI/command_sender.py

@@ -106,11 +106,7 @@ class Ros2CommandSender(QObject):
     # (drone_id, sysid, success, message)
     send_result = pyqtSignal(str, int, bool, str)
 
-    # 閉環設計：goto setpoint 由 mission_executor 週期性重送、換點靠 GPS 到達判定，
+    DEFAULT_TIMEOUT_SEC = 2.0
-    # 所以這裡的 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__()

src/GUI/communication.py

@@ -69,18 +69,6 @@ except ImportError as e:
     _log("ERROR", f"錯誤: {e}")
     GnssRaw = None
 
-try:
-    from fc_interfaces.msg import SystemDiagnosticsRaw
-except ImportError as e:
-    _log("WARN", f"SystemDiagnosticsRaw 尚不可用，略過 sys_diags 訂閱: {e}")
-    SystemDiagnosticsRaw = None
-
-try:
-    from fc_interfaces.msg import FcNetworkLog
-except ImportError as e:
-    _log("WARN", f"FcNetworkLog 尚不可用，略過 FC Network 紀錄訂閱: {e}")
-    FcNetworkLog = None
-
 class DroneSignals(QObject):
     update_signal = pyqtSignal(str, str, object)  # (msg_type, drone_id, data)
 
@@ -813,16 +801,6 @@ class DroneMonitor(Node):
         # 主题检测定时器
         self.create_timer(1.0, self.scan_topics)
     
-        # FC Network 使用專用結構化紀錄 topic，不再解析 /rosout。
-        self.fc_network_log_sub = None
-        if FcNetworkLog is not None:
-            self.fc_network_log_sub = self.create_subscription(
-                FcNetworkLog,
-                '/fc_network/logs',
-                self.fc_network_log_callback,
-                200
-            )
-    
     def get_next_socket_id(self):
         """取得目前最小的未使用 socket_id（從 0 開始）。"""
         with self.socket_id_lock:
@@ -991,32 +969,6 @@ class DroneMonitor(Node):
                         setattr(self, subs_attr, subs)
                     except Exception:
                         pass
-                if isinstance(subs, dict) and 'sys_diags' not in subs and SystemDiagnosticsRaw is not None:
-                    base_topic = f'/fc_network/vehicle/{sys_id}'
-                    try:
-                        sys_diags_sub = self.create_subscription(
-                            SystemDiagnosticsRaw,
-                            f'{base_topic}/sys_diags',
-                            lambda msg, sid=sys_id: self.sys_diags_callback(sid, msg),
-                            10
-                        )
-                        subs['sys_diags'] = sys_diags_sub
-                        setattr(self, subs_attr, subs)
-                    except Exception:
-                        pass
-                if isinstance(subs, dict) and 'status_text' not in subs:
-                    base_topic = f'/fc_network/vehicle/{sys_id}'
-                    try:
-                        status_text_sub = self.create_subscription(
-                            String,
-                            f'{base_topic}/status_text',
-                            lambda msg, sid=sys_id: self.status_text_callback(sid, msg),
-                            10
-                        )
-                        subs['status_text'] = status_text_sub
-                        setattr(self, subs_attr, subs)
-                    except Exception:
-                        pass
     
     def setup_drone(self, sys_id):
         # sys_id 格式： sys11, sys12, ...
@@ -1089,21 +1041,6 @@ class DroneMonitor(Node):
                 10
             )
         
-        if SystemDiagnosticsRaw is not None:
-            subs['sys_diags'] = self.create_subscription(
-                SystemDiagnosticsRaw,
-                f'{base_topic}/sys_diags',
-                lambda msg, sid=sys_id: self.sys_diags_callback(sid, msg),
-                10
-            )
-
-        subs['status_text'] = self.create_subscription(
-            String,
-            f'{base_topic}/status_text',
-            lambda msg, sid=sys_id: self.status_text_callback(sid, msg),
-            10
-        )
-        
         setattr(self, f'drone_{sys_id}_subs', subs)
 
     # ================================================================================
@@ -1362,67 +1299,6 @@ class DroneMonitor(Node):
             'voltage': msg.voltage
         }
 
-    def sys_diags_callback(self, sys_id, msg):
-        """轉送 /fc_network/vehicle/sysN/sys_diags 到 GUI 紀錄區。"""
-        stamp = getattr(msg, 'stamp', None)
-        data = {
-            'stamp': {
-                'sec': getattr(stamp, 'sec', 0),
-                'nanosec': getattr(stamp, 'nanosec', 0),
-            },
-            'sensors_install_mask': int(msg.sensors_install_mask),
-            'sensors_enabled_mask': int(msg.sensors_enabled_mask),
-            'sensors_health_mask': int(msg.sensors_health_mask),
-            'mcu_load': int(msg.mcu_load),
-            'mcu_load_percent': float(msg.mcu_load) / 10.0,
-            'bus_error_rate': int(msg.bus_error_rate),
-            'bus_error_rate_percent': float(msg.bus_error_rate) / 10.0,
-            'bus_error_count': int(msg.bus_error_count),
-            'errors_count1': int(msg.errors_count1),
-            'errors_count2': int(msg.errors_count2),
-            'errors_count3': int(msg.errors_count3),
-            'errors_count4': int(msg.errors_count4),
-        }
-        self.signals.update_signal.emit('sys_diags', sys_id, data)
-
-    def status_text_callback(self, sys_id, msg):
-        """轉送飛控 STATUSTEXT（飛行檢查、警告與錯誤）。"""
-        raw = msg.data.strip()
-        if not raw:
-            return
-
-        match = re.match(r'^\[([\d.]+)\]\s*\[(-?\d+)\]\s*(.*)$', raw)
-        if match:
-            vehicle_timestamp = float(match.group(1))
-            severity = int(match.group(2))
-            text = match.group(3)
-        else:
-            vehicle_timestamp = None
-            severity = -1
-            text = raw
-
-        self.signals.update_signal.emit('status_text', sys_id, {
-            'severity': severity,
-            'text': text,
-            'vehicle_timestamp': vehicle_timestamp,
-            'raw': raw,
-        })
-
-    def fc_network_log_callback(self, msg):
-        """接收 /fc_network/logs 的結構化紀錄。"""
-        stamp = getattr(msg, 'stamp', None)
-        self.signals.update_signal.emit('fc_network_log', msg.source or 'fc_network', {
-            'stamp': {
-                'sec': getattr(stamp, 'sec', 0),
-                'nanosec': getattr(stamp, 'nanosec', 0),
-            },
-            'level': int(msg.level),
-            'source': msg.source,
-            'event_code': msg.event_code,
-            'sysid': int(msg.sysid),
-            'message': msg.message,
-        })
-
     def state_callback(self, drone_id, msg):
         mode = msg.mode
         if mode in self.filtered_modes:

src/GUI/gui.py

@@ -18,9 +18,11 @@ import re
 import threading
 from concurrent.futures import ThreadPoolExecutor
 
+
 def _log(level, message):
     print(f"[{level}] {message}", flush=True)
 
+
 # 導入分離的類別
 from communication import DroneMonitor, UDPMavlinkReceiver, WebSocketMavlinkReceiver
 from map_layout import DroneMap
@@ -146,7 +148,7 @@ class ToggleSwitch(QWidget):
 class ControlStationUI(QMainWindow):
     planning_finished = pyqtSignal(object)
 
-    VERSION = '2.7.1'
+    VERSION = '2.6.0'
     FONT_SCALE_MIN = 70
     FONT_SCALE_MAX = 180
     FONT_SCALE_DEFAULT = 100
@@ -209,15 +211,8 @@ class ControlStationUI(QMainWindow):
         self._attitude_cache = {}
         self._overview_cache = {}
         self._map_dirty_drones = set()
-        # 「紀錄」分頁分成三個獨立來源；message_history 保留為 GUI 操作紀錄的
+        self.message_history = []
-        # 相容別名，避免既有程式仍存取舊屬性時失效。
-        self.drone_message_history = []
-        self.gui_operation_history = []
-        self.fcnetwork_history = []
-        self.message_history = self.gui_operation_history
         self.max_message_history = 500
-        self._sys_diags_warning_signatures = {}
-        self._suppressed_status_message = None
         
         # 初始化UI
         self.drones = {}
@@ -453,19 +448,19 @@ class ControlStationUI(QMainWindow):
         self.statusBar().messageChanged.connect(self._on_status_bar_message_changed)
 
     def _create_message_history_tab(self):
-        """建立上、中、下三區的紀錄分頁。"""
+        """建立左側訊息歷史分頁。"""
         widget = QWidget()
         layout = QVBoxLayout(widget)
         layout.setContentsMargins(10, 10, 10, 10)
         layout.setSpacing(8)
 
         header_layout = QHBoxLayout()
-        title = QLabel("訊息與操作紀錄")
+        title = QLabel("操作與訊息歷史")
         title.setStyleSheet("color: #DDD; font-size: 14px; font-weight: bold;")
         header_layout.addWidget(title)
         header_layout.addStretch()
 
-        clear_btn = QPushButton("全部清空")
+        clear_btn = QPushButton("清空")
         clear_btn.setStyleSheet("""
             QPushButton { background-color: #555; color: white; border: none;
                 padding: 5px 10px; border-radius: 4px; font-size: 12px; }
@@ -475,7 +470,9 @@ class ControlStationUI(QMainWindow):
         header_layout.addWidget(clear_btn)
         layout.addLayout(header_layout)
 
-        history_style = """
+        self.message_history_view = QPlainTextEdit()
+        self.message_history_view.setReadOnly(True)
+        self.message_history_view.setStyleSheet("""
             QPlainTextEdit {
                 background-color: #1E1E1E;
                 color: #DDD;
@@ -485,67 +482,9 @@ class ControlStationUI(QMainWindow):
                 font-family: monospace;
                 font-size: 12px;
             }
-        """
+        """)
-
+        self.message_history_view.setPlaceholderText("左下角狀態訊息會顯示在這裡...")
-        def create_section(section_title, category):
+        layout.addWidget(self.message_history_view)
-            section = QWidget()
-            section_layout = QVBoxLayout(section)
-            section_layout.setContentsMargins(0, 0, 0, 0)
-            section_layout.setSpacing(4)
-
-            section_header = QHBoxLayout()
-            section_label = QLabel(section_title)
-            section_label.setStyleSheet(
-                "color: #CCC; font-size: 12px; font-weight: bold;")
-            section_header.addWidget(section_label)
-            section_header.addStretch()
-
-            section_clear_btn = QPushButton("清空")
-            section_clear_btn.setStyleSheet("""
-                QPushButton { background-color: #444; color: #DDD; border: none;
-                    padding: 3px 8px; border-radius: 3px; font-size: 11px; }
-                QPushButton:hover { background-color: #555; }
-            """)
-            section_clear_btn.clicked.connect(
-                lambda _checked=False, name=category:
-                    self._clear_message_history(name)
-            )
-            section_header.addWidget(section_clear_btn)
-            section_layout.addLayout(section_header)
-
-            view = QPlainTextEdit()
-            view.setReadOnly(True)
-            view.setStyleSheet(history_style)
-            view.document().setMaximumBlockCount(self.max_message_history)
-            section_layout.addWidget(view)
-            return section, view
-
-        history_splitter = QSplitter(Qt.Orientation.Vertical)
-        history_splitter.setChildrenCollapsible(False)
-
-        gui_section, self.gui_operation_history_view = create_section(
-            "操作紀錄", "gui")
-        drone_section, self.drone_message_history_view = create_section(
-            "無人機狀態", "drone")
-        fcnetwork_section, self.fcnetwork_history_view = create_section(
-            "FC Network", "fcnetwork")
-
-        history_splitter.addWidget(gui_section)
-        history_splitter.addWidget(drone_section)
-        history_splitter.addWidget(fcnetwork_section)
-        history_splitter.setStretchFactor(0, 1)
-        history_splitter.setStretchFactor(1, 1)
-        history_splitter.setStretchFactor(2, 1)
-        history_splitter.setSizes([220, 220, 220])
-        layout.addWidget(history_splitter)
-
-        self._history_views = {
-            'drone': self.drone_message_history_view,
-            'gui': self.gui_operation_history_view,
-            'fcnetwork': self.fcnetwork_history_view,
-        }
-        # 舊名稱指向 GUI 操作紀錄，保留向後相容性。
-        self.message_history_view = self.gui_operation_history_view
 
         return widget
 
@@ -802,114 +741,27 @@ class ControlStationUI(QMainWindow):
 
         return super().eventFilter(obj, event)
 
-    def _history_list(self, category):
+    def _clear_message_history(self):
-        return {
+        """清空訊息歷史。"""
-            'drone': self.drone_message_history,
+        self.message_history.clear()
-            'gui': self.gui_operation_history,
+        if hasattr(self, 'message_history_view'):
-            'fcnetwork': self.fcnetwork_history,
+            self.message_history_view.clear()
-        }.get(category)
 
-    def _append_history(self, category, message):
+    def _on_status_bar_message_changed(self, message):
-        """加入指定來源的紀錄並讓畫面保持在最新一筆。"""
+        """同步狀態列訊息到歷史紀錄。"""
         if not message:
             return
-
         timestamp = time.strftime("%H:%M:%S")
         entry = f"[{timestamp}] {message}"
-        history = self._history_list(category)
+        self.message_history.append(entry)
-        if history is None:
+        if len(self.message_history) > self.max_message_history:
-            return
+            self.message_history = self.message_history[-self.max_message_history:]
-        history.append(entry)
-        if len(history) > self.max_message_history:
-            del history[:-self.max_message_history]
 
-        view = getattr(self, '_history_views', {}).get(category)
+        if hasattr(self, 'message_history_view'):
-        if view is not None:
+            self.message_history_view.appendPlainText(entry)
-            view.appendPlainText(entry)
+            scrollbar = self.message_history_view.verticalScrollBar()
-            scrollbar = view.verticalScrollBar()
             scrollbar.setValue(scrollbar.maximum())
 
-    def _clear_message_history(self, category=None):
-        """清空單一來源；未指定來源時清空全部。"""
-        categories = (category,) if category else ('drone', 'gui', 'fcnetwork')
-        for name in categories:
-            history = self._history_list(name)
-            if history is not None:
-                history.clear()
-            view = getattr(self, '_history_views', {}).get(name)
-            if view is not None:
-                view.clear()
-
-    def _record_drone_status(self, sys_id, data):
-        """顯示由 status_text topic 收到的飛行檢查與報錯。"""
-        severity_labels = {
-            0: 'EMERGENCY', 1: 'ALERT', 2: 'CRITICAL', 3: 'ERROR',
-            4: 'WARNING', 5: 'NOTICE', 6: 'INFO', 7: 'DEBUG'
-        }
-        severity = data.get('severity', -1)
-        label = severity_labels.get(severity, 'STATUS')
-        self._append_history(
-            'drone', f"/fc_network/vehicle/{sys_id}/status_text "
-                     f"[{label}] {data.get('text', '')}")
-
-    def _record_sys_diags_warning(self, sys_id, data):
-        """sys_diags 只在診斷值異常或內容改變時顯示警告。"""
-        installed = int(data.get('sensors_install_mask', 0))
-        enabled = int(data.get('sensors_enabled_mask', 0))
-        healthy = int(data.get('sensors_health_mask', 0))
-        unhealthy_mask = installed & enabled & ~healthy
-        load = float(data.get('mcu_load_percent', 0.0))
-        bus_rate = float(data.get('bus_error_rate_percent', 0.0))
-        error_counts = tuple(int(data.get(f'errors_count{i}', 0)) for i in range(1, 5))
-
-        warnings = []
-        if unhealthy_mask:
-            warnings.append(f"感測器異常 mask=0x{unhealthy_mask:08X}")
-        if load >= 90.0:
-            warnings.append(f"MCU 負載過高 {load:.1f}%")
-        if bus_rate > 0.0 or int(data.get('bus_error_count', 0)) > 0:
-            warnings.append(
-                f"匯流排錯誤率 {bus_rate:.1f}% / "
-                f"count={int(data.get('bus_error_count', 0))}")
-        if any(error_counts):
-            warnings.append(f"錯誤計數={error_counts}")
-
-        signature = tuple(warnings)
-        if not signature:
-            self._sys_diags_warning_signatures.pop(sys_id, None)
-            return
-        if self._sys_diags_warning_signatures.get(sys_id) == signature:
-            return
-        self._sys_diags_warning_signatures[sys_id] = signature
-        self._append_history(
-            'drone', f"/fc_network/vehicle/{sys_id}/sys_diags [WARNING] "
-                     + "；".join(warnings))
-
-    def _record_fcnetwork_log(self, logger_name, data):
-        """顯示 /fc_network/logs 的結構化 FC Network 紀錄。"""
-        level_labels = {
-            10: 'DEBUG', 20: 'INFO', 30: 'WARN', 40: 'ERROR', 50: 'FATAL'
-        }
-        level = level_labels.get(data.get('level'), str(data.get('level', '')))
-        context = []
-        if data.get('event_code'):
-            context.append(data['event_code'])
-        if data.get('sysid', -1) >= 0:
-            context.append(f"sys{data['sysid']}")
-        context_text = f" [{' / '.join(context)}]" if context else ''
-        self._append_history(
-            'fcnetwork', f"/fc_network/logs [{level}] [{logger_name}]"
-                         f"{context_text} {data.get('message', '')}")
-
-    def _on_status_bar_message_changed(self, message):
-        """同步 GUI 狀態列訊息到操作紀錄。"""
-        if not message:
-            return
-        if message == self._suppressed_status_message:
-            self._suppressed_status_message = None
-            return
-        self._append_history('gui', message)
-
     def _setup_stream_redirector(self):
         """將 stdout/stderr 同步到左下角狀態列與訊息紀錄。"""
         self._original_stdout = sys.stdout
@@ -931,14 +783,10 @@ class ControlStationUI(QMainWindow):
             sys.stderr = self._original_stderr
 
     def show_in_bottom_left(self, text):
-        """背景輸出只短暫顯示在狀態列，不混入三類紀錄。"""
+        """將重導向的輸出顯示在左下角狀態列。"""
         if not text:
             return
-        # 背景輸出仍沿用既有狀態列提示，但避免又被當成 GUI 操作紀錄。
-        self._suppressed_status_message = text
         self.statusBar().showMessage(text, 5000)
-        if self._suppressed_status_message == text:
-            self._suppressed_status_message = None
     
 
     # ================================================================================
@@ -1475,7 +1323,6 @@ 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)
@@ -1813,15 +1660,6 @@ class ControlStationUI(QMainWindow):
 
     def update_ui(self, msg_type, drone_id, data):
         """只做數據快取，不在這裡更新 UI"""
-        if msg_type == 'status_text':
-            self._record_drone_status(drone_id, data)
-            return
-        if msg_type == 'sys_diags':
-            self._record_sys_diags_warning(drone_id, data)
-            return
-        if msg_type == 'fc_network_log':
-            self._record_fcnetwork_log(drone_id, data)
-            return
         if msg_type == 'connection_type':
             conn_type = data.get('type', 'Unknown')
             parts = drone_id.split('_')
@@ -2295,40 +2133,14 @@ 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：狀態列提示 + 更新總覽表『任務狀態』列"""
+        """MissionExecutor.task_status_changed slot：把 goto 失敗/重試/fallback/barrier 丟到 status bar"""
-        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":

src/GUI/map_layout.py

@@ -377,8 +377,6 @@ class DroneMap:
                 var clickStartPos = null;
                 var lastManualMapMoveAt = 0;
                 var autoCenteringMap = false;
-                const autoFollowPanThresholdRatio = 0.25;
-                var lastAutoFollowPanAt = 0;
 
                 function markManualMapMove() {
                     if (!autoCenteringMap) {
@@ -388,42 +386,6 @@ class DroneMap:
 
                 map.on('dragstart drag dragend zoomstart zoomend', markManualMapMove);
                 
-                function isOutsideAutoFollowWindow(latlng) {
-                    if (!latlng) {
-                        return false;
-                    }
-
-                    var size = map.getSize();
-                    if (!size || size.x <= 0 || size.y <= 0) {
-                        return false;
-                    }
-
-                    var centerPoint = map.latLngToContainerPoint(map.getCenter());
-                    var targetPoint = map.latLngToContainerPoint(latlng);
-                    var dx = Math.abs(targetPoint.x - centerPoint.x);
-                    var dy = Math.abs(targetPoint.y - centerPoint.y);
-
-                    return dx >= size.x * autoFollowPanThresholdRatio ||
-                           dy >= size.y * autoFollowPanThresholdRatio;
-                }
-
-                function autoFollowPanTo(latlng) {
-                    if (!isOutsideAutoFollowWindow(latlng)) {
-                        return;
-                    }
-                    if (Date.now() - lastManualMapMoveAt < 1000) {
-                        return;
-                    }
-                    if (Date.now() - lastAutoFollowPanAt < 250) {
-                        return;
-                    }
-
-                    lastAutoFollowPanAt = Date.now();
-                    autoCenteringMap = true;
-                    map.panTo(latlng, {animate: true});
-                    setTimeout(() => { autoCenteringMap = false; }, 300);
-                }
-                
                 // 路徑標記變量 (跟隨模式用)
                 var routePoints = [];
                 var routeMarkers = [];
@@ -776,8 +738,13 @@ class DroneMap:
 
                 setInterval(() => {
                     if (focusedId && markers[focusedId]) {
+                        if (Date.now() - lastManualMapMoveAt < 1000) {
+                            return;
+                        }
                         var latlng = markers[focusedId].getLatLng();
-                        autoFollowPanTo(latlng);
+                        autoCenteringMap = true;
+                        map.panTo(latlng);
+                        setTimeout(() => { autoCenteringMap = false; }, 300);
                     }
                 }, 1000);
 
@@ -793,9 +760,6 @@ class DroneMap:
                             .setLatLng([lat, lon])
                             .setRotationAngle(heading);
                         idLabels[id].setLatLng([lat, lon]);
-                        if (id === focusedId) {
-                            autoFollowPanTo(markers[id].getLatLng());
-                        }
                     } else {
                         initTrajectory(id);
                         addTrajectoryPoint(id, lat, lon);

src/GUI/mission_executor.py

@@ -3,19 +3,14 @@
 任務執行模組
 管理多架無人機的 GUIDED 模式飛行控制迴圈
 
-設計 (閉環 / closed-loop):
+設計:
     - 每架無人機持有一個航點序列，逐點推進
     - 各自到達就各自切換到下一個航點
-    - 閉環發送：以固定週期「持續重送」當前目標 setpoint（GUIDED 位置目標冪等），
+    - 事件驅動發送：航點切換時送一次，收到 send_result 才決定重送/前進
-      掉包會在下個週期自癒；換點完全靠 GPS 到達判定驅動，不依賴通訊 ACK
+    - 失敗策略 (b)：單機重試 MAX_RETRY 次仍失敗 → 該機 fallback LOITER，其他機繼續
-    - 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
@@ -47,7 +42,7 @@ class DroneTask:
     """單架無人機的任務資料"""
     __slots__ = (
         'drone_id', 'sysid', 'waypoints', 'wp_index', 'done',
-        'last_sent_at', 'fail_count', 'status', 'waiting_since',
+        'sent_current_wp', 'fail_count', 'status', 'waiting_since',
         'entered_radius_at', 'last_log_at',
     )
 
@@ -57,8 +52,7 @@ class DroneTask:
         self.waypoints = waypoints
         self.wp_index = 0
         self.done = len(waypoints) == 0
-        # 上次送出當前目標的 monotonic 時間；0.0 = 尚未送過（下個 tick 立即送）
+        self.sent_current_wp = False
-        self.last_sent_at = 0.0
         self.fail_count = 0
         self.status = TaskStatus.NORMAL
         self.waiting_since = 0.0  # monotonic time 進入 WAITING_AT_BARRIER 的瞬間
@@ -91,7 +85,7 @@ class MissionExecutor(QObject):
         }
     """
 
-    MAX_RETRY = 3  # 已停用：閉環下 echo 驗證失敗不再累計切 LOITER（保留常數以防外部引用）
+    MAX_RETRY = 3
 
     drone_waypoint_reached = pyqtSignal(str, int, int)   # (drone_id, wp_index, total)
     task_status_changed = pyqtSignal(str, str, str)       # (drone_id, status, message)
@@ -101,16 +95,12 @@ 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）
@@ -162,7 +152,6 @@ 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}",
         )
@@ -260,21 +249,19 @@ 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.last_sent_at = now
+            task.sent_current_wp = True
             self.sender.send_position_global(
                 task.drone_id, task.sysid, tgt_lat, tgt_lon, tgt_alt
             )
@@ -306,14 +293,12 @@ class MissionExecutor(QObject):
         )
 
     def _advance_waypoint(self, task, arrived_distance):
-        """把 task 推進一個航點，重置發送計時。不處理 barrier 邏輯。"""
+        """把 task 推進一個航點，重置發送旗標。不處理 barrier 邏輯。"""
         task.wp_index += 1
-        task.last_sent_at = 0.0  # 新 wp 下個 tick 立即送
+        task.sent_current_wp = False
         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(
@@ -384,46 +369,42 @@ 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 等待，舊指令的遲到回應不要再干擾狀態
+        # 若已在 barrier 等待，舊指令的遲到回應不要再觸發重試/fallback
         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, "send verified")
+                self.task_status_changed.emit(drone_id, task.status.value, "recovered")
             task.fail_count = 0
             return
 
-        # echo 驗證失敗：僅記錄 + UI 提示，不改變飛行（交給 GPS 續飛、下個週期重送）
         task.fail_count += 1
-        task.status = TaskStatus.RETRYING  # 純觀測用途，Phase 1/3 不會因此停飛
+        _log("WARN", f"{drone_id} 發送失敗 {task.fail_count}/{self.MAX_RETRY}: {message}")
-        _log(
+
-            "WARN",
+        if task.fail_count < self.MAX_RETRY:
-            f"{drone_id} 送出未驗證 x{task.fail_count}（指令已送達飛控，由 GPS 續飛）: {message}",
+            task.status = TaskStatus.RETRYING
-        )
+            task.sent_current_wp = False  # 下個 tick 會重送
-        self.task_status_changed.emit(
+            self.task_status_changed.emit(
-            drone_id, task.status.value,
+                drone_id, task.status.value,
-            f"send unverified x{task.fail_count} (GPS 續飛): {message}"
+                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)
 
     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

src/GUI/overview_table.py

@@ -6,38 +6,34 @@ class OverviewTable(QTableWidget):
     """總覽表格，顯示所有無人機的狀態資訊"""
     
     # 默認的資訊類型和映射
-    # 「任務狀態」放最上面：閉環跑點時一眼看每台的 WP 進度 + 通訊(送出)狀態。
+    DEFAULT_INFO_TYPES = ["模式", "ARM", "電壓", "經度", "緯度", "FixType", "衛星數", "EPH", "EPV",
-    # 此列不來自遙測 QLabel，由 MissionExecutor 訊號經 update_table(field='mission') 餵入。
-    DEFAULT_INFO_TYPES = ["任務狀態",
-                          "模式", "ARM", "電壓", "經度", "緯度", "FixType", "衛星數", "EPH", "EPV",
                           "高度", "XY位置", "XY速度", "地速", "航向", "空速", "油門", "海拔高度",
                           "爬升率", "Roll", "Pitch", "Yaw", "丟包", "延遲"]
     
     DEFAULT_INFO_TYPE_MAP = {
-        "mission": 0,
+        "mode": 0,
-        "mode": 1,
+        "armed": 1,
-        "armed": 2,
+        "battery": 2,
-        "battery": 3,
+        "longitude": 3,
-        "longitude": 4,
+        "latitude": 4,
-        "latitude": 5,
+        "fix_type": 5,
-        "fix_type": 6,
+        "satellites_visible": 6,
-        "satellites_visible": 7,
+        "eph": 7,
-        "eph": 8,
+        "epv": 8,
-        "epv": 9,
+        "altitude": 9,
-        "altitude": 10,
+        "local": 10,
-        "local": 11,
+        "velocity": 11,
-        "velocity": 12,
+        "groundspeed": 12,
-        "groundspeed": 13,
+        "heading": 13,
-        "heading": 14,
+        "airspeed": 14,
-        "airspeed": 15,
+        "throttle": 15,
-        "throttle": 16,
+        "hud_alt": 16,
-        "hud_alt": 17,
+        "climb": 17,
-        "climb": 18,
+        "roll": 18,
-        "roll": 19,
+        "pitch": 19,
-        "pitch": 20,
+        "yaw": 20,
-        "yaw": 21,
+        "loss_rate": 21,
-        "loss_rate": 22,
+        "ping": 22
-        "ping": 23
     }
     
     def __init__(self, info_types=None, info_type_map=None, parent=None):
@@ -47,8 +43,6 @@ 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)
@@ -82,10 +76,6 @@ class OverviewTable(QTableWidget):
             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)
 
@@ -114,12 +104,8 @@ 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():
-                if field == 'mission':
+                lbl = panel.findChild(QLabel, f"{did}_{field}")
-                    # 任務狀態非遙測，取自獨立保存，避免被 "--" 蓋掉
+                val = lbl.text() if lbl else "--"
-                    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)

src/fc_interfaces/CMakeLists.txt

@@ -15,7 +15,6 @@ rosidl_generate_interfaces(${PROJECT_NAME}
   "msg/AttitudeRaw.msg"
   "msg/GnssRaw.msg"
   "msg/SystemDiagnosticsRaw.msg"
-  "msg/FcNetworkLog.msg"
   "msg/ServiceAckResult.msg"
   "srv/MavPing.srv"
   "srv/MavCommandLong.srv"

src/fc_interfaces/msg/FcNetworkLog.msg

@@ -1,12 +0,0 @@
-uint8 DEBUG=10
-uint8 INFO=20
-uint8 WARN=30
-uint8 ERROR=40
-uint8 FATAL=50
-
-builtin_interfaces/Time stamp
-uint8 level
-string source
-string event_code
-int32 sysid
-string message

src/fc_network_adapter/fc_network_adapter/mainOrchestrator.py

@@ -436,7 +436,7 @@ class ControlPanel:
                         menu_stack.pop()
                         idx_stack.pop()
                     elif selected.action == "SET_SERIAL_COMM_XBEE_ESP":
-                        state.serial_info_temp["CommunicationType"] = "XBee(API-API) espv1"
+                        state.serial_info_temp["CommunicationType"] = "XBee(API-API)espv1"
                         menu_stack.pop()
                         idx_stack.pop()
                     elif selected.action == "SET_SERIAL_COMM_TELEMETRY":
@@ -1600,7 +1600,7 @@ class Orchestrator:
         # 定義通訊類型映射表
         COMM_TYPE_MAP = {
             "XBee(API-AT)": sm.SerialMode.XBEEAPI2AT,
-            "XBee(API-AT)": sm.SerialMode.XBEEAPI_espv1,
+            "XBee(API-API)espv1": sm.SerialMode.XBEEAPI_espv1,
             "TELEMETRY": sm.SerialMode.STRAIGHT,
             # 新增區
         }

src/fc_network_adapter/fc_network_adapter/mavlinkROS2Nodes.py

@@ -1,6 +1,6 @@
 """
 MAVLink ROS2 Nodes
-主要包含四個獨立的 ROS2 Node :
+主要包含三個獨立的 ROS2 Node :
 1. VehicleStatusPublisher - 發布載具狀態到 ROS2 topics
     從 vehicle_registry 讀取狀態數據，頻率控制，模組化設計
 2. MavlinkCommandService - 提供 MAVLink 指令 service 介面
@@ -8,7 +8,6 @@ MAVLink ROS2 Nodes
     並不會包含額外的功能
 3. RtcmRelay - 訂閱 RTCM topic 並轉發為 MAVLink GPS_RTCM_DATA 給所有載具
     過期丟棄、去重、節流、分片
-4. FcNetworkLogPublisher - 將 FC Network logging queue 發布到 /fc_network/logs
 
 與一個節點管理器
 - fc_ros_manager
@@ -19,7 +18,6 @@ import os
 import time
 import math
 import hashlib
-import queue
 import threading
 from typing import Dict, Optional
 
@@ -44,7 +42,7 @@ from fc_interfaces.msg import ServiceAckResult
 # 自定義 imports
 from . import mavlinkVehicleView as mvv
 from . import mavlinkObject as mo
-from .utils import get_ros_log_queue, setup_logger
+from .utils import setup_logger
 
 logger = setup_logger(os.path.basename(__file__))
 MODULE_VER = "2.50"
@@ -1181,58 +1179,6 @@ class RtcmRelay(Node):
         # logger.info("RtcmRelay stopped")
 
 
-# ============================================================================
-#                    FC Network Log Publisher Node
-# ============================================================================
-
-class FcNetworkLogPublisher(Node):
-    """從 Python logging queue 發布結構化 /fc_network/logs 訊息。"""
-
-    def __init__(self):
-        super().__init__('fc_network_log_publisher')
-        qos = QoSProfile(
-            history=HistoryPolicy.KEEP_LAST,
-            depth=200,
-            reliability=ReliabilityPolicy.RELIABLE,
-            durability=DurabilityPolicy.VOLATILE,
-        )
-        self.publisher = self.create_publisher(
-            fcmsg.FcNetworkLog, '/fc_network/logs', qos)
-        self.log_queue = get_ros_log_queue()
-        self.running = True
-        self.timer = self.create_timer(0.05, self._drain_queue)
-
-    def _drain_queue(self):
-        if not self.running:
-            return
-
-        for _ in range(100):
-            try:
-                item = self.log_queue.get_nowait()
-            except queue.Empty:
-                break
-
-            created = max(0.0, float(item.get('created', 0.0)))
-            seconds = int(created)
-            nanoseconds = int((created - seconds) * 1_000_000_000)
-
-            msg = fcmsg.FcNetworkLog()
-            msg.stamp.sec = seconds
-            msg.stamp.nanosec = min(max(nanoseconds, 0), 999_999_999)
-            msg.level = min(max(int(item.get('level', 20)), 0), 255)
-            msg.source = str(item.get('source', 'fc_network'))
-            msg.event_code = str(item.get('event_code', ''))
-            msg.sysid = min(
-                max(int(item.get('sysid', -1)), -2_147_483_648),
-                2_147_483_647
-            )
-            msg.message = str(item.get('message', ''))
-            self.publisher.publish(msg)
-
-    def stop(self):
-        self.running = False
-
-
 # ============================================================================
 #                         ROS2 節點管理器
 # ============================================================================
@@ -1246,11 +1192,10 @@ class fc_ros_manager:
     stop 是停下止 ROS2 nodes 的運行，但不銷毀節點實例，允許後續再次 start。
     shutdown 是完全關閉 ROS2 並銷毀節點實例。
 
-    管理四個獨立的 ROS2 Node :
+    管理三個獨立的 ROS2 Node :
     - VehicleStatusPublisher
     - MavlinkCommandService
     - RtcmRelay
-    - FcNetworkLogPublisher
     
     提供統一的啟動/停止介面給 mainOrchestrator
 
@@ -1270,7 +1215,6 @@ class fc_ros_manager:
         self.status_publisher: Optional[VehicleStatusPublisher] = None
         self.command_service: Optional[MavlinkCommandService] = None
         self.rtcm_relay: Optional[RtcmRelay] = None
-        self.log_publisher: Optional[FcNetworkLogPublisher] = None
         
         # Executor & Thread
         self.spin_thread: Optional[threading.Thread] = None
@@ -1291,19 +1235,12 @@ class fc_ros_manager:
             self.status_publisher = VehicleStatusPublisher()
             self.command_service = MavlinkCommandService()
             self.rtcm_relay = RtcmRelay()
-            if hasattr(fcmsg, 'FcNetworkLog'):
-                self.log_publisher = FcNetworkLogPublisher()
-            else:
-                logger.warning(
-                    "FcNetworkLog interface unavailable; /fc_network/logs disabled")
     
             # 創建執行者 MultiThreadedExecutor 並把 node 加入其中
             self.executor = MultiThreadedExecutor()
             self.executor.add_node(self.status_publisher)
             self.executor.add_node(self.command_service)
             self.executor.add_node(self.rtcm_relay)
-            if self.log_publisher:
-                self.executor.add_node(self.log_publisher)
             
             self.initialized = True
             # logger.info("fc_ros_manager initialized")
@@ -1329,8 +1266,6 @@ class fc_ros_manager:
             self.status_publisher.running = True
             self.command_service.running = True
             self.rtcm_relay.running = True
-            if self.log_publisher:
-                self.log_publisher.running = True
             
             self.spin_thread = threading.Thread(
                 target=self._spin_executor,
@@ -1456,8 +1391,6 @@ class fc_ros_manager:
                 self.command_service.stop()
             if self.rtcm_relay:
                 self.rtcm_relay.stop()
-            if self.log_publisher:
-                self.log_publisher.stop()
             
             # 等待 spin 執行緒結束
             if self.spin_thread and self.spin_thread.is_alive():
@@ -1484,8 +1417,6 @@ class fc_ros_manager:
                     self.command_service.destroy_node()
                 if self.rtcm_relay:
                     self.rtcm_relay.destroy_node()
-                if self.log_publisher:
-                    self.log_publisher.destroy_node()
                 
                 # 關閉 ROS2
                 if rclpy.ok():
@@ -1504,7 +1435,6 @@ class fc_ros_manager:
             'status_publisher_active': self.status_publisher is not None and self.status_publisher.running,
             'command_service_active': self.command_service is not None,
             'rtcm_relay_active': self.rtcm_relay is not None and self.rtcm_relay.running,
-            'log_publisher_active': self.log_publisher is not None and self.log_publisher.running,
         }
 
     
@@ -1559,3 +1489,4 @@ TODO
 1. service 部分會需要跟 mavlinkobject 大量互動 也許需要考慮對方的生命週期
 
 '''
+

src/fc_network_adapter/fc_network_adapter/serialManager.py

@@ -37,7 +37,7 @@ from .utils import pollStrategy
 # ====================== 分割線 =====================
 
 logger = setup_logger(os.path.basename(__file__))
-MODULE_VER = "2.00"
+MODULE_VER = "2.02"
 
 rx_module_ack = RingBuffer(capacity=64, buffer_id=253)
 
@@ -147,10 +147,13 @@ class XBeeFrameProcessor_Base(FrameProcessor):
 
     DEST_ADDR64_BRAODCAST = b'\x00\x00\x00\x00\x00\x00\x00\x00' 
     
+
     def __init__(self, at_handler: "ATCommandHandler" = None):
         super().__init__()
         self.at_handler = at_handler
 
+
+
     # ---- 對外契約 ----
     def process_incoming(self, data: bytes) -> bytes:
         """處理 XBee API 幀並提取 payload"""
@@ -231,7 +234,7 @@ class XBeeFrameProcessor_Base(FrameProcessor):
     def _encapsulate(
         data: bytes,
         dest_addr64: bytes = DEST_ADDR64_BRAODCAST,
-        dest_addr16 = DEST_ADDR16_BRAODCAST,
+        dest_addr16: bytes = DEST_ADDR16_BRAODCAST,
         frame_id: int = 0x01,
     ) -> bytes:
         """
@@ -247,6 +250,8 @@ class XBeeFrameProcessor_Base(FrameProcessor):
         frame += dest_addr64 + dest_addr16
         frame += struct.pack(">BB", broadcast_radius, options) + data
         checksum = 0xFF - (sum(frame) & 0xFF)
+        # ret = b'\x7E' + struct.pack(">H", len(frame)) + frame + struct.pack("B", checksum) 
+        # logger.debug(ret.hex())
         return b'\x7E' + struct.pack(">H", len(frame)) + frame + struct.pack("B", checksum)
 
     @staticmethod
@@ -290,10 +295,7 @@ class XBeeFrameProcessor_ESPv1(XBeeFrameProcessor_Base):
     MAX_PAYLOAD_PER_FRAME = 80
     CHUNK_SEND_INTERVAL_SEC = 0.01
 
-    # ADDR16 選項
-    DEST_ADDR16_BRAODCAST = b'\xFF\xFF'
    
-    DEST_ADDR64_BRAODCAST = b'\x00\x00\x00\x00\x00\x00\xFF\xFF'
 
     class Esp32DeviceInfo:
         def __init__(self, system_id, address_64, last_hello_time):
@@ -306,6 +308,8 @@ class XBeeFrameProcessor_ESPv1(XBeeFrameProcessor_Base):
             self.last_done_time = 0.0 # 最後送出Done的時間
             self.received_len = 0 # 收到封包累計
 
+            
+
     def __init__(self, at_handler: "ATCommandHandler" = None):
         super().__init__(at_handler)
 
@@ -316,6 +320,10 @@ class XBeeFrameProcessor_ESPv1(XBeeFrameProcessor_Base):
         self.operator_busy = False
         self.operator_running = False
 
+        # ADDR16 選項
+        self.DEST_ADDR16_BRAODCAST = b'\xFF\xFE'
+        self.DEST_ADDR64_BRAODCAST = b'\x00\x00\x00\x00\x00\x00\xFF\xFF'
+
         self.serial_writer: Optional[Callable[[bytes], None]] = None
         self.event_loop: Optional[asyncio.AbstractEventLoop] = None
         self.serial_baudrate = 115200
@@ -329,8 +337,8 @@ class XBeeFrameProcessor_ESPv1(XBeeFrameProcessor_Base):
 
         self.last_discovery_time = 0.0 # 這個是最後做廣播 discovery 的時間  
         self.last_recieve_mavlink = 0.0 # 這個是最後收到 mavlink payload 時間 為了定義 poll-done 之間不要超時用的
-        self.MAX_mavPack_interval_timeout = 100  # ms，poll 期間 MAVLink/DONE 最大閒置間隔
+        self.mavPack_interval_timeout = 150  # ms，poll 期間 MAVLink/DONE 最大閒置間隔
-        self.discovery_interval_seconds = 30.0 # 每次做 discovery 程序的間隔時間
+        self.discovery_interval_seconds = 200.0 # 每次做 discovery 程序的間隔時間
         self.device_offline_timeout = self.discovery_interval_seconds * 2 # 遠端沒有回應會被踢出 超時時限
         self.operator_tick_interval_seconds = 0.03 # 
         self.guard_milliseconds = 50 # POLL DONE 的保底時間間隔
@@ -390,12 +398,12 @@ class XBeeFrameProcessor_ESPv1(XBeeFrameProcessor_Base):
             return None
 
         if frame_type == self.FRAME_TYPE_TX_STATUS:
-            length = (frame[1] << 8) | frame[2]
+            # length = (frame[1] << 8) | frame[2]
-            logger.debug(
+            # logger.debug(
-                f"TX Status raw={frame.hex()}, api_len={length}, "
+            #     f"TX Status raw={frame.hex()}, api_len={length}, "
-                f"fid=0x{frame[4]:02X}, dest16=0x{(frame[5]<<8)|frame[6]:04X}, "
+            #     f"fid=0x{frame[4]:02X}, dest16=0x{(frame[5]<<8)|frame[6]:04X}, "
-                f"retry={frame[7]}, delivery={frame[8]}, discovery={frame[9]}"
+            #     f"retry={frame[7]}, delivery={frame[8]}, discovery={frame[9]}"
-            )
+            # )
             return None
 
         logger.warning(f"Unknown XBee frame type: 0x{frame_type:02X}")
@@ -404,7 +412,8 @@ class XBeeFrameProcessor_ESPv1(XBeeFrameProcessor_Base):
     # ---- DISC / POLL 封裝 ----
 
     def pack_discovery(self) -> bytes:
-        return self._encapsulate(self.DISC_HEADER, frame_id=0x00)
+        # logger.debug(f"pack discovery")
+        return self._encapsulate(self.DISC_HEADER, dest_addr16 = self.DEST_ADDR16_BRAODCAST, dest_addr64 = self.DEST_ADDR64_BRAODCAST , frame_id=0x00)
 
     # 處理每個裝置回傳的 Hello 訊息
     def handle_hello_report(self, payload: bytes, sender_address_64: bytes) -> None:   
@@ -450,8 +459,11 @@ class XBeeFrameProcessor_ESPv1(XBeeFrameProcessor_Base):
         if remote_device is None:
             return
 
+
+        system_id, sent_length, remain_length = struct.unpack('>BHH', payload[4:9])
+        remote_device.remain_bytes = remain_length
+
         # 這段是有問題的 因為會有整數封包切割問題 以及載具端的 buffer 存量不足 故回傳的資訊量會與要求的不一致
-        # system_id, sent_length, remain_length = struct.unpack('>BHH', payload[4:9])
         # if sent_length != remote_device.received_len:
         #     logger.info(
         #         f"POLL may be missing packets sent={sent_length} "
@@ -461,7 +473,6 @@ class XBeeFrameProcessor_ESPv1(XBeeFrameProcessor_Base):
         # TODO 傳送速率
         # TODO 累積速率預測
         remote_device.received_len = 0
-        remote_device.remain_bytes = remain_length
         remote_device.last_done_time = time.time()
 
         if (
@@ -582,12 +593,9 @@ class XBeeFrameProcessor_ESPv1(XBeeFrameProcessor_Base):
     def _should_run_discovery(self) -> bool:
         # 條件1. 目前沒有任何遠端ESP裝置被紀錄 或者 手動啟動
         if (not self.esp32_address_mapping) or (self.pending_manual_discovery):
-            return True
+            return (time.time() - self.last_discovery_time) > 2
         # 條件2. 每個固定週期 會做一次
-        return (
+        return (time.time() - self.last_discovery_time) > self.discovery_interval_seconds
-            time.time() - self.last_discovery_time
-            >= self.discovery_interval_seconds
-        )
 
     # ---- 手動請求（thread-safe 對外 API）----
 
@@ -754,19 +762,20 @@ class XBeeFrameProcessor_ESPv1(XBeeFrameProcessor_Base):
         self.pending_manual_discovery = False
 
     async def _wait_poll_done_with_idle_timeout(self) -> bool:
-        idle_timeout_sec = self.MAX_mavPack_interval_timeout / 1000.0
+        idle_timeout_sec = self.mavPack_interval_timeout / 1000.0
-        poll_tick = min(0.02, idle_timeout_sec / 2)
+        poll_tick = min(0.02, idle_timeout_sec)
 
         while not self.poll_done_event.is_set():
             if time.time() - self.last_recieve_mavlink >= idle_timeout_sec:
                 return False
-            try:
+            # try:
-                await asyncio.wait_for(
+            await asyncio.wait_for(
-                    self.poll_done_event.wait(),
+                self.poll_done_event.wait(),
-                    timeout=poll_tick,
+                timeout=poll_tick,
-                )
+            )
-            except asyncio.TimeoutError:
+            
-                continue
+            # except asyncio.TimeoutError:
+                # continue
         return True
 
     # poll 程序
@@ -1374,51 +1383,51 @@ if __name__ == '__main__':
     # UDP_REMOTE_PORT = 14571
     # sm.create_serial_link(SERIAL_PORT, SERIAL_BAUDRATE, UDP_REMOTE_PORT, SerialMode.STRAIGHT)
 
-    # 測試項二
+    # # 測試項二
-    print("運行 測試項二")
+    # print("運行 測試項二")
-    SERIAL_PORT = '/dev/ttyUSB0'  # 手動指定
+    # SERIAL_PORT = '/dev/ttyUSB0'  # 手動指定
-    SERIAL_BAUDRATE = 115200
+    # SERIAL_BAUDRATE = 115200
-    UDP_REMOTE_PORT = 14561
+    # UDP_REMOTE_PORT = 14561
-    sm.create_serial_link(SERIAL_PORT, SERIAL_BAUDRATE, UDP_REMOTE_PORT, SerialMode.XBEEAPI2AT)
+    # sm.create_serial_link(SERIAL_PORT, SERIAL_BAUDRATE, UDP_REMOTE_PORT, SerialMode.XBEEAPI2AT)
-    serial_id = 1
+    # serial_id = 1
-    device_sys_id = 10
+    # device_sys_id = 10
 
-    linked_serial = sm.get_serial_link()
+    # linked_serial = sm.get_serial_link()
-    print(f"連結完成 : {linked_serial}. 等待兩秒")
+    # print(f"連結完成 : {linked_serial}. 等待兩秒")
 
-    # 等 connection_made 完成 writer 注入，再發一筆 AT 指令測試
+    # # 等 connection_made 完成 writer 注入，再發一筆 AT 指令測試
-    time.sleep(2)
+    # time.sleep(2)
-    rssi_request = ATRequest(command=b'DB', parameter=b'', frame_id=device_sys_id)
+    # rssi_request = ATRequest(command=b'DB', parameter=b'', frame_id=device_sys_id)
-    print(f"手動送出 DB AT Command:")
+    # print(f"手動送出 DB AT Command:")
-    for i in range(20):
+    # for i in range(20):
-        sm.send_at_command(1, rssi_request)
+    #     sm.send_at_command(1, rssi_request)
-        time.sleep(1)
+    #     time.sleep(1)
 
-    sm.remove_serial_link(1)
+    # sm.remove_serial_link(1)
-    time.sleep(2)
+    # time.sleep(2)
-    sm.shutdown()
+    # sm.shutdown()
-    print("結束運行")
+    # print("結束運行")
 
-    # # 測試項三
+    # 測試項三
-    # SERIAL_PORT = '/dev/ttyUSB0'
+    SERIAL_PORT = '/dev/ttyUSB0'
-    # SERIAL_BAUDRATE = 115200
+    SERIAL_BAUDRATE = 115200
-    # UDP_REMOTE_PORT = 14561
+    UDP_REMOTE_PORT = 14561
-    # sm.create_serial_link(SERIAL_PORT, SERIAL_BAUDRATE, UDP_REMOTE_PORT, SerialMode.XBEEAPI_espv1)
+    sm.create_serial_link(SERIAL_PORT, SERIAL_BAUDRATE, UDP_REMOTE_PORT, SerialMode.XBEEAPI_espv1)
 
-    # time.sleep(2)  # 等 serial 連線與 operator 啟動
+    time.sleep(2)  # 等 serial 連線與 operator 啟動
 
-    # serial_id = 1
+    serial_id = 1
-    # processor = sm.get_espv1_processor(serial_id)
+    processor = sm.get_espv1_processor(serial_id)
-    # if processor is not None:
+    if processor is not None:
-    #     processor.request_discovery()
+        processor.request_discovery()
-    #     processor.request_poll(target_system_id=1)
+        processor.request_poll(target_system_id=1)
-    #     processor.request_poll(target_system_id=1, grant_bytes=200)
+        processor.request_poll(target_system_id=1, grant_bytes=200)
-    #     print(processor.get_status_snapshot())
+        print(processor.get_status_snapshot())
-    #     print(processor.get_gcs_queue_byte_count())
+        print(processor.get_gcs_queue_byte_count())
-
+
-    # sm.remove_serial_link(serial_id)
+    time.sleep(120)
-    # time.sleep(30)
+    sm.remove_serial_link(serial_id)
-    # sm.shutdown()
+    sm.shutdown()
 
 '''
 ================= 改版記錄 ============================

src/fc_network_adapter/fc_network_adapter/utils/__init__.py

@@ -2,6 +2,6 @@
 共用工具模組
 """
 from .ringBuffer import RingBuffer
-from .theLogger import get_ros_log_queue, setup_logger
+from .theLogger import setup_logger
 
-__all__ = ['RingBuffer', 'get_ros_log_queue', 'setup_logger']
+__all__ = ['RingBuffer', 'setup_logger']

src/fc_network_adapter/fc_network_adapter/utils/theLogger.py

@@ -1,47 +1,9 @@
 import logging
 import os
-import queue
 from logging.handlers import TimedRotatingFileHandler
 
 # 全域 Logger 實例
 _global_logger = None
-_ros_log_queue = queue.Queue(maxsize=2000)
-
-
-class RosTopicQueueHandler(logging.Handler):
-    """將 FC Network log record 非阻塞地排入 ROS publisher 佇列。"""
-
-    def emit(self, record: logging.LogRecord) -> None:
-        try:
-            sysid = int(getattr(record, 'sysid', -1))
-        except (TypeError, ValueError):
-            sysid = -1
-
-        item = {
-            'created': float(record.created),
-            'level': int(record.levelno),
-            'source': str(record.name),
-            'event_code': str(getattr(record, 'event_code', '')),
-            'sysid': sysid,
-            'message': record.getMessage(),
-        }
-        try:
-            _ros_log_queue.put_nowait(item)
-        except queue.Full:
-            # GUI 不應反過來拖慢通訊；滿載時淘汰最舊一筆。
-            try:
-                _ros_log_queue.get_nowait()
-            except queue.Empty:
-                return
-            try:
-                _ros_log_queue.put_nowait(item)
-            except queue.Full:
-                pass
-
-
-def get_ros_log_queue():
-    """供 ROS publisher node 取得共用、thread-safe 的紀錄佇列。"""
-    return _ros_log_queue
 
 def setup_logger(name: str, log_dir: str = "logs", level=logging.DEBUG) -> logging.Logger:
     global _global_logger
@@ -75,10 +37,6 @@ def setup_logger(name: str, log_dir: str = "logs", level=logging.DEBUG) -> loggi
         console_handler.setFormatter(formatter)
         _global_logger.addHandler(console_handler)
 
-        ros_queue_handler = RosTopicQueueHandler()
-        ros_queue_handler.setLevel(logging.INFO)
-        _global_logger.addHandler(ros_queue_handler)
-
     # 為每個模組建立子 Logger，並設定名稱
     module_logger = _global_logger.getChild(name)
     module_logger.name = name  # 修改子 Logger 的名稱，僅保留子 Logger 名稱