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>

README.md

@@ -67,6 +67,8 @@ cd ~/AirTrapMine/src/ # 這是範例!!!
 python -m fc_network_adapter.fc_network_adapter.mainOrchestrator
 python -m fc_network_adapter.tests.demo_integration
 python -m someotherpkg.src.example_wholeMoving
+
+python fc_network_apps/vehicleDiagShow.py
 ```
 
 2. 
@@ -83,17 +85,16 @@ python gui.py
     建立、維護與飛控韌體的連接 
     構築 mavlink 封包
     處理無線模組的通訊格式 (XBee)
-    --同時處理與 Gazebo 的 ardupilot_plugin 溝通的 FDM/JSON 訊息 (移除)--
+2. fc_interfaces (必要)
-2. fc_interfaces (重要)
+    自定義的 ROS2 介面檔 沒有這個核心會運作不了
-    自定義的 ROS2 介面檔 沒啥好說的 沒有這個核心會運作不了
 3. fc_network_module (重要)
     非核心 但是支援載具的重要附屬功能 
     需要該功能時 作為一個 ros2 節點打開
     例如 : ntrip rtk 訊號轉接
 4. fc_network_apps
-    與 fc_network_adapter 銜接做高階功能包裝的應用小程式 
+    是 fc_network_adapter 高階包裝API
     利於開發GUI或其他應用 使用者的外層包裝
-    這裡的定位是 "核心功能的高階包裝" 可以完全不去用
+    可以不使用 或者當作一個範例程式來看
 5. someotherpkg
     如何使用 fc_network_apps 的範例檔案
 6. GUI 

serial_udp_update.py

@@ -1,193 +0,0 @@
-import asyncio
-import serial_asyncio
-import struct
-import os
-import sys
-import serial
-import signal
-import traceback
-from pymavlink import mavutil
-
-# === 設定區 ===
-SERIAL_PORT = 'COM15'  # 手動指定
-SERIAL_BAUDRATE = 57600
-UDP_REMOTE_IP = '127.0.0.1'
-UDP_REMOTE_PORT = 14550
-DEBUG_MODE = False
-TARGET_ADDR64 = b'\x00\x00\x00\x00\x00\x00\xFF\xFF'  # 廣播
-
-# === 工具函數 ===
-def check_serial_port():
-    try:
-        ser = serial.Serial(SERIAL_PORT, SERIAL_BAUDRATE)
-        ser.close()
-        return True
-    except serial.SerialException as e:
-        print(f"錯誤：串口設備 {SERIAL_PORT} 被占用或無法訪問：{str(e)}")
-        return False
-    except Exception as e:
-        print(f"錯誤：檢查串口時發生未知錯誤：{str(e)}")
-        return False
-
-def build_api_tx_frame(data: bytes, dest_addr64: bytes, frame_id=0x01) -> bytes:
-    frame_type = 0x10
-    dest_addr16 = b'\xFF\xFE'
-    broadcast_radius = 0x00
-    options = 0x00
-
-    frame = struct.pack(">B", frame_type) + struct.pack(">B", frame_id)
-    frame += dest_addr64 + dest_addr16
-    frame += struct.pack(">BB", broadcast_radius, options) + data
-    checksum = 0xFF - (sum(frame) & 0xFF)
-    return b'\x7E' + struct.pack(">H", len(frame)) + frame + struct.pack("B", checksum)
-
-# === Serial Protocol 實作 ===
-class SerialToUDP(asyncio.Protocol):
-    def __init__(self, udp_protocol):
-        self.udp_protocol = udp_protocol
-        self.buffer = bytearray()
-
-    def connection_made(self, transport):
-        self.transport = transport
-        if hasattr(self.udp_protocol, 'set_serial_transport'):
-            self.udp_protocol.set_serial_transport(self)
-        print(f"Serial connection established on {SERIAL_PORT}")
-
-    def data_received(self, data):
-        self.buffer.extend(data)
-        while True:
-            if len(self.buffer) < 3:
-                return
-            if self.buffer[0] != 0x7E:
-                self.buffer.pop(0)
-                continue
-            length = (self.buffer[1] << 8) | self.buffer[2]
-            full_length = 3 + length + 1
-            if len(self.buffer) < full_length:
-                return
-            frame = self.buffer[:full_length]
-            del self.buffer[:full_length]
-            if hasattr(self.udp_protocol, 'send_udp'):
-                self.udp_protocol.send_udp(bytes(frame))
-
-    def write_to_serial(self, data):
-        try:
-            api_frame = build_api_tx_frame(data, TARGET_ADDR64)
-            pass
-            self.transport.write(api_frame)
-        except Exception as e:
-            print(f"[TX Error] 無法封裝或傳送資料: {e}")
-
-# === UDP Protocol 實作 ===
-class UDPHandler(asyncio.DatagramProtocol):
-    def __init__(self):
-        self.serial_transport = None
-        self.transport = None
-        self.mav_decoder = mavutil.mavlink.MAVLink(None)
-
-    def connection_made(self, transport):
-        self.transport = transport
-        print("UDP transport ready.")
-
-    def set_serial_transport(self, serial_transport):
-        self.serial_transport = serial_transport
-
-    def datagram_received(self, data, addr):
-        if self.serial_transport:
-            self.serial_transport.write_to_serial(data)
-
-    def send_udp(self, data):
-        decoded_data = self.decapsulate_data(data)
-        if decoded_data is None:
-            pass
-            return
-        self.decode_mavlink_data(decoded_data)
-        if self.transport:
-            self.transport.sendto(decoded_data, (UDP_REMOTE_IP, UDP_REMOTE_PORT))
-
-    def decapsulate_data(self, data):
-        try:
-            if not data or data[0] != 0x7E:
-                return None
-            length = (data[1] << 8) | data[2]
-            if len(data) < length + 4:
-                return None
-            frame_type = data[3]
-            if frame_type == 0x90:
-                rf_data_start = 3 + 12
-                return data[rf_data_start:3 + length]
-            else:
-                return None
-        except Exception as e:
-            print(f"[XBee 解封錯誤] {e}")
-            return None
-
-    def decode_mavlink_data(self, data):
-        try:
-            msg = self.mav_decoder.parse_char(data)
-            if msg:
-                if msg.get_type() == "HEARTBEAT":
-                    pass  # 不輸出任何訊息
-                else:
-                    pass
-        except Exception as e:
-            print(f"[MAVLink Decode Error] {e}")
-
-# === 主流程 ===
-async def main():
-    if not check_serial_port():
-        print("程式終止：串口檢查失敗")
-        return
-
-    loop = asyncio.get_running_loop()
-    if os.name != 'nt':  # Windows 不支援 add_signal_handler
-        for sig in (signal.SIGINT, signal.SIGTERM):
-            loop.add_signal_handler(sig, lambda: asyncio.create_task(shutdown(loop)))
-
-    udp_handler = UDPHandler()
-
-    try:
-        udp_transport, _ = await loop.create_datagram_endpoint(
-            lambda: udp_handler,
-            local_addr=('0.0.0.0', 0)
-        )
-    except Exception as e:
-        print(f"無法創建 UDP 端點：{str(e)}")
-        return
-
-    sock = udp_transport.get_extra_info('socket')
-    print(f"UDP listening on {sock.getsockname()}")
-
-    try:
-        serial_proto = SerialToUDP(udp_handler)
-        await serial_asyncio.create_serial_connection(
-            loop, lambda: serial_proto, SERIAL_PORT, baudrate=SERIAL_BAUDRATE
-        )
-    except Exception as e:
-        print(f"無法建立串口連接：{str(e)}")
-        traceback.print_exc()
-        udp_transport.close()
-        return
-
-    print("等待串口資料...")
-    try:
-        await asyncio.Future()
-    except asyncio.CancelledError:
-        pass
-
-async def shutdown(loop):
-    print("Shutting down...")
-    tasks = [t for t in asyncio.all_tasks() if t is not asyncio.current_task()]
-    for task in tasks:
-        task.cancel()
-    await asyncio.gather(*tasks, return_exceptions=True)
-    loop.stop()
-
-if __name__ == '__main__':
-    try:
-        asyncio.run(main())
-    except KeyboardInterrupt:
-        print("程式被使用者中斷")
-    except Exception as e:
-        print("程式執行錯誤：")
-        traceback.print_exc()

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()
+        history_style = """
-        self.message_history_view.setReadOnly(True)
-        self.message_history_view.setStyleSheet("""
             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):
+    def _history_list(self, category):
-        """清空訊息歷史。"""
+        return {
-        self.message_history.clear()
+            'drone': self.drone_message_history,
-        if hasattr(self, 'message_history_view'):
+            'gui': self.gui_operation_history,
-            self.message_history_view.clear()
+            '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)
+        history = self._history_list(category)
-        if len(self.message_history) > self.max_message_history:
+        if history is None:
-            self.message_history = self.message_history[-self.max_message_history:]
+            return
+        history.append(entry)
+        if len(history) > self.max_message_history:
+            del history[:-self.max_message_history]
 
-        if hasattr(self, 'message_history_view'):
+        view = getattr(self, '_history_views', {}).get(category)
-            self.message_history_view.appendPlainText(entry)
+        if view is not None:
-            scrollbar = self.message_history_view.verticalScrollBar()
+            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;
+                        autoFollowPanTo(latlng);
-                        map.panTo(latlng);
-                        setTimeout(() => { autoCenteringMap = false; }, 300);
                     }
                 }, 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 才決定重送/前進
+    - 閉環發送：以固定週期「持續重送」當前目標 setpoint（GUIDED 位置目標冪等），
-    - 失敗策略 (b)：單機重試 MAX_RETRY 次仍失敗 → 該機 fallback LOITER，其他機繼續
+      掉包會在下個週期自癒；換點完全靠 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}")
+        task.status = TaskStatus.RETRYING  # 純觀測用途，Phase 1/3 不會因此停飛
-
+        _log(
-        if task.fail_count < self.MAX_RETRY:
+            "WARN",
-            task.status = TaskStatus.RETRYING
+            f"{drone_id} 送出未驗證 x{task.fail_count}（指令已送達飛控，由 GPS 續飛）: {message}",
-            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"retry {task.fail_count}/{self.MAX_RETRY}: {message}"
+            f"send unverified x{task.fail_count} (GPS 續飛): {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,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,
+        "mission": 0,
-        "armed": 1,
+        "mode": 1,
-        "battery": 2,
+        "armed": 2,
-        "longitude": 3,
+        "battery": 3,
-        "latitude": 4,
+        "longitude": 4,
-        "fix_type": 5,
+        "latitude": 5,
-        "satellites_visible": 6,
+        "fix_type": 6,
-        "eph": 7,
+        "satellites_visible": 7,
-        "epv": 8,
+        "eph": 8,
-        "altitude": 9,
+        "epv": 9,
-        "local": 10,
+        "altitude": 10,
-        "velocity": 11,
+        "local": 11,
-        "groundspeed": 12,
+        "velocity": 12,
-        "heading": 13,
+        "groundspeed": 13,
-        "airspeed": 14,
+        "heading": 14,
-        "throttle": 15,
+        "airspeed": 15,
-        "hud_alt": 16,
+        "throttle": 16,
-        "climb": 17,
+        "hud_alt": 17,
-        "roll": 18,
+        "climb": 18,
-        "pitch": 19,
+        "roll": 19,
-        "yaw": 20,
+        "pitch": 20,
-        "loss_rate": 21,
+        "yaw": 21,
-        "ping": 22
+        "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}")
+                if field == 'mission':
-                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

@@ -14,6 +14,8 @@ find_package(builtin_interfaces REQUIRED)
 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_interfaces/msg/SystemDiagnosticsRaw.msg

@@ -0,0 +1,11 @@
+builtin_interfaces/Time stamp
+uint32 sensors_install_mask
+uint32 sensors_enabled_mask
+uint32 sensors_health_mask
+uint16 mcu_load
+uint16 bus_error_rate
+uint16 bus_error_count
+uint16 errors_count1
+uint16 errors_count2
+uint16 errors_count3
+uint16 errors_count4

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.10"
+PROJECT_VER = "v1.20"
 
 class PanelState:
     def __init__(self):
@@ -435,6 +435,10 @@ class ControlPanel:
                         state.serial_info_temp["CommunicationType"] = "XBee(API-AT)"
                         menu_stack.pop()
                         idx_stack.pop()
+                    elif selected.action == "SET_SERIAL_COMM_XBEE_ESP":
+                        state.serial_info_temp["CommunicationType"] = "XBee(API-API) espv1"
+                        menu_stack.pop()
+                        idx_stack.pop()
                     elif selected.action == "SET_SERIAL_COMM_TELEMETRY":
                         state.serial_info_temp["CommunicationType"] = "TELEMETRY"
                         menu_stack.pop()
@@ -812,6 +816,7 @@ class ControlPanel:
                 port_menu = MenuNode(f"{port}", children=[
                     MenuNode("Set Comm Type", "設定通訊形態", "SET_SERIAL_COMM", children=[
                         MenuNode("XBee(API-AT)", "XBee 模式", "SET_SERIAL_COMM_XBEE"),
+                        MenuNode("XBee(API-API)", "XBee 模式(with ESP)", "SET_SERIAL_COMM_XBEE_ESP"),
                         MenuNode("Telemetry", "數傳模式", "SET_SERIAL_COMM_TELEMETRY"),
                     ]),
                     MenuNode("Set Baud", "設定 Baud", "TEXT_BAUD_SERIAL"),
@@ -1106,7 +1111,7 @@ class ControlPanel:
                 0: "HB", 1: "S_STAT", 2: "S_TIME", 24: "GPS_RAW", 27: "RAW_IMU",
                 30: "ATT", 32: "LOC_POS", 33: "GLB_POS", 62: "NAV_CTL",
                 74: "VFR_HUD", 147: "BATT_ST", 136: "TERRAIN", 241: "VIBRAT",
-                125: "POW_STA",
+                125: "POW_STA", 253: "STAT_TXT",
             }
 
             # ardupilot mega 
@@ -1198,6 +1203,8 @@ class Orchestrator:
             'vfr_hud': 1.0,
             'mode': 0.0,
             'summary': 1.0,
+            'sys_diags': 1.0,
+            'status_text': 1.0,
         }
 
     def engageWholeSystem(self):
@@ -1593,6 +1600,7 @@ class Orchestrator:
         # 定義通訊類型映射表
         COMM_TYPE_MAP = {
             "XBee(API-AT)": sm.SerialMode.XBEEAPI2AT,
+            "XBee(API-AT)": sm.SerialMode.XBEEAPI_espv1,
             "TELEMETRY": sm.SerialMode.STRAIGHT,
             # 新增區
         }
@@ -1636,25 +1644,7 @@ class Orchestrator:
 
 def main():
 
-    # =========== 各項模組的版本先驗 ===========
+    logger.info(f"Each Module Running Version at mavlinkObkect:{mo.MODULE_VER}, mavlinkROS2Nodes:{mros.MODULE_VER}, mavlinkVehicleView:{mvv.MODULE_VER}, serialManager:{sm.MODULE_VER}")
-    # 除非你有在做這幾項模組的改版 不然動到這邊的版本號 代表執行環境有很大的問題!!!!!!
-    version_check = True
-    if mo.MODULE_VER != "1.50":
-        print("Module Version Error! : mavlinkObkect")
-        version_check = False
-    if mros.MODULE_VER != "2.10":
-        print("Module Version Error! : mavlinkROS2Nodes")
-        version_check = False
-    if mvv.MODULE_VER != "1.00":
-        print("Module Version Error! : mavlinkVehicleView")
-        version_check = False
-    if sm.MODULE_VER != "0.80":
-        print("Module Version Error! : serialManager")
-        version_check = False
-    if version_check == False:
-        print("Environment Obstacle! Check YOUR Execution System Path First!!")
-        return
-    # ========================================
 
     stop_evt = threading.Event()
     def signal_handler(signum, frame):

src/fc_network_adapter/fc_network_adapter/mavlinkObject.py

@@ -47,11 +47,11 @@ from pymavlink.dialects.v20 import ardupilotmega as mav_ardupilot
 
 # 自定義的 import
 from .mavlinkVehicleView import (
-    vehicle_registry,
+    vehicle_registry, # 儲存全部物件的地方
-    VehicleView,
+    VehicleView, # 代表每台載具的最外層
     VehicleComponent,
     ComponentType,
-    ConnectionType
+    StatusTextEntry,
 )
 from .utils import RingBuffer, setup_logger
 
@@ -108,13 +108,15 @@ class mavlink_bridge:
     def _init_message_handlers(self):
         """初始化訊息處理器映射表，提高處理效率"""
         self.message_handlers = {
-            0: self._handle_heartbeat,        # HEARTBEAT
+            0: self._handle_heartbeat,          # HEARTBEAT
-            24: self._handle_gps_raw_int,     # GPS_RAW_INT
+            1: self._handle_vehicle_sys_status, # SYS_STATUS
-            30: self._handle_attitude,        # ATTITUDE
+            24: self._handle_gps_raw_int,       # GPS_RAW_INT
-            32: self._handle_local_position,  # LOCAL_POSITION_NED
+            30: self._handle_attitude,          # ATTITUDE
-            33: self._handle_global_position, # GLOBAL_POSITION_INT
+            32: self._handle_local_position,    # LOCAL_POSITION_NED
-            74: self._handle_vfr_hud,         # VFR_HUD
+            33: self._handle_global_position,   # GLOBAL_POSITION_INT
-            147: self._handle_battery_status, # BATTERY_STATUS
+            74: self._handle_vfr_hud,           # VFR_HUD
+            147: self._handle_battery_status,   # BATTERY_STATUS
+            253: self._handle_status_text,      # STATUSTEXT
         }
 
     def start(self):
@@ -231,6 +233,29 @@ class mavlink_bridge:
             'vx': msg.vx, 'vy': msg.vy, 'vz': msg.vz
         }
 
+    def _handle_vehicle_sys_status(self, vehicle, component, msg, timestamp):
+        """處理 SYS_STATUS 訊息 (msg_id: 1)"""
+        diag = component.status.sys_diag
+        diag.sensors_install_mask = msg.onboard_control_sensors_present
+        diag.sensors_enabled_mask = msg.onboard_control_sensors_enabled
+        diag.sensors_health_mask = msg.onboard_control_sensors_health
+        diag.mcuLoad = msg.load
+        diag.busErrorRate = msg.drop_rate_comm
+        diag.busErrorCount = msg.errors_comm
+        diag.errors_count1 = msg.errors_count1
+        diag.errors_count2 = msg.errors_count2
+        diag.errors_count3 = msg.errors_count3
+        diag.errors_count4 = msg.errors_count4
+        diag.timestamp = timestamp
+
+    def _handle_status_text(self, vehicle, component, msg, timestamp):
+        """處理 STATUSTEXT 訊息 (msg_id: 253)"""
+        text = msg.text.rstrip('\x00') if msg.text else ''
+        if not text:
+            return
+        component.status.status_text_queue.append(
+            StatusTextEntry(text=text, severity=msg.severity, timestamp=timestamp)
+        )
 
     def _handle_global_position(self, vehicle, component, msg, timestamp):
         """處理 GLOBAL_POSITION_INT 訊息 (msg_id: 33)"""
@@ -395,9 +420,9 @@ class mavlink_object:
             self.MAVLink = mav_common.MAVLink(self.mavlinkPipeline, srcSystem=254, srcComponent=191)
         
         # 記錄訊息過濾類型 (可選)
-        # 0 HEARTBEAT, 24 GPS_RAW_INT, 30 ATTITUDE, 
+        # 0 HEARTBEAT, 1 SYS_STATUS, 24 GPS_RAW_INT, 30 ATTITUDE, 
-        # 32 LOCAL_POSITION_NED, 33 GLOBAL_POSITION_INT, 74 VFR_HUD, 147 BATTERY_STATUS
+        # 32 LOCAL_POSITION_NED, 33 GLOBAL_POSITION_INT, 74 VFR_HUD, 147 BATTERY_STATUS, 253 STATUSTEXT
-        self.bridge_msg_types = set([0, 24, 30, 32, 33, 74, 147])  
+        self.bridge_msg_types = set([0, 1, 24, 30, 32, 33, 74, 147, 253])  
         self.return_msg_types = set([])
 
         # 轉發到別的 mavlink object 作為目標端口 的列表
@@ -834,5 +859,8 @@ if __name__ == '__main__':
 1. async_io_manager.managed_objects 與 mavlink_object.mavlinkObjects 功能重複整合 保留 mavlink_object.mavlinkObjects
 2. async_io_manager 的 _stop_event 無效變數移除
 
+2026年 06月 10日
+1. 增加 SYS_STATUS 與 STATUSTEXT 訊息的處理機制
+
 '''
 

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,10 +44,10 @@ 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.10"
+MODULE_VER = "2.50"
 
 FC_ROS_DOMAIN_ID = "0"
 NODE_KEYS = ("status_publisher", "command_service", "rtcm_relay")
@@ -68,14 +70,16 @@ class PublishRateController:
         # 注意 這邊是定義區 不要把參數寫在這裡 所以預設全部關閉
         # 以這個專案 請看 mainOrchestrator.py 的 Orchestrator 初始化階段
         self.topic_intervals = {
-            'position_gnss': 0.0,  # GNSS位置
+            'summary': 0.0,         # 載具摘要 (sysid 飛行模式 解鎖上鎖 gps狀態)
-            'position_ned': 0.0,   # LOCAL_POSITION_NED (位置+速度)
+            'position_gnss': 0.0,   # GNSS位置 (海拔高度)
-            'attitude': 0.0,       # 姿態 (pitch yaw row 與其加速狀態)
+            'position_ned': 0.0,    # LOCAL_POSITION_NED (位置+速度+相對高度)
-            'velocity': 0.0,       # 速度 (已經包含在 vfr_hud 未來移除)
+            'attitude': 0.0,        # 姿態 (pitch yaw row 與其加速狀態)
-            'battery': 0.0,        # 電池
+            'battery': 0.0,         # 電池
-            'vfr_hud': 0.0,        # VFR HUD (地速 空速 絕對高度 爬升率 航向 油門)
+            'vfr_hud': 0.0,         # VFR HUD (地速 空速 絕對高度 爬升率 航向 油門)
-            'mode': 0.0,           # 飛行模式 (已經在 summary 裡 未來移除)
+            'sys_diags': 0.0,       # SYS_STATUS 系統診斷
-            'summary': 0.0,        # 載具摘要 (sysid 飛行模式 解鎖上鎖 gps狀態)
+            'status_text': 0.0,     # STATUSTEXT 飛控文字（佇列驅動，>0 僅作啟用旗標）
+            'mode': 0.0,            # 飛行模式 (已經在 summary 裡 未來移除)
+            'velocity': 0.0,        # 速度 (已經包含在 vfr_hud 未來移除)
             # 在這裡新增更多 topics...
         }
         # 記錄每個 topic 的最後發布時間 {(sysid, topic): timestamp}
@@ -112,6 +116,10 @@ class PublishRateController:
         
         return False
     
+    def is_topic_enabled(self, topic: str) -> bool:
+        """檢查 topic 是否啟用（interval > 0）"""
+        return self.topic_intervals.get(topic, 0) > 0
+    
     def reset(self):
         """重置所有計時器"""
         self.last_publish_time.clear()
@@ -122,7 +130,7 @@ class VehicleStatusPublisher(Node):
     
     職責:
     - 定期從 vehicle_registry 讀取載具狀態
-    - 頻率控制（位置/姿態 2Hz，電池/摘要 1Hz）
+    - 頻率控制 (位置/姿態 2Hz, 電池/摘要 1Hz)
     - 發布標準 ROS2 消息類型
     - 檢測訂閱者，按需發布
     """
@@ -181,11 +189,13 @@ class VehicleStatusPublisher(Node):
         self._publish_position_gnss(sysid, status)
         self._publish_position_ned(sysid, status)
         self._publish_attitude(sysid, status)
-        self._publish_velocity(sysid, status)
         self._publish_battery(sysid, status)
         self._publish_vfr_hud(sysid, status)
-        self._publish_mode(sysid, status)
         self._publish_summary(vehicle)
+        self._publish_system_diagnostics(sysid, status)
+        self._publish_status_text(sysid, status)
+        self._publish_velocity(sysid, status)
+        self._publish_mode(sysid, status)
         # 在這裡新增更多 publish 方法調用...
     
     def _get_or_create_publisher(self, sysid: int, topic: str, msg_type, qos: int = 1):
@@ -206,7 +216,7 @@ class VehicleStatusPublisher(Node):
             topic_name = f'{self.topicString_prefix}/sys{sysid}/{topic}'
             publisher = self.create_publisher(msg_type, topic_name, qos)
             self.fc_publishers[key] = publisher
-            logger.info(f"Created publisher: {topic_name}")
+            logger.debug(f"Created publisher: {topic_name}")
         return self.fc_publishers[key]
     
     def _publish_position_gnss(self, sysid: int, status: mvv.ComponentStatus):
@@ -439,11 +449,7 @@ class VehicleStatusPublisher(Node):
             'armed': status.armed if status.armed is not None else False,
             # 'mode_custom': status.mode.custom_mode if status.mode.custom_mode else 0,
             'mode_name': status.mode.mode_name if status.mode.mode_name else "UNKNOWN",
-            # 'latitude': status.position.latitude if status.position.latitude else 0.0,
+            'mav_status': status.system_status if status.system_status is not None else 0,
-            # 'longitude': status.position.longitude if status.position.longitude else 0.0,
-            # 'altitude': status.position.altitude if status.position.altitude else 0.0,
-            # 'battery_percent': status.battery.remaining if status.battery.remaining else 0,
-            # 'gps_fix': status.gps.fix_type if status.gps.fix_type else 0,
             'connection_type': vehicle.connected_via.value,
             'last_update': component.packet_stats.last_msg_time if component.packet_stats.last_msg_time else 0.0,
         }
@@ -452,6 +458,62 @@ class VehicleStatusPublisher(Node):
         msg.data = json.dumps(summary)
         publisher.publish(msg)
     
+    def _publish_system_diagnostics(self, sysid: int, status: mvv.ComponentStatus):
+        """發布 SYS_STATUS 系統診斷資訊"""
+        if not self.rate_controller.should_publish(sysid, 'sys_diags'):
+            return
+        
+        diag = status.sys_diag
+        if diag.sensors_install_mask is None:
+            return
+        
+        publisher = self._get_or_create_publisher(
+            sysid, 'sys_diags', fcmsg.SystemDiagnosticsRaw
+        )
+        
+        if publisher.get_subscription_count() == 0:
+            return
+        
+        msg = fcmsg.SystemDiagnosticsRaw()
+        msg.stamp = self.get_clock().now().to_msg()
+        msg.sensors_install_mask = int(diag.sensors_install_mask)
+        msg.sensors_enabled_mask = int(diag.sensors_enabled_mask or 0)
+        msg.sensors_health_mask = int(diag.sensors_health_mask or 0)
+        msg.mcu_load = int(diag.mcuLoad or 0)
+        msg.bus_error_rate = int(diag.busErrorRate or 0)
+        msg.bus_error_count = int(diag.busErrorCount or 0)
+        msg.errors_count1 = int(diag.errors_count1 or 0)
+        msg.errors_count2 = int(diag.errors_count2 or 0)
+        msg.errors_count3 = int(diag.errors_count3 or 0)
+        msg.errors_count4 = int(diag.errors_count4 or 0)
+        
+        publisher.publish(msg)
+    
+    def _publish_status_text(self, sysid: int, status: mvv.ComponentStatus):
+        """發布 STATUSTEXT 飛控文字 (佇列 drain, 無訂閱者直接丟棄) """
+        # 是否啟用
+        if not self.rate_controller.is_topic_enabled('status_text'):
+            return
+        # 是否有資料
+        queue = status.status_text_queue
+        if not queue:
+            return
+
+        publisher = self._get_or_create_publisher(sysid, 'status_text', std_msgs.msg.String)
+
+        # 是否有監聽者
+        if publisher.get_subscription_count() == 0:
+            queue.clear()
+            return
+
+        while queue:
+            entry = queue.popleft()
+            msg = std_msgs.msg.String()
+            ts = entry.timestamp if entry.timestamp is not None else 0.0
+            sev = entry.severity if entry.severity is not None else -1
+            msg.data = f'[{ts:.3f}] [{sev}] {entry.text}'
+            publisher.publish(msg)
+    
     # ═══════════════════════════════════════════════════════════════
     # 【新增 Topic 位置 3/4】
     # 若要新增 topic，請在此處實作對應的發布方法
@@ -470,29 +532,6 @@ class VehicleStatusPublisher(Node):
     #     # ... 實作發布邏輯
     # ═══════════════════════════════════════════════════════════════
     
-    @staticmethod
-    def _euler_to_quaternion(roll, pitch, yaw):
-        """
-        歐拉角轉四元數
-        
-        Args:
-            roll: 橫滾角 (弧度)
-            pitch: 俯仰角 (弧度)
-            yaw: 偏航角 (弧度)
-            
-        Returns:
-            tuple: (qx, qy, qz, qw)
-        """
-        qx = math.sin(roll/2) * math.cos(pitch/2) * math.cos(yaw/2) - \
-             math.cos(roll/2) * math.sin(pitch/2) * math.sin(yaw/2)
-        qy = math.cos(roll/2) * math.sin(pitch/2) * math.cos(yaw/2) + \
-             math.sin(roll/2) * math.cos(pitch/2) * math.sin(yaw/2)
-        qz = math.cos(roll/2) * math.cos(pitch/2) * math.sin(yaw/2) - \
-             math.sin(roll/2) * math.sin(pitch/2) * math.cos(yaw/2)
-        qw = math.cos(roll/2) * math.cos(pitch/2) * math.cos(yaw/2) + \
-             math.sin(roll/2) * math.sin(pitch/2) * math.sin(yaw/2)
-        return (qx, qy, qz, qw)
-    
     def stop(self):
         """停止發布"""
         self.running = False
@@ -530,6 +569,14 @@ class MavlinkCommandService(Node):
     講白話一點就是
     每次接到一個 service 請求 要整個系統丟某種指令給載具時
     會做兩件事 1."丟出mavlink封包" 2."創造一個臨時信箱 Pending"
+    
+    然後透過每次 manager spin 
+    會去呼叫 return_router() 方法
+    這個方法會監聽 return_packet_ring 跟臨時信箱的 Pending 做配對
+    配對到的解開 Pending 
+
+    解開後 相對應的 handle_XXX 就會開始做事
+    
 
     """
 
@@ -1134,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 節點管理器
 # ============================================================================
@@ -1147,12 +1246,17 @@ class fc_ros_manager:
     stop 是停下止 ROS2 nodes 的運行，但不銷毀節點實例，允許後續再次 start。
     shutdown 是完全關閉 ROS2 並銷毀節點實例。
 
-    管理三個獨立的 ROS2 Node :
+    管理四個獨立的 ROS2 Node :
     - VehicleStatusPublisher
     - MavlinkCommandService
     - RtcmRelay
+    - FcNetworkLogPublisher
     
     提供統一的啟動/停止介面給 mainOrchestrator
+
+    另外 這邊用到 MultiThreadedExecutor 會開出額外的 thread 的特性 
+    使得就算 executor 在跑一些需要等待的方法 
+    常態的 spin_once 也不會被 block (spin_thread 是另一個支線)
     """
     
     def __init__(self):
@@ -1166,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
@@ -1186,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")
@@ -1217,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,
@@ -1342,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():
@@ -1368,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():
@@ -1386,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,
         }
 
     
@@ -1432,8 +1551,11 @@ ros2_manager = fc_ros_manager()
 2. schedule_restart_node / _restart_node : 手動重啟單一 node (spin thread 內執行）
 3. orchestrator cmd: ("RESTART_ROS_NODE", node_key), node_key 見 NODE_KEYS
 
+2026.06.10
+1. 增加了 _publish_system_diagnostics 與 _publish_status_text 功能
+
+
 TODO 
 1. service 部分會需要跟 mavlinkobject 大量互動 也許需要考慮對方的生命週期
 
 '''
-

src/fc_network_adapter/fc_network_adapter/mavlinkVehicleView.py

@@ -5,6 +5,7 @@ VehicleView - Pure State Container
 """
 
 import os
+from collections import deque
 from typing import Dict, Optional, Any, Tuple
 from dataclasses import dataclass, field
 from enum import Enum
@@ -12,7 +13,7 @@ from enum import Enum
 from .utils import setup_logger
 
 logger = setup_logger(os.path.basename(__file__))
-MODULE_VER = "1.00"
+MODULE_VER = "1.10"
 
 # ====================== Enums =====================
 
@@ -117,6 +118,30 @@ class VFR:
     timestamp: Optional[float] = None  # 時間戳記
 
 
+@dataclass
+class StatusTextEntry:
+    """飛控狀態文字（來源：MAVLink STATUSTEXT）"""
+    text: str
+    severity: Optional[int] = None
+    timestamp: Optional[float] = None
+
+
+@dataclass
+class SystemDiagnostics:
+    """系統診斷資訊（來源：MAVLink SYS_STATUS，不含電池欄位）"""
+    sensors_install_mask: Optional[int] = None
+    sensors_enabled_mask: Optional[int] = None
+    sensors_health_mask: Optional[int] = None
+    mcuLoad: Optional[int] = None          # 單位 1%
+    busErrorRate: Optional[int] = None     # 單位 0.1%
+    busErrorCount: Optional[int] = None
+    errors_count1: Optional[int] = None
+    errors_count2: Optional[int] = None
+    errors_count3: Optional[int] = None
+    errors_count4: Optional[int] = None
+    timestamp: Optional[float] = None
+
+
 @dataclass
 class ComponentStatus:
     """組件狀態容器"""
@@ -127,6 +152,8 @@ class ComponentStatus:
     ekf: EKF = field(default_factory=EKF)
     gps: GPS = field(default_factory=GPS)
     vfr: VFR = field(default_factory=VFR)
+    sys_diag: SystemDiagnostics = field(default_factory=SystemDiagnostics)
+    status_text_queue: deque = field(default_factory=lambda: deque(maxlen=64))
     
     # 系統狀態
     system_status: Optional[int] = None  # MAV_STATE
@@ -160,6 +187,7 @@ class RFStatus:
 @dataclass
 class SocketInfo:
     """Socket連接資訊"""
+    src64_addr: Optional[bytes] = None # 模組的物理定址
     ip: Optional[str] = None  # IP位址
     port: Optional[int] = None  # 埠號
     local_ip: Optional[str] = None  # 本地IP
@@ -283,6 +311,7 @@ class RFModule:
 
 class VehicleView:
     """
+    最上層
     載具視圖 - 純狀態容器
     
     特點:

src/fc_network_adapter/fc_network_adapter/serialManager.py

@@ -14,20 +14,30 @@ import signal
 import time
 import threading
 import struct
+from collections import deque
 from enum import Enum, auto
 from abc import ABC, abstractmethod
 from dataclasses import dataclass
+from typing import Callable, Optional
+from typing import List
 
 # # XBee 模組
 # from xbee.frame import APIFrame
 
 # 自定義的 import
+from .mavlinkVehicleView import (
+    vehicle_registry, # 儲存全部物件的地方
+    VehicleView,
+    RFModule,
+    RFModuleType,
+)
 from .utils import RingBuffer, setup_logger
+from .utils import pollStrategy
 
 # ====================== 分割線 =====================
 
 logger = setup_logger(os.path.basename(__file__))
-MODULE_VER = "0.80"
+MODULE_VER = "2.00"
 
 rx_module_ack = RingBuffer(capacity=64, buffer_id=253)
 
@@ -37,8 +47,8 @@ rx_module_ack = RingBuffer(capacity=64, buffer_id=253)
 class SerialMode(Enum):
     """連接類型"""
     STRAIGHT = auto()      # 原始數據直通
-    XBEEAPI2AT = auto()    # XBee API 模式
+    XBEEAPI2AT = auto()    # XBee API-AT 模式
-    XBEEAPI_POLL = auto()
+    XBEEAPI_espv1 = auto()  # XBee API-API 模式 esp v1
     NOT_USE = auto()       # 不使用
 
 
@@ -101,19 +111,28 @@ class RawFrameProcessor(FrameProcessor):
         return data
 
 
-class XBeeFrameProcessor(FrameProcessor):
+class XBeeFrameProcessor_Base(FrameProcessor):
     """
     XBee API 協議處理器
+    處理 XBEE API 端口 對應 -> 遠端 XBEE AT Mode
+    For SerialMode.XBEEAPI2AT
 
     職責：
     - XBee API frame 的拆幀 / 組幀
     - 0x90 (RX Packet)   -> 解出 payload 回傳
     - 0x88 (AT Response) -> 轉交 at_handler 處理（若有注入）
-    - 0x8B (TX Status)   -> 目前忽略
+    - 0x8B (TX Status)   -> 目前寫LOG (開發用)
     - 其他 frame type    -> 記 warning 忽略
 
-    若未來要做變化型 XBee（例如 API2 escape mode、不同 addressing），
+    若未來要做變化型 XBee (例如 API2 escape mode、不同 addressing)
     繼承此類並覆寫 _encapsulate / _decapsulate / _try_extract_frame 即可。
+
+    硬體產品系列 (Product Family) : XB3-24
+    晶片世代: Digi XBee 3 代架構（搭載 Silicon Labs EFR32 微控制器）。
+    運作頻段: 2.4 GHz RF。
+    硬體構型: TH (Through-Hole)，標準針腳插孔式引腳設計。
+    運行協議 (Protocol / Function Set): 802.15.4
+    韌體版本 (Firmware Version): 2014 -> 2 (XBee 3 平台) + 0 (802.15.4 協議代碼) + 14 (次要版本號)。
     """
 
     # XBee API frame type
@@ -122,6 +141,12 @@ class XBeeFrameProcessor(FrameProcessor):
     FRAME_TYPE_TX_STATUS   = 0x8B
     FRAME_TYPE_RX_PACKET   = 0x90
 
+    # ADDR16 選項
+    DEST_ADDR16_UNICAST = b'\xFF\xFE'
+    DEST_ADDR16_BRAODCAST = b'\xFF\xFF'
+
+    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
@@ -174,11 +199,15 @@ class XBeeFrameProcessor(FrameProcessor):
         return None
 
     def _dispatch_frame(self, frame: bytes) -> bytes:
-        """根據 frame type 分派；若是 RX payload 回傳 bytes，其餘回傳 None"""
+        """
+        根據 frame type 分派
+        1. 若是 RX payload 則 return bytes 遞交出去
+        2. 若是 AT command 則 進到相依類別 ATCommandHandler 處理
+        """
         frame_type = frame[3]
 
         if frame_type == self.FRAME_TYPE_RX_PACKET: # mavlink
-            return self._decapsulate(frame)
+            return self._decapsulate(frame)[0]
 
         if frame_type == self.FRAME_TYPE_AT_RESPONSE: # AT command
             if self.at_handler is not None:
@@ -186,6 +215,12 @@ class XBeeFrameProcessor(FrameProcessor):
             return None
 
         if frame_type == self.FRAME_TYPE_TX_STATUS:
+            length = (frame[1] << 8) | frame[2] # for debug
+            logger.info(
+                f"TX Status raw={frame.hex()}, api_len={length}, "
+                f"fid=0x{frame[4]:02X}, dest16=0x{(frame[5]<<8)|frame[6]:04X}, "
+                f"retry={frame[7]}, delivery={frame[8]}, discovery={frame[9]}"
+            ) # for debug
             return None
 
         logger.warning(f"Unknown XBee frame type: 0x{frame_type:02X}")
@@ -195,7 +230,8 @@ class XBeeFrameProcessor(FrameProcessor):
     @staticmethod
     def _encapsulate(
         data: bytes,
-        dest_addr64: bytes = b'\x00\x00\x00\x00\x00\x00\xFF\xFF',
+        dest_addr64: bytes = DEST_ADDR64_BRAODCAST,
+        dest_addr16 = DEST_ADDR16_BRAODCAST,
         frame_id: int = 0x01,
     ) -> bytes:
         """
@@ -203,8 +239,7 @@ class XBeeFrameProcessor(FrameProcessor):
         - 使用廣播地址
         - 添加適當的頭部和校驗和
         """
-        frame_type = XBeeFrameProcessor.FRAME_TYPE_TX_REQUEST
+        frame_type = XBeeFrameProcessor_Base.FRAME_TYPE_TX_REQUEST
-        dest_addr16 = b'\xFF\xFE'
         broadcast_radius = 0x00
         options = 0x00
 
@@ -219,7 +254,557 @@ class XBeeFrameProcessor(FrameProcessor):
         """從 RX Packet (0x90) 取出 payload"""
         length = (frame[1] << 8) | frame[2]
         rf_data_start = 3 + 12
-        return frame[rf_data_start:3 + length]
+        payload = frame[rf_data_start:3 + length]
+        senderAddr = frame[4:12]
+        return payload, senderAddr
+
+
+class XBeeFrameProcessor_ESPv1(XBeeFrameProcessor_Base):
+    '''
+    ESP32 封包分類
+    - Mavlink 資料封包 : 
+        - Payload = mavlink [方向] GCS -> UAV (broadcast) 
+        - Payload = mavlink [方向] UAV -> GCS (with src64 Addr)
+    - Discovery 階段
+        - Payload = DISC_HEADER [方向] GCS -> UAV (broadcast)
+        - Payload = HELLO_HEADER + esp_sysid(1) [方向] UAV -> GCS (with src64 Addr)
+    - Poll 階段
+        - Payload = POLL_HEADER + esp_sysid(1) + grant_bytes(2) [方向] GCS -> UAV (with src64 Addr)
+        - Payload = DONE_HEADER + sysid(1) + sent_len(2) + remain_len(2) [方向] UAV -> GCS (with src64 Addr)
+
+    這邊認定 esp_sysid 會等於 mavlink sysid
+
+    硬體產品系列 (Product Family) : XBP9B-DM
+    晶片世代: XBee PRO 900HP 200K
+    運作頻段: 900 MHz RF。
+    運行協議 (Protocol / Function Set): DigiMesh
+    韌體版本 (Firmware Version): 8075
+    '''
+
+    DISC_HEADER = b'DISC'
+    HELLO_HEADER = b'HELO'
+    POLL_HEADER = b'POLL'
+    DONE_HEADER = b'DONE'
+
+    MAX_BYTES_PER_FLUSH = 150
+    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):
+            self.system_id = system_id
+            self.address_64 = address_64
+            self.last_hello_time = last_hello_time
+
+            self.remain_bytes = 0 # 剩餘 buffer 量
+            self.last_poll_time = 0.0
+            self.last_done_time = 0.0 # 最後送出Done的時間
+            self.received_len = 0 # 收到封包累計
+
+    def __init__(self, at_handler: "ATCommandHandler" = None):
+        super().__init__(at_handler)
+
+        self.max_discovery_window_ms = 220
+        self.is_discovery_phase = False
+        self.esp32_address_mapping = {}
+
+        self.operator_busy = False
+        self.operator_running = False
+
+        self.serial_writer: Optional[Callable[[bytes], None]] = None
+        self.event_loop: Optional[asyncio.AbstractEventLoop] = None
+        self.serial_baudrate = 115200
+
+        self.gcs_transmit_queue: deque[bytearray] = deque()
+        self.poll_scheduler_state = pollStrategy.PollSchedulerState()
+
+        self.command_pending_event: Optional[asyncio.Event] = None
+        self.poll_done_event: Optional[asyncio.Event] = None
+        self.current_poll_address_64: Optional[bytes] = None
+
+        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.discovery_interval_seconds = 30.0 # 每次做 discovery 程序的間隔時間
+        self.device_offline_timeout = self.discovery_interval_seconds * 2 # 遠端沒有回應會被踢出 超時時限
+        self.operator_tick_interval_seconds = 0.03 # 
+        self.guard_milliseconds = 50 # POLL DONE 的保底時間間隔
+
+        self.pending_manual_discovery = False
+        self.pending_manual_poll: Optional[tuple[int, Optional[int]]] = None
+
+    # ---- 注入與設定 ----
+
+    def set_writer(self, writer: Callable[[bytes], None]) -> None:
+        self.serial_writer = writer
+
+    def set_event_loop(self, loop: asyncio.AbstractEventLoop) -> None:
+        self.event_loop = loop
+        self._ensure_async_primitives()
+
+    def set_serial_baudrate(self, baudrate: int) -> None:
+        self.serial_baudrate = baudrate
+
+    def _ensure_async_primitives(self) -> None:
+        if self.command_pending_event is None:
+            self.command_pending_event = asyncio.Event()
+        if self.poll_done_event is None:
+            self.poll_done_event = asyncio.Event()
+
+    # 有急事 叫醒 operator 做下一個 tick
+    def wake_operator(self) -> None:
+        self._ensure_async_primitives()
+        self.command_pending_event.set()
+
+    # ---- 拆幀分派 ----
+
+    def _dispatch_frame(self, frame: bytes) -> Optional[bytes]:
+        frame_type = frame[3]
+
+        if frame_type == self.FRAME_TYPE_RX_PACKET:
+            payload, sender_address_64 = self._decapsulate(frame)
+
+            if payload.startswith(self.HELLO_HEADER) and len(payload) == 5:
+                self.handle_hello_report(payload, sender_address_64)
+                return None
+
+            if payload.startswith(self.DONE_HEADER) and len(payload) == 9:
+                self.handle_done_report(payload, sender_address_64)
+                return None
+
+            remote_device = self.esp32_address_mapping.get(sender_address_64)
+            if remote_device is not None:
+                remote_device.received_len += len(payload)
+                if (self.operator_busy and self.current_poll_address_64 == sender_address_64):
+                    self.last_recieve_mavlink = time.time()
+            return payload
+
+        if frame_type == self.FRAME_TYPE_AT_RESPONSE:
+            if self.at_handler is not None:
+                self.at_handler.handle_frame(frame)
+            return None
+
+        if frame_type == self.FRAME_TYPE_TX_STATUS:
+            length = (frame[1] << 8) | frame[2]
+            logger.debug(
+                f"TX Status raw={frame.hex()}, api_len={length}, "
+                f"fid=0x{frame[4]:02X}, dest16=0x{(frame[5]<<8)|frame[6]:04X}, "
+                f"retry={frame[7]}, delivery={frame[8]}, discovery={frame[9]}"
+            )
+            return None
+
+        logger.warning(f"Unknown XBee frame type: 0x{frame_type:02X}")
+        return None
+
+    # ---- DISC / POLL 封裝 ----
+
+    def pack_discovery(self) -> bytes:
+        return self._encapsulate(self.DISC_HEADER, frame_id=0x00)
+
+    # 處理每個裝置回傳的 Hello 訊息
+    def handle_hello_report(self, payload: bytes, sender_address_64: bytes) -> None:   
+        system_id = payload[4]
+        remote_device = self.esp32_address_mapping.get(sender_address_64)
+
+        if remote_device is None:
+            self.esp32_address_mapping[sender_address_64] = self.Esp32DeviceInfo(
+                system_id, sender_address_64, time.time()
+            )
+            logger.debug(
+                f"new HELO system_id={system_id}, address_64={sender_address_64.hex()}")
+        elif remote_device.address_64 == sender_address_64:
+            remote_device.last_hello_time = time.time()
+        else:
+            logger.warning(
+                f"SYSID duplicated system_id={system_id}, "
+                f"address_64={sender_address_64.hex()}"
+            )
+        
+        if vehicle:=vehicle_registry.get(system_id):
+            if not vehicle.rf_module:
+                vehicle.rf_module = RFModule(RFModuleType.XBEE)
+
+    def pack_poll(self, target_address_64: bytes, grant_bytes: int = 0) -> Optional[bytes]:
+        remote_device = self.esp32_address_mapping.get(target_address_64)
+        if remote_device is None:
+            return None
+
+        poll_payload = self.POLL_HEADER + struct.pack(
+            '>BH', remote_device.system_id, grant_bytes
+        )
+        remote_device.received_len = 0
+        return self._encapsulate(
+            poll_payload,
+            dest_addr64=remote_device.address_64,
+            dest_addr16=self.DEST_ADDR16_UNICAST,
+            frame_id=0x02,
+        )
+
+    def handle_done_report(self, payload: bytes, sender_address_64: bytes) -> None:
+        remote_device = self.esp32_address_mapping.get(sender_address_64)
+        if remote_device is None:
+            return
+
+        # 這段是有問題的 因為會有整數封包切割問題 以及載具端的 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} "
+        #         f"received={remote_device.received_len} system_id={system_id}"
+        #     )
+
+        # TODO 傳送速率
+        # TODO 累積速率預測
+        remote_device.received_len = 0
+        remote_device.remain_bytes = remain_length
+        remote_device.last_done_time = time.time()
+
+        if (
+            self.current_poll_address_64 is not None
+            and sender_address_64 == self.current_poll_address_64
+            and self.poll_done_event is not None
+        ):
+            self.poll_done_event.set()
+
+    # ---- UDP到Serial 佇列 ----
+
+    def enqueue_gcs_transmit(self, payload: bytes) -> None:
+        if not payload:
+            return
+        self.gcs_transmit_queue.append(bytearray(payload))
+
+    # 只是 show 狀態
+    def get_gcs_queue_byte_count(self) -> int:
+        return sum(len(packet) for packet in self.gcs_transmit_queue)
+
+    # 只是 show 狀態
+    def get_gcs_queue_packet_count(self) -> int:
+        return len(self.gcs_transmit_queue)
+
+    # 把 gcs_transmit_queue 的 mavlink 封包依照大小打包出來
+    def _pop_flush_batch(self, max_bytes: int) -> List[bytes]:
+        if not self.gcs_transmit_queue:
+            return []
+
+        batch: List[bytes] = []
+        total_bytes = 0
+
+        while self.gcs_transmit_queue:
+            next_packet = bytes(self.gcs_transmit_queue[0])
+            packet_length = len(next_packet)
+
+            if not batch:
+                batch.append(self.gcs_transmit_queue.popleft())
+                total_bytes = packet_length
+                if packet_length > max_bytes:
+                    break
+                continue
+
+            if total_bytes + packet_length <= max_bytes:
+                batch.append(self.gcs_transmit_queue.popleft())
+                total_bytes += packet_length
+            else:
+                break
+
+        return batch
+
+    # 把數據封裝好以後 交給 serial_writer 排程送出
+    async def _send_gcs_packet(self, packet: bytes) -> None:
+        # 重複判斷
+        # if self.serial_writer is None:
+        #     return
+
+        sent_offset = 0
+        while sent_offset < len(packet):
+            chunk_end = min(
+                sent_offset + self.MAX_PAYLOAD_PER_FRAME,
+                len(packet),
+            )
+            chunk = packet[sent_offset:chunk_end]
+            sent_offset = chunk_end
+            self.serial_writer(self._encapsulate(chunk))
+            await asyncio.sleep(self.CHUNK_SEND_INTERVAL_SEC)
+
+    # 把上面兩個步驟打包起來 處理從 UDP 來的資訊 丟給 Serial
+    async def flush_gcs_transmit_queue(
+        self,
+        max_bytes: int = MAX_BYTES_PER_FLUSH,
+    ) -> None:
+        if self.serial_writer is None:
+            logger.warning("GCS flush skipped: serial writer not ready")
+            return
+
+        for packet in self._pop_flush_batch(max_bytes):
+            await self._send_gcs_packet(packet)
+
+    # ---- POLL 排程輔助 ----
+
+    # 計算下次要 poll 的對象跟大小
+    def _pick_poll_target(self):
+        poll_devices = [
+            pollStrategy.PollDevice(
+                address_64=address_64,
+                # system_id=device.system_id,
+                remain_bytes=device.remain_bytes,
+                last_done_time=device.last_done_time,
+            )
+            for address_64, device in self.esp32_address_mapping.items()
+        ]
+
+        return pollStrategy.pick_next(
+            poll_devices,
+            self.poll_scheduler_state
+        )
+
+    # 移除長時間沒有 HELLO 或 DONE 的 Dongle
+    def _prune_stale_devices(self) -> None:
+        now = time.time()
+        stale_addresses = [
+            address_64
+            for address_64, device in self.esp32_address_mapping.items()
+            if (now - device.last_hello_time > self.device_offline_timeout) and \
+               (now - device.last_done_time > self.device_offline_timeout)
+        ]
+        for address_64 in stale_addresses:
+            device = self.esp32_address_mapping.pop(address_64, None)
+            if device is not None:
+                logger.info(
+                    f"Removed stale device system_id={device.system_id} "
+                    f"address_64={address_64.hex()}"
+                )
+
+    # 判斷是否進入 discovery 程序
+    def _should_run_discovery(self) -> bool:
+        # 條件1. 目前沒有任何遠端ESP裝置被紀錄 或者 手動啟動
+        if (not self.esp32_address_mapping) or (self.pending_manual_discovery):
+            return True
+        # 條件2. 每個固定週期 會做一次
+        return (
+            time.time() - self.last_discovery_time
+            >= self.discovery_interval_seconds
+        )
+
+    # ---- 手動請求（thread-safe 對外 API）----
+
+    def request_discovery(self) -> bool:
+        """
+        手動觸發 discovery 可從任意 thread 呼叫。
+        實際排程在 serial_manager 的 event loop 內執行。
+        """
+        if self.event_loop is None:
+            logger.warning("ESPv1 request_discovery: event loop not ready")
+            return False
+        if not self.operator_running:
+            logger.warning("ESPv1 request_discovery: operator not running")
+            return False
+        self.event_loop.call_soon_threadsafe(self._apply_request_discovery)
+        return True
+
+    def request_poll(
+        self,
+        target_system_id: int,
+        grant_bytes: Optional[int] = None,
+    ) -> bool:
+        """
+        手動對指定 system_id 排入一輪 POLL 可從任意 thread 呼叫。
+        實際排程在 serial_manager 的 event loop 內執行。
+        """
+        if self.event_loop is None:
+            logger.warning("ESPv1 request_poll: event loop not ready")
+            return False
+        if not self.operator_running:
+            logger.warning("ESPv1 request_poll: operator not running")
+            return False
+        self.event_loop.call_soon_threadsafe(
+            self._apply_request_poll,
+            target_system_id,
+            grant_bytes,
+        )
+        return True
+
+    def _apply_request_discovery(self) -> None:
+        self.pending_manual_discovery = True
+        self.wake_operator()
+
+    def _apply_request_poll(
+        self,
+        target_system_id: int,
+        grant_bytes: Optional[int],
+    ) -> None:
+        self.pending_manual_poll = (target_system_id, grant_bytes)
+        self.wake_operator()
+
+    def get_status_snapshot(self) -> dict:
+        """
+        唯讀狀態快照 for debug
+        從其他 thread 讀取時不保證與 operator 原子一致。
+        """
+        now = time.time()
+        devices = []
+        for address_64, device in self.esp32_address_mapping.items():
+            devices.append({
+                "system_id": device.system_id,
+                "address_64": address_64.hex(),
+                "remain_bytes": device.remain_bytes,
+                "last_hello_age_seconds": now - device.last_hello_time,
+                "last_done_age_seconds": (
+                    now - device.last_done_time if device.last_done_time else None
+                ),
+            })
+        return {
+            "operator_busy": self.operator_busy,
+            "is_discovery_phase": self.is_discovery_phase,
+            "gcs_queue_bytes": self.get_gcs_queue_byte_count(),
+            "gcs_queue_packets": self.get_gcs_queue_packet_count(),
+            "devices": devices,
+        }
+
+    # ---- operator 主循環 ----
+
+    # operator 的運行鐘 有點像是 spin_once
+    async def operator_loop(self) -> None:
+        self._ensure_async_primitives()
+        logger.info("ESPv1 operator loop started")
+
+        while self.operator_running:
+            try:
+                await self._operator_tick() # TODO 最好不要在 while loop 塞 try 想辦法改掉
+            except asyncio.CancelledError:
+                raise
+            except Exception as exc:
+                logger.error(f"ESPv1 operator tick error: {exc}")
+
+            await self._wait_for_next_tick()
+
+        logger.info("ESPv1 operator loop stopped")
+
+    # 安排啥時要醒來做事
+    async def _wait_for_next_tick(self) -> None:
+        self.command_pending_event.clear()
+
+        # 固定時間醒來
+        sleep_task = asyncio.create_task( asyncio.sleep(self.operator_tick_interval_seconds) )
+        # 有"急事"被叫醒
+        wake_task = asyncio.create_task(self.command_pending_event.wait())
+
+        done, pending = await asyncio.wait(
+            {sleep_task, wake_task},
+            return_when=asyncio.FIRST_COMPLETED,
+        )
+
+        for task in pending:
+            task.cancel()
+
+    async def _operator_tick(self) -> None:
+        # 1. 移除沒反應 dongle
+        self._prune_stale_devices()
+
+        # 2. 忙碌時 略過這次循環
+        if self.operator_busy:
+            return
+
+        # 3. (最優先) 處理從 UDP 過來的封包 並且透過 Serial 送出
+        if self.gcs_transmit_queue:
+            await self.flush_gcs_transmit_queue()
+            return
+
+        # 4. 檢測要不要跑 discovery 程序
+        if self._should_run_discovery():
+            await self._run_discovery()
+            return
+
+        # 5. POLL 程序 (若有手動先執行 若無自動則策略決策)
+        if self.pending_manual_poll is not None:
+            target_system_id, grant_bytes = self.pending_manual_poll
+            self.pending_manual_poll = None
+            target_address = self._find_address_by_system_id(target_system_id)
+        else:
+            target_address, grant_bytes = self._pick_poll_target()
+
+        if target_address is None:
+            return
+        await self._run_one_poll(target_address, grant_bytes if grant_bytes is not None else 0)
+
+    def _find_address_by_system_id(self, target_system_id: int) -> Optional[bytes]:
+        for address_64, device in self.esp32_address_mapping.items():
+            if device.system_id == target_system_id:
+                return address_64
+        return None
+
+    # discovery 程序 
+    async def _run_discovery(self) -> None:
+        if self.serial_writer is None:
+            self.pending_manual_discovery = False
+            return
+
+        self.operator_busy = True
+        self.is_discovery_phase = True
+        
+        self.serial_writer(self.pack_discovery())
+        self.last_discovery_time = time.time()
+        await asyncio.sleep(self.max_discovery_window_ms / 1000.0)
+
+        self.operator_busy = False
+        self.is_discovery_phase = False
+        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
+        poll_tick = min(0.02, idle_timeout_sec / 2)
+
+        while not self.poll_done_event.is_set():
+            if time.time() - self.last_recieve_mavlink >= idle_timeout_sec:
+                return False
+            try:
+                await asyncio.wait_for(
+                    self.poll_done_event.wait(),
+                    timeout=poll_tick,
+                )
+            except asyncio.TimeoutError:
+                continue
+        return True
+
+    # poll 程序
+    async def _run_one_poll(self, target_address_64: bytes, grant_bytes: int) -> None:
+        if self.serial_writer is None:
+            return
+
+        # 這邊把要求封包最小量放在36 是讓載具端正好可以回傳一個有簽章的 HEARTBEAT 封包的大小
+        # 算是 "探測封包" 這樣讓系統可以更快速的知道載具端殘餘的資料量
+        grant_bytes = max(36, min(int(grant_bytes), 65535))
+
+        poll_frame = self.pack_poll(target_address_64, grant_bytes)
+        if poll_frame is None:
+            return
+
+        self._ensure_async_primitives()
+        self.operator_busy = True
+        self.current_poll_address_64 = target_address_64
+        self.poll_done_event.clear()
+        self.serial_writer(poll_frame)
+        self.last_recieve_mavlink = time.time()
+
+        try:
+            completed = await self._wait_poll_done_with_idle_timeout()
+            if not completed:
+                logger.warning(
+                    f"POLL timeout address_64={target_address_64.hex()} "
+                    f"grant_bytes={grant_bytes}"
+                )
+        finally:
+            self.operator_busy = False
+            self.current_poll_address_64 = None
+            await asyncio.sleep(self.guard_milliseconds / 1000.0)
+
+    def stop_operator(self) -> None:
+        self.operator_running = False
+        self.gcs_transmit_queue.clear()
+        self.wake_operator()
 
 
 # ====================== Dongle Command Handler =====================
@@ -281,21 +866,21 @@ class ATCommandHandler:
     # ---- 接收端 ----
     def handle_frame(self, frame: bytes) -> None:
         """
-        接收一整個 AT Response frame：
+        接收一整個 AT Response frame:
         1. 解析成 ATResponse
         2. 推進 rx_module_ack 供其他模組消費
         3. 本地 dispatch 給對應的 _handle_xxx
         """
-        parsed = self._parse(frame)
+        parsed_at_ack = self._parse(frame)
-        if parsed is None:
+        if parsed_at_ack is None:
             return
 
-        if not rx_module_ack.put(parsed):
+        if not rx_module_ack.put(parsed_at_ack):
             logger.warning(
-                f"[{self.serial_port}] rx_module_ack overflow, drop {parsed.command!r}"
+                f"[{self.serial_port}] rx_module_ack overflow, drop {parsed_at_ack.command!r}"
             )
 
-        self._dispatch(parsed)
+        self._dispatch(parsed_at_ack)
 
     @staticmethod
     def _parse(frame: bytes) -> ATResponse:
@@ -326,30 +911,30 @@ class ATCommandHandler:
 
         handler = self.handlers.get(response.command)
         if handler:
-            handler(response.data)
+            handler(response)
         else:
             logger.debug(
                 f"[{self.serial_port}] 未處理的 AT 指令: "
                 f"{response.command.decode()}"
             )
 
-    def _handle_rssi(self, data: bytes):
+    def _handle_rssi(self, response: ATResponse):
         """處理 DB (RSSI) 回應：單 byte 無號值，單位 dBm"""
-        pass
+
-        if data:
+        # print(f"[{self.serial_port}] RSSI = -{data[0]} dBm")  # dev
-            print(f"[{self.serial_port}] RSSI = -{data[0]} dBm")  # dev
+        logger.debug(f"[{self.serial_port}] RSSI = -{response.data[0]} dBm") # dev
-            # logger.debug(f"[{self.serial_port}] RSSI = -{data[0]} dBm") # dev
+
-            pass
+
-        
+
-    def _handle_serial_high(self, data: bytes):
+
+    def _handle_serial_high(self, response: ATResponse):
         """處理 SH (Serial Number High)"""
         pass
 
-    def _handle_serial_low(self, data: bytes):
+    def _handle_serial_low(self, response: ATResponse):
         """處理 SL (Serial Number Low)"""
         pass
 
-
 # ================ Serial UDP Socket Object ==============
 class SerialHandler(asyncio.Protocol):
     """asyncio.Protocol 用於處理 Serial 收發"""
@@ -368,13 +953,13 @@ class SerialHandler(asyncio.Protocol):
         if self.serial_mode == SerialMode.STRAIGHT:
             return RawFrameProcessor()
 
-        if self.serial_mode == SerialMode.XBEEAPI2AT:
+        elif self.serial_mode == SerialMode.XBEEAPI2AT:
             at_handler = ATCommandHandler(self.serial_port_str)
-            return XBeeFrameProcessor(at_handler=at_handler)
+            return XBeeFrameProcessor_Base(at_handler=at_handler)
 
-        # if self.serial_mode == SerialMode.XBEEAPI_POLL:
+        elif self.serial_mode == SerialMode.XBEEAPI_espv1:
-        #     at_handler = ATCommandHandler_new(self.serial_port_str)
+            at_handler = ATCommandHandler(self.serial_port_str)
-        #     return XBeeFrameProcessor(at_handler=at_handler)
+            return XBeeFrameProcessor_ESPv1(at_handler=at_handler)
 
         logger.warning(f"Unknown serial mode: {self.serial_mode}, using Raw")
         return RawFrameProcessor()
@@ -387,6 +972,13 @@ class SerialHandler(asyncio.Protocol):
         if self.serial_mode == SerialMode.XBEEAPI2AT:
             self.processor.at_handler.set_writer(self.transport.write)
 
+        elif self.serial_mode == SerialMode.XBEEAPI_espv1:
+            if isinstance(self.processor, XBeeFrameProcessor_ESPv1):
+                self.processor.set_writer(self.transport.write)
+                self.processor.set_event_loop(asyncio.get_running_loop())
+                if self.processor.at_handler is not None:
+                    self.processor.at_handler.set_writer(self.transport.write)
+
         if hasattr(self.udp_handler, 'set_serial_handler'):
             self.udp_handler.set_serial_handler(self)
         logger.debug(f"Serial port {self.serial_port_str} connected")
@@ -429,11 +1021,15 @@ class UDPHandler(asyncio.DatagramProtocol):
         if not self.serial_handler:
             logger.warning("Serial handler not set, dropping UDP packet")
             return
-        
+
-        # 使用 processor 封裝數據
+        if self.serial_mode == SerialMode.XBEEAPI_espv1:
+            processor = self.serial_handler.processor
+            # if isinstance(processor, XBeeFrameProcessor_ESPv1): # 預設唯一綁定 少一個 if 多一點效率
+            processor.enqueue_gcs_transmit(data)
+            processor.wake_operator()
+            return
+
         processed_data = self.serial_handler.processor.process_outgoing(data)
-        
-        # 發送到串口
         self.serial_handler.transport.write(processed_data)
 
 
@@ -454,6 +1050,7 @@ class serial_manager:
             self.protocol = None
             self.udp_handler = None
             self.serial_handler = None
+            self.operator_task = None
 
     def __init__(self):
         self.thread = None
@@ -594,6 +1191,16 @@ class serial_manager:
             
             logger.debug(f"Serial connection created for {serial_port}")
 
+            if serial_mode == SerialMode.XBEEAPI_espv1:
+                processor = serial_obj.serial_handler.processor
+                if isinstance(processor, XBeeFrameProcessor_ESPv1):
+                    processor.set_serial_baudrate(baudrate)
+                    processor.operator_running = True
+                    serial_obj.operator_task = asyncio.create_task(
+                        processor.operator_loop()
+                    )
+                    logger.debug(f"ESPv1 operator task started for {serial_port}")
+
             # 將 serial_object 加入管理列表
             serial_id = self.serial_count + 1
             self.serial_objects[serial_id] = serial_obj
@@ -646,7 +1253,19 @@ class serial_manager:
         
         try:
             serial_obj = self.serial_objects[serial_id]
-            
+
+            if serial_obj.serial_mode == SerialMode.XBEEAPI_espv1:
+                processor = serial_obj.serial_handler.processor
+                if isinstance(processor, XBeeFrameProcessor_ESPv1):
+                    processor.stop_operator()
+                if serial_obj.operator_task is not None:
+                    serial_obj.operator_task.cancel()
+                    try:
+                        await serial_obj.operator_task
+                    except asyncio.CancelledError:
+                        pass
+                    serial_obj.operator_task = None
+
             # 關閉 UDP transport
             if hasattr(serial_obj, 'transport') and serial_obj.transport:
                 serial_obj.transport.close()
@@ -674,7 +1293,7 @@ class serial_manager:
 
     def send_at_command(self, serial_id, request: ATRequest) -> bool:
         """
-        對指定 serial_id 的 XBee dongle 發送一筆 AT 指令（thread-safe）
+        對指定 serial_id 的 XBee dongle 發送一筆 AT 指令 (thread-safe)
         - serial_id: create_serial_link 取得的編號
         - request: ATRequest 物件，攜帶 command / parameter / frame_id
         回傳是否成功排進事件圈
@@ -687,6 +1306,7 @@ class serial_manager:
             logger.error(f"Serial object {serial_id} not found")
             return False
 
+        # TODO 這邊的防呆 應該可以不用 if 有空再改
         serial_obj = self.serial_objects[serial_id]
         if serial_obj.serial_mode != SerialMode.XBEEAPI2AT:
             logger.error(
@@ -699,6 +1319,18 @@ class serial_manager:
         self.loop.call_soon_threadsafe(at_handler.send_command, request)
         return True
 
+    def get_espv1_processor(self, serial_id: int) -> Optional[XBeeFrameProcessor_ESPv1]:
+        """依 serial_id 取得 ESPv1 processor 不存在或模式不符回傳 None。"""
+        if serial_id not in self.serial_objects:
+            return None
+        serial_obj = self.serial_objects[serial_id]
+        if serial_obj.serial_mode != SerialMode.XBEEAPI_espv1:
+            return None
+        processor = serial_obj.serial_handler.processor
+        if not isinstance(processor, XBeeFrameProcessor_ESPv1):
+            return None
+        return processor
+
     @staticmethod
     def check_serial_port(serial_port, baudrate):
         """檢查串口是否存在與可用"""
@@ -736,42 +1368,105 @@ if __name__ == '__main__':
     sm = serial_manager()
     sm.start()
 
+    # # 測試項一 
     # SERIAL_PORT = '/dev/ttyACM0'  # 手動指定
     # SERIAL_BAUDRATE = 115200
     # UDP_REMOTE_PORT = 14571
     # sm.create_serial_link(SERIAL_PORT, SERIAL_BAUDRATE, UDP_REMOTE_PORT, SerialMode.STRAIGHT)
 
+    # 測試項二
+    print("運行 測試項二")
     SERIAL_PORT = '/dev/ttyUSB0'  # 手動指定
     SERIAL_BAUDRATE = 115200
     UDP_REMOTE_PORT = 14561
     sm.create_serial_link(SERIAL_PORT, SERIAL_BAUDRATE, UDP_REMOTE_PORT, SerialMode.XBEEAPI2AT)
+    serial_id = 1
+    device_sys_id = 10
 
     linked_serial = sm.get_serial_link()
-    print(linked_serial)
+    print(f"連結完成 : {linked_serial}. 等待兩秒")
 
     # 等 connection_made 完成 writer 注入，再發一筆 AT 指令測試
-    time.sleep(5)
+    time.sleep(2)
-    rssi_request = ATRequest(command=b'DB', parameter=b'', frame_id=0x52)
+    rssi_request = ATRequest(command=b'DB', parameter=b'', frame_id=device_sys_id)
-    for i in range(60):
+    print(f"手動送出 DB AT Command:")
+    for i in range(20):
         sm.send_at_command(1, rssi_request)
         time.sleep(1)
 
     sm.remove_serial_link(1)
-    time.sleep(3)
+    time.sleep(2)
     sm.shutdown()
+    print("結束運行")
 
+    # # 測試項三
+    # SERIAL_PORT = '/dev/ttyUSB0'
+    # SERIAL_BAUDRATE = 115200
+    # UDP_REMOTE_PORT = 14561
+    # sm.create_serial_link(SERIAL_PORT, SERIAL_BAUDRATE, UDP_REMOTE_PORT, SerialMode.XBEEAPI_espv1)
 
+    # time.sleep(2)  # 等 serial 連線與 operator 啟動
+
+    # serial_id = 1
+    # processor = sm.get_espv1_processor(serial_id)
+    # if processor is not None:
+    #     processor.request_discovery()
+    #     processor.request_poll(target_system_id=1)
+    #     processor.request_poll(target_system_id=1, grant_bytes=200)
+    #     print(processor.get_status_snapshot())
+    #     print(processor.get_gcs_queue_byte_count())
+
+    # sm.remove_serial_link(serial_id)
+    # time.sleep(30)
+    # sm.shutdown()
 
 '''
 ================= 改版記錄 ============================
 2026.4.20
 1. XBeeFrameHandler 結構移除
-2. XBeeFrameProcessor 新增 _encapsulate, _decapsulate 編碼解碼 xbee 封包的功能 (原來在 XBeeFrameHandler 中)
+2. XBeeFrameProcessor_Base 新增 _encapsulate, _decapsulate 編碼解碼 xbee 封包的功能 (原來在 XBeeFrameHandler 中)
-3. XBeeFrameProcessor 新增 _try_extract_frame 處理被可能截斷的 UART 封包
+3. XBeeFrameProcessor_Base 新增 _try_extract_frame 處理被可能截斷的 UART 封包
-4. XBeeFrameProcessor 新增 _dispatch_frame 分配封包到 UDP 或者 Dongle Command Handler 
+4. XBeeFrameProcessor_Base 新增 _dispatch_frame 分配封包到 UDP 或者 Dongle Command Handler 
 5. ATCommandHandler 新增 _parse 去拆解 0x88 AT Command Response
 6. ATCommandHandler 新增 _dispatch 把拆解的結果 分配到 _handle_XXX
 7. ATCommandHandler 新增各項 _handle_XXX (未實作)
 
+2026.06.15
+1. 修改 XBeeFrameProcessor_Base _decapsulate 使其解出發送端 dongle 的 src64 地址
+2. 新增 XBeeFrameProcessor_ESPv1 這個類別繼承 XBeeFrameProcessor_Base 
+3. 新增 XBeeFrameProcessor_ESPv1 組合 DISC 跟 POLL 訊息 處理 HELO 跟 DONE 的能力
+4. DISC 自動化與手動
+5. POLL 的自動化
+
+2026.06.16
+1. 移除 DongleCommandHandler_ESPv1
+2. ESPv1 對外 API（request_discovery / request_poll）移至 XBeeFrameProcessor_ESPv1，thread-safe
+3. serial_manager 僅保留 get_espv1_processor(serial_id) lookup
+
+
+註解1 : FRAME_TYPE_TX_STATUS 的對應解說 (我不喜歡程式塞太多為了顯示而顯示的東西 錯誤碼自己下來看)
+    TX_DELIVERY_STATUS
+        0x00: "Success",
+        0x01: "No ACK received",
+        0x02: "CCA failure",
+        0x15: "Invalid destination endpoint",
+        0x21: "Network ACK failure",
+        0x22: "Not joined to network",
+        0x23: "Self-addressed",
+        0x24: "Address not found",
+        0x25: "Route not found",
+        0x26: "Broadcast source relay",
+        0x27: "Insufficient data",
+        0x28: "TX buffered",
+        0x32: "Invalid send flag",
+        0x74: "Resource error",
+
+    TX_DISCOVERY_STATUS
+        0x00: "No discovery overhead",
+        0x01: "Address discovery",
+        0x02: "Route discovery",
+        0x03: "Address and route discovery",
+
+
 '''
 

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

@@ -0,0 +1,48 @@
+"""
+POLL 輪詢策略，供 XBeeFrameProcessor_ESPv1 使用。
+
+不依賴 serialManager，僅接受裝置快照與排程狀態。
+"""
+
+from dataclasses import dataclass
+from typing import List
+
+
+@dataclass(frozen=True)
+class PollDevice:
+    """從 esp32AddrMapping 抽出的唯讀快照。"""
+    address_64: bytes
+    # system_id: int
+    remain_bytes: int
+    last_done_time: float
+
+
+@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)。
+    """
+    if not devices:
+        return None, 0
+
+    device_count = len(devices)
+    selected_index = scheduler_state.round_robin_index % device_count
+    selected_device = devices[selected_index]
+    scheduler_state.round_robin_index += 1
+
+    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/fc_network_apps/vehicleDiagShow.py

@@ -0,0 +1,544 @@
+#!/usr/bin/env python3
+"""Standalone curses TUI for fc_network vehicle diagnostics (sys_diags, status_text, summary)."""
+
+from __future__ import annotations
+
+import curses
+import json
+import re
+import sys
+import threading
+import time
+from dataclasses import dataclass, field
+from typing import Dict, List, Optional, Tuple
+
+import rclpy
+from rclpy.node import Node
+from std_msgs.msg import String
+
+from fc_interfaces.msg import SystemDiagnosticsRaw
+
+VEHICLE_TOPIC_RE = re.compile(
+    r'^/fc_network/vehicle/(sys\d+)/(sys_diags|status_text|summary)$'
+)
+STATUS_TEXT_RE = re.compile(r'^\[([\d.]+)\]\s*\[(-?\d+)\]\s*(.*)$')
+
+STATUS_TEXT_TTL_SEC = 30.0
+STALE_SEC = 3.0
+RESCAN_SEC = 2.0
+OFFLINE_SEC = 10.0
+DRAW_HZ = 5.0
+
+# MAV_SYS_STATUS_SENSOR bit labels (MAVLink common)
+SENSOR_BITS: Tuple[Tuple[int, str], ...] = (
+    (268435456, 'PreArm'),
+    (1, 'Gyro'),
+    (2, 'Accel'),
+    (4, 'Mag'),
+    (8, 'Baro'),
+    (16, 'DiffPress'),
+    (32, 'GPS'),
+    (64, 'OptFlow'),
+    (128, 'Vision'),
+    (256, 'Laser'),
+    (512, 'ExtGT'),
+    (1024, 'AngRate'),
+    (2048, 'AttStab'),
+    (4096, 'YawPos'),
+    (8192, 'AltCtrl'),
+    (16384, 'XYCtrl'),
+    (32768, 'Motor'),
+    (65536, 'RC'),
+    (131072, 'Gyro2'),
+    (262144, 'Accel2'),
+    (524288, 'Mag2'),
+    (1048576, 'Geofence'),
+    (2097152, 'AHRS'),
+    (4194304, 'Terrain'),
+    (8388608, 'RevMotor'),
+    (16777216, 'Log'),
+    (33554432, 'Batt'),
+    (67108864, 'Prox'),
+    (134217728, 'Satcom'),
+    (536870912, 'ObsAvoid'),
+    (1073741824, 'Propulsion'),
+)
+
+MAV_STATE_LABELS = {
+    0: 'UNINIT',
+    1: 'BOOT',
+    2: 'CAL',
+    3: 'STANDBY',
+    4: 'ACTIVE',
+    5: 'CRITICAL',
+    6: 'EMERGENCY',
+    7: 'OFF',
+    8: 'TERM',
+}
+
+SEVERITY_LABELS = {
+    0: 'EMERG',
+    1: 'ALERT',
+    2: 'CRIT',
+    3: 'ERROR',
+    4: 'WARN',
+    5: 'NOTICE',
+    6: 'INFO',
+    7: 'DEBUG',
+}
+
+
+@dataclass
+class StatusTextEntry:
+    received_at: float
+    severity: int
+    text: str
+    raw: str
+    sys_key: str
+
+
+@dataclass
+class VehicleDiagState:
+    sys_key: str
+    sysid: int
+    summary: Optional[dict] = None
+    diag: Optional[SystemDiagnosticsRaw] = None
+    status_entries: List[StatusTextEntry] = field(default_factory=list)
+    last_summary_ts: float = 0.0
+    last_diag_ts: float = 0.0
+    last_topic_seen_ts: float = 0.0
+    offline: bool = False
+
+
+def sys_key_to_id(sys_key: str) -> int:
+    return int(sys_key.replace('sys', '', 1))
+
+
+def purge_status_entries(entries: List[StatusTextEntry], now: float, ttl: float = STATUS_TEXT_TTL_SEC) -> None:
+    cutoff = now - ttl
+    entries[:] = [e for e in entries if e.received_at >= cutoff]
+
+
+def parse_status_text(raw: str, sys_key: str, received_at: float) -> StatusTextEntry:
+    match = STATUS_TEXT_RE.match(raw.strip())
+    if match:
+        severity = int(match.group(2))
+        text = match.group(3)
+    else:
+        severity = -1
+        text = raw
+    return StatusTextEntry(
+        received_at=received_at,
+        severity=severity,
+        text=text,
+        raw=raw,
+        sys_key=sys_key,
+    )
+
+
+def decode_sensors(install: int, enabled: int, health: int) -> List[Tuple[str, str]]:
+    """Return list of (label, status) where status is OK / FAIL / OFF."""
+    items: List[Tuple[str, str]] = []
+    for bit, label in SENSOR_BITS:
+        if not (install & bit):
+            continue
+        if not (enabled & bit):
+            items.append((label, 'OFF'))
+        elif health & bit:
+            items.append((label, 'OK'))
+        else:
+            items.append((label, 'FAIL'))
+    return items
+
+
+def format_age(seconds: float) -> str:
+    if seconds < 0:
+        return 'never'
+    if seconds < 1.0:
+        return f'{seconds * 1000:.0f}ms'
+    if seconds < 60.0:
+        return f'{seconds:.1f}s'
+    return f'{seconds / 60:.1f}m'
+
+
+def truncate(text: str, width: int) -> str:
+    if width <= 0:
+        return ''
+    if len(text) <= width:
+        return text
+    if width <= 3:
+        return text[:width]
+    return text[: width - 3] + '...'
+
+
+class VehicleDiagNode(Node):
+    """ROS2 node: discover vehicles and subscribe to diagnostic topics."""
+
+    def __init__(self) -> None:
+        super().__init__('vehicle_diag_show')
+        self._lock = threading.Lock()
+        self._vehicles: Dict[str, VehicleDiagState] = {}
+        self._subs: Dict[str, Dict[str, object]] = {}
+        self._scan_timer = self.create_timer(RESCAN_SEC, self._rescan_topics)
+        self._rescan_topics()
+
+    def snapshot(self) -> Dict[str, VehicleDiagState]:
+        now = time.monotonic()
+        with self._lock:
+            for state in self._vehicles.values():
+                purge_status_entries(state.status_entries, now)
+                if state.last_topic_seen_ts > 0:
+                    state.offline = (now - state.last_topic_seen_ts) > OFFLINE_SEC
+            return {k: state for k, state in self._vehicles.items()}
+
+    def _get_or_create_state(self, sys_key: str) -> VehicleDiagState:
+        if sys_key not in self._vehicles:
+            self._vehicles[sys_key] = VehicleDiagState(
+                sys_key=sys_key,
+                sysid=sys_key_to_id(sys_key),
+            )
+        return self._vehicles[sys_key]
+
+    def _rescan_topics(self) -> None:
+        now = time.monotonic()
+        found: set = set()
+        for topic_name, _ in self.get_topic_names_and_types():
+            match = VEHICLE_TOPIC_RE.match(topic_name)
+            if not match:
+                continue
+            sys_key = match.group(1)
+            found.add(sys_key)
+            with self._lock:
+                state = self._get_or_create_state(sys_key)
+                state.last_topic_seen_ts = now
+                state.offline = False
+            self._ensure_subscriptions(sys_key)
+
+        with self._lock:
+            for sys_key, state in self._vehicles.items():
+                if sys_key not in found and state.last_topic_seen_ts > 0:
+                    state.offline = (now - state.last_topic_seen_ts) > OFFLINE_SEC
+
+    def _ensure_subscriptions(self, sys_key: str) -> None:
+        if sys_key in self._subs:
+            return
+        base = f'/fc_network/vehicle/{sys_key}'
+        subs = {
+            'summary': self.create_subscription(
+                String,
+                f'{base}/summary',
+                lambda msg, sk=sys_key: self._on_summary(sk, msg),
+                10,
+            ),
+            'sys_diags': self.create_subscription(
+                SystemDiagnosticsRaw,
+                f'{base}/sys_diags',
+                lambda msg, sk=sys_key: self._on_sys_diags(sk, msg),
+                10,
+            ),
+            'status_text': self.create_subscription(
+                String,
+                f'{base}/status_text',
+                lambda msg, sk=sys_key: self._on_status_text(sk, msg),
+                10,
+            ),
+        }
+        with self._lock:
+            self._subs[sys_key] = subs
+
+    def _on_summary(self, sys_key: str, msg: String) -> None:
+        now = time.monotonic()
+        try:
+            summary = json.loads(msg.data)
+        except json.JSONDecodeError:
+            summary = {'raw': msg.data}
+        with self._lock:
+            state = self._get_or_create_state(sys_key)
+            state.summary = summary
+            state.last_summary_ts = now
+            state.last_topic_seen_ts = now
+
+    def _on_sys_diags(self, sys_key: str, msg: SystemDiagnosticsRaw) -> None:
+        now = time.monotonic()
+        with self._lock:
+            state = self._get_or_create_state(sys_key)
+            state.diag = msg
+            state.last_diag_ts = now
+            state.last_topic_seen_ts = now
+
+    def _on_status_text(self, sys_key: str, msg: String) -> None:
+        now = time.monotonic()
+        entry = parse_status_text(msg.data, sys_key, now)
+        with self._lock:
+            state = self._get_or_create_state(sys_key)
+            state.status_entries.append(entry)
+            purge_status_entries(state.status_entries, now)
+            state.last_topic_seen_ts = now
+
+
+class DiagCursesApp:
+    """Curses front-end for vehicle diagnostics."""
+
+    def __init__(self, node: VehicleDiagNode) -> None:
+        self._node = node
+        self._running = True
+
+    def run(self) -> None:
+        curses.wrapper(self._main)
+
+    def _init_colors(self) -> None:
+        curses.start_color()
+        curses.use_default_colors()
+        curses.init_pair(1, curses.COLOR_GREEN, -1)    # OK / DISARMED / INFO
+        curses.init_pair(2, curses.COLOR_RED, -1)        # FAIL / ARMED / ERROR+
+        curses.init_pair(3, curses.COLOR_YELLOW, -1)   # WARN / OFF
+        curses.init_pair(4, curses.COLOR_CYAN, -1)     # header / INFO
+        curses.init_pair(5, curses.COLOR_WHITE, -1)    # dim / offline
+        curses.init_pair(6, curses.COLOR_MAGENTA, -1)  # NOTICE
+
+    def _main(self, stdscr) -> None:
+        curses.curs_set(0)
+        stdscr.nodelay(True)
+        stdscr.keypad(True)
+        self._init_colors()
+
+        while self._running and rclpy.ok():
+            try:
+                self._draw(stdscr)
+            except curses.error:
+                pass
+            key = stdscr.getch()
+            if key in (ord('q'), ord('Q'), 27):
+                self._running = False
+            time.sleep(1.0 / DRAW_HZ)
+
+    def _draw(self, stdscr) -> None:
+        stdscr.erase()
+        height, width = stdscr.getmaxyx()
+        now = time.monotonic()
+        vehicles = self._node.snapshot()
+        sorted_keys = sorted(vehicles.keys(), key=sys_key_to_id)
+
+        header = f' Vehicle Diagnostics Monitor  vehicles:{len(sorted_keys)}  Q:quit '
+        stdscr.attron(curses.color_pair(4) | curses.A_BOLD)
+        stdscr.addstr(0, 0, truncate(header.ljust(width - 1), width - 1))
+        stdscr.attroff(curses.color_pair(4) | curses.A_BOLD)
+
+        row = 1
+        if not sorted_keys:
+            msg = ' Waiting for vehicles... (need /fc_network/vehicle/sysN/* topics) '
+            if row < height - 1:
+                stdscr.addstr(row, max(0, (width - len(msg)) // 2), truncate(msg, width - 1))
+            row += 2
+
+        vehicle_rows_end = max(row, height - 8)
+        for sys_key in sorted_keys:
+            if row >= vehicle_rows_end:
+                break
+            state = vehicles[sys_key]
+            row = self._draw_vehicle_block(stdscr, row, width, state, now)
+            if row < height - 1:
+                try:
+                    stdscr.addstr(row, 0, truncate('-' * (width - 1), width - 1))
+                except curses.error:
+                    pass
+                row += 1
+
+        log_title = ' Status Text (last 30s) '
+        if row < height - 1:
+            stdscr.attron(curses.color_pair(4))
+            stdscr.addstr(row, 0, truncate(log_title.ljust(width - 1), width - 1))
+            stdscr.attroff(curses.color_pair(4))
+            row += 1
+
+        all_entries: List[StatusTextEntry] = []
+        for sys_key in sorted_keys:
+            all_entries.extend(vehicles[sys_key].status_entries)
+        all_entries.sort(key=lambda e: e.received_at)
+
+        log_rows = height - row - 1
+        if log_rows <= 0:
+            stdscr.refresh()
+            return
+
+        if not all_entries:
+            empty = ' (no messages in last 30s) '
+            if row < height - 1:
+                stdscr.addstr(row, 0, truncate(empty, width - 1))
+        else:
+            visible = all_entries[-log_rows:]
+            for entry in visible:
+                if row >= height - 1:
+                    break
+                self._draw_status_line(stdscr, row, width, entry)
+                row += 1
+
+        stdscr.refresh()
+
+    def _draw_vehicle_block(
+        self,
+        stdscr,
+        row: int,
+        width: int,
+        state: VehicleDiagState,
+        now: float,
+    ) -> int:
+        summary = state.summary or {}
+        mode = summary.get('mode_name', 'UNKNOWN')
+        armed = summary.get('armed', False)
+        conn = summary.get('connection_type', '?')
+        socket_id = summary.get('socket_id', -1)
+        mav_status = summary.get('mav_status', 0)
+        mav_label = MAV_STATE_LABELS.get(int(mav_status), str(mav_status))
+
+        last_update = max(state.last_summary_ts, state.last_diag_ts)
+        age_sec = (now - last_update) if last_update > 0 else -1.0
+        stale = age_sec >= 0 and age_sec > STALE_SEC
+
+        armed_str = 'ARMED' if armed else 'DISARMED'
+        flags = []
+        if state.offline:
+            flags.append('OFFLINE')
+        if stale and not state.offline:
+            flags.append('STALE')
+        flag_str = f"  [{' '.join(flags)}]" if flags else ''
+
+        line1 = (
+            f' {state.sys_key} (MAV {state.sysid})  {mode}  {armed_str}  '
+            f'{conn}  socket:{socket_id}  {mav_label}  '
+            f'updated {format_age(age_sec)}{flag_str}'
+        )
+        if row < stdscr.getmaxyx()[0] - 1:
+            if state.offline:
+                stdscr.attron(curses.color_pair(5))
+                stdscr.addstr(row, 0, truncate(line1, width - 1))
+                stdscr.attroff(curses.color_pair(5))
+            else:
+                stdscr.addstr(row, 0, truncate(line1, width - 1))
+                armed_pos = line1.find(armed_str)
+                if armed_pos >= 0:
+                    pair = curses.color_pair(2) if armed else curses.color_pair(1)
+                    stdscr.attron(pair | curses.A_BOLD)
+                    stdscr.addstr(row, armed_pos, armed_str)
+                    stdscr.attroff(pair | curses.A_BOLD)
+        row += 1
+
+        if state.diag is not None and row < stdscr.getmaxyx()[0] - 1:
+            load_pct = state.diag.mcu_load / 10.0
+            drop_pct = state.diag.bus_error_rate / 10.0
+            line2 = f' MCU {load_pct:.1f}%  CommDrop {drop_pct:.1f}%'
+            stdscr.addstr(row, 0, truncate(line2, width - 1))
+            row += 1
+
+            sensors = decode_sensors(
+                state.diag.sensors_install_mask,
+                state.diag.sensors_enabled_mask,
+                state.diag.sensors_health_mask,
+            )
+            row = self._draw_sensor_line(stdscr, row, width, sensors)
+        elif row < stdscr.getmaxyx()[0] - 1:
+            stdscr.addstr(row, 0, truncate(' (no sys_diags yet)', width - 1))
+            row += 1
+
+        return row
+
+    def _draw_sensor_line(
+        self,
+        stdscr,
+        row: int,
+        width: int,
+        sensors: List[Tuple[str, str]],
+    ) -> int:
+        max_row, _ = stdscr.getmaxyx()
+        if not sensors:
+            if row < max_row - 1:
+                stdscr.addstr(row, 0, truncate(' Sensors: (none installed)', width - 1))
+            return row + 1
+
+        lines: List[List[Tuple[str, str]]] = [[]]
+        line_len = len(' Sensors: ')
+        for label, status in sensors:
+            token = f'{label}:{status}  '
+            if lines[-1] and line_len + len(token) >= width - 1:
+                lines.append([])
+                line_len = 2
+            lines[-1].append((label, status))
+            line_len += len(token)
+
+        for idx, line_items in enumerate(lines):
+            if row >= max_row - 1:
+                break
+            prefix = ' Sensors: ' if idx == 0 else '  '
+            col = 0
+            stdscr.addstr(row, col, prefix)
+            col += len(prefix)
+            for label, status in line_items:
+                token = f'{label}:{status}  '
+                if col + len(token) >= width - 1:
+                    stdscr.addstr(row, col, truncate('...', width - col - 1))
+                    return row + 1
+                pair = curses.A_NORMAL
+                if status == 'OK':
+                    pair = curses.color_pair(1)
+                elif status == 'FAIL':
+                    pair = curses.color_pair(2) | curses.A_BOLD
+                elif status == 'OFF':
+                    pair = curses.color_pair(3)
+                stdscr.attron(pair)
+                stdscr.addstr(row, col, token)
+                stdscr.attroff(pair)
+                col += len(token)
+            row += 1
+        return row
+
+    def _draw_status_line(
+        self,
+        stdscr,
+        row: int,
+        width: int,
+        entry: StatusTextEntry,
+    ) -> None:
+        sev_label = SEVERITY_LABELS.get(entry.severity, str(entry.severity))
+        prefix = f'[{entry.sys_key:5}] [{entry.severity} {sev_label:5}] '
+        body = truncate(entry.text, max(0, width - len(prefix) - 2))
+        line = prefix + body
+
+        pair = curses.A_NORMAL
+        if entry.severity <= 3:
+            pair = curses.color_pair(2)
+        elif entry.severity == 4:
+            pair = curses.color_pair(3)
+        elif entry.severity == 5:
+            pair = curses.color_pair(6)
+        elif entry.severity == 6:
+            pair = curses.color_pair(1)
+        elif entry.severity == 7:
+            pair = curses.color_pair(5)
+
+        stdscr.attron(pair)
+        stdscr.addstr(row, 0, truncate(line, width - 1))
+        stdscr.attroff(pair)
+
+
+def main() -> int:
+    if not sys.stdout.isatty():
+        print('vehicleDiagShow requires an interactive terminal (TTY).', file=sys.stderr)
+        return 1
+
+    rclpy.init()
+    node = VehicleDiagNode()
+    spin_thread = threading.Thread(target=rclpy.spin, args=(node,), daemon=True)
+    spin_thread.start()
+
+    try:
+        DiagCursesApp(node).run()
+    finally:
+        node.destroy_node()
+        rclpy.shutdown()
+        spin_thread.join(timeout=2.0)
+
+    return 0
+
+
+if __name__ == '__main__':
+    sys.exit(main())

src/fc_network_module/fc_network_module/ntrip_client.py

@@ -2,17 +2,72 @@ import socket
 import base64
 import threading
 import time
+from typing import Optional, Tuple
 
 import rclpy
 from rclpy.node import Node
 from rclpy.qos import QoSProfile, HistoryPolicy, ReliabilityPolicy, DurabilityPolicy
 from mavros_msgs.msg import RTCM
 
+# TODO: ROS_DOMAIN_ID 要補一下
+
+class GGA_stream():
+
+    @classmethod
+    def nmea_checksum(cls, body: str) -> str:
+        """body 不含 '$' 與 '*checksum'，例如 'GPGGA,123519,...'"""
+        value = 0
+        for ch in body:
+            value ^= ord(ch)
+        return f"{value:02X}"
+
+    @classmethod
+    def decimal_to_nmea_dm(cls, 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
+
+    @classmethod
+    def build_gga_sentence(cls, 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 = cls.decimal_to_nmea_dm(lat_deg, is_latitude=True)
+        lon_dm, ew = cls.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}*{cls.nmea_checksum(body)}\r\n"
+        return sentence.encode("ascii")
+
+    @classmethod
+    def send_gga(cls, sock: socket.socket, lat_deg: float, lon_deg: float, alt_m: float = 100.0) -> str:
+        """送出 GGA，回傳可讀句子供列印。"""
+        payload = cls.build_gga_sentence(lat_deg, lon_deg, alt_m)
+        sock.sendall(payload)
+        return payload.decode("ascii").strip()
 
 class NtripClientNode(Node):
-    """連線 NTRIP caster，接收 RTCM3 資料流並發布至 ROS2 topic。"""
+    """連線 NTRIP caster, 接收 RTCM3 資料流並發布至 ROS2 topic """
 
-    RECONNECT_BASE_SEC = 2.0
+    RECONNECT_BASE_SEC = 10.0
     RECONNECT_MAX_SEC = 60.0
 
     def __init__(self):
@@ -21,9 +76,15 @@ class NtripClientNode(Node):
         self.declare_parameter('host', 'rtk2go.com')
         self.declare_parameter('port', 2101)
         self.declare_parameter('mountpoint', '')
-        self.declare_parameter('username', '')
+        self.declare_parameter('username', 'template@mail.com')
         self.declare_parameter('password', '')
 
+        self.declare_parameter('gga_on', False)
+        self.declare_parameter('gga_lat', None)
+        self.declare_parameter('gga_lon', None)
+        self.declare_parameter('gga_alt_m', 100)
+        self.declare_parameter('gga_interval_sec', 60)
+
         rtcm_qos = QoSProfile(
             history=HistoryPolicy.KEEP_LAST,
             depth=1,
@@ -32,7 +93,9 @@ class NtripClientNode(Node):
         )
         self.publisher_ = self.create_publisher(RTCM, '/fc_network/rtcm_input', rtcm_qos)
 
-        self._running = True
+        self._stop_event = threading.Event()
+        self._sock_lock = threading.Lock()
+        self._active_sock: Optional[socket.socket] = None
         self._thread = threading.Thread(target=self._receive_loop, daemon=True)
         self._thread.start()
 
@@ -40,31 +103,50 @@ class NtripClientNode(Node):
 
     # ── NTRIP 連線 + RTCM 接收迴圈 ──────────────────────────────
 
+    def _request_stop(self):
+        self._stop_event.set()
+        with self._sock_lock:
+            if self._active_sock is not None:
+                try:
+                    self._active_sock.close()
+                except OSError:
+                    pass
+
     def _receive_loop(self):
         backoff = self.RECONNECT_BASE_SEC
 
-        while self._running:
+        host = self.get_parameter('host').value
-            host = self.get_parameter('host').value
+        port = self.get_parameter('port').value
-            port = self.get_parameter('port').value
+        mountpoint = self.get_parameter('mountpoint').value
-            mount = self.get_parameter('mountpoint').value
+        user = self.get_parameter('username').value
-            user = self.get_parameter('username').value
+        pwd = self.get_parameter('password').value
-            pwd = self.get_parameter('password').value
 
-            if not mount:
+        if not mountpoint:
-                self.get_logger().error('mountpoint 參數未設定，等待重試...')
+            self.get_logger().error('mountpoint 參數未設定...')
-                time.sleep(backoff)
+            return
-                continue
+
+        while not self._stop_event.is_set():
 
             sock = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
             sock.settimeout(10)
+            with self._sock_lock:
+                self._active_sock = sock
+
+            self.gga_lat = self.get_parameter('gga_lat').value
+            self.gga_lon = self.get_parameter('gga_lon').value
+            self.gga_alt_m = self.get_parameter('gga_alt_m').value
+            self.gga_interval_sec = self.get_parameter('gga_interval_sec').value
+            self.gga_on = self.get_parameter('gga_on').value
+            self.gga_on = self.gga_lat is not None and self.gga_lon is not None and self.gga_on
+
 
             try:
-                self.get_logger().info(f'正在連線 {host}:{port}/{mount} ...')
+                self.get_logger().info(f'正在連線 {host}:{port}/{mountpoint} ...')
                 sock.connect((host, port))
 
                 auth = base64.b64encode(f'{user}:{pwd}'.encode()).decode()
                 request = (
-                    f'GET /{mount} HTTP/1.0\r\n'
+                    f'GET /{mountpoint} HTTP/1.0\r\n'
                     f'User-Agent: NTRIP PythonClient\r\n'
                     f'Authorization: Basic {auth}\r\n'
                     f'Connection: close\r\n\r\n'
@@ -78,29 +160,54 @@ class NtripClientNode(Node):
                     )
                     raise ConnectionError('handshake failed')
 
-                self.get_logger().info(f'已連接至 {mount}，開始接收 RTCM 資料流')
+                self.get_logger().info(f'已成功連接至 {mountpoint}')
+
+
                 backoff = self.RECONNECT_BASE_SEC
 
                 self._read_stream(sock)
 
             except Exception as e:
-                if self._running:
+                if self._stop_event.is_set():
-                    self.get_logger().warn(f'連線中斷: {e}，{backoff:.0f}s 後重連')
+                    break
-                    time.sleep(backoff)
+                self.get_logger().warn(f'連線中斷: {e}，{backoff:.0f}s 後重連')
-                    backoff = min(backoff * 2, self.RECONNECT_MAX_SEC)
+                if self._stop_event.wait(timeout=backoff):
+                    break
+                backoff = min(backoff * 2, self.RECONNECT_MAX_SEC)
             finally:
-                sock.close()
+                with self._sock_lock:
+                    if self._active_sock is sock:
+                        self._active_sock = None
+                try:
+                    sock.close()
+                except OSError:
+                    pass
 
     def _read_stream(self, sock: socket.socket):
         buf = b''
+        last_gga_time = 0.0
+
+        while not self._stop_event.is_set():
+            now = time.time()
+            if self.gga_on and (now - last_gga_time >= self.gga_interval_sec):
+                line = GGA_stream.send_gga(sock, self.gga_lat, self.gga_lon, self.gga_alt_m)
+                self.get_logger().info(f"[GGA] {line}")
+                last_gga_time = now
 
-        while self._running:
             try:
                 chunk = sock.recv(4096)
             except socket.timeout:
+                if self._stop_event.is_set():
+                    break
                 continue
+            except OSError:
+                if self._stop_event.is_set():
+                    break
+                raise
             if not chunk:
-                break
+                if self._stop_event.is_set():
+                    break
+                raise ConnectionError("stream ended. Empty Chunk")
             buf += chunk
 
             while len(buf) >= 6:
@@ -126,8 +233,11 @@ class NtripClientNode(Node):
         self.publisher_.publish(msg)
 
     def destroy_node(self):
-        self._running = False
+        self._request_stop()
-        self._thread.join(timeout=3.0)
+        if self._thread.is_alive():
+            self._thread.join(timeout=5.0)
+            if self._thread.is_alive():
+                self.get_logger().warn('receive thread 未在時限內結束')
         super().destroy_node()
 
 

src/unitdev02/unitdev02/devnote.txt

@@ -8,7 +8,8 @@
  RTK
    跟 RTK2go 抓取列表 Done
    從特定 mount point 得到數據 Done
-   做一個 ros2 service 接到數據並包裝 
+   做一個 ros2 service 接到數據並包裝 Done
+   RTK 實機測試
  
 
 下一步
@@ -34,7 +35,11 @@ python -m fc_network_module.fc_network_module.ntrip_client --ros-args -p mountpo
 
 python -m fc_network_module.fc_network_module.ntrip_client --ros-args -p host:=210.241.63.193 -p port:=81 -p mountpoint:=2020_GNSS -p username:=uavlab6061 -p password:=iamsupersmart
 
-export ROS_DOMAIN_ID=13
+python3 -m fc_network_module.fc_network_module.ntrip_client --ros-args -p host:=210.241.63.193 -p port:=81 -p mountpoint:=2020_GNSS -p username:=uavlab6061 -p password:=iamsupersmart -p gga_on:=True -p gga_lat:=24.155792000 -p gga_lon:=120.630679000 -p gga_interval_sec:=60
+
+watch GPS_RAW_INT
+
+export ROS_DOMAIN_ID=1
 ros2 daemon stop
 ros2 daemon start