fix(GUI): 多機跑點閉環化，解決實飛只有一台跑點的問題

根因：adapter 的 pos_global_int service 被序列化(預設 mutually-exclusive
callback group) + 阻塞等 msg87 echo。實飛慢速鏈路下多機序列化，只有頭一台
能在 client timeout 內拿到 echo，其餘被判失敗 → 重試 → fallback LOITER 停飛。
(SITL echo 快故全過)。指令其實在等 echo 前就已送達飛控。

閉環改法(全在 src/GUI/，繞過 adapter 瓶頸)：
- mission_executor: 每點只送一次 → 每 resend_interval_sec(預設2.5s) 週期性
  重送當前 setpoint(GUIDED 目標冪等，掉包自癒)；換點純靠 GPS 到達判定；
  _on_send_result 降級為觀測，不再累計失敗切 LOITER。
- command_sender: Ros2CommandSender echo timeout 2.0→0.5s，echo miss 快速
  放棄換下一台，避免序列化塞爆 client poll。
- gui: 建構子接 resend_interval_sec。

順帶：總覽表新增「任務狀態」列，每台一格即時顯示 WP 進度 + 通訊狀態
(送出OK/送出未驗證/等待同伴)，實飛可一眼分辨哪台通訊不穩 vs 卡住；
此列獨立保存，不被遙測 refresh 蓋掉。

SITL 驗證通過。

Co-Authored-By: Claude Opus 4.8 <noreply@anthropic.com>

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__()

src/GUI/communication.py

@@ -69,6 +69,18 @@ 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)
 
@@ -800,6 +812,16 @@ 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 開始）。"""
@@ -969,6 +991,32 @@ 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, ...
@@ -1040,6 +1088,21 @@ class DroneMonitor(Node):
                 lambda msg, sid=sys_id: self.attitude_callback(sid, msg),
                 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)
 
@@ -1299,6 +1362,67 @@ 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,11 +18,9 @@ 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
@@ -148,7 +146,7 @@ class ToggleSwitch(QWidget):
 class ControlStationUI(QMainWindow):
     planning_finished = pyqtSignal(object)
 
-    VERSION = '2.6.0'
+    VERSION = '2.7.1'
     FONT_SCALE_MIN = 70
     FONT_SCALE_MAX = 180
     FONT_SCALE_DEFAULT = 100
@@ -211,8 +209,15 @@ class ControlStationUI(QMainWindow):
         self._attitude_cache = {}
         self._overview_cache = {}
         self._map_dirty_drones = set()
-        self.message_history = []
+        # 「紀錄」分頁分成三個獨立來源；message_history 保留為 GUI 操作紀錄的
+        # 相容別名，避免既有程式仍存取舊屬性時失效。
+        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 = {}
@@ -448,19 +453,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; }
@@ -470,9 +475,7 @@ class ControlStationUI(QMainWindow):
         header_layout.addWidget(clear_btn)
         layout.addLayout(header_layout)
 
-        self.message_history_view = QPlainTextEdit()
-        self.message_history_view.setReadOnly(True)
-        self.message_history_view.setStyleSheet("""
+        history_style = """
             QPlainTextEdit {
                 background-color: #1E1E1E;
                 color: #DDD;
@@ -482,9 +485,67 @@ class ControlStationUI(QMainWindow):
                 font-family: monospace;
                 font-size: 12px;
             }
-        """)
-        self.message_history_view.setPlaceholderText("左下角狀態訊息會顯示在這裡...")
-        layout.addWidget(self.message_history_view)
+        """
+
+        def create_section(section_title, category):
+            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
 
@@ -741,27 +802,114 @@ class ControlStationUI(QMainWindow):
 
         return super().eventFilter(obj, event)
 
-    def _clear_message_history(self):
-        """清空訊息歷史。"""
-        self.message_history.clear()
-        if hasattr(self, 'message_history_view'):
-            self.message_history_view.clear()
+    def _history_list(self, category):
+        return {
+            'drone': self.drone_message_history,
+            'gui': self.gui_operation_history,
+            'fcnetwork': self.fcnetwork_history,
+        }.get(category)
 
-    def _on_status_bar_message_changed(self, message):
-        """同步狀態列訊息到歷史紀錄。"""
+    def _append_history(self, category, message):
+        """加入指定來源的紀錄並讓畫面保持在最新一筆。"""
         if not message:
             return
+
         timestamp = time.strftime("%H:%M:%S")
         entry = f"[{timestamp}] {message}"
-        self.message_history.append(entry)
-        if len(self.message_history) > self.max_message_history:
-            self.message_history = self.message_history[-self.max_message_history:]
+        history = self._history_list(category)
+        if history is None:
+            return
+        history.append(entry)
+        if len(history) > self.max_message_history:
+            del history[:-self.max_message_history]
 
-        if hasattr(self, 'message_history_view'):
-            self.message_history_view.appendPlainText(entry)
-            scrollbar = self.message_history_view.verticalScrollBar()
+        view = getattr(self, '_history_views', {}).get(category)
+        if view is not None:
+            view.appendPlainText(entry)
+            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
@@ -783,10 +931,14 @@ 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
     
 
     # ================================================================================
@@ -1323,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)
@@ -1660,6 +1813,15 @@ 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('_')
@@ -2133,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":

src/GUI/map_layout.py

@@ -377,6 +377,8 @@ class DroneMap:
                 var clickStartPos = null;
                 var lastManualMapMoveAt = 0;
                 var autoCenteringMap = false;
+                const autoFollowPanThresholdRatio = 0.25;
+                var lastAutoFollowPanAt = 0;
 
                 function markManualMapMove() {
                     if (!autoCenteringMap) {
@@ -385,6 +387,42 @@ 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 = [];
@@ -738,13 +776,8 @@ class DroneMap:
 
                 setInterval(() => {
                     if (focusedId && markers[focusedId]) {
-                        if (Date.now() - lastManualMapMoveAt < 1000) {
-                            return;
-                        }
                         var latlng = markers[focusedId].getLatLng();
-                        autoCenteringMap = true;
-                        map.panTo(latlng);
-                        setTimeout(() => { autoCenteringMap = false; }, 300);
+                        autoFollowPanTo(latlng);
                     }
                 }, 1000);
 
@@ -760,6 +793,9 @@ 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,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

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)

src/fc_interfaces/CMakeLists.txt

@@ -15,6 +15,7 @@ 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

@@ -0,0 +1,12 @@
+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

@@ -29,7 +29,7 @@ from .utils import acquireSerial, acquirePort
 from .utils.acquirePort import find_available_port
 
 logger = setup_logger(os.path.basename(__file__))
-PROJECT_VER = "v1.25"
+PROJECT_VER = "v1.20"
 
 class PanelState:
     def __init__(self):
@@ -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":
@@ -490,10 +490,6 @@ class ControlPanel:
                             # 一樣退兩層
                                 menu_stack.pop()
                                 idx_stack.pop()
-                    
-                    elif selected.action == "DO_DISCOVERY_PANEL":
-                        if hasattr(selected, 'serial_id'):
-                            cmd_q.put(("DO_DISCOVERY", selected.serial_id))
 
                     # related with -> 列表選單
                     elif selected.action in ("PREV_PAGE", "NEXT_PAGE"):
@@ -872,7 +868,6 @@ class ControlPanel:
                 obj_menu = MenuNode(f"Serial #{serial_id}", f"連結口 {serial_id}", None, children=[
                     MenuNode("Info", "查看詳細資訊", "INSPECT_LINKED_SERIAL"),
                     MenuNode("Remove", "移除此連結口", "REMOVE_LINKED_SERIAL"),
-                    MenuNode("Discovery", "搜尋遠端RF(Only XBEE-ESP32)", "DO_DISCOVERY_PANEL"),
                     # MenuNode("Change UDP Target", "變更目標 UDP (工程)", "CHANGE_LINKED_SERIAL_TARGET"),
                     MenuNode("GoUp", "回到列表", "BACK"),
                 ])
@@ -1324,11 +1319,6 @@ class Orchestrator:
                         elif action == "REMOVE_LINKED_SERIAL":
                             serial_id = cmd[1]
                             self.plumber.remove_serial_link(serial_id)
-                        elif action == "DO_DISCOVERY":
-                            serial_id = cmd[1]
-                            processor = self.plumber.get_espv1_processor(serial_id)
-                            if processor:
-                                processor.request_discovery()
 
                         elif action == "INSPECT_VEHICLE":
                             sysid, compid = cmd[1], cmd[2]
@@ -1610,7 +1600,7 @@ class Orchestrator:
         # 定義通訊類型映射表
         COMM_TYPE_MAP = {
             "XBee(API-AT)": sm.SerialMode.XBEEAPI2AT,
-            "XBee(API-API)espv1": sm.SerialMode.XBEEAPI_espv1,
+            "XBee(API-AT)": sm.SerialMode.XBEEAPI_espv1,
             "TELEMETRY": sm.SerialMode.STRAIGHT,
             # 新增區
         }
@@ -1696,8 +1686,4 @@ if __name__ == "__main__":
 2025.05.28
 1. 工程模式 新增 Node 重啟的功能
 
-2026.08.05 (ver 1.25)
-1. 增加對於 esp32 連線模式的支援
-
-
 '''

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,6 +8,7 @@ MAVLink ROS2 Nodes
     並不會包含額外的功能
 3. RtcmRelay - 訂閱 RTCM topic 並轉發為 MAVLink GPS_RTCM_DATA 給所有載具
     過期丟棄、去重、節流、分片
+4. FcNetworkLogPublisher - 將 FC Network logging queue 發布到 /fc_network/logs
 
 與一個節點管理器
 - fc_ros_manager
@@ -18,6 +19,7 @@ import os
 import time
 import math
 import hashlib
+import queue
 import threading
 from typing import Dict, Optional
 
@@ -42,7 +44,7 @@ from fc_interfaces.msg import ServiceAckResult
 # 自定義 imports
 from . import mavlinkVehicleView as mvv
 from . import mavlinkObject as mo
-from .utils import setup_logger
+from .utils import get_ros_log_queue, setup_logger
 
 logger = setup_logger(os.path.basename(__file__))
 MODULE_VER = "2.50"
@@ -1179,6 +1181,58 @@ 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 節點管理器
 # ============================================================================
@@ -1192,10 +1246,11 @@ class fc_ros_manager:
     stop 是停下止 ROS2 nodes 的運行，但不銷毀節點實例，允許後續再次 start。
     shutdown 是完全關閉 ROS2 並銷毀節點實例。
 
-    管理三個獨立的 ROS2 Node :
+    管理四個獨立的 ROS2 Node :
     - VehicleStatusPublisher
     - MavlinkCommandService
     - RtcmRelay
+    - FcNetworkLogPublisher
     
     提供統一的啟動/停止介面給 mainOrchestrator
 
@@ -1215,6 +1270,7 @@ 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
@@ -1235,12 +1291,19 @@ 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")
@@ -1266,6 +1329,8 @@ 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,
@@ -1391,6 +1456,8 @@ 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():
@@ -1417,6 +1484,8 @@ 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():
@@ -1435,6 +1504,7 @@ 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,
         }
 
     
@@ -1489,4 +1559,3 @@ TODO
 1. service 部分會需要跟 mavlinkobject 大量互動 也許需要考慮對方的生命週期
 
 '''
-

src/fc_network_adapter/fc_network_adapter/mavlinkVehicleView.py

@@ -37,7 +37,6 @@ class ComponentType(Enum):
 class RFModuleType(Enum):
     """RF模組類型"""
     XBEE = "xbee"
-    XBEE_ESP = "xbee_esp"
     UDP = "udp"
     TCP = "tcp"
     OTHER = "other"
@@ -182,7 +181,6 @@ class RFStatus:
     at_response: Optional[str] = None  # AT 命令回應
     link_quality: Optional[int] = None  # 連接品質
     timestamp: Optional[float] = None  # 時間戳記
-    avg_bandwidth: Optional[float] = None # 量測到的傳輸速度 bytes/ms
     custom_status: Dict[str, Any] = field(default_factory=dict)  # 其他自定義狀態
 
 
@@ -276,18 +274,6 @@ class RFModule:
         self.type = rf_type
         self.status = RFStatus()
         self.socket_info = SocketInfo()
-
-        # # only for ESP32
-        # self.poll_data_times: deque[int] = None
-        # self.poll_data_sizes: deque[int] = None
-        # self.poll_total_time = None
-        # self.poll_total_size = None
-        # self.remain_bytes = None
-
-    def update_avg_bandwidth(self, bandwidth: float, timestamp: Optional[float] = None) -> None:
-        self.status.avg_bandwidth = bandwidth
-        if timestamp:
-            self.status.timestamp = timestamp
     
     def update_rssi(self, rssi: int, timestamp: Optional[float] = None) -> None:
         """更新RSSI"""

src/fc_network_adapter/fc_network_adapter/utils/__init__.py

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

src/fc_network_adapter/fc_network_adapter/utils/pollStrategy.py

@@ -7,54 +7,42 @@ POLL 輪詢策略，供 XBeeFrameProcessor_ESPv1 使用。
 from dataclasses import dataclass
 from typing import List
 
-MIN_TRIGGER_INTERVAL_MS = 5
 
 @dataclass(frozen=True)
 class PollDevice:
     """從 esp32AddrMapping 抽出的唯讀快照。"""
     address_64: bytes
+    # system_id: int
     remain_bytes: int
-    last_poll_time: float
+    last_done_time: float
 
 
-def pick_next(devices: List[PollDevice], now_time):
+@dataclass
+class PollSchedulerState:
+    """每條 serial link 一份，保存 round-robin 索引。"""
+    round_robin_index: int = 0
+
+
+def pick_next(
+    devices: List[PollDevice],
+    scheduler_state: PollSchedulerState
+):
     """
     選下一個 POLL 目標。
 
     回傳 (target_address_64, grant_bytes)；devices 為空時回傳 (None, 0)。
-
-    poll time = 0.2305 * grant_bytes + 58.094 (R平方為0.979 高線性相關)  @-40db 
     """
     if not devices:
         return None, 0
 
-    grant_bytes = 0
     device_count = len(devices)
-    weights = [0] * device_count
+    selected_index = scheduler_state.round_robin_index % device_count
+    selected_device = devices[selected_index]
+    scheduler_state.round_robin_index += 1
 
-    for i in range(device_count):
-        gap = int((now_time - devices[i].last_poll_time)*1000)
-        # # 時間大於觸發間隔 才會計算
-        # if gap >= MIN_TRIGGER_INTERVAL_MS:
-        #     weights[i] += devices[i].remain_bytes
-        #     weights[i] += gap
-
-        weights[i] += devices[i].remain_bytes
-        weights[i] += gap
-
-    maxarg = weights.index(max(weights))
-
-    if weights[maxarg] == 0:
-        return None, 0
-    
-    selected_device = devices[maxarg]
-
-    if selected_device.remain_bytes < 500:
-        grant_bytes = 170  # 這個大概是 100 ms 總傳輸時間
-    elif selected_device.remain_bytes < 1200:
-        grant_bytes = 610  # 這個大概是 200 ms 總傳輸時間
-    else:
-        grant_bytes = 1050 # 這個大概是 300 ms 總傳輸時間
+    grant_bytes = 0
+    if selected_device.remain_bytes > 0:
+        grant_bytes = min( 65535, max(0, selected_device.remain_bytes))
 
     return selected_device.address_64, grant_bytes
 

src/fc_network_adapter/fc_network_adapter/utils/theLogger.py

@@ -1,9 +1,47 @@
 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
@@ -37,6 +75,10 @@ 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 名稱

src/unitdev02/unitdev02/devnote.txt

@@ -27,7 +27,6 @@
 python -m fc_network_adapter.tests.test_vehicleStatusPublisher
 python -m fc_network_adapter.tests.test_ringBuffer
 python -m fc_network_adapter.fc_network_adapter.mainOrchestrator
-python -m fc_network_adapter.fc_network_adapter.serialManager
 
 python -m someotherpkg.src.example_takeoff_land
 python -m someotherpkg.src.example_change_mode

src/unitdev02/unitdev02/esp32.py

@@ -1,641 +0,0 @@
-from machine import UART
-import time
-import struct
-import gc
-
-# =========================================================
-# ESP32 / MicroPython：UAV 端 XBee <-> Flight Controller Bridge
-# Packet-size TDMA + DONE + SYSID / GCS address 自動學習版
-#
-# 本版已刪除「一般 DISC / 強制 DSCF」雙模式。
-# 現在只保留一種 discovery 封包，名稱統一叫 DISC。
-#
-# 重要：
-# - DISC 的功能等同於原本的 DSCF，也就是「強制 discovery」。
-# - UAV 收到 DISC 後，只要已知 MY_SYSID 與 DEST_64，就會重新回 HELO。
-# - 若收到 DISC 時 MY_SYSID 尚未學到，會設定 DISC_PENDING；
-#   之後一旦從飛控 MAVLink 學到 SYSID，就自動補送 HELO。
-#
-# 封包格式：
-# GCS -> UAV:
-#   DISC
-#   POLL + target_sysid(1) + grant_bytes(2)
-#
-# UAV -> GCS:
-#   HELO + sysid(1)
-#   DONE + sysid(1) + sent_len(2) + remain_len(2)
-#
-# 注意：
-# - HELO 不帶 remain。
-# - DONE 保留 remain。
-# - MY_SYSID 不寫死，由飛控 MAVLink 自動學。
-# - DEST_64 不寫死，由 GCS 封包的 XBee 0x90 src64 自動學。
-# =========================================================
-
-
-# ================= 設定區 =================
-FC_BAUDRATE = 115200
-XB_BAUDRATE = 115200
-
-DEST_64 = None
-MY_SYSID = None
-
-SYSID_LEARN_CONFIRM_COUNT = 3
-_sysid_candidate = None
-_sysid_candidate_count = 0
-
-# 若收到 DISC 時還沒學到 MY_SYSID，就先記住。
-# 等之後從 FC MAVLink 學到 MY_SYSID 後，自動補送 HELO。
-DISC_PENDING = False
-
-XBEE_MAX_PAYLOAD = 100
-
-# Discovery：只保留 DISC；功能等同原本 DSCF，收到後強制回 HELO。
-DISC_MAGIC = b'DISC'
-
-# UAV -> GCS hello：
-# HELO + sysid(1)
-HELLO_MAGIC = b'HELO'
-
-HELLO_MIN_INTERVAL_MS = 1000
-_last_hello_ms = 0
-
-# 保留此變數只作狀態記錄；本版 DISC 會 force=True，所以不會被 HELLO_SENT 擋掉。
-HELLO_SENT = False
-
-POLL_MAGIC = b'POLL'
-DEFAULT_GRANT_BYTES = 600
-
-DONE_MAGIC = b'DONE'
-
-MAX_BUF_SIZE = 6144
-READ_FC_BYTES = 250
-
-FC_UART_ID = 1
-FC_TX_PIN = 32
-FC_RX_PIN = 33
-
-XB_UART_ID = 2
-XB_TX_PIN = 25
-XB_RX_PIN = 26
-# =========================================
-
-
-uart_fc = UART(FC_UART_ID, baudrate=FC_BAUDRATE, tx=FC_TX_PIN, rx=FC_RX_PIN, rxbuf=4096)
-uart_xb = UART(XB_UART_ID, baudrate=XB_BAUDRATE, tx=XB_TX_PIN, rx=XB_RX_PIN, rxbuf=4096)
-
-tx_buf = bytearray()  # FC -> GCS 等待 TDMA poll 的 MAVLink stream
-rx_buf = bytearray()  # XBee API frame parser buffer
-
-
-# =========================================================
-# 基本工具函式
-# =========================================================
-
-def get_checksum(data):
-    return 0xFF - (sum(data) & 0xFF)
-
-
-def is_valid_addr64(addr):
-    if addr is None:
-        return False
-    if len(addr) != 8:
-        return False
-    if addr == b'\x00\x00\x00\x00\x00\x00\x00\x00':
-        return False
-    return True
-
-
-def learn_dest64_from_src64(src64):
-    """
-    UAV 收到 GCS 的 XBee 0x90 frame 時，
-    0x90 裡面的 src64 就是 GCS / coordinator XBee 的 64-bit address。
-    """
-    global DEST_64
-    global HELLO_SENT
-    global DISC_PENDING
-
-    if not is_valid_addr64(src64):
-        return False
-
-    src64 = bytes(src64)
-
-    if DEST_64 is None:
-        DEST_64 = src64
-        HELLO_SENT = False
-        return True
-
-    if DEST_64 != src64:
-        DEST_64 = src64
-        HELLO_SENT = False
-        DISC_PENDING = False
-        return True
-
-    return False
-
-
-def build_api_tx_frame(payload):
-    """
-    建立 XBee API frame type 0x10，把 payload 送到 DEST_64。
-    若 DEST_64 尚未學到，回傳 None。
-    """
-    if DEST_64 is None:
-        return None
-
-    frame_content = bytearray()
-    frame_content.append(0x10)
-    frame_content.append(0x00)
-    frame_content.extend(DEST_64)
-    frame_content.extend(b'\xFF\xFE')
-    frame_content.append(0x00)
-    frame_content.append(0x00)
-    frame_content.extend(payload)
-
-    length = len(frame_content)
-
-    packet = bytearray()
-    packet.append(0x7E)
-    packet.append((length >> 8) & 0xFF)
-    packet.append(length & 0xFF)
-    packet.extend(frame_content)
-    packet.append(get_checksum(frame_content))
-
-    return packet
-
-
-def send_to_xbee_chunked(payload):
-    """
-    把 payload 依 XBEE_MAX_PAYLOAD 切成多個 XBee API TX frame 送出。
-    """
-    if DEST_64 is None:
-        return False
-
-    total_len = len(payload)
-    sent_len = 0
-
-    while sent_len < total_len:
-        end_len = min(sent_len + XBEE_MAX_PAYLOAD, total_len)
-        chunk = payload[sent_len:end_len]
-        sent_len = end_len
-
-        pkt = build_api_tx_frame(chunk)
-        if pkt is None:
-            return False
-
-        uart_xb.write(pkt)
-        time.sleep_ms(1)
-
-    return True
-
-
-# =========================================================
-# HELO / DONE 回報
-# =========================================================
-
-def send_hello_report(force=False):
-    """
-    UAV 回報自己存在：
-        HELO + MY_SYSID
-
-    本版 DISC 預設以 force=True 呼叫，因此每次收到 DISC 都會重新回 HELO。
-    """
-    global _last_hello_ms
-    global HELLO_SENT
-
-    if MY_SYSID is None:
-        return False
-    if DEST_64 is None:
-        return False
-
-    if HELLO_SENT and not force:
-        return False
-
-    now = time.ticks_ms()
-    if not force:
-        if time.ticks_diff(now, _last_hello_ms) < HELLO_MIN_INTERVAL_MS:
-            return False
-
-    payload = HELLO_MAGIC + struct.pack('>B', MY_SYSID)
-    pkt = build_api_tx_frame(payload)
-    if pkt is None:
-        return False
-
-    uart_xb.write(pkt)
-    _last_hello_ms = now
-    HELLO_SENT = True
-
-    time.sleep_ms(1)
-    return True
-
-
-def try_send_pending_hello():
-    """
-    若先收到 DISC、後學到 MY_SYSID，這裡會自動補送 HELO。
-    """
-    global DISC_PENDING
-
-    if DISC_PENDING and MY_SYSID is not None and DEST_64 is not None:
-        delay_ms = 20 + (((MY_SYSID * 37) + (time.ticks_ms() & 0xFF)) % 180)
-        time.sleep_ms(delay_ms)
-
-        if send_hello_report(force=True):
-            DISC_PENDING = False
-
-
-def send_done_report(sent_len, remain_len):
-    """
-    DONE payload:
-        b'DONE' + sysid(1) + sent_len(2) + remain_len(2)
-    """
-    if MY_SYSID is None:
-        return False
-    if DEST_64 is None:
-        return False
-
-    if sent_len > 65535:
-        sent_len = 65535
-    if remain_len > 65535:
-        remain_len = 65535
-
-    payload = DONE_MAGIC + struct.pack('>BHH', MY_SYSID, sent_len, remain_len)
-    pkt = build_api_tx_frame(payload)
-    if pkt is None:
-        return False
-
-    uart_xb.write(pkt)
-    time.sleep_ms(1)
-    return True
-
-
-# =========================================================
-# MAVLink frame 解析與 MY_SYSID 自動學習
-# =========================================================
-
-def find_first_mavlink_magic(buf):
-    pos_fe = buf.find(b'\xFE')
-    pos_fd = buf.find(b'\xFD')
-
-    if pos_fe == -1:
-        return pos_fd
-    if pos_fd == -1:
-        return pos_fe
-    return pos_fe if pos_fe < pos_fd else pos_fd
-
-
-def mavlink_frame_length(buf, start_idx):
-    if start_idx >= len(buf):
-        return None
-
-    magic = buf[start_idx]
-
-    if magic == 0xFE:
-        if len(buf) - start_idx < 2:
-            return None
-        payload_len = buf[start_idx + 1]
-        total_len = payload_len + 8
-        if len(buf) - start_idx < total_len:
-            return None
-        return total_len
-
-    if magic == 0xFD:
-        if len(buf) - start_idx < 3:
-            return None
-        payload_len = buf[start_idx + 1]
-        incompat_flags = buf[start_idx + 2]
-        signed = (incompat_flags & 0x01) != 0
-        total_len = payload_len + 12 + (13 if signed else 0)
-        if len(buf) - start_idx < total_len:
-            return None
-        return total_len
-
-    return None
-
-
-def get_mavlink_sysid(buf, start_idx):
-    if start_idx >= len(buf):
-        return None
-
-    magic = buf[start_idx]
-
-    if magic == 0xFE:
-        if len(buf) - start_idx >= 6:
-            return buf[start_idx + 3]
-
-    if magic == 0xFD:
-        if len(buf) - start_idx >= 10:
-            return buf[start_idx + 5]
-
-    return None
-
-
-def learn_my_sysid_from_tx_buf():
-    """
-    從 FC -> ESP32 的 tx_buf 中找完整 MAVLink frame，
-    並自動學習飛控的 MAVLink SYSID。
-
-    若更改飛控 SYSID，建議重新上電 ESP32，讓 MY_SYSID 重新學習。
-    """
-    global MY_SYSID
-    global _sysid_candidate
-    global _sysid_candidate_count
-    global HELLO_SENT
-    global tx_buf
-
-    if len(tx_buf) == 0:
-        return
-
-    idx = 0
-    checked = 0
-
-    while idx < len(tx_buf) and checked < 8:
-        sub = tx_buf[idx:]
-        rel_start = find_first_mavlink_magic(sub)
-        if rel_start == -1:
-            return
-
-        start = idx + rel_start
-        frame_len = mavlink_frame_length(tx_buf, start)
-        if frame_len is None:
-            return
-
-        sysid = get_mavlink_sysid(tx_buf, start)
-
-        if sysid is not None and sysid > 0:
-            if MY_SYSID is not None:
-                return
-
-            if _sysid_candidate == sysid:
-                _sysid_candidate_count += 1
-            else:
-                _sysid_candidate = sysid
-                _sysid_candidate_count = 1
-
-            if _sysid_candidate_count >= SYSID_LEARN_CONFIRM_COUNT:
-                MY_SYSID = _sysid_candidate
-                HELLO_SENT = False
-                return
-
-        idx = start + frame_len
-        checked += 1
-
-
-# =========================================================
-# tx_buf MAVLink frame 取出與裁切
-# =========================================================
-
-def pop_mavlink_frames_by_quota(quota_bytes):
-    global tx_buf
-
-    if quota_bytes <= 0 or len(tx_buf) == 0:
-        return b''
-
-    start = find_first_mavlink_magic(tx_buf)
-    if start == -1:
-        tx_buf = bytearray()
-        return b''
-
-    if start > 0:
-        tx_buf = tx_buf[start:]
-
-    out = bytearray()
-
-    while len(tx_buf) > 0:
-        if tx_buf[0] not in (0xFE, 0xFD):
-            start = find_first_mavlink_magic(tx_buf)
-            if start == -1:
-                tx_buf = bytearray()
-                break
-            tx_buf = tx_buf[start:]
-
-        frame_len = mavlink_frame_length(tx_buf, 0)
-        if frame_len is None:
-            break
-
-        if len(out) > 0 and (len(out) + frame_len) > quota_bytes:
-            break
-
-        if len(out) == 0 and frame_len > quota_bytes:
-            out.extend(tx_buf[:frame_len])
-            tx_buf = tx_buf[frame_len:]
-            break
-
-        out.extend(tx_buf[:frame_len])
-        tx_buf = tx_buf[frame_len:]
-
-        if len(out) >= quota_bytes:
-            break
-
-    return bytes(out)
-
-
-def trim_tx_buffer_if_needed():
-    global tx_buf
-
-    if len(tx_buf) <= MAX_BUF_SIZE:
-        return
-
-    target_size = MAX_BUF_SIZE // 2
-
-    while len(tx_buf) > target_size:
-        start = find_first_mavlink_magic(tx_buf)
-        if start == -1:
-            tx_buf = bytearray()
-            return
-
-        if start > 0:
-            tx_buf = tx_buf[start:]
-            continue
-
-        frame_len = mavlink_frame_length(tx_buf, 0)
-        if frame_len is None:
-            if len(tx_buf) > target_size:
-                tx_buf = tx_buf[-target_size:]
-            return
-
-        tx_buf = tx_buf[frame_len:]
-
-
-def flush_tx_buffer(grant_bytes):
-    global tx_buf
-
-    if MY_SYSID is None: # TODO 這個重複判斷了 應該刪 不要浪費效率
-        return
-
-    if DEST_64 is None:
-        return
-
-    if grant_bytes <= 0:
-        send_done_report(0, len(tx_buf))
-        return
-
-    data_to_send = pop_mavlink_frames_by_quota(grant_bytes)
-    sent_len = len(data_to_send)
-
-    if sent_len > 0:
-        send_to_xbee_chunked(data_to_send)
-
-    send_done_report(sent_len, len(tx_buf))
-
-
-# =========================================================
-# GCS control payload 解析
-# =========================================================
-
-def parse_poll_payload(real_data):
-    if not real_data.startswith(POLL_MAGIC):
-        return None, None
-
-    if len(real_data) == 5:
-        return real_data[4], DEFAULT_GRANT_BYTES
-
-    if len(real_data) == 7:
-        target_sysid = real_data[4]
-        grant_bytes = (real_data[5] << 8) | real_data[6]
-        return target_sysid, grant_bytes
-
-    return None, None
-
-
-def is_discovery_payload(real_data):
-    return real_data.startswith(DISC_MAGIC)
-
-
-# =========================================================
-# XBee API RX parser
-# =========================================================
-
-def process_xbee_buffer():
-    global rx_buf
-    global DISC_PENDING
-
-    while True:
-        start_pos = rx_buf.find(b'\x7E')
-        if start_pos == -1:
-            rx_buf = bytearray()
-            return
-
-        if start_pos > 0:
-            rx_buf = rx_buf[start_pos:]
-
-        if len(rx_buf) < 3:
-            return
-
-        pkt_len = (rx_buf[1] << 8) | rx_buf[2]
-        total_len = pkt_len + 4
-
-        if pkt_len > 300:
-            rx_buf = rx_buf[1:]
-            continue
-
-        if len(rx_buf) < total_len:
-            return
-
-        packet = rx_buf[3:3 + pkt_len]
-        checksum_recv = rx_buf[3 + pkt_len]
-
-        if get_checksum(packet) == checksum_recv:
-            if len(packet) > 0 and packet[0] == 0x90:
-                # XBee Receive Packet 0x90:
-                # packet = 90 | src64(8) | src16(2) | options(1) | RF data
-                if len(packet) >= 12:
-                    src64 = bytes(packet[1:9])
-                    real_data = packet[12:]
-
-                    # -------------------------------------------------
-                    # DISC：本版唯一 discovery 封包。
-                    # 功能等同原本 DSCF：收到後強制回 HELO。
-                    # -------------------------------------------------
-                    if is_discovery_payload(real_data):
-                        learn_dest64_from_src64(src64)
-                        learn_my_sysid_from_tx_buf() # TODO 加一個 if MY_SYSID is None
-
-                        if MY_SYSID is not None and DEST_64 is not None:
-                            delay_ms = 20 + ((MY_SYSID * 37) + (time.ticks_ms() & 0xFF)) % 180 # TODO 這邊會有多少秒延遲? 為啥是37? 不能21或者17? 20 ~ 199  210 / 30000
-                            time.sleep_ms(delay_ms)
-                            send_hello_report(force=True)
-                            DISC_PENDING = False
-                        else:
-                            DISC_PENDING = True
-
-                    # -------------------------------------------------
-                    # 判斷是不是 POLL
-                    # -------------------------------------------------
-                    else: # TODO 這邊要用 elif 去判斷 POLL
-                        target_sysid, grant_bytes = parse_poll_payload(real_data)
-
-                        if target_sysid is not None:
-                            learn_dest64_from_src64(src64)
-                            learn_my_sysid_from_tx_buf()
-                            try_send_pending_hello() # TODO 這邊三行怪怪的 GCS 下 poll 然後會回應 HELO 這個很怪 然後為何要每次重做上面兩行 拖效率
-
-                            if MY_SYSID is not None and target_sysid == MY_SYSID:
-                                flush_tx_buffer(grant_bytes)
-
-                        else: 
-                            # 一般 GCS -> FC MAVLink 下行資料
-                            if DEST_64 is None: # TODO 這個概念不對 隨便一個 poll 就把本來有做過 DISC 的權限搶走了 危險!!!
-                                learn_dest64_from_src64(src64)
-
-                            if DEST_64 is not None and src64 == DEST_64: # TODO 從 GCS 到 FC 的 mavlink 封包 切開不要跟 POLL 放在同一個判斷
-                                uart_fc.write(real_data)
-
-            rx_buf = rx_buf[total_len:]
-
-        else:
-            rx_buf = rx_buf[1:]
-
-
-
-# =========================================================
-# 初始清理
-# =========================================================
-
-try:
-    gc.collect()
-except Exception:
-    pass
-
-
-# =========================================================
-# 主迴圈
-# =========================================================
-
-while True:
-    try:
-        # -------------------------------------------------
-        # FC -> ESP32 tx buffer
-        # -------------------------------------------------
-        if uart_fc.any():
-            data = uart_fc.read(READ_FC_BYTES)
-            if data:
-                tx_buf.extend(data)
-
-                learn_my_sysid_from_tx_buf()
-                try_send_pending_hello()
-                trim_tx_buffer_if_needed()
-
-        # -------------------------------------------------
-        # XBee -> ESP32
-        # -------------------------------------------------
-        if uart_xb.any():
-            chunk = uart_xb.read()
-            if chunk:
-                rx_buf.extend(chunk)
-
-            process_xbee_buffer()
-
-        time.sleep_ms(1)
-
-    except MemoryError:
-        tx_buf = bytearray()
-        rx_buf = bytearray()
-
-        try:
-            gc.collect()
-        except Exception:
-            pass
-
-        time.sleep_ms(10)
-
-    except Exception:
-        time.sleep_ms(2)

src/unitdev02/unitdev02/ntrip_dev.py

@@ -1,209 +0,0 @@
-# import socket
-
-# def get_rtk2go_source_table():
-#     host = "rtk2go.com"
-#     port = 2101
-#
-#     print(f"正在連線至 {host}:{port} 獲取掛載點列表...\n")
-#     ...
-
-from __future__ import annotations
-
-import socket
-import base64
-import time
-
-
-# ── NMEA GGA（告訴 Caster「我在哪」，觸發 RTCM 推流）────────────────
-
-def nmea_checksum(body: str) -> str:
-    """body 不含 '$' 與 '*checksum'，例如 'GPGGA,123519,...'"""
-    value = 0
-    for ch in body:
-        value ^= ord(ch)
-    return f"{value:02X}"
-
-
-def decimal_to_nmea_dm(deg: float, *, is_latitude: bool) -> tuple[str, str]:
-    """十進位度 → NMEA 的 (d)dmm.mmmm 與半球字元。"""
-    if is_latitude:
-        hemi = "N" if deg >= 0 else "S"
-        deg_width = 2
-    else:
-        hemi = "E" if deg >= 0 else "W"
-        deg_width = 3
-    deg = abs(deg)
-    d = int(deg)
-    m = (deg - d) * 60.0
-    return f"{d:0{deg_width}d}{m:07.4f}", hemi
-
-
-def build_gga_sentence(lat_deg: float, lon_deg: float, alt_m: float = 100.0) -> bytes:
-    """
-    組一筆 $GPGGA 句子（含 checksum），回傳 bytes 可直接 sock.sendall。
-
-    lat_deg / lon_deg：十進位經緯度（北、東為正）。
-    測試時請改成 mount 服務範圍內的近似位置；正式使用應來自 GNSS 真實定位。
-    """
-    utc = time.gmtime()
-    t_str = f"{utc.tm_hour:02d}{utc.tm_min:02d}{utc.tm_sec:02d}.00"
-
-    lat_dm, ns = decimal_to_nmea_dm(lat_deg, is_latitude=True)
-    lon_dm, ew = decimal_to_nmea_dm(lon_deg, is_latitude=False)
-
-    # quality=1（GPS fix）、8 顆星、HDOP=1.0 僅供測試示意
-    body = (
-        f"GPGGA,{t_str},{lat_dm},{ns},{lon_dm},{ew},"
-        f"1,08,1.0,{alt_m:.1f},M,0.0,M,,"
-    )
-    sentence = f"${body}*{nmea_checksum(body)}\r\n"
-    return sentence.encode("ascii")
-
-
-def send_gga(sock: socket.socket, lat_deg: float, lon_deg: float, alt_m: float = 100.0) -> str:
-    """送出 GGA，回傳可讀句子供列印。"""
-    payload = build_gga_sentence(lat_deg, lon_deg, alt_m)
-    sock.sendall(payload)
-    return payload.decode("ascii").strip()
-
-
-# ── NTRIP 連線 + RTCM 接收 ─────────────────────────────────────────
-
-def get_rtcm_payload(
-    host,
-    port,
-    mountpoint,
-    user="",
-    password="",
-    target_ids=None,
-    *,
-    gga_lat=None,
-    gga_lon=None,
-    gga_alt_m=100.0,
-    gga_interval_sec=5.0,
-    filter_ids=True,
-):
-    """
-  連線 NTRIP caster 並解析 RTCM3。
-
-  gga_lat / gga_lon：若兩者皆有設定，握手成功後會週期送出 GGA（許多 Caster 如 SNIP 至少需一次）。
-  gga_interval_sec：GGA 重送間隔（秒）。
-  filter_ids：True 時只印 target_ids 內的 message；False 印全部（方便學習觀察）。
-    """
-    if target_ids is None:
-        target_ids = []
-    need_gga = gga_lat is not None and gga_lon is not None
-
-    sock = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
-    sock.settimeout(10)
-
-    try:
-        sock.connect((host, port))
-
-        auth = base64.b64encode(f"{user}:{password}".encode()).decode()
-        request = f"GET /{mountpoint} HTTP/1.0\r\n"
-        request += "User-Agent: NTRIP PythonClient\r\n"
-        request += f"Authorization: Basic {auth}\r\n"
-        request += "Connection: close\r\n\r\n"
-        sock.sendall(request.encode())
-
-        response = sock.recv(4096)
-        if b"ICY 200 OK" not in response:
-            print(f"連線失敗: {response.decode(errors='ignore')}")
-            return
-
-        print(f"已成功連接至 {mountpoint}")
-        if need_gga:
-            print(
-                f"GGA 模式：每 {gga_interval_sec}s 回報位置 "
-                f"({gga_lat:.6f}, {gga_lon:.6f})，觸發 Caster 推流"
-            )
-        else:
-            print("未設定 GGA（部分 Caster 如 RTK2GO 仍可推流；SNIP/區域站常需 GGA）")
-        print(f"RTCM 過濾目標: {target_ids if filter_ids else '(關閉，顯示全部)'}\n")
-
-        last_gga_time = 0.0
-        buffer = b""
-
-        while True:
-            now = time.time()
-            if need_gga and (now - last_gga_time >= gga_interval_sec):
-                line = send_gga(sock, gga_lat, gga_lon, gga_alt_m)
-                print(f"[GGA] {line}")
-                last_gga_time = now
-
-            try:
-                chunk = sock.recv(4096)
-            except socket.timeout:
-                continue
-
-            if not chunk:
-                print("Caster 關閉連線")
-                break
-
-            buffer += chunk
-
-            while len(buffer) >= 3:
-                if buffer[0] != 0xD3:
-                    buffer = buffer[1:]
-                    continue
-
-                length = ((buffer[1] & 0x03) << 8) | buffer[2]
-                total_len = length + 6
-
-                if len(buffer) < total_len:
-                    break
-
-                packet = buffer[:total_len]
-                buffer = buffer[total_len:]
-                msg_id = (packet[3] << 4) | (packet[4] >> 4)
-
-                if not filter_ids or str(msg_id) in target_ids:
-                    print(f"Found ID: {msg_id} | Length: {total_len} bytes")
-
-    except KeyboardInterrupt:
-        print("\n使用者中斷")
-    except Exception as e:
-        print(f"發生錯誤: {e}")
-    finally:
-        sock.close()
-
-
-if __name__ == "__main__":
-    TARGET_LIST = ["1007", "1230", "1074"]
-
-    # ── Case 1：RTK2GO（通常可不送 GGA）────────────────────────────
-    # USER = "chiyu1468@hotmail.com"
-    # HOST = "rtk2go.com"
-    # PORT = 2101
-    # MOUNT = "No1bio_02"
-    # get_rtcm_payload(
-    #     HOST, PORT, MOUNT,
-    #     user=USER,
-    #     target_ids=TARGET_LIST,
-    #     filter_ids=False,
-    # )
-
-    # ── Case 2：區域 Caster（需 GGA，座標請改成服務區內近似點）──────
-    USER = "uavlab6061"
-    PW = "iamsupersmart"
-    HOST = "210.241.63.193"
-    PORT = 81
-    MOUNT = "2020_GNSS"
-    # 十進位度；請依 SNIP / 基準站服務範圍調
-    GGA_LAT = 24.155792000
-    GGA_LON = 120.630679000
-
-    get_rtcm_payload(
-        HOST,
-        PORT,
-        MOUNT,
-        user=USER,
-        password=PW,
-        target_ids=TARGET_LIST,
-        gga_lat=GGA_LAT,
-        gga_lon=GGA_LON,
-        gga_alt_m=100.0,
-        gga_interval_sec=30.0,
-        filter_ids=False,
-    )

src/unitdev02/unitdev02/tryByteDict.py

@@ -1,33 +0,0 @@
-import timeit
-
-# 建立 1000 個元素的字典，避免單一 Key 的特殊雜湊值干擾
-d_int = {i: True for i in range(1000)}
-d_bytes = {f"k_{i:06d}".encode(): True for i in range(1000)}
-
-# 正確命中中間的值
-target_int = 500
-target_bytes_same = f"k_{500:06d}".encode()
-
-# 將變數傳入 timeit 的全域環境
-setup_env = lambda: None
-globals_dict = {
-    'd_int': d_int,
-    'd_bytes': d_bytes,
-    'target_int': target_int,
-    'target_bytes_same': target_bytes_same
-}
-
-# 1. int 查詢
-t_int = timeit.timeit("d_int.get(target_int)", globals=globals_dict, number=50000000)
-
-# 2. bytes 查詢 (同物件：直接傳入現成變數)
-t_bytes_same = timeit.timeit("d_bytes.get(target_bytes_same)", globals=globals_dict, number=50000000)
-
-# 3. bytes 查詢 (真・新物件：每次都在查詢時動態切片生成新物件，強迫重新計算 Hash)
-globals_dict['dynamic_source'] = f"k_{500:06d}".encode() + b"dummy"
-t_bytes_new = timeit.timeit("d_bytes.get(dynamic_source[:8])", globals=globals_dict, number=50000000)
-
-print(f"修正後測試結果 (50,000,000 次)：")
-print(f"int 查詢          : {t_int:.4f} 秒 (1.0x)")
-print(f"bytes 查詢 (同物件): {t_bytes_same:.4f} 秒 (約 {t_bytes_same/t_int:.2f}x)")
-print(f"bytes 查詢 (新物件): {t_bytes_new:.4f} 秒 (約 {t_bytes_new/t_int:.2f}x)")