tmpCommit1

- 修正 POLL-DONE 程序的 timeout 判定方式
- 修正為 py3.8 語法
- 不在判定要求量跟實收量一致性

.gitignore

@@ -32,3 +32,8 @@ CLAUDE.md
 my_env_notes.md
 notes/
 **/eeprom.bin
+
+# MAVProxy / SITL 執行產生的 log 與參數 dump
+*.tlog
+*.tlog.raw
+mav.parm

README.md

@@ -2,9 +2,15 @@
 
 ===
 ## 功能簡介
-1. mavlink 多對多支援平台
-2. 不允許進到 ros 系統有相同 sysid
-3. 假設一台載具上所有 component 共用同一 socket
+1. mavlink 多對多串流與交換
+2. 支援 xbee 與 telemetry
+3. 支援 udp 管理
+4. ROS2 介面監控載具
+
+===
+使用限制
+1. 不允許進到 ros 系統有相同 sysid
+2. 假設一台載具上所有 component 共用同一 socket
 
 ===
 ## 運行環境
@@ -61,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. 
@@ -77,17 +85,16 @@ python gui.py
     建立、維護與飛控韌體的連接 
     構築 mavlink 封包
     處理無線模組的通訊格式 (XBee)
-    --同時處理與 Gazebo 的 ardupilot_plugin 溝通的 FDM/JSON 訊息 (移除)--
-2. fc_interfaces (重要)
-    自定義的 ROS2 介面檔 沒啥好說的 沒有這個核心會運作不了
+2. fc_interfaces (必要)
+    自定義的 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/comm_panel.py

@@ -247,7 +247,7 @@ class CommPanel(QWidget):
         
         self.serial_baudrate_combo = QComboBox()
         self.serial_baudrate_combo.addItems(['9600', '19200', '38400', '57600', '115200'])
-        self.serial_baudrate_combo.setCurrentText('57600')
+        self.serial_baudrate_combo.setCurrentText('115200')
         self.serial_baudrate_combo.setFixedWidth(100)
         _set_scaled_stylesheet(self.serial_baudrate_combo, """
             QComboBox {

src/GUI/communication.py

@@ -83,7 +83,10 @@ class JsonTelemetryProcessor:
         "position": {"lat": 24.0, "lon": 120.0},
         "heading": 90
     }
-    Serial JSON uses this same shape; only the transport/framing differs.
+    Serial JSON also accepts the compact UAV.py shape:
+    {"s": 1, "m": "GUIDED", "a": 1, "b": 85, "h": 10.0,
+     "v": 4.2, "p": [24.0, 120.0], "ypr": [90.0, 0.0, 0.0],
+     "g": 3, "d": [0.8, 1.2]}
     """
 
     def _emit_json_connection_type(self, drone_id):
@@ -103,19 +106,21 @@ class JsonTelemetryProcessor:
             if not isinstance(data, dict):
                 return
 
-            system_id = data.get('system_id', data.get('sysid'))
+            system_id = data.get('system_id', data.get('sysid', data.get('s')))
             if system_id is None:
                 return
 
             drone_id = f"s{self.socket_id}_{system_id}"
             self._emit_json_connection_type(drone_id)
 
-            mode = data.get('mode', data.get('mode_name'))
+            mode = data.get('mode', data.get('mode_name', data.get('m')))
             state = {}
             if mode is not None:
                 state['mode'] = mode
             if 'armed' in data:
                 state['armed'] = data.get('armed')
+            elif 'a' in data:
+                state['armed'] = bool(data.get('a'))
             if state:
                 self.signals.update_signal.emit('state', drone_id, state)
 
@@ -144,6 +149,10 @@ class JsonTelemetryProcessor:
                 if 'battery_voltage' in data:
                     battery_data['voltage'] = data.get('battery_voltage')
                 self.signals.update_signal.emit('battery', drone_id, battery_data)
+            elif 'b' in data:
+                self.signals.update_signal.emit('battery', drone_id, {
+                    'percentage': data.get('b')
+                })
 
             pos = data.get('position')
             if isinstance(pos, dict):
@@ -159,6 +168,22 @@ class JsonTelemetryProcessor:
                     'lon': data.get('lon', data.get('longitude', 0)),
                     'alt': data.get('alt', data.get('altitude', 0))
                 })
+            elif isinstance(data.get('p'), (list, tuple)) and len(data.get('p')) >= 2:
+                dop = data.get('d') if isinstance(data.get('d'), (list, tuple)) else []
+                gps_data = {
+                    'lat': data['p'][0],
+                    'lon': data['p'][1],
+                    'alt': data.get('h', 0)
+                }
+                if 'g' in data:
+                    gps_data['fix_type'] = data.get('g')
+                if len(dop) >= 1:
+                    gps_data['eph'] = dop[0]
+                    gps_data['hdop'] = dop[0]
+                if len(dop) >= 2:
+                    gps_data['epv'] = dop[1]
+                    gps_data['vdop'] = dop[1]
+                self.signals.update_signal.emit('gps', drone_id, gps_data)
 
             local = data.get('local_position', data.get('local_pose', data.get('local')))
             if isinstance(local, dict):
@@ -183,6 +208,29 @@ class JsonTelemetryProcessor:
                 self.signals.update_signal.emit('altitude', drone_id, {
                     'altitude': alt
                 })
+            elif 'h' in data:
+                height = data.get('h', 0.0)
+                self.signals.update_signal.emit('local_pose', drone_id, {
+                    'x': 0.0,
+                    'y': 0.0,
+                    'z': height
+                })
+                self.signals.update_signal.emit('altitude', drone_id, {
+                    'altitude': height
+                })
+            elif (
+                isinstance(data.get('p'), (list, tuple))
+                or 'lat' in data
+                or 'latitude' in data
+            ):
+                self.signals.update_signal.emit('local_pose', drone_id, {
+                    'x': 0.0,
+                    'y': 0.0,
+                    'z': 0.0
+                })
+                self.signals.update_signal.emit('altitude', drone_id, {
+                    'altitude': 0.0
+                })
 
             velocity = data.get('velocity')
             if isinstance(velocity, dict):
@@ -191,6 +239,12 @@ class JsonTelemetryProcessor:
                     'vy': velocity.get('vy', velocity.get('y', 0.0)),
                     'vz': velocity.get('vz', velocity.get('z', 0.0))
                 })
+            elif 'v' in data:
+                self.signals.update_signal.emit('velocity', drone_id, {
+                    'vx': data.get('v', 0.0),
+                    'vy': 0.0,
+                    'vz': 0.0
+                })
 
             attitude = data.get('attitude')
             if isinstance(attitude, dict):
@@ -200,12 +254,28 @@ class JsonTelemetryProcessor:
                     'yaw': attitude.get('yaw', 0.0),
                     'rates': attitude.get('rates', (0.0, 0.0, 0.0))
                 })
+            elif isinstance(data.get('ypr'), (list, tuple)) and len(data.get('ypr')) >= 3:
+                yaw, pitch, roll = data['ypr'][0], data['ypr'][1], data['ypr'][2]
+                self.signals.update_signal.emit('attitude', drone_id, {
+                    'roll': roll,
+                    'pitch': pitch,
+                    'yaw': yaw,
+                    'rates': (0.0, 0.0, 0.0)
+                })
 
             hud = data.get('hud', {})
             if not isinstance(hud, dict):
                 hud = {}
             if 'heading' in data:
                 hud['heading'] = data.get('heading')
+            elif 'y' in data:
+                hud['heading'] = data.get('y')
+            elif isinstance(data.get('ypr'), (list, tuple)) and len(data.get('ypr')) >= 1:
+                hud['heading'] = data['ypr'][0]
+            if 'v' in data and 'groundspeed' not in hud:
+                hud['groundspeed'] = data.get('v')
+            if 'h' in data and 'alt' not in hud and 'altitude' not in hud:
+                hud['alt'] = data.get('h')
             if hud:
                 self.signals.update_signal.emit('hud', drone_id, {
                     'heading': hud.get('heading', 0.0),
@@ -229,6 +299,7 @@ class UDPMavlinkReceiver(threading.Thread):
         self.connection_name = connection_name
         self.monitor = monitor  # 保存 monitor 引用
         self.socket_id = monitor.get_next_socket_id() if monitor else 0
+        self._socket_id_released = False
         self.running = False
         self.sock = None
         
@@ -258,6 +329,7 @@ class UDPMavlinkReceiver(threading.Thread):
         finally:
             if self.sock:
                 self.sock.close()
+            self._release_socket_id()
                 
     def process_mavlink_message(self, msg):
         """處理 MAVLink 訊息"""
@@ -343,6 +415,12 @@ class UDPMavlinkReceiver(threading.Thread):
     def stop(self):
         """停止接收器"""
         self.running = False
+        self._release_socket_id()
+
+    def _release_socket_id(self):
+        if self.monitor and not self._socket_id_released:
+            self.monitor.release_socket_id(self.socket_id)
+            self._socket_id_released = True
 
 class SerialMavlinkReceiver(threading.Thread, JsonTelemetryProcessor):
     """串口遙測接收器，可自動處理 MAVLink 或 WebSocket 格式 JSON。"""
@@ -354,6 +432,7 @@ class SerialMavlinkReceiver(threading.Thread, JsonTelemetryProcessor):
         self.connection_name = connection_name
         self.monitor = monitor  # 保存 monitor 引用
         self.socket_id = monitor.get_next_socket_id() if monitor else 0
+        self._socket_id_released = False
         self.source_type = 'Serial'
         self.running = False
         self.serial_conn = None
@@ -402,6 +481,7 @@ class SerialMavlinkReceiver(threading.Thread, JsonTelemetryProcessor):
                     self.serial_conn.close()
                 except Exception:
                     pass
+            self._release_socket_id()
 
     def _process_mavlink_byte(self, byte):
         try:
@@ -562,6 +642,12 @@ class SerialMavlinkReceiver(threading.Thread, JsonTelemetryProcessor):
                 self.serial_conn.close()
             except Exception:
                 pass
+        self._release_socket_id()
+
+    def _release_socket_id(self):
+        if self.monitor and not self._socket_id_released:
+            self.monitor.release_socket_id(self.socket_id)
+            self._socket_id_released = True
 
 class WebSocketMavlinkReceiver(threading.Thread, JsonTelemetryProcessor):
     """WebSocket MAVLink 接收器"""
@@ -572,6 +658,7 @@ class WebSocketMavlinkReceiver(threading.Thread, JsonTelemetryProcessor):
         self.connection_name = connection_name
         self.monitor = monitor  # 保存 monitor 引用
         self.socket_id = monitor.get_next_socket_id() if monitor else 0  # 一次性分配 socket_id
+        self._socket_id_released = False
         self.source_type = 'WS'
         self.running = False
         self.max_retries = 5
@@ -629,6 +716,7 @@ class WebSocketMavlinkReceiver(threading.Thread, JsonTelemetryProcessor):
                     break
         
         print(f"WebSocket client {self.connection_name} stopped")
+        self._release_socket_id()
     
     def process_websocket_message(self, data):
         """處理 WebSocket 訊息"""
@@ -637,6 +725,12 @@ class WebSocketMavlinkReceiver(threading.Thread, JsonTelemetryProcessor):
     def stop(self):
         """停止接收器"""
         self.running = False
+        self._release_socket_id()
+
+    def _release_socket_id(self):
+        if self.monitor and not self._socket_id_released:
+            self.monitor.release_socket_id(self.socket_id)
+            self._socket_id_released = True
 
 class DroneMonitor(Node):
     # Subscribe to drone ROS2 topics
@@ -675,7 +769,7 @@ class DroneMonitor(Node):
         # 【新增】Socket ID 重新分配機制 (從 0 開始)
         # ================================================================================
         self.socket_id_mapping = {}  # {原始socket_id: 重新分配的socket_id}
-        self.socket_id_counter = 0   # 當前分配到的最大socket_id
+        self.active_socket_ids = set()  # 目前通訊連線使用中的 socket_id
         self.socket_id_lock = Lock()  # 線程安全鎖
         # ================================================================================
         
@@ -708,23 +802,55 @@ class DroneMonitor(Node):
         self.create_timer(1.0, self.scan_topics)
     
     def get_next_socket_id(self):
-        """获取下一个可用的 socket_id（从 0 开始连续分配）"""
+        """取得目前最小的未使用 socket_id（從 0 開始）。"""
         with self.socket_id_lock:
-            current_id = self.socket_id_counter
-            self.socket_id_counter += 1
-            return current_id
+            socket_id = 0
+            while socket_id in self.active_socket_ids:
+                socket_id += 1
+            self.active_socket_ids.add(socket_id)
+            return socket_id
+
+    def release_socket_id(self, socket_id):
+        """釋放通訊連線使用的 socket_id，讓後續連線可重用最小空缺。"""
+        try:
+            socket_id = int(socket_id)
+        except (TypeError, ValueError):
+            return
+
+        with self.socket_id_lock:
+            if socket_id in getattr(self, 'socket_id_mapping', {}).values():
+                return
+            if socket_id in getattr(self, 'sys_to_socket_id', {}).values():
+                return
+            self.active_socket_ids.discard(socket_id)
     
     def get_or_assign_socket_id(self, original_socket_id):
-        """根據原始 socket_id 分配或獲取對應的 socket_id（從 0 開始連續分配）
-        同一個原始 socket_id 會得到同一個分配的 ID
+        """ROS2 socket_id 映射。
+
+        有原始 socket_id=N 時優先使用 N；若 N 已被其他通訊占用，
+        才改用目前最小未使用 ID。同一個原始 socket_id 會得到同一個映射。
         """
         original_socket_id = str(original_socket_id)
         
         with self.socket_id_lock:
             if original_socket_id not in self.socket_id_mapping:
-                # 分配新的 socket_id
-                self.socket_id_mapping[original_socket_id] = self.socket_id_counter
-                self.socket_id_counter += 1
+                try:
+                    preferred_socket_id = int(original_socket_id)
+                except (TypeError, ValueError):
+                    preferred_socket_id = None
+
+                if (
+                    preferred_socket_id is not None
+                    and preferred_socket_id >= 0
+                    and preferred_socket_id not in self.active_socket_ids
+                ):
+                    socket_id = preferred_socket_id
+                else:
+                    socket_id = 0
+                    while socket_id in self.active_socket_ids:
+                        socket_id += 1
+                self.active_socket_ids.add(socket_id)
+                self.socket_id_mapping[original_socket_id] = socket_id
             
             return self.socket_id_mapping[original_socket_id]
        
@@ -796,8 +922,7 @@ class DroneMonitor(Node):
             # 为每个 sys_id 分配 socket_id（如果还没有分配）
             # 注意：如果后续 summary 提供了 socket_id，会使用 summary 的映射覆盖
             if sys_id not in self.sys_to_socket_id:
-                # 暂时所有 ROS2 topic 共享同一个 socket_id = 0
-                self.sys_to_socket_id[sys_id] = 0
+                self.sys_to_socket_id[sys_id] = self.get_next_socket_id()
             
             subs_attr = f'drone_{sys_id}_subs'
             if not hasattr(self, subs_attr):
@@ -1065,6 +1190,39 @@ class DroneMonitor(Node):
             traceback.print_exc()
             return False
 
+    async def reboot_drone(self, drone_id):
+        """使用 CommandLongClient 執行飛控重啟。"""
+        try:
+            parts = drone_id.split('_')
+            if len(parts) < 2:
+                _log("ERROR", f"[REBOOT] 無效的 drone_id 格式: {drone_id}")
+                return False
+            sysid = int(parts[-1])
+
+            _log("INFO", f"[REBOOT] {drone_id} -> 飛控重啟")
+
+            client = self.get_or_create_client(drone_id)
+            if not client:
+                _log("ERROR", "[REBOOT] CommandLongClient 無法初始化")
+                return False
+
+            result = await client.reboot_autopilot_async(
+                target_sysid=sysid,
+                target_compid=0,
+                timeout_sec=5.0,
+            )
+
+            if result and result.success:
+                _log("INFO", f"[REBOOT] {drone_id} 重啟命令已送出")
+                return True
+
+            _log("ERROR", f"[REBOOT] 重啟失敗 (message={result.message if result else 'None'})")
+            return False
+        except Exception as e:
+            _log("ERROR", f"[REBOOT] 例外錯誤: {e}")
+            traceback.print_exc()
+            return False
+
     def send_setpoint(self, drone_id, x, y, z):
         """Send setpoint position command"""
         if drone_id not in self.setpoint_pubs:
@@ -1161,8 +1319,12 @@ class DroneMonitor(Node):
             # 從 summary 獲取原始 socket_id，並映射到分配的 socket_id
             original_socket_id = data.get('socket_id')
             if original_socket_id is not None:
+                fallback_socket_id = self.sys_to_socket_id.pop(sys_id, None)
+                if fallback_socket_id is not None:
+                    self.release_socket_id(fallback_socket_id)
                 # 使用原始 socket_id 獲取或分配統一的 socket_id
                 assigned_socket_id = self.get_or_assign_socket_id(original_socket_id)
+                self.sys_to_socket_id[sys_id] = assigned_socket_id
             else:
                 # 如果沒有 socket_id，使用 sys_to_socket_id 映射
                 assigned_socket_id = self.sys_to_socket_id.get(sys_id, 0)
@@ -1328,7 +1490,7 @@ class DroneMonitor(Node):
             'ping': msg.data
         }
 
-    def start_serial_connection(self, port='/dev/ttyUSB0', baudrate=57600):
+    def start_serial_connection(self, port='/dev/ttyUSB0', baudrate=115200):
         """啟動串口遙測連接（自動辨識 MAVLink / JSON）"""
         connection_name = f"Serial_{port.replace('/', '_')}"
         receiver = SerialMavlinkReceiver(port, baudrate, self.signals, connection_name, self)

src/GUI/drone_panel.py

@@ -290,6 +290,7 @@ class DronePanel(QWidget):
 
         # 分隔符
         separator1 = QLabel(" - ")
+        separator1.setObjectName(f"{self.drone_id}_battery_sep1")
         _set_scaled_stylesheet(separator1, "color: #DDD;")
 
         # 顯示電壓
@@ -299,6 +300,7 @@ class DronePanel(QWidget):
 
         # 分隔符
         separator2 = QLabel(" - ")
+        separator2.setObjectName(f"{self.drone_id}_battery_sep2")
         _set_scaled_stylesheet(separator2, "color: #DDD;")
 
         # 顯示電池節數 (S count)

src/GUI/gui.py

@@ -4,7 +4,8 @@ from PyQt6.QtWidgets import (QApplication, QMainWindow, QVBoxLayout, QHBoxLayout
                             QWidget, QLabel, QSplitter, QScrollArea,
                             QSizePolicy, QTabWidget, QTableWidget, QTableWidgetItem,
                             QHeaderView, QPushButton, QCheckBox, QLineEdit,
-                            QComboBox, QDialog, QPlainTextEdit, QSlider)
+                            QComboBox, QDialog, QPlainTextEdit, QSlider,
+                            QMessageBox)
 from PyQt6.QtCore import Qt, QTimer, QObject, pyqtSignal, QEvent, QPropertyAnimation, pyqtProperty, QEasingCurve
 from PyQt6.QtGui import QColor, QFont, QPainter, QPen
 import sys
@@ -147,7 +148,7 @@ class ToggleSwitch(QWidget):
 class ControlStationUI(QMainWindow):
     planning_finished = pyqtSignal(object)
 
-    VERSION = '2.4.1'
+    VERSION = '2.6.0'
     FONT_SCALE_MIN = 70
     FONT_SCALE_MAX = 180
     FONT_SCALE_DEFAULT = 100
@@ -219,23 +220,24 @@ class ControlStationUI(QMainWindow):
         self.socket_types = {}
         
         self.socket_colors = {
-            '0': '#00BFFF',  # 天藍色 (DeepSkyBlue)
-            '1': '#FFD700',  # 金色 (Gold)
-            '2': '#FF6969',  # 淺紅色 (Light Red)
-            '3': '#FF69B4',  # 熱粉紅 (HotPink)
-            '4': '#00FA9A',  # 中春綠 (MediumSpringGreen)
-            '5': '#9370DB',  # 中紫色 (MediumPurple) - 串口
-            '6': '#FFA500',  # 橙色 (Orange)
-            '7': '#20B2AA',  # 淺海綠 (LightSeaGreen)
-            '8': '#7CFC00',  # 草綠色 (LawnGreen)
-            '9': '#FF8C00',  # 深橙色 (DarkOrange)
-            'default': '#AAAAAA'  # 灰色
+            '0': '#00ACC1',  # Cyan
+            '1': "#FDD835",  # Yellow
+            '2': '#F8BBD0',  # Pink
+            '3': '#90EE90',  # Green
+            '4': '#FB8C00',  # Orange
+            '5': '#8E24AA',  # Purple
+            '6': '#1E88E5',  # Blue
+            '7': '#E53935',  # Red
+            '8': '#C0CA33',  # Lime
+            '9': '#6D4C41',  # Brown
+            'default': '#E0E0E0'  # Light gray
         }
 
         self.drone_positions = {}
         self.drone_headings = {}
         # 初始化地圖
         self.drone_map = DroneMap()
+        self.drone_map.set_socket_colors(self.socket_colors)
 
         # 地圖更新獨立節流（10Hz），避免 WebEngine / JS map 拖慢 panel 更新
         self.map_timer = QTimer()
@@ -378,7 +380,7 @@ class ControlStationUI(QMainWindow):
 
         verify_label = QLabel("模擬驗證")
         verify_label.setStyleSheet("color: #CCC; font-size: 12px;")
-        self.verify_toggle = ToggleSwitch(checked=True)
+        self.verify_toggle = ToggleSwitch(checked=False)
         group_header.addWidget(verify_label)
         group_header.addWidget(self.verify_toggle)
 
@@ -1102,6 +1104,7 @@ class ControlStationUI(QMainWindow):
         panel.mode_change_requested.connect(self._handle_group_mode_change)
         panel.arm_requested.connect(self._handle_group_arm)
         panel.takeoff_requested.connect(self._handle_group_takeoff)
+        panel.reboot_requested.connect(self._handle_group_reboot)
         panel.box_select_requested.connect(self._handle_box_select)
         panel.select_all_requested.connect(self._handle_select_all_for_group)
         panel.clear_group_requested.connect(self._handle_clear_group)
@@ -1181,6 +1184,16 @@ class ControlStationUI(QMainWindow):
                 panel.update_drone_list()
                 panel.update_status()
 
+        self._sync_map_group_colors()
+
+    def _sync_map_group_colors(self):
+        """同步每台 drone 的 group 顏色到地圖箭頭外框。"""
+        drone_group_colors = {}
+        for group in self.mission_groups.values():
+            for drone_id in group.selected_drone_ids:
+                drone_group_colors[drone_id] = group.color
+        self.drone_map.set_drone_group_colors(drone_group_colors)
+
     def _get_drone_checkbox_style(self, blocked=False):
         """回傳 drone checkbox 樣式；被其他 group 佔用時顯示紅色填滿。"""
         blocked_style = """
@@ -1270,6 +1283,8 @@ class ControlStationUI(QMainWindow):
             # 只有當操作目標組是 active 組時，才更新 UI
             if group_id == self.active_group_id:
                 self.refresh_selection_ui()
+            else:
+                self._sync_map_group_colors()
 
             panel = self.group_panels.get(group_id)
             if panel:
@@ -1295,12 +1310,18 @@ class ControlStationUI(QMainWindow):
                 self.drone_map.set_mission_mode(mission_type)
 
     def _create_executor_for_group(self, group):
-        """為群組建立 MissionExecutor"""
+        """為群組建立 MissionExecutor，讀取 panel 上的執行參數"""
+        exec_params = {}
+        panel = self.group_panels.get(group.group_id)
+        if panel and hasattr(panel, 'get_execution_params'):
+            exec_params = panel.get_execution_params()
+
         executor = MissionExecutor(
             sender=self.command_sender,
             drone_gps=self.monitor.drone_gps,
             monitor=self.monitor,
-            arrival_radius=2.0,
+            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,
         )
         executor.drone_waypoint_reached.connect(self.on_drone_waypoint_reached)
@@ -1319,6 +1340,15 @@ class ControlStationUI(QMainWindow):
             return
         if group.executor is None:
             self._create_executor_for_group(group)
+        else:
+            # 已存在的 executor 也要重讀執行參數，讓使用者調整能生效
+            panel = self.group_panels.get(group_id)
+            if panel and hasattr(panel, 'get_execution_params'):
+                ep = panel.get_execution_params()
+                group.executor.arrival_radius = ep.get(
+                    'arrival_radius', group.executor.arrival_radius)
+                group.executor.hover_stable_sec = ep.get(
+                    'hover_stable_sec', group.executor.hover_stable_sec)
         group.executor.start(group.planned_waypoints)
         panel = self.group_panels.get(group_id)
         if panel:
@@ -1426,6 +1456,39 @@ class ControlStationUI(QMainWindow):
             future = self.monitor.takeoff_drone(drone_id, altitude)
             loop.create_task(self.handle_service_response(future, f"起飛 {drone_id} ({altitude}m)"))
 
+    def _handle_group_reboot(self, group_id):
+        """重啟群組內所有選中的飛控。"""
+        group = self.mission_groups.get(group_id)
+        if not group:
+            _log("WARN", f"找不到群組: {group_id}")
+            return
+
+        selected = list(group.selected_drone_ids)
+        if not selected:
+            self.statusBar().showMessage(f"群組 {group_id} 中沒有無人機", 3000)
+            return
+
+        reply = QMessageBox.warning(
+            self,
+            "確認重啟飛控",
+            f"即將透過 command long 重啟 Group {group_id} 的 {len(selected)} 台飛控：\n"
+            f"{', '.join(selected)}\n\n"
+            "這會中斷飛控連線，請確認無人機已在安全狀態。",
+            QMessageBox.StandardButton.Yes | QMessageBox.StandardButton.No,
+            QMessageBox.StandardButton.No,
+        )
+        if reply != QMessageBox.StandardButton.Yes:
+            _log("WARN", f"Group {group_id} 飛控重啟已取消")
+            return
+
+        _log("INFO", f"Group {group_id} 批次重啟飛控: {', '.join(selected)}")
+        self.statusBar().showMessage(f"Group {group_id}: 正在送出重啟命令", 3000)
+
+        loop = asyncio.get_event_loop()
+        for drone_id in selected:
+            future = self.monitor.reboot_drone(drone_id)
+            loop.create_task(self.handle_service_response(future, f"重啟飛控 {drone_id}"))
+
     def _handle_box_select(self, group_id):
         """觸發地圖框選 → 框選完成後直接分配到該群組"""
         self._pending_box_assign = group_id
@@ -1457,6 +1520,8 @@ class ControlStationUI(QMainWindow):
 
         if group_id == self.active_group_id:
             self.refresh_selection_ui()
+        else:
+            self._sync_map_group_colors()
 
         panel = self.group_panels.get(group_id)
         if panel:
@@ -1493,6 +1558,8 @@ class ControlStationUI(QMainWindow):
         # 只有當操作目標組是 active 組時，才更新 UI
         if group_id == self.active_group_id:
             self.refresh_selection_ui()
+        else:
+            self._sync_map_group_colors()
 
         panel = self.group_panels.get(group_id)
         if panel:
@@ -1517,6 +1584,8 @@ class ControlStationUI(QMainWindow):
         # 只有當操作目標組是 active 組時，才更新 UI
         if group_id == self.active_group_id:
             self.refresh_selection_ui()
+        else:
+            self._sync_map_group_colors()
 
         panel = self.group_panels.get(group_id)
         if panel:
@@ -1563,6 +1632,7 @@ class ControlStationUI(QMainWindow):
             if self.group_tab_widget.count() > 0:
                 self.group_tab_widget.setCurrentIndex(0)
             # _on_group_tab_changed 會自動設定新的 active group
+        self._sync_map_group_colors()
         
         self.statusBar().showMessage(f"已刪除 Group {group_id}", 3000)
         
@@ -1928,7 +1998,7 @@ class ControlStationUI(QMainWindow):
         self.statusBar().showMessage(
             f"Group {group.group_id}: 正在規劃 {group.mission_type} ({len(selected_drones)} 台)", 2000)
 
-        drone_gps_positions = self._collect_drone_gps(selected_drones, base_alt)
+        drone_gps_positions = self._collect_drone_gps(selected_drones)
         if drone_gps_positions is None:
             return
 
@@ -1974,7 +2044,7 @@ class ControlStationUI(QMainWindow):
         base_alt = params.get('altitude', 10.0)
         self.statusBar().showMessage(
             f"Group {group.group_id}: 正在規劃 Grid Sweep ({len(selected_drones)} 台)", 2000)
-        drone_gps_positions = self._collect_drone_gps(selected_drones, base_alt)
+        drone_gps_positions = self._collect_drone_gps(selected_drones)
         if drone_gps_positions is None:
             return
 
@@ -2036,7 +2106,7 @@ class ControlStationUI(QMainWindow):
         self.statusBar().showMessage(
             f"Group {group.group_id}: 正在規劃跟隨模式 ({len(selected_drones)} 台)", 2000)
 
-        drone_gps_positions = self._collect_drone_gps(selected_drones, base_alt)
+        drone_gps_positions = self._collect_drone_gps(selected_drones)
         if drone_gps_positions is None:
             return
 
@@ -2084,18 +2154,20 @@ class ControlStationUI(QMainWindow):
     # 輔助方法
     # ================================================================================
 
-    def _collect_drone_gps(self, selected_drones, base_alt):
+    def _collect_drone_gps(self, selected_drones):
+        # 只收集無人機的水平位置 (lat/lon)；高度固定填 0.0 當 placeholder。
+        # 各任務的指令高度一律取自 mission params（base_altitude/altitude），不依賴無人機
+        # 回報的當下高度，因此這裡的 z 不是規劃輸入，planner 與驗證視窗都會忽略它。
         drone_gps_positions = []
         for drone_id in selected_drones:
             if hasattr(self.monitor, 'drone_gps') and drone_id in self.monitor.drone_gps:
                 gps_data = self.monitor.drone_gps[drone_id]
-                drone_gps_positions.append((gps_data['lat'], gps_data['lon'], gps_data.get('alt', base_alt)))
+                drone_gps_positions.append((gps_data['lat'], gps_data['lon'], 0.0))
             elif drone_id in self.drone_positions:
                 pos = self.drone_positions[drone_id]
                 lat_drone = 24.0 + pos[1] / 111000
                 lon_drone = 120.0 + pos[0] / (111000 * 0.9)
-                alt_drone = pos[2] if len(pos) > 2 else base_alt
-                drone_gps_positions.append((lat_drone, lon_drone, alt_drone))
+                drone_gps_positions.append((lat_drone, lon_drone, 0.0))
             else:
                 self.statusBar().showMessage(f"找不到 {drone_id} 的位置資料", 3000)
                 return None
@@ -2279,17 +2351,30 @@ class ControlStationUI(QMainWindow):
                         self.queue_overview_update(drone_id, 'armed', arm_text)
                     
                     elif msg_type == 'battery':
-                        voltage = data.get('voltage', 16)
-                        cells = round(voltage / 3.95)
-                        percentage = (voltage / cells - 3.7) / 0.5 * 100 if cells > 0 else 0
+                        voltage = data.get('voltage')
+                        cells = round(voltage / 3.95) if voltage is not None else None
+                        percentage = (
+                            (voltage / cells - 3.7) / 0.5 * 100
+                            if voltage is not None and cells and cells > 0
+                            else data.get('percentage', 0)
+                        )
                         if percentage < 20: voltage_color = '#FF6464'
                         elif percentage < 50: voltage_color = '#FFA500'
                         else: voltage_color = '#FFFFFF'
                         percentage = data.get('percentage', percentage)
                         self.update_field(panel, drone_id, 'battery_pct', f"{percentage:.0f}%", voltage_color)
-                        self.update_field(panel, drone_id, 'battery_vol', f"{voltage:.2f}V")
-                        self.update_field(panel, drone_id, 'battery_cells', f"{cells}S")
-                        self.queue_overview_update(drone_id, 'battery', f"{voltage:.2f}V")
+                        if voltage is not None:
+                            self.update_field(panel, drone_id, 'battery_sep1', " - ")
+                            self.update_field(panel, drone_id, 'battery_vol', f"{voltage:.2f}V")
+                            self.update_field(panel, drone_id, 'battery_sep2', " - ")
+                            self.update_field(panel, drone_id, 'battery_cells', f"{cells}S")
+                            self.queue_overview_update(drone_id, 'battery', f"{voltage:.2f}V")
+                        else:
+                            self.update_field(panel, drone_id, 'battery_sep1', "")
+                            self.update_field(panel, drone_id, 'battery_vol', "")
+                            self.update_field(panel, drone_id, 'battery_sep2', "")
+                            self.update_field(panel, drone_id, 'battery_cells', "")
+                            self.queue_overview_update(drone_id, 'battery', f"{percentage:.0f}%")
                     
                     elif msg_type == 'altitude':
                         altitude = data.get('altitude', 0)
@@ -2328,7 +2413,6 @@ class ControlStationUI(QMainWindow):
                         lat, lon = gps_data.get('lat', 0), gps_data.get('lon', 0)
                         self.drone_positions[drone_id] = (lat, lon)
                         self._map_dirty_drones.add(drone_id)
-                        alt = gps_data.get('alt', 0)
                         if not hasattr(self.monitor, 'drone_gps'):
                             self.monitor.drone_gps = {}
                         self.monitor.drone_gps[drone_id] = gps_data.copy()

src/GUI/map_layout.py

@@ -1,4 +1,6 @@
 #!/usr/bin/env python3
+import json
+
 from PyQt6.QtWebEngineWidgets import QWebEngineView
 from PyQt6.QtCore import QTimer, pyqtSignal, QObject, pyqtSlot
 from PyQt6.QtWebChannel import QWebChannel
@@ -17,6 +19,8 @@ class DroneMap:
         self.map_loaded = False
         self.pending_map_updates = {}
         self.font_scale = 1.0
+        self.socket_colors = {}
+        self.drone_group_colors = {}
         
         # 創建橋接對象
         self.bridge = MapBridge()
@@ -281,38 +285,37 @@ class DroneMap:
                     }, 100);
                 });
 
-                function createArrowIcon(color) {
+                function createArrowIcon(color, groupColor) {
+                const outlineColor = groupColor || 'transparent';
                 return L.divIcon({
                     className: 'drone-arrow',
                     html: `
                     <div style="
-                        width: 30px; height: 30px;
+                        width: 34px; height: 34px;
                         display: flex;
                         align-items: center;
                         justify-content: center;
                     ">
-                        <svg width="30" height="30" viewBox="0 0 30 30">
-                            <polygon points="15,0 30,30  15,25  0,30" fill="${color || '#FF0000'}" />
+                        <svg width="34" height="34" viewBox="0 0 34 34">
+                            <polygon
+                                points="17,3 31,31 17,26 3,31"
+                                fill="${color || '#FF0000'}"
+                                stroke="${outlineColor}"
+                                stroke-width="3"
+                                stroke-linejoin="round"
+                            />
                         </svg>
                     </div>
                     `,
-                    iconSize: [30, 30],
-                    iconAnchor: [15, 15]
+                    iconSize: [34, 34],
+                    iconAnchor: [17, 17]
                 });
                 }
 
                 function getColorBySocketId(id) {
-                    if (id.startsWith("s0_")) return "#00BFFF";
-                    if (id.startsWith("s1_")) return "#FFD700";
-                    if (id.startsWith("s2_")) return "#FF6969";
-                    if (id.startsWith("s3_")) return "#FF69B4";
-                    if (id.startsWith("s4_")) return "#00FA9A";
-                    if (id.startsWith("s5_")) return "#9370DB";
-                    if (id.startsWith("s6_")) return "#FFA500";
-                    if (id.startsWith("s7_")) return "#20B2AA";
-                    if (id.startsWith("s8_")) return "#7CFC00";
-                    if (id.startsWith("s9_")) return "#FF8C00";
-                    return "#AAAAAA";
+                    const match = /^s([^_]+)_/.exec(id || '');
+                    const socketId = match ? match[1] : 'default';
+                    return socketColors[socketId] || socketColors.default || "#E0E0E0";
                 }
 
                 function createIdIcon(id) {
@@ -340,8 +343,11 @@ class DroneMap:
                 var markers = {};
                 var idLabels = {};
                 var trajectories = {};
-                const maxTrajectoryPoints = 300;
+                const maxTrajectoryPoints = 2000;
+                const trajectoryMinDistanceMeters = 0.05;
                 var trajectoryLines = {};
+                var socketColors = {};
+                var droneGroupColors = {};
                 var focusedId = null;
                 var initialized = false;
                 var trajectoryGroup = L.layerGroup().addTo(map);
@@ -369,6 +375,16 @@ class DroneMap:
                 var drawStartPoint = null;
                 var mapDragStarted = false;
                 var clickStartPos = null;
+                var lastManualMapMoveAt = 0;
+                var autoCenteringMap = false;
+
+                function markManualMapMove() {
+                    if (!autoCenteringMap) {
+                        lastManualMapMoveAt = Date.now();
+                    }
+                }
+
+                map.on('dragstart drag dragend zoomstart zoomend', markManualMapMove);
                 
                 // 路徑標記變量 (跟隨模式用)
                 var routePoints = [];
@@ -693,7 +709,7 @@ class DroneMap:
                             color: color,
                             weight: 3,
                             opacity: 0.7,
-                            smoothFactor: 1
+                            smoothFactor: 0.1
                         }).addTo(trajectoryGroup);
                     }
                 }
@@ -715,13 +731,20 @@ class DroneMap:
                     if (!markers[id]) return;
                     focusedId = id;
                     var latlng = markers[id].getLatLng();
+                    autoCenteringMap = true;
                     map.flyTo(latlng, map.getZoom());
+                    setTimeout(() => { autoCenteringMap = false; }, 500);
                 }
 
                 setInterval(() => {
                     if (focusedId && markers[focusedId]) {
+                        if (Date.now() - lastManualMapMoveAt < 1000) {
+                            return;
+                        }
                         var latlng = markers[focusedId].getLatLng();
+                        autoCenteringMap = true;
                         map.panTo(latlng);
+                        setTimeout(() => { autoCenteringMap = false; }, 300);
                     }
                 }, 1000);
 
@@ -729,7 +752,7 @@ class DroneMap:
                     if (markers[id]) {
                         const lastPos = markers[id].getLatLng();
                         const distance = lastPos.distanceTo([lat, lon]);
-                        if (distance > 1) {
+                        if (distance >= trajectoryMinDistanceMeters) {
                             addTrajectoryPoint(id, lat, lon);
                         }
                         
@@ -743,7 +766,7 @@ class DroneMap:
                         
                         const color = getColorBySocketId(id);
                         markers[id] = L.marker([lat, lon], {
-                            icon: createArrowIcon(color),
+                            icon: createArrowIcon(color, droneGroupColors[id]),
                             rotationAngle: heading,
                             rotationOrigin: 'center'
                         })
@@ -781,6 +804,33 @@ class DroneMap:
                     }
                 }
 
+                function setDroneGroupColors(groupColors) {
+                    droneGroupColors = groupColors || {};
+                    Object.keys(markers).forEach(id => {
+                        markers[id].setIcon(
+                            createArrowIcon(getColorBySocketId(id), droneGroupColors[id])
+                        );
+                        if (typeof markers[id].setRotationAngle === 'function') {
+                            markers[id].setRotationAngle(markers[id].options.rotationAngle || 0);
+                        }
+                    });
+                }
+
+                function setSocketColors(colors) {
+                    socketColors = colors || {};
+                    Object.keys(markers).forEach(id => {
+                        markers[id].setIcon(
+                            createArrowIcon(getColorBySocketId(id), droneGroupColors[id])
+                        );
+                        if (typeof markers[id].setRotationAngle === 'function') {
+                            markers[id].setRotationAngle(markers[id].options.rotationAngle || 0);
+                        }
+                    });
+                    Object.keys(trajectoryLines).forEach(id => {
+                        trajectoryLines[id].setStyle({ color: getColorBySocketId(id) });
+                    });
+                }
+
                 function clearAllTrajectories() {
                     trajectories = {};
                     Object.values(trajectoryLines).forEach(line => {
@@ -887,6 +937,8 @@ class DroneMap:
         if ok:
             self.map_loaded = True
             self.set_font_scale(self.font_scale)
+            self.set_socket_colors(self.socket_colors)
+            self.set_drone_group_colors(self.drone_group_colors)
         else:
             _log("ERROR", "地圖載入失敗")
     
@@ -901,7 +953,7 @@ class DroneMap:
         
         js_commands = []
         for drone_id, (lat, lon, heading) in self.pending_map_updates.items():
-            js_commands.append(f"updateDrone({lat:.6f}, {lon:.6f}, '{drone_id}', {heading:.1f});")
+            js_commands.append(f"updateDrone({lat:.8f}, {lon:.8f}, '{drone_id}', {heading:.1f});")
         
         if js_commands:
             combined_js = "\n".join(js_commands)
@@ -913,6 +965,20 @@ class DroneMap:
         """清除所有軌跡"""
         if self.map_loaded:
             self.map_view.page().runJavaScript("clearAllTrajectories();")
+
+    def set_drone_group_colors(self, drone_group_colors):
+        """設定每台無人機的 group 外框顏色；未分組者不傳入即為透明。"""
+        self.drone_group_colors = dict(drone_group_colors or {})
+        if self.map_loaded:
+            payload = json.dumps(self.drone_group_colors)
+            self.map_view.page().runJavaScript(f"setDroneGroupColors({payload});")
+
+    def set_socket_colors(self, socket_colors):
+        """設定 socket 顏色，供地圖箭頭本體與軌跡線使用。"""
+        self.socket_colors = dict(socket_colors or {})
+        if self.map_loaded:
+            payload = json.dumps(self.socket_colors)
+            self.map_view.page().runJavaScript(f"setSocketColors({payload});")
     
     def focus_on_drone(self, drone_id):
         """聚焦到指定無人機"""

src/GUI/mission_executor.py

@@ -43,6 +43,7 @@ class DroneTask:
     __slots__ = (
         'drone_id', 'sysid', 'waypoints', 'wp_index', 'done',
         'sent_current_wp', 'fail_count', 'status', 'waiting_since',
+        'entered_radius_at', 'last_log_at',
     )
 
     def __init__(self, drone_id, sysid, waypoints):
@@ -55,6 +56,8 @@ class DroneTask:
         self.fail_count = 0
         self.status = TaskStatus.NORMAL
         self.waiting_since = 0.0  # monotonic time 進入 WAITING_AT_BARRIER 的瞬間
+        self.entered_radius_at = 0.0  # 進入 arrival_radius 的時間，0 = 尚未進入
+        self.last_log_at = 0.0  # 上次印 distance log 的時間
 
     @property
     def current_target(self):
@@ -89,14 +92,21 @@ class MissionExecutor(QObject):
     mission_completed = pyqtSignal()
 
     def __init__(self, sender, drone_gps, monitor=None,
-                 arrival_radius=2.0, tick_rate_hz=2.0,
-                 barrier_timeout_sec=20.0):
+                 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):
         super().__init__()
         self.sender = sender
         self.drone_gps = drone_gps
         self.monitor = monitor  # 用於失敗 fallback 到 LOITER
         self.arrival_radius = arrival_radius
         self.barrier_timeout_sec = barrier_timeout_sec
+        # hover-stable 判定：進入 radius 後須穩定停留 hover_stable_sec 秒才算到達，
+        # 容忍 GPS 抖動跨越邊界（用 radius * 1.5 作 hysteresis）
+        self.hover_stable_sec = hover_stable_sec
+        self.hysteresis_factor = 1.5
+        self.progress_log_interval_sec = progress_log_interval_sec
         self.state = MissionState.IDLE
         self.tasks = {}
         self.rendezvous_indices = set()
@@ -140,7 +150,7 @@ class MissionExecutor(QObject):
             "INFO",
             f"任務已啟動: {len(self.tasks)} 架無人機, "
             f"共 {total_wps} 個航點, "
-            f"到達半徑={self.arrival_radius}m, "
+            f"到達半徑={self.arrival_radius}m (hover-stable {self.hover_stable_sec}s), "
             f"tick 週期={self._interval_ms}ms, "
             f"barrier timeout={self.barrier_timeout_sec}s, "
             f"{rv_info}",
@@ -177,7 +187,7 @@ class MissionExecutor(QObject):
         """
         now = time.monotonic()
 
-        # ---- Phase 1: 到達判定 ----
+        # ---- Phase 1: 到達判定（hover-stable） ----
         for task in self.tasks.values():
             if task.done or task.status == TaskStatus.FALLBACK_LOITER:
                 continue
@@ -195,10 +205,32 @@ class MissionExecutor(QObject):
 
             tgt_lat, tgt_lon, _ = target
             distance = _haversine(gps['lat'], gps['lon'], tgt_lat, tgt_lon)
-            if distance >= self.arrival_radius:
+
+            # hover-stable 判定：
+            #   - 首次進入 radius → 記錄時間
+            #   - 在 radius * hysteresis 內持續 → 保留計時
+            #   - 離開 hysteresis → 重置
+            #   - 在 radius 內穩定 hover_stable_sec 秒 → 視為到達
+            hysteresis_radius = self.arrival_radius * self.hysteresis_factor
+            if distance < self.arrival_radius:
+                if task.entered_radius_at == 0.0:
+                    task.entered_radius_at = now
+                stable_for = now - task.entered_radius_at
+                if stable_for < self.hover_stable_sec:
+                    self._maybe_log_progress(task, now, distance, stable_for)
+                    continue
+            elif distance < hysteresis_radius and task.entered_radius_at > 0.0:
+                # 還在 hysteresis 內，計時不重置（容忍 GPS 抖動跨越邊界）
+                self._maybe_log_progress(task, now, distance, now - task.entered_radius_at)
+                continue
+            else:
+                # 離開 hysteresis：重置計時，繼續等待到達
+                task.entered_radius_at = 0.0
+                self._maybe_log_progress(task, now, distance, 0.0)
                 continue
 
             # 到達當前 wp_index
+            task.entered_radius_at = 0.0  # 重置給下個 wp 用
             if (task.wp_index in self.rendezvous_indices
                     and task.wp_index < task.total_waypoints - 1):
                 # rendezvous 點 → 不推進，進入 barrier 等待
@@ -245,11 +277,28 @@ class MissionExecutor(QObject):
             self.mission_completed.emit()
             _log("INFO", "任務全部完成")
 
+    def _maybe_log_progress(self, task, now, distance, stable_for):
+        """週期性印出 drone 與當前 wp 的距離，方便實飛 debug 卡點問題。"""
+        if now - task.last_log_at < self.progress_log_interval_sec:
+            return
+        task.last_log_at = now
+        if stable_for > 0:
+            stable_info = f", 在內側 {stable_for:.1f}s/{self.hover_stable_sec:.1f}s"
+        else:
+            stable_info = ""
+        _log(
+            "INFO",
+            f"{task.drone_id} → WP {task.wp_index}: "
+            f"距離 {distance:.1f}m (半徑 {self.arrival_radius:.1f}m{stable_info})",
+        )
+
     def _advance_waypoint(self, task, arrived_distance):
         """把 task 推進一個航點，重置發送旗標。不處理 barrier 邏輯。"""
         task.wp_index += 1
         task.sent_current_wp = False
         task.fail_count = 0
+        task.entered_radius_at = 0.0
+        task.last_log_at = 0.0
         if task.wp_index >= task.total_waypoints:
             task.done = True
             self.drone_waypoint_reached.emit(

src/GUI/mission_group.py

@@ -13,14 +13,13 @@ from mission_executor import MissionExecutor, MissionState
 
 # 群組顏色（循環使用）
 GROUP_COLORS = [
-    '#4A9EFF',  # 藍
-    '#FF8C42',  # 橘
-    '#56C87A',  # 綠
-    '#E85D75',  # 紅
-    '#B07CED',  # 紫
-    '#F5C542',  # 黃
-    '#42C9C9',  # 青
-    '#FF6B9D',  # 粉
+    "#E53935",  # Deep Red
+    "#FB8C00",  # Deep Orange
+    "#FDD835",  # Deep Yellow
+    "#43A047",  # Deep Green
+    "#1E88E5",  # Deep Blue
+    "#8E24AA",  # Deep Purple
+    "#D81B60",  # Deep Pink
 ]
 
 DEFAULT_MISSION_PARAM_VALUES = {
@@ -33,9 +32,16 @@ DEFAULT_MISSION_PARAM_VALUES = {
     'lateral_offset': '3.0',
     'longitudinal_spacing': '5.0',
     'line_spacing': '5.0',
+    # 執行參數（所有任務類型共用）
+    'arrival_radius': '4.0',     # 到達半徑 (m)，實飛建議 3~5，SITL 可調小到 2
+    'hover_stable_sec': '2.0',   # 在 radius 內穩定停留多久才視為到達 (s)
 }
 
 
+# 執行參數的 key 集合（所有 mission type 共用，不隨類型切換）
+EXECUTION_PARAM_KEYS = ('arrival_radius', 'hover_stable_sec')
+
+
 class MissionGroup:
     """單一任務群組的資料"""
 
@@ -156,6 +162,7 @@ class GroupPanel(QWidget):
     mode_change_requested = pyqtSignal(str, str)       # group_id, mode
     arm_requested = pyqtSignal(str)                    # group_id
     takeoff_requested = pyqtSignal(str, float)         # group_id, altitude
+    reboot_requested = pyqtSignal(str)                 # group_id
     box_select_requested = pyqtSignal(str)             # group_id — 框選直接分配
     select_all_requested = pyqtSignal(str)             # group_id — 全選直接分配
     clear_group_requested = pyqtSignal(str)            # group_id — 清除分組
@@ -370,6 +377,23 @@ class GroupPanel(QWidget):
 
         right.addStretch()
 
+        # ============================
+        # 飛控操作
+        # ============================
+        reboot_col = QVBoxLayout()
+        reboot_col.setSpacing(3)
+
+        reboot_title = QLabel("飛控操作")
+        reboot_title.setStyleSheet(TITLE)
+        reboot_col.addWidget(reboot_title)
+
+        reboot_btn = QPushButton("重啟飛控")
+        reboot_btn.setStyleSheet(BTN.format(bg='#8A3A3A', fg='#FFF', hover='#A84646'))
+        reboot_btn.clicked.connect(
+            lambda: self.reboot_requested.emit(self.group.group_id))
+        reboot_col.addWidget(reboot_btn)
+        reboot_col.addStretch()
+
         # ============================
         # 第四欄：任務參數
         # ============================
@@ -407,6 +431,12 @@ class GroupPanel(QWidget):
             ],
         }
 
+        # 所有 mission type 共用的執行參數（不隨類型切換隱藏）
+        self._exec_param_defs = [
+            ('arrival_radius', '到達半徑 (m)', self.default_params['arrival_radius']),
+            ('hover_stable_sec', '停留判定 (s)', self.default_params['hover_stable_sec']),
+        ]
+
         # 建立所有參數列的 widget（先全部建好，切換時顯示/隱藏）
         self._param_widgets = {}  # key → (label_widget, input_widget)
         self._param_rows = []     # 所有 row layout 對應的 container widget
@@ -430,6 +460,23 @@ class GroupPanel(QWidget):
                 self._param_widgets[key] = (row_w, inp)
                 self._param_rows.append(row_w)
 
+        # 執行參數列（所有 mission type 共用，永遠顯示）
+        for key, label_text, default in self._exec_param_defs:
+            row_w = QWidget()
+            row_l = QHBoxLayout(row_w)
+            row_l.setContentsMargins(0, 0, 0, 0)
+            row_l.setSpacing(3)
+            lbl = QLabel(label_text)
+            lbl.setStyleSheet(LBL)
+            inp = QLineEdit(default)
+            inp.setStyleSheet(INPUT)
+            inp.setFixedWidth(50)
+            row_l.addWidget(lbl, 1)
+            row_l.addWidget(inp)
+            param_col.addWidget(row_w)
+            self._param_widgets[key] = (row_w, inp)
+            self._param_rows.append(row_w)
+
         # LEADER_FOLLOWER 專用：領隊下拉選單
         self._leader_row = QWidget()
         leader_layout = QHBoxLayout(self._leader_row)
@@ -453,7 +500,7 @@ class GroupPanel(QWidget):
         # 當任務類型切換時更新參數顯示
         self.type_combo.currentTextChanged.connect(self._update_param_visibility)
 
-        # ── 組裝四欄：控制指令 > 任務規劃 > 任務參數 > 選取與分組 ──
+        # ── 組裝欄位：控制指令 > 任務規劃 > 任務參數 > 選取與分組 > 飛控操作 ──
         # 使用伸展因子 0 讓列根據內容自動調整寬度，而不是均等分配
         cols.addLayout(left, 0)
         cols.addWidget(self._make_sep())
@@ -462,7 +509,9 @@ class GroupPanel(QWidget):
         cols.addLayout(param_col, 0)
         cols.addWidget(self._make_sep())
         cols.addLayout(right, 0)
-        cols.addStretch()  # 填充剩餘空間，使四列置左
+        cols.addWidget(self._make_sep())
+        cols.addLayout(reboot_col, 0)
+        cols.addStretch()  # 填充剩餘空間，使欄位置左
 
         layout.addLayout(cols)
 
@@ -527,15 +576,17 @@ class GroupPanel(QWidget):
         self.delete_group_btn.setEnabled(enabled)
 
     def _update_param_visibility(self, _=None):
-        """根據當前任務類型，顯示/隱藏對應的參數列"""
+        """根據當前任務類型，顯示/隱藏對應的參數列。執行參數永遠顯示。"""
         mission_type = self.type_combo.currentText()
-        visible_keys = {d[0] for d in self._param_defs.get(mission_type, [])}
+        mission_keys = {d[0] for d in self._param_defs.get(mission_type, [])}
+        exec_keys = {d[0] for d in self._exec_param_defs}
+        visible_keys = mission_keys | exec_keys
         for key, (row_w, _inp) in self._param_widgets.items():
             row_w.setVisible(key in visible_keys)
         self._leader_row.setVisible(mission_type == 'LEADER_FOLLOWER')
 
     def get_mission_params(self):
-        """讀取當前顯示的參數值，回傳 dict"""
+        """讀取當前任務類型的規劃參數，回傳 dict（不含執行參數）"""
         mission_type = self.type_combo.currentText()
         params = {}
         for key, _label, default in self._param_defs.get(mission_type, []):
@@ -547,6 +598,18 @@ class GroupPanel(QWidget):
                     params[key] = float(default)
         return params
 
+    def get_execution_params(self):
+        """讀取執行參數（arrival_radius, hover_stable_sec 等），回傳 dict"""
+        params = {}
+        for key, _label, default in self._exec_param_defs:
+            if key in self._param_widgets:
+                _row_w, inp = self._param_widgets[key]
+                try:
+                    params[key] = float(inp.text())
+                except ValueError:
+                    params[key] = float(default)
+        return params
+
     def set_param_value(self, key, value):
         """更新指定參數欄位的文字值。"""
         if key not in self._param_widgets:

src/GUI/mission_planner.py

@@ -193,11 +193,13 @@ class FormationPlanner:
             final_y = center_y + radius * math.sin(angle_rad)
             final_z = altitude
 
-            current_x, current_y, current_z = drone_positions[i]
+            current_x, current_y, _current_z = drone_positions[i]
+            # stage1 高度直接用 final_z：先爬升到目標高度再水平集合，
+            # 避免用 current_z 做中點插值（current_z 在實飛時可能是 AMSL 而非相對 home，會導致衝高）
             stage1_positions.append((
                 current_x + (final_x - current_x) * 0.5,
                 current_y + (final_y - current_y) * 0.5,
-                current_z + (final_z - current_z) * 0.5
+                final_z,
             ))
             stage2_positions.append((final_x, final_y, final_z))
 

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/ServiceAckResult.msg"
   "srv/MavPing.srv"
   "srv/MavCommandLong.srv"
   "srv/MavPositionTargetGlobalInt.srv"

src/fc_interfaces/msg/ServiceAckResult.msg

@@ -0,0 +1,43 @@
+# ServiceAckResult.msg
+# ACK code definition for command-like service responses.
+# 這邊只是定義常數的檔案
+#
+# Design:
+# - 0~10   : MAVLink MAV_RESULT compatible range (autopilot/firmware ACK) 大多不會用到 直接吃飛控回傳值
+# - 50~89  : System-level ACK codes (local bridge/orchestrator status)
+# - 90~99  : Unknown/reserved fallback
+#
+# Rule:
+# - If FC ACK is received, return MAV_RESULT_* code.
+# - If FC ACK is not reachable / local pipeline failed, return SYS_* code.
+
+# ---- MAVLink MAV_RESULT (standard) ---- 詳細請自行去網路找 mavlink MAV_RESULT 
+uint8 MAV_RESULT_ACCEPTED=0             # 封包正常接受並執行
+uint8 MAV_RESULT_TEMPORARILY_REJECTED=1 # 封包正常接受 但是飛控很忙 不執行 需要等待後重試
+uint8 MAV_RESULT_DENIED=2               # 封包不正常 參數可能給錯了
+uint8 MAV_RESULT_UNSUPPORTED=3          # 封包不正常 根本認不出是啥
+uint8 MAV_RESULT_FAILED=4               # 封包正常接受 但是載具的狀態異常 不執行
+uint8 MAV_RESULT_IN_PROGRESS=5
+uint8 MAV_RESULT_CANCELLED=6
+uint8 MAV_RESULT_COMMAND_LONG_ONLY=7
+uint8 MAV_RESULT_COMMAND_INT_ONLY=8
+uint8 MAV_RESULT_COMMAND_UNSUPPORTED_MAV_FRAME=9
+uint8 MAV_RESULT_NOT_IN_CONTROL=10
+
+# ---- System-level result codes (custom) ----
+# 50 : 有接到預期的封包 但是訊息建議再確認
+# 51 : some messages will not have obviously ack, so monitering feature message. however, we do NOT get the feature message. ex. 85/86 pair 
+# 
+# 52 : mavlink communication timeout 可能是通訊丟包了
+# 53 : there already has a command being execution for this sysid, try later
+# 54 : this sysid is not found
+
+uint8 FCSYS_GET_EXPECTED_FEEDBACK=50
+uint8 FCSYS_NO_EXPECTED_FEEDBACK=51
+uint8 FCSYS_MAVLINK_TIMEOUT=52
+uint8 FCSYS_BUSY=53
+uint8 FCSYS_DEVICE_NOT_FOUND=54
+
+
+# ---- Fallback ----
+uint8 FCSYS_UNKNOWN=90

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/ackEnum.py

@@ -1,31 +0,0 @@
-
-'''
-單獨出來定義回傳碼的意義
-若跟飛控韌體有正常的通訊 (無論是否被拒絕) 則以 mavlink 定義的結果回傳
-若跟飛控韌體無法通訊 才使用這邊的錯誤代碼
-所以要避開重疊的號碼
-
-這邊備註 Mavlink 的 MAV_RESULT 參數意義
-0 : 封包正常接受並執行
-1 : 封包正常接受 但是正在忙不執行 需要等待後重試
-4 : 封包正常接受 但是載具的狀態異常 不執行
-
-2 : 封包不正常 參數可能給錯了
-3 : 封包不正常 根本認不出是啥
-
-'''
-
-
-from enum import IntEnum
-
-class serviceAckResult(IntEnum):
-    MAVLINK_TIMEOUT = 51 # mavlink communication timeout
-
-    MAVLINK_SEND_BUT_NO_EXP_STEAM = 30 # some messages will not have obviously ack, so monitering feature message. however, we do NOT get the feature message. ex. 85/86 pair 
-
-    MAVLINK_BUSY = 53 # there already has a command being execution for this sysid, try later
-    MAVLINK_DEV_NOTFOUND = 54 # this sysid not fount
-
-    UNKNOWN = 90 # i did not capture this error, try read the logs
-
-

src/fc_network_adapter/fc_network_adapter/fc_network_adapter.md

@@ -37,6 +37,7 @@
 - *self.bridge* 存放 mavlink_bridge 實例 
 - *self.plumber* 存放 serial_manager 實例
 - *self.vehicle_registry* 存放 vehicle_registry 實例
+- *self.fc_ros_manager* 存放 fc_ros_manager 實例 
 
 - *self.panel_thread* 面板的執行緒
 - *self.panelState* 暫存面板與調配者互動的資料流動區
@@ -240,8 +241,14 @@
 8. 基礎載具流量觀測
 9. 載具狀態收集與彙整
 10. a. ros2 topic 應用開發介面 (基礎框架)
+        - GNSS (alt lan fix_type epv eph)
+        - attitude position_ned (pitch yaw row local_position)
+        - battery
     b. ros2 service 應用開發介面 (基礎框架)
-11. RTCM 轉發 - 訂閱 RTCM topic 轉為 MAVLink GPS_RTCM_DATA 廣播給所有載具
+    c. RTCM 轉發 - 訂閱 RTCM topic 轉為 MAVLink GPS_RTCM_DATA 廣播給所有載具
+11. ros2 topic service rtcm Node 重啟功能
+12. 可以定義 ROS_DOMAIN_ID
+
 
 ### 待開發功能
 5-1. 建立 serial 連線 並可以對接收器下達AT指令

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 = "v0.70"
+PROJECT_VER = "v1.25"
 
 class PanelState:
     def __init__(self):
@@ -177,11 +177,18 @@ class ControlPanel:
                     MenuNode("New+", "新增 Serial 連接埠", action = "LIST_SERIAL_RES"),
                     MenuNode("ListAll", "顯示並管理已連線的 Serial", action = "LIST_SERIAL_LINKS"),
                 ]),
-                MenuNode("Vehicles Insp.", "檢視已連線的遠端載具", action = "INSPECT_VEHICLES"),
+                MenuNode("Vehicles Insp.", "檢視已連線的遠端載具", action = "SHOW_VEHICLES"),
                 MenuNode("Engineer Mode", "工程模式", children=[
-                    MenuNode("Stop Manager", "停止 Mavlink 物件管理", "STOP_MANAGER"),
+                    MenuNode("Stop Manager", "停止 Mavlink_Object 管理", "STOP_MANAGER"),
                     MenuNode("Stop Bridge", "停止 Mavlink-ROS 橋接", "STOP_BRIDGE"),
-                    MenuNode("Stop Serial M.", "停止 Serial 端口轉接", "STOP_SERIAL_MANAGER"),
+                    MenuNode("Stop Serial M.", "停止 Serial 端口", "STOP_SERIAL_MANAGER"),
+                    MenuNode("Edit ROS Nodes", "管理 ROS NODE", children=[
+                        MenuNode("Reboot Topics", "重建 ROS Topics", "REBOOT_ROS_TOPICS"),
+                        MenuNode("Reboot Services", "重建 ROS Services", "REBOOT_ROS_SERVICES"),
+                        MenuNode("Reboot RTCM", "重建 RTK RTCM", "REBOOT_ROS_RTCM"),
+                        MenuNode("Stop ROS Manager", "停止 ROS 功能", "STOP_ROS_MANAGER"),
+                        MenuNode("Start ROS Manager", "啟動 ROS 功能", "START_ROS_MANAGER"),
+                    ]),
                 ]),
                 MenuNode("Shutdown", "關閉整個系統", children=[
                     MenuNode("Return", "繼續運行", "BACK"),
@@ -384,6 +391,7 @@ class ControlPanel:
                         state.intoTERMINATION()
                         pre_panel_shutdown()
 
+                    # related with -> UDP mavlink Object 
                     elif selected.action == "TEXT_UDP_IP":
                         result = ControlPanel.input_dialog(stdscr, "請輸入監聽的 IP 位址: ")
                         if result is not None:
@@ -412,6 +420,8 @@ class ControlPanel:
                             # menu_stack.pop()
                             # idx_stack.pop()
 
+
+                    # related with -> Serial Connection
                     elif selected.action == "TEXT_BAUD_SERIAL":
                         result = ControlPanel.input_dialog(stdscr, "請輸入 Baud Rate (e.g., 9600, 115200): ")
                         if result is not None:
@@ -425,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()
@@ -476,13 +490,12 @@ class ControlPanel:
                             # 一樣退兩層
                                 menu_stack.pop()
                                 idx_stack.pop()
+                    
+                    elif selected.action == "DO_DISCOVERY_PANEL":
+                        if hasattr(selected, 'serial_id'):
+                            cmd_q.put(("DO_DISCOVERY", selected.serial_id))
 
-                    elif selected.action == "LIST_MAV_OBJECT":
-                        # 動態生成 mavlink_object 列表選單
-                        created_list_menu = self.create_object_list_menu(state, page=0)
-                        menu_stack.append(created_list_menu)
-                        idx_stack.append(0)
-
+                    # related with -> 列表選單
                     elif selected.action in ("PREV_PAGE", "NEXT_PAGE"):
                         if hasattr(selected, 'page'):
                             current_list_menu = menu_stack[-1]
@@ -507,6 +520,13 @@ class ControlPanel:
                             menu_stack.append(created_list_menu)
                             idx_stack.append(0)
 
+                    # related with -> mavlink object list
+                    elif selected.action == "LIST_MAV_OBJECT":
+                        # 動態生成 mavlink_object 列表選單
+                        created_list_menu = self.create_object_list_menu(state, page=0)
+                        menu_stack.append(created_list_menu)
+                        idx_stack.append(0)
+
                     elif selected.action == "INSPECT_MAV_OBJECT":
                         # 顯示物件詳細資訊
                         if hasattr(selected, 'socket_id'):
@@ -551,6 +571,7 @@ class ControlPanel:
                             if target_id is not None:
                                 cmd_q.put(("MAVOBJ_ADD_TARGET", selected.socket_id, target_id))
 
+                    # related with -> Engineer Mode
                     elif selected.action == "STOP_MANAGER":
                         if state.panel_status != "Engineer":
                             state.panel_info_msg_list.append(("Not in Engineer Mode.", time.time()))
@@ -568,8 +589,40 @@ class ControlPanel:
                             state.panel_info_msg_list.append(("Not in Engineer Mode.", time.time()))
                             continue  # 只有在工程模式下才能操作
                         cmd_q.put("SHUTDOWN_SERIAL_MANAGER")
+                    
+                    elif selected.action == "STOP_ROS_MANAGER":
+                        if state.panel_status != "Engineer":
+                            state.panel_info_msg_list.append(("Not in Engineer Mode.", time.time()))
+                            continue  # 只有在工程模式下才能操作
+                        cmd_q.put("STOP_ROS_MANAGER")
 
-                    elif selected.action == "INSPECT_VEHICLES":
+                    elif selected.action == "START_ROS_MANAGER":
+                        if state.panel_status != "Engineer":
+                            state.panel_info_msg_list.append(("Not in Engineer Mode.", time.time()))
+                            continue  # 只有在工程模式下才能操作
+                        cmd_q.put("START_ROS_MANAGER")
+                        
+                    
+                    elif selected.action == "REBOOT_ROS_TOPICS":
+                        if state.panel_status != "Engineer":
+                            state.panel_info_msg_list.append(("Not in Engineer Mode.", time.time()))
+                            continue  # 只有在工程模式下才能操作
+                        cmd_q.put(("RESTART_ROS_NODE", "status_publisher")) # 參照一下 mavlinkROS2Nodes.NODE_KEYS
+                    
+                    elif selected.action == "REBOOT_ROS_SERVICES":
+                        if state.panel_status != "Engineer":
+                            state.panel_info_msg_list.append(("Not in Engineer Mode.", time.time()))
+                            continue  # 只有在工程模式下才能操作
+                        cmd_q.put(("RESTART_ROS_NODE", "command_service")) # 參照一下 mavlinkROS2Nodes.NODE_KEYS
+
+                    elif selected.action == "REBOOT_ROS_RTCM":
+                        if state.panel_status != "Engineer":
+                            state.panel_info_msg_list.append(("Not in Engineer Mode.", time.time()))
+                            continue  # 只有在工程模式下才能操作
+                        cmd_q.put(("RESTART_ROS_NODE", "rtcm_relay")) # 參照一下 mavlinkROS2Nodes.NODE_KEYS
+
+                    # related with -> 進 bridge 的設備 列表
+                    elif selected.action == "SHOW_VEHICLES":
                         # 進入載具檢視選單
                         cmd_q.put("UPDATE_VEHICLES_LIST")
                         created_list_menu = self.create_vehicles_list_menu(state, page=0)
@@ -767,6 +820,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"),
@@ -818,6 +872,7 @@ class ControlPanel:
                 obj_menu = MenuNode(f"Serial #{serial_id}", f"連結口 {serial_id}", None, children=[
                     MenuNode("Info", "查看詳細資訊", "INSPECT_LINKED_SERIAL"),
                     MenuNode("Remove", "移除此連結口", "REMOVE_LINKED_SERIAL"),
+                    MenuNode("Discovery", "搜尋遠端RF(Only XBEE-ESP32)", "DO_DISCOVERY_PANEL"),
                     # MenuNode("Change UDP Target", "變更目標 UDP (工程)", "CHANGE_LINKED_SERIAL_TARGET"),
                     MenuNode("GoUp", "回到列表", "BACK"),
                 ])
@@ -1061,7 +1116,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 
@@ -1153,6 +1208,8 @@ class Orchestrator:
             'vfr_hud': 1.0,
             'mode': 0.0,
             'summary': 1.0,
+            'sys_diags': 1.0,
+            'status_text': 1.0,
         }
 
     def engageWholeSystem(self):
@@ -1173,6 +1230,8 @@ class Orchestrator:
 
     def mainLoop(self):
         logger.info("Main orchestrator started <-")
+        self.ros2_retry_time = None
+        self.ros2_manual_stop = False
         try:
             # while not self.stop_evt.is_set():
             while self.panel_thread.is_alive():
@@ -1204,6 +1263,17 @@ class Orchestrator:
                 else:
                     self.panelState.ros2_manager_state = 'Stopped'
 
+                    # 這邊做一個簡易的重啟 掛掉要等待一秒再重啟 避免過度頻繁 # 算是高技術債的方式構建 但是未來有需要在還吧 (煙
+                    if not self.ros2_manual_stop:
+                        # logger.info(f"mark A {self.ros2_retry_time}")
+                        if self.ros2_retry_time:
+                            if (time.time() - self.ros2_retry_time) > 1:
+                                self.fc_ros_manager.start()
+                                self.fc_ros_manager.schedule_restart_node("command_service")
+                                self.ros2_retry_time = None
+                        else:
+                            self.ros2_retry_time = time.time()
+
                 # B. 更新載具列表（從 vehicle_registry 獲取）
                 self._update_vehicles_list()
 
@@ -1254,6 +1324,11 @@ class Orchestrator:
                         elif action == "REMOVE_LINKED_SERIAL":
                             serial_id = cmd[1]
                             self.plumber.remove_serial_link(serial_id)
+                        elif action == "DO_DISCOVERY":
+                            serial_id = cmd[1]
+                            processor = self.plumber.get_espv1_processor(serial_id)
+                            if processor:
+                                processor.request_discovery()
 
                         elif action == "INSPECT_VEHICLE":
                             sysid, compid = cmd[1], cmd[2]
@@ -1268,6 +1343,12 @@ class Orchestrator:
                         elif action == "UNREGISTER_VEHICLE":
                             sysid = cmd[1]
                             self.vehicle_registry.unregister(sysid)
+                        elif action == "RESTART_ROS_NODE":
+                            node_key = cmd[1]
+                            if not self.fc_ros_manager.schedule_restart_node(node_key):
+                                logger.warning(
+                                    f"RESTART_ROS_NODE failed: {node_key!r}"
+                                )
 
                     elif cmd == "CREATE_UDP_INBOUND":
                         self.panelState.udp_info_temp["direction"] = "inbound"
@@ -1277,12 +1358,19 @@ class Orchestrator:
                         self.create_udp_object()
                     elif cmd == "CREATE_SERIAL_PORT":
                         self.create_serial_port_object()
+
                     elif cmd == "SHUTDOWN_BRIDGE":
                         self.bridge.stop()
                     elif cmd == "SHUTDOWN_MANAGER":
                         self.manager.shutdown()
                     elif cmd == "SHUTDOWN_SERIAL_MANAGER":
                         self.plumber.shutdown()
+                    elif cmd == "STOP_ROS_MANAGER":
+                        self.fc_ros_manager.stop()
+                        self.ros2_manual_stop = True
+                    elif cmd == "START_ROS_MANAGER":
+                        self.fc_ros_manager.start()
+                        self.ros2_manual_stop = False
 
                 except queue.Empty:
                     pass
@@ -1522,6 +1610,7 @@ class Orchestrator:
         # 定義通訊類型映射表
         COMM_TYPE_MAP = {
             "XBee(API-AT)": sm.SerialMode.XBEEAPI2AT,
+            "XBee(API-API)espv1": sm.SerialMode.XBEEAPI_espv1,
             "TELEMETRY": sm.SerialMode.STRAIGHT,
             # 新增區
         }
@@ -1565,25 +1654,7 @@ class Orchestrator:
 
 def main():
 
-    # =========== 各項模組的版本先驗 ===========
-    # 除非你有在做這幾項模組的改版 不然動到這邊的版本號 代表執行環境有很大的問題!!!!!!
-    version_check = True
-    if mo.MODULE_VER != "1.50":
-        print("Module Version Error! : mavlinkObkect")
-        version_check = False
-    if mros.MODULE_VER != "1.50":
-        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
-    # ========================================
+    logger.info(f"Each Module Running Version at mavlinkObkect:{mo.MODULE_VER}, mavlinkROS2Nodes:{mros.MODULE_VER}, mavlinkVehicleView:{mvv.MODULE_VER}, serialManager:{sm.MODULE_VER}")
 
     stop_evt = threading.Event()
     def signal_handler(signum, frame):
@@ -1622,4 +1693,11 @@ if __name__ == "__main__":
 2025.04.13
 1. 加入各項模組的版本先驗 檢驗失敗就直接離開
 
+2025.05.28
+1. 工程模式 新增 Node 重啟的功能
+
+2026.08.05 (ver 1.25)
+1. 增加對於 esp32 連線模式的支援
+
+
 '''

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,
-    VehicleView,
+    vehicle_registry, # 儲存全部物件的地方
+    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
-            24: self._handle_gps_raw_int,     # GPS_RAW_INT
-            30: self._handle_attitude,        # ATTITUDE
-            32: self._handle_local_position,  # LOCAL_POSITION_NED
-            33: self._handle_global_position, # GLOBAL_POSITION_INT
-            74: self._handle_vfr_hud,         # VFR_HUD
-            147: self._handle_battery_status, # BATTERY_STATUS
+            0: self._handle_heartbeat,          # HEARTBEAT
+            1: self._handle_vehicle_sys_status, # SYS_STATUS
+            24: self._handle_gps_raw_int,       # GPS_RAW_INT
+            30: self._handle_attitude,          # ATTITUDE
+            32: self._handle_local_position,    # LOCAL_POSITION_NED
+            33: self._handle_global_position,   # GLOBAL_POSITION_INT
+            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, 
-        # 32 LOCAL_POSITION_NED, 33 GLOBAL_POSITION_INT, 74 VFR_HUD, 147 BATTERY_STATUS
-        self.bridge_msg_types = set([0, 24, 30, 32, 33, 74, 147])  
+        # 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, 253 STATUSTEXT
+        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

@@ -37,7 +37,7 @@ import mavros_msgs.msg
 # ROS2 Service imports
 import fc_interfaces.srv as fcsrv
 import fc_interfaces.msg as fcmsg
-from .ackEnum import serviceAckResult
+from fc_interfaces.msg import ServiceAckResult
 
 # 自定義 imports
 from . import mavlinkVehicleView as mvv
@@ -45,7 +45,10 @@ from . import mavlinkObject as mo
 from .utils import setup_logger
 
 logger = setup_logger(os.path.basename(__file__))
-MODULE_VER = "1.50"
+MODULE_VER = "2.50"
+
+FC_ROS_DOMAIN_ID = "0"
+NODE_KEYS = ("status_publisher", "command_service", "rtcm_relay")
 
 # ============================================================================
 #                       VehicleStatusPublisher Node
@@ -65,14 +68,16 @@ class PublishRateController:
         # 注意 這邊是定義區 不要把參數寫在這裡 所以預設全部關閉
         # 以這個專案 請看 mainOrchestrator.py 的 Orchestrator 初始化階段
         self.topic_intervals = {
-            'position_gnss': 0.0,  # GNSS位置
-            'position_ned': 0.0,   # LOCAL_POSITION_NED (位置+速度)
-            'attitude': 0.0,       # 姿態 (pitch yaw row 與其加速狀態)
-            'velocity': 0.0,       # 速度 (已經包含在 vfr_hud 未來移除)
-            'battery': 0.0,        # 電池
-            'vfr_hud': 0.0,        # VFR HUD (地速 空速 絕對高度 爬升率 航向 油門)
-            'mode': 0.0,           # 飛行模式 (已經在 summary 裡 未來移除)
-            'summary': 0.0,        # 載具摘要 (sysid 飛行模式 解鎖上鎖 gps狀態)
+            'summary': 0.0,         # 載具摘要 (sysid 飛行模式 解鎖上鎖 gps狀態)
+            'position_gnss': 0.0,   # GNSS位置 (海拔高度)
+            'position_ned': 0.0,    # LOCAL_POSITION_NED (位置+速度+相對高度)
+            'attitude': 0.0,        # 姿態 (pitch yaw row 與其加速狀態)
+            'battery': 0.0,         # 電池
+            'vfr_hud': 0.0,         # VFR HUD (地速 空速 絕對高度 爬升率 航向 油門)
+            'sys_diags': 0.0,       # SYS_STATUS 系統診斷
+            'status_text': 0.0,     # STATUSTEXT 飛控文字（佇列驅動，>0 僅作啟用旗標）
+            'mode': 0.0,            # 飛行模式 (已經在 summary 裡 未來移除)
+            'velocity': 0.0,        # 速度 (已經包含在 vfr_hud 未來移除)
             # 在這裡新增更多 topics...
         }
         # 記錄每個 topic 的最後發布時間 {(sysid, topic): timestamp}
@@ -109,6 +114,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()
@@ -119,7 +128,7 @@ class VehicleStatusPublisher(Node):
     
     職責:
     - 定期從 vehicle_registry 讀取載具狀態
-    - 頻率控制（位置/姿態 2Hz，電池/摘要 1Hz）
+    - 頻率控制 (位置/姿態 2Hz, 電池/摘要 1Hz)
     - 發布標準 ROS2 消息類型
     - 檢測訂閱者，按需發布
     """
@@ -178,11 +187,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):
@@ -203,7 +214,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):
@@ -436,11 +447,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,
-            # '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,
+            'mav_status': status.system_status if status.system_status is not None 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,
         }
@@ -449,6 +456,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，請在此處實作對應的發布方法
@@ -467,29 +530,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
@@ -527,6 +567,14 @@ class MavlinkCommandService(Node):
     講白話一點就是
     每次接到一個 service 請求 要整個系統丟某種指令給載具時
     會做兩件事 1."丟出mavlink封包" 2."創造一個臨時信箱 Pending"
+    
+    然後透過每次 manager spin 
+    會去呼叫 return_router() 方法
+    這個方法會監聽 return_packet_ring 跟臨時信箱的 Pending 做配對
+    配對到的解開 Pending 
+
+    解開後 相對應的 handle_XXX 就會開始做事
+    
 
     """
 
@@ -535,7 +583,7 @@ class MavlinkCommandService(Node):
     def __init__(self):
         super().__init__('mavlink_command_service')
         
-        # 狀態標記
+        # 狀態標記 
         self.running = True
         
         # # mavlinkObject 的引用（將由外部設置） 用不到
@@ -581,7 +629,7 @@ class MavlinkCommandService(Node):
             self.handle_set_position_target_global_int
         )
 
-        logger.info("MavlinkCommandService initialized")
+        # logger.info("MavlinkCommandService initialized")
 
     def _index(self, target_sysid, target_compid):
 
@@ -608,20 +656,19 @@ class MavlinkCommandService(Node):
         藉由 event.set() 解開 service 中的 block 
         '''
         return_tuple = mo.return_packet_ring.get()
-        # 確認 return_packet_ring 有資料
         if return_tuple == None: return
 
         socketid, timestamp, msg = return_tuple
         sysid = msg.get_srcSystem()
 
         _pending = self._pending_by_sysid.get(sysid)
-        # 確認是否有 service 在等待回應 若無直接 return 此封包也會被忽略
-        if _pending == None:
+        if _pending is None:
             return
 
         if _pending.match_fn(msg):
             _pending.result_mav_msg = msg
             _pending.event.set()
+        return
 
     # ═══════════════════════════════════════════════════════════════════════
     # Service Handler: 發送 TIMESYNC 可以作為模板
@@ -706,20 +753,20 @@ class MavlinkCommandService(Node):
         # 設定失效回應
         response.success = False
         response.message = "Unknown error"
-        response.ack_result = serviceAckResult.UNKNOWN
+        response.ack_result = ServiceAckResult.FCSYS_UNKNOWN
         timeout_sec = request.timeout_sec
 
         # 1) 確認是否已經有相同 sysid 的其他需求正在 pending
         if request.target_sysid in self._pending_by_sysid:
             response.message = f"sysid {request.target_sysid} already has pending request"
-            response.ack_result = serviceAckResult.MAVLINK_BUSY
+            response.ack_result = ServiceAckResult.FCSYS_BUSY
             return response
 
         # 2) 找到 socket 標地
         socketObject = self._index(request.target_sysid, request.target_compid)
         if socketObject is None:
             response.message = "This system id not found."
-            response.ack_result = serviceAckResult.MAVLINK_DEV_NOTFOUND
+            response.ack_result = ServiceAckResult.FCSYS_DEVICE_NOT_FOUND
             return response
 
         # 3) 接收封包系統 的設定
@@ -750,12 +797,13 @@ class MavlinkCommandService(Node):
         # 5) 等待回應封包
         if not evt.wait(timeout_sec):
             response.message = "mavlink timeout - CommLONG"
-            response.ack_result = serviceAckResult.MAVLINK_TIMEOUT
+            response.ack_result = ServiceAckResult.FCSYS_MAVLINK_TIMEOUT
             fail_skip = True
 
         # 6) 處理回應封包
         if not fail_skip:
             ack_msg = _pending.result_mav_msg
+            # response.success = ServiceAckResult.FCSYS_MAVLINK_BREAK # dev 故意觸發錯誤用的
             response.success = (ack_msg.result == 0) # mavutil.mavlink.MAV_RESULT_ACCEPTED
             response.message = "" # 沒有消息就是好消息
             response.ack_result = ack_msg.result
@@ -779,20 +827,20 @@ class MavlinkCommandService(Node):
         '''
         # 設定失效回應
         response.message = "Unknown error"
-        response.ack_result = serviceAckResult.UNKNOWN
+        response.ack_result = ServiceAckResult.FCSYS_UNKNOWN
         timeout_sec = request.timeout_sec
 
         # 1) 確認是否已經有相同 sysid 的其他需求正在 pending
         if request.target_sysid in self._pending_by_sysid:
             response.message = f"sysid {request.target_sysid} already has pending request"
-            response.ack_result = serviceAckResult.MAVLINK_BUSY
+            response.ack_result = ServiceAckResult.FCSYS_BUSY
             return response
 
         # 2) 找到 socket 標地
         socketObject = self._index(request.target_sysid, request.target_compid)
         if socketObject is None:
             response.message = "This system id not found."
-            response.ack_result = serviceAckResult.MAVLINK_DEV_NOTFOUND
+            response.ack_result = ServiceAckResult.FCSYS_DEVICE_NOT_FOUND
             return response
 
         # 3) 接收封包系統 的設定
@@ -827,7 +875,7 @@ class MavlinkCommandService(Node):
         # 5) 等待回應封包
         if not evt.wait(timeout_sec):
             response.message = "mavlink timeout - SET POSITION 86"
-            response.ack_result = MAVLINK_SEND_BUT_NO_EXP_STEAM
+            response.ack_result = ServiceAckResult.FCSYS_NO_EXPECTED_FEEDBACK
             fail_skip = True
 
         # 6) 處理回應封包
@@ -835,7 +883,7 @@ class MavlinkCommandService(Node):
             ack_msg = _pending.result_mav_msg
 
             response.message = ""
-            response.ack_result = 0
+            response.ack_result = ServiceAckResult.MAV_RESULT_ACCEPTED
             response.r_time_boot_ms = ack_msg.time_boot_ms 
             response.r_type_mask = ack_msg.type_mask
             response.r_lat_int = ack_msg.lat_int
@@ -952,11 +1000,10 @@ class MavlinkCommandService(Node):
         return response
 
 
-
     def stop(self):
         """停止服務"""
         self.running = False
-        logger.info("MavlinkCommandService stopped")
+        # logger.info("MavlinkCommandService stopped")
 
 
 # ============================================================================
@@ -1025,7 +1072,7 @@ class RtcmRelay(Node):
             rtcm_qos,
         )
 
-        logger.info("RtcmRelay initialized")
+        # logger.info("RtcmRelay initialized")
 
     # ── callback ──────────────────────────────────────────────────────
 
@@ -1129,7 +1176,7 @@ class RtcmRelay(Node):
     def stop(self):
         """停止轉發"""
         self.running = False
-        logger.info("RtcmRelay stopped")
+        # logger.info("RtcmRelay stopped")
 
 
 # ============================================================================
@@ -1145,17 +1192,24 @@ class fc_ros_manager:
     stop 是停下止 ROS2 nodes 的運行，但不銷毀節點實例，允許後續再次 start。
     shutdown 是完全關閉 ROS2 並銷毀節點實例。
 
-    管理三個獨立的 ROS2 Node：
+    管理三個獨立的 ROS2 Node :
     - VehicleStatusPublisher
     - MavlinkCommandService
     - RtcmRelay
     
     提供統一的啟動/停止介面給 mainOrchestrator
+
+    另外 這邊用到 MultiThreadedExecutor 會開出額外的 thread 的特性 
+    使得就算 executor 在跑一些需要等待的方法 
+    常態的 spin_once 也不會被 block (spin_thread 是另一個支線)
     """
     
     def __init__(self):
         self.initialized = False
         self.running = False
+        self._restarting = False
+        self._pending_restart: Optional[str] = None
+        self._restart_lock = threading.Lock()
         
         # node 實例
         self.status_publisher: Optional[VehicleStatusPublisher] = None
@@ -1173,7 +1227,7 @@ class fc_ros_manager:
             return False
         
         try:
-            # 初始化 ROS2
+            os.environ.setdefault("ROS_DOMAIN_ID", FC_ROS_DOMAIN_ID)
             if not rclpy.ok():
                 rclpy.init(args=None)
             
@@ -1208,6 +1262,10 @@ class fc_ros_manager:
         
         try:
             self.running = True
+
+            self.status_publisher.running = True
+            self.command_service.running = True
+            self.rtcm_relay.running = True
             
             self.spin_thread = threading.Thread(
                 target=self._spin_executor,
@@ -1223,24 +1281,105 @@ class fc_ros_manager:
             logger.error(f"Failed to start fc_ros_manager: {e}")
             self.running = False
             return False
-    
+
+    # 給 orchestrator 呼叫的函數
+    def schedule_restart_node(self, node_key: str) -> bool:
+        """由 orchestrator 排程單一 node 重啟（於 spin thread 下一輪執行）"""
+        if node_key not in NODE_KEYS:
+            logger.warning(f"schedule_restart_node: unknown key {node_key!r}")
+            return False
+        if not self.running:
+            logger.warning("schedule_restart_node: fc_ros_manager not running")
+            return False
+        if not self.spin_thread or not self.spin_thread.is_alive():
+            logger.warning("schedule_restart_node: spin thread not alive")
+            return False
+        with self._restart_lock:
+            self._pending_restart = node_key
+        logger.info(f"schedule_restart_node: queued {node_key}")
+        return True
+
     # 循環執行的地方
     def _spin_executor(self):
         """在 thread 中運行的 executor"""
         try:
-            # logger.info("ROS2 executor spinning...")
             while self.running:
                 self.executor.spin_once(timeout_sec=0.1)
-                self.command_service.return_router()
+
+                pending = None
+                with self._restart_lock:
+                    if self._pending_restart:
+                        pending = self._pending_restart
+                        self._pending_restart = None
+
+                if pending:
+                    self._restart_node(pending)
+                elif self.command_service and not self._restarting:
+                    self.command_service.return_router()
         except Exception as e:
             logger.error(f"fc_ros_manager error in spinning executor: {e}")
-    
+        finally:
+            self.running = False
+
+    def _restart_node(self, node_key: str) -> bool:
+        """僅由 spin thread 呼叫 : destroy 並重建單一 node"""
+        old_node = getattr(self, node_key, None)
+        if old_node is None:
+            logger.error(f"_restart_node: {node_key} is None")
+            return False
+
+        # 重置前保留舊有或處理設定
+        saved_topic_intervals = None
+        if node_key == "status_publisher":
+            saved_topic_intervals = dict(old_node.rate_controller.topic_intervals)
+
+        # 如果是 command_service 清除所有等待的 pending 並且清除 return message 
+        if node_key == "command_service":
+            old_node._pending_by_sysid.clear()
+            for sockObj in mo.mavlink_object.mavlinkObjects.values():
+                sockObj.set_return_message_types([])
+
+        try:
+            self._restarting = True
+
+            # 停止運作 移出執行迴圈 銷毀資源
+            old_node.stop()
+            self.executor.remove_node(old_node)
+            old_node.destroy_node()
+
+            # 建立新的 node
+            if node_key == "status_publisher":
+                new_node = VehicleStatusPublisher()
+                if saved_topic_intervals is not None:
+                    new_node.rate_controller.topic_intervals = saved_topic_intervals
+            elif node_key == "command_service":
+                new_node = MavlinkCommandService()
+            elif node_key == "rtcm_relay":
+                new_node = RtcmRelay()
+            else:
+                return False
+
+            # 加回去管理系統中
+            setattr(self, node_key, new_node)
+            self.executor.add_node(new_node)
+            logger.info(f"_restart_node: {node_key} restarted")
+            return True
+        except Exception as e:
+            logger.error(f"_restart_node failed for {node_key}: {e}")
+            return False
+        finally:
+            self._restarting = False
+
     def stop(self):
         """停止 ROS2 nodes"""
-        if not self.running:
-            logger.warning("fc_ros_manager not running")
+        # if not self.running:
+        #     logger.warning("fc_ros_manager not running")
+        #     return False
+
+        if not self.spin_thread:
+            logger.warning("fc_ros_manager without thread to stop")
             return False
-        
+
         try:            
             # 標記停止
             self.running = False
@@ -1289,7 +1428,7 @@ class fc_ros_manager:
             except Exception as e:
                 logger.error(f"Error during shutdown: {e}")
     
-    def get_status(self) -> dict:
+    def get_status(self):
         return {
             'initialized': self.initialized,
             'running': self.running,
@@ -1298,6 +1437,12 @@ class fc_ros_manager:
             'rtcm_relay_active': self.rtcm_relay is not None and self.rtcm_relay.running,
         }
 
+    
+
+
+        
+
+
 
 # ============================================================================
 #                              全域實例
@@ -1331,8 +1476,17 @@ ros2_manager = fc_ros_manager()
 2. 實作過期丟棄(1s)、blake2s hash 去重、最短轉發間隔節流、分片(180 bytes/片)
 3. 接入 fc_ros_manager 的 initialize/start/stop/shutdown 生命週期
 
+2026.05.22
+1. initialize() 設定 ROS_DOMAIN_ID ( FC_ROS_DOMAIN_ID )
+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 大量互動 也許需要考慮對方的生命週期
-2. 還沒測試 如果有失效狀態讓 fc_ros_manager 中斷了 如何恢復的問題
+
 '''
 

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 =====================
 
@@ -36,6 +37,7 @@ class ComponentType(Enum):
 class RFModuleType(Enum):
     """RF模組類型"""
     XBEE = "xbee"
+    XBEE_ESP = "xbee_esp"
     UDP = "udp"
     TCP = "tcp"
     OTHER = "other"
@@ -117,6 +119,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 +153,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
@@ -154,12 +182,14 @@ class RFStatus:
     at_response: Optional[str] = None  # AT 命令回應
     link_quality: Optional[int] = None  # 連接品質
     timestamp: Optional[float] = None  # 時間戳記
+    avg_bandwidth: Optional[float] = None # 量測到的傳輸速度 bytes/ms
     custom_status: Dict[str, Any] = field(default_factory=dict)  # 其他自定義狀態
 
 
 @dataclass
 class SocketInfo:
     """Socket連接資訊"""
+    src64_addr: Optional[bytes] = None # 模組的物理定址
     ip: Optional[str] = None  # IP位址
     port: Optional[int] = None  # 埠號
     local_ip: Optional[str] = None  # 本地IP
@@ -246,6 +276,18 @@ class RFModule:
         self.type = rf_type
         self.status = RFStatus()
         self.socket_info = SocketInfo()
+
+        # # only for ESP32
+        # self.poll_data_times: deque[int] = None
+        # self.poll_data_sizes: deque[int] = None
+        # self.poll_total_time = None
+        # self.poll_total_size = None
+        # self.remain_bytes = None
+
+    def update_avg_bandwidth(self, bandwidth: float, timestamp: Optional[float] = None) -> None:
+        self.status.avg_bandwidth = bandwidth
+        if timestamp:
+            self.status.timestamp = timestamp
     
     def update_rssi(self, rssi: int, timestamp: Optional[float] = None) -> None:
         """更新RSSI"""
@@ -283,6 +325,7 @@ class RFModule:
 
 class VehicleView:
     """
+    最上層
     載具視圖 - 純狀態容器
     
     特點:

src/fc_network_adapter/fc_network_adapter/utils/pollStrategy.py

@@ -0,0 +1,60 @@
+"""
+POLL 輪詢策略，供 XBeeFrameProcessor_ESPv1 使用。
+
+不依賴 serialManager，僅接受裝置快照與排程狀態。
+"""
+
+from dataclasses import dataclass
+from typing import List
+
+MIN_TRIGGER_INTERVAL_MS = 5
+
+@dataclass(frozen=True)
+class PollDevice:
+    """從 esp32AddrMapping 抽出的唯讀快照。"""
+    address_64: bytes
+    remain_bytes: int
+    last_poll_time: float
+
+
+def pick_next(devices: List[PollDevice], now_time):
+    """
+    選下一個 POLL 目標。
+
+    回傳 (target_address_64, grant_bytes)；devices 為空時回傳 (None, 0)。
+
+    poll time = 0.2305 * grant_bytes + 58.094 (R平方為0.979 高線性相關)  @-40db 
+    """
+    if not devices:
+        return None, 0
+
+    grant_bytes = 0
+    device_count = len(devices)
+    weights = [0] * device_count
+
+    for i in range(device_count):
+        gap = int((now_time - devices[i].last_poll_time)*1000)
+        # # 時間大於觸發間隔 才會計算
+        # if gap >= MIN_TRIGGER_INTERVAL_MS:
+        #     weights[i] += devices[i].remain_bytes
+        #     weights[i] += gap
+
+        weights[i] += devices[i].remain_bytes
+        weights[i] += gap
+
+    maxarg = weights.index(max(weights))
+
+    if weights[maxarg] == 0:
+        return None, 0
+    
+    selected_device = devices[maxarg]
+
+    if selected_device.remain_bytes < 500:
+        grant_bytes = 170  # 這個大概是 100 ms 總傳輸時間
+    elif selected_device.remain_bytes < 1200:
+        grant_bytes = 610  # 這個大概是 200 ms 總傳輸時間
+    else:
+        grant_bytes = 1050 # 這個大概是 300 ms 總傳輸時間
+
+    return selected_device.address_64, grant_bytes
+

src/fc_network_apps/longCommand.py

@@ -14,6 +14,7 @@ from fc_interfaces.srv import MavCommandLong
 COMMAND_DO_SET_MODE = 176
 COMMAND_NAV_LAND = 21
 COMMAND_NAV_TAKEOFF = 22
+COMMAND_PREFLIGHT_REBOOT_SHUTDOWN = 246
 COMMAND_COMPONENT_ARM_DISARM = 400
 
 DEFAULT_SERVICE_NAME = "/fc_network/vehicle/send_command_long"
@@ -191,6 +192,28 @@ class CommandLongClient(Node):
             timeout_sec=timeout_sec,
         )
 
+    def reboot_autopilot(
+        self,
+        *,
+        target_sysid: int,
+        target_compid: int = 0,
+        timeout_sec: float = DEFAULT_TIMEOUT_SEC,
+    ) -> CommandLongResult:
+        return self._send_command_long(
+            target_sysid=target_sysid,
+            target_compid=target_compid,
+            command=COMMAND_PREFLIGHT_REBOOT_SHUTDOWN,
+            confirmation=0,
+            param1=1.0,
+            param2=0.0,
+            param3=0.0,
+            param4=0.0,
+            param5=0.0,
+            param6=0.0,
+            param7=0.0,
+            timeout_sec=timeout_sec,
+        )
+
     # ============================================================================
     # 【新增】非阻塞 async 包裝方法（用於 GUI 的非阻塞調用）
     # 這些方法在 ThreadPoolExecutor 中運行同步版本，以避免阻塞事件循環
@@ -353,6 +376,29 @@ class CommandLongClient(Node):
             timeout_sec=timeout_sec,
         )
 
+    async def reboot_autopilot_async(
+        self,
+        *,
+        target_sysid: int,
+        target_compid: int = 0,
+        timeout_sec: float = DEFAULT_TIMEOUT_SEC,
+    ) -> CommandLongResult:
+        """非阻塞 async 版本的 autopilot reboot."""
+        return await self._send_command_long_async(
+            target_sysid=target_sysid,
+            target_compid=target_compid,
+            command=COMMAND_PREFLIGHT_REBOOT_SHUTDOWN,
+            confirmation=0,
+            param1=1.0,
+            param2=0.0,
+            param3=0.0,
+            param4=0.0,
+            param5=0.0,
+            param6=0.0,
+            param7=0.0,
+            timeout_sec=timeout_sec,
+        )
+
     async def land_async(
         self,
         *,

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
-            port = self.get_parameter('port').value
-            mount = self.get_parameter('mountpoint').value
-            user = self.get_parameter('username').value
-            pwd = self.get_parameter('password').value
+        host = self.get_parameter('host').value
+        port = self.get_parameter('port').value
+        mountpoint = self.get_parameter('mountpoint').value
+        user = self.get_parameter('username').value
+        pwd = self.get_parameter('password').value
 
-            if not mount:
-                self.get_logger().error('mountpoint 參數未設定，等待重試...')
-                time.sleep(backoff)
-                continue
+        if not mountpoint:
+            self.get_logger().error('mountpoint 參數未設定...')
+            return
+
+        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:
-                    self.get_logger().warn(f'連線中斷: {e}，{backoff:.0f}s 後重連')
-                    time.sleep(backoff)
-                    backoff = min(backoff * 2, self.RECONNECT_MAX_SEC)
+                if self._stop_event.is_set():
+                    break
+                self.get_logger().warn(f'連線中斷: {e}，{backoff:.0f}s 後重連')
+                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._thread.join(timeout=3.0)
+        self._request_stop()
+        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

@@ -1,46 +1,55 @@
 備選需要的功能
- - serail 對於 telemetry 的支援
  - serial_manager.serial_object.transport 這些變數可能不需要
 
 不用動
  - mavlink_object 的 send_message 確認一下 mavlink_bridge 的 _send_to_socket 是不是應該做成 async
- - 不同 socket 上面有重複的 sysid 分開儲存 (不做 不允許sysid重複) 
 
 這一步
- 希望 ackEnum 可以擺到正確的位置
- node 增加 GNSS 訊息 (2d fix 3d fix rtk float...)
  RTK
-   跟 RTK2go 抓取列表
-   從特定 mount point 得到數據
-   做一個 ros2 service(action?) 接到數據並包裝
+   跟 RTK2go 抓取列表 Done
+   從特定 mount point 得到數據 Done
+   做一個 ros2 service 接到數據並包裝 Done
+   RTK 實機測試
  
 
 下一步
-  NODE 重啟
-  可以 refresh node topic
+  重啟做好了 但是還沒自動化
 
 下下一步
- 
+  整合 xbee api-api 功能 
 
 後面
- rssi 資訊提取s
 
 
-mavlink timeout - CommLONG 這個是 mavlink bus 沒有回應 可能是丟包了
 
 
 自己的常用指令
 python -m fc_network_adapter.tests.test_vehicleStatusPublisher
 python -m fc_network_adapter.tests.test_ringBuffer
 python -m fc_network_adapter.fc_network_adapter.mainOrchestrator
+python -m fc_network_adapter.fc_network_adapter.serialManager
 
 python -m someotherpkg.src.example_takeoff_land
 python -m someotherpkg.src.example_change_mode
 
 python -m fc_network_module.fc_network_module.ntrip_client --ros-args -p mountpoint:=No1bio_02 -p username:=chiyu1468@hotmail.com
 
+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
+
+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
+
 ros2 topic list
 ros2 topic echo /fc_network/vehicle/sys3/battery
+ros2 topic echo hz /fc_network/vehicle/sys3/battery
+
+ros2 topic bw /fc_network/vehicle/sys3/attitude
+ros2 topic hz /fc_network/vehicle/sys3/attitude
 
 
 /home/picars/ardupilot/build/sitl/bin/arducopter -S --model + --speedup 1 --slave 0 --defaults /home/picars/ardupilot/Tools/autotest/default_params/copter.parm --sim-address=127.0.0.1 -I3 --sysid 7
@@ -51,7 +60,7 @@ ros2 service call /mavlink/add_two_ints example_interfaces/srv/AddTwoInts "{a: 5
 ros2 service call /fc_network/vehicle/mav_ping fc_interfaces/srv/MavPing "{target_sysid: 3, target_compid: 0, ping_seq: 1}"
 
 ros2 service call /fc_network/vehicle/send_command_long fc_interfaces/srv/MavCommandLong "{target_sysid: 3, target_compid: 0, command: 176, confirmation: 0, param1: 1, param2: 4, param3: 0,param4: 0,param5: 0,param6: 0,param7: 0, timeout_sec: 2}"
-ros2 service call /fc_network/vehicle/send_command_long fc_interfaces/srv/MavCommandLong "{target_sysid: 3, target_compid: 0, command: 176, confirmation: 0, param1: 1, param2: 4, timeout_sec: 2}"
+ros2 service call /fc_network/vehicle/send_command_long fc_interfaces/srv/MavCommandLong "{target_sysid: 10, target_compid: 0, command: 176, confirmation: 0, param1: 1, param2: 4, timeout_sec: 2}"
 
 ros2 service call /fc_network/vehicle/pos_global_int fc_interfaces/srv/MavPositionTargetGlobalInt "{target_sysid: 3, target_compid: 1, coordinate_frame: 6, type_mask: 3576, lat_int: -35376655, lon_int: 149157011, alt: 20.0, timeout_sec: 5.0}"
 
@@ -60,10 +69,4 @@ sudo tcpdump -i lo 'udp dst port 14561' -X
 sudo tcpdump -i lo 'udp dst port 14550' -X -vv
 sudo tcpdump -i lo -X udp port 14550
 
-幾個要討論的
-1. 專案文件中 .mat 檔案是幹嘛的?
-2. GUI 的 print [Panel/Map Update] 11 Hz 希望關閉
-3. readme 那串文字來源? 用途?
-4. GUI介面 加一下海拔高 跟 相對高 速度跟位置 變更為 XY速度 XY位置
-5. pitch yaw roll 出來了 弄一下
-6.
+

src/unitdev02/unitdev02/esp32.py

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

src/unitdev02/unitdev02/ntrip_dev.py

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

src/unitdev02/unitdev02/tryByteDict.py

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