fix(GUI): 修正實飛三個關鍵 bug — 高度偏移累加、Leader-Follower 不動、起點重疊

1. mission_planner: origin_alt 固定為 0，避免 GPS 橢球高度疊加到相對高度指令
2. mission_planner: 移除圓弧插值，直接用路徑點作為航點，消除微航點導致飛控無法執行
3. mission_planner: lookup 改為外推（extrapolate），解決偶數 rank 起終點重疊
4. communication: UDP/Serial 的 GLOBAL_POSITION_INT 補上 relative_alt 欄位

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

README.md

@@ -73,25 +73,30 @@ python gui.py
 
 ===
 ## 資料夾說明
-1. unitdev 為各自協作者做開發時的測試區
+1. fc_network_adapter (核心)
-    01 -> 晉凱(ken)
-    02 -> 其宇(chiyu)
-    03 -> 文鈞
-    04 -> 倫渝
-2. fc_network_adapter (核心)
     建立、維護與飛控韌體的連接 
     構築 mavlink 封包
     處理無線模組的通訊格式 (XBee)
     --同時處理與 Gazebo 的 ardupilot_plugin 溝通的 FDM/JSON 訊息 (移除)--
-3. fc_interfaces (重要)
+2. fc_interfaces (重要)
-    自定義的 ROS2 介面檔
+    自定義的 ROS2 介面檔 沒啥好說的 沒有這個核心會運作不了
+3. fc_network_module (重要)
+    非核心 但是支援載具的重要附屬功能 
+    需要該功能時 作為一個 ros2 節點打開
+    例如 : ntrip rtk 訊號轉接
 4. fc_network_apps
-    與 fc_network_adapter 銜接做高階功能包裝的應用小程式 利於開發GUI或其他應用
+    與 fc_network_adapter 銜接做高階功能包裝的應用小程式 
-    使用者的外層包裝
+    利於開發GUI或其他應用 使用者的外層包裝
+    這裡的定位是 "核心功能的高階包裝" 可以完全不去用
 5. someotherpkg
     如何使用 fc_network_apps 的範例檔案
 6. GUI 
     由 PyQt6 開發的互動式介面
-N. logs 是執行時期的記錄檔
+7. unitdev 為各自協作者做開發時的測試區
+    01 -> 晉凱(ken)
+    02 -> 其宇(chiyu)
+    03 -> 文鈞
+    04 -> 倫渝
+8. logs 是執行時期的記錄檔
 
 ===

src/GUI/communication.py

@@ -12,6 +12,10 @@ import socket
 import sys
 import os
 import traceback
+try:
+    import serial
+except ImportError:
+    serial = None
 from pymavlink import mavutil
 from geometry_msgs.msg import Point, Vector3, Vector3Stamped, PoseWithCovarianceStamped
 from sensor_msgs.msg import BatteryState, NavSatFix, Imu
@@ -58,9 +62,163 @@ except ImportError as e:
     _log("ERROR", f"錯誤: {e}")
     AttitudeRaw = None
 
+try:
+    from fc_interfaces.msg import GnssRaw
+except ImportError as e:
+    _log("WARN", "無法導入 GnssRaw")
+    _log("ERROR", f"錯誤: {e}")
+    GnssRaw = None
+
 class DroneSignals(QObject):
     update_signal = pyqtSignal(str, str, object)  # (msg_type, drone_id, data)
 
+class JsonTelemetryProcessor:
+    """共用 WebSocket JSON telemetry 轉換器。
+
+    Canonical JSON fields:
+    {
+        "system_id": 1,
+        "mode": "GUIDED",
+        "battery": 85,
+        "position": {"lat": 24.0, "lon": 120.0},
+        "heading": 90
+    }
+    Serial JSON uses this same shape; only the transport/framing differs.
+    """
+
+    def _emit_json_connection_type(self, drone_id):
+        self.signals.update_signal.emit('connection_type', drone_id, {
+            'type': self.source_type
+        })
+
+    def process_json_telemetry_message(self, data):
+        """處理 WebSocket JSON 格式的遙測資料。"""
+        try:
+            if isinstance(data, list):
+                for item in data:
+                    if isinstance(item, dict):
+                        self.process_json_telemetry_message(item)
+                return
+
+            if not isinstance(data, dict):
+                return
+
+            system_id = data.get('system_id', data.get('sysid'))
+            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'))
+            state = {}
+            if mode is not None:
+                state['mode'] = mode
+            if 'armed' in data:
+                state['armed'] = data.get('armed')
+            if state:
+                self.signals.update_signal.emit('state', drone_id, state)
+
+            if 'battery' in data:
+                battery = data['battery']
+                if isinstance(battery, dict):
+                    battery_data = {}
+                    if 'percentage' in battery:
+                        battery_data['percentage'] = battery.get('percentage')
+                    elif 'percent' in battery:
+                        battery_data['percentage'] = battery.get('percent')
+                    if 'voltage' in battery:
+                        battery_data['voltage'] = battery.get('voltage')
+                    elif 'voltage_v' in battery:
+                        battery_data['voltage'] = battery.get('voltage_v')
+                    if battery_data:
+                        self.signals.update_signal.emit('battery', drone_id, battery_data)
+                else:
+                    self.signals.update_signal.emit('battery', drone_id, {
+                        'percentage': battery
+                    })
+            elif 'battery_percentage' in data or 'battery_voltage' in data:
+                battery_data = {}
+                if 'battery_percentage' in data:
+                    battery_data['percentage'] = data.get('battery_percentage')
+                if 'battery_voltage' in data:
+                    battery_data['voltage'] = data.get('battery_voltage')
+                self.signals.update_signal.emit('battery', drone_id, battery_data)
+
+            pos = data.get('position')
+            if isinstance(pos, dict):
+                gps_data = {
+                    'lat': pos.get('lat', pos.get('latitude', 0)),
+                    'lon': pos.get('lon', pos.get('longitude', 0)),
+                    'alt': pos.get('alt', pos.get('altitude', 0))
+                }
+                self.signals.update_signal.emit('gps', drone_id, gps_data)
+            elif 'lat' in data or 'latitude' in data:
+                self.signals.update_signal.emit('gps', drone_id, {
+                    'lat': data.get('lat', data.get('latitude', 0)),
+                    'lon': data.get('lon', data.get('longitude', 0)),
+                    'alt': data.get('alt', data.get('altitude', 0))
+                })
+
+            local = data.get('local_position', data.get('local_pose', data.get('local')))
+            if isinstance(local, dict):
+                x = local.get('x', 0.0)
+                y = local.get('y', 0.0)
+                z = local.get('z', 0.0)
+                self.signals.update_signal.emit('local_pose', drone_id, {
+                    'x': x,
+                    'y': y,
+                    'z': z
+                })
+                self.signals.update_signal.emit('altitude', drone_id, {
+                    'altitude': z
+                })
+            elif isinstance(pos, dict):
+                alt = pos.get('alt', pos.get('altitude', 0.0))
+                self.signals.update_signal.emit('local_pose', drone_id, {
+                    'x': 0.0,
+                    'y': 0.0,
+                    'z': alt
+                })
+                self.signals.update_signal.emit('altitude', drone_id, {
+                    'altitude': alt
+                })
+
+            velocity = data.get('velocity')
+            if isinstance(velocity, dict):
+                self.signals.update_signal.emit('velocity', drone_id, {
+                    'vx': velocity.get('vx', velocity.get('x', 0.0)),
+                    'vy': velocity.get('vy', velocity.get('y', 0.0)),
+                    'vz': velocity.get('vz', velocity.get('z', 0.0))
+                })
+
+            attitude = data.get('attitude')
+            if isinstance(attitude, dict):
+                self.signals.update_signal.emit('attitude', drone_id, {
+                    'roll': attitude.get('roll', 0.0),
+                    'pitch': attitude.get('pitch', 0.0),
+                    'yaw': attitude.get('yaw', 0.0),
+                    'rates': attitude.get('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')
+            if hud:
+                self.signals.update_signal.emit('hud', drone_id, {
+                    'heading': hud.get('heading', 0.0),
+                    'groundspeed': hud.get('groundspeed', 0.0),
+                    'airspeed': hud.get('airspeed', 0.0),
+                    'throttle': hud.get('throttle', 0.0),
+                    'alt': hud.get('alt', hud.get('altitude', 0.0)),
+                    'climb': hud.get('climb', 0.0)
+                })
+
+        except Exception as e:
+            print(f"{self.source_type} JSON telemetry processing error: {e}")
+
 class UDPMavlinkReceiver(threading.Thread):
     """UDP MAVLink 接收器"""
     def __init__(self, ip, port, signals, connection_name, monitor=None):
@@ -129,9 +287,11 @@ class UDPMavlinkReceiver(threading.Thread):
             elif msg_type == "GLOBAL_POSITION_INT":
                 latitude = msg.lat / 1e7
                 longitude = msg.lon / 1e7
+                relative_alt = msg.relative_alt / 1000.0
                 self.signals.update_signal.emit('gps', drone_id, {
                     'lat': latitude,
-                    'lon': longitude
+                    'lon': longitude,
+                    'alt': relative_alt,
                 })
 
             elif msg_type == "GPS_RAW_INT":
@@ -184,8 +344,8 @@ class UDPMavlinkReceiver(threading.Thread):
         """停止接收器"""
         self.running = False
 
-class SerialMavlinkReceiver(threading.Thread):
+class SerialMavlinkReceiver(threading.Thread, JsonTelemetryProcessor):
-    """串口 MAVLink 接收器"""
+    """串口遙測接收器，可自動處理 MAVLink 或 WebSocket 格式 JSON。"""
     def __init__(self, port, baudrate, signals, connection_name, monitor=None):
         super().__init__(daemon=True)
         self.port = port
@@ -194,47 +354,125 @@ class SerialMavlinkReceiver(threading.Thread):
         self.connection_name = connection_name
         self.monitor = monitor  # 保存 monitor 引用
         self.socket_id = monitor.get_next_socket_id() if monitor else 0
+        self.source_type = 'Serial'
         self.running = False
-        self.mav = None
+        self.serial_conn = None
+        self.mav_parser = mavutil.mavlink.MAVLink(None)
+        self.mav_parser.robust_parsing = True
+        self.json_buffer = []
+        self.json_depth = 0
+        self.json_in_string = False
+        self.json_escape = False
+        self._detected_protocols = set()
         
     def run(self):
-        """執行串口接收循環"""
+        """執行串口接收循環。"""
         self.running = True
         try:
-            print(f"Serial MAVLink receiver started on {self.port} at {self.baudrate} baud")
+            print(f"Serial receiver started on {self.port} at {self.baudrate} baud (MAVLink/JSON auto detect)")
+            if serial is None:
+                raise RuntimeError("pyserial 未安裝，無法啟動 Serial 連線")
 
-            # 創建 MAVLink 串口連接
+            self.serial_conn = serial.Serial(
-            self.mav = mavutil.mavlink_connection(
                 self.port,
-                baud=self.baudrate,
+                self.baudrate,
-                source_system=255
+                timeout=0.2
             )
 
-            print(f"Waiting for heartbeat from {self.port}...")
-            self.mav.wait_heartbeat()
-            print(f"Heartbeat received from system {self.mav.target_system}, component {self.mav.target_component}")
-            
             while self.running:
                 try:
-                    msg = self.mav.recv_match(blocking=True, timeout=1.0)
+                    chunk = self.serial_conn.read(256)
-                    if msg is None:
+                    if not chunk:
                         continue
 
-                    self.process_mavlink_message(msg)
+                    for raw_byte in chunk:
+                        byte = bytes([raw_byte])
+                        self._process_json_byte(byte)
+                        self._process_mavlink_byte(byte)
 
                 except Exception as e:
                     if self.running:
-                        print(f"Error receiving MAVLink message from serial: {e}")
+                        print(f"Error receiving serial telemetry: {e}")
 
         except Exception as e:
             print(f"Serial receiver error: {e}")
         finally:
-            if self.mav:
+            if self.serial_conn:
                 try:
-                    self.mav.close()
+                    self.serial_conn.close()
-                except:
+                except Exception:
                     pass
 
+    def _process_mavlink_byte(self, byte):
+        try:
+            msg = self.mav_parser.parse_char(byte)
+            if msg is None:
+                return
+            if 'MAVLink' not in self._detected_protocols:
+                print(f"Serial {self.connection_name} detected MAVLink")
+                self._detected_protocols.add('MAVLink')
+            self.process_mavlink_message(msg)
+        except Exception:
+            # MAVLink parser is deliberately fed the whole stream, including JSON bytes.
+            return
+
+    def _process_json_byte(self, byte):
+        try:
+            char = byte.decode('utf-8')
+        except UnicodeDecodeError:
+            if self.json_buffer:
+                self._reset_json_framing()
+            return
+
+        if not self.json_buffer:
+            if char.isspace():
+                return
+            if char not in ('{', '['):
+                return
+            self.json_depth = 1
+            self.json_in_string = False
+            self.json_escape = False
+            self.json_buffer = [char]
+            return
+
+        self.json_buffer.append(char)
+
+        if self.json_in_string:
+            if self.json_escape:
+                self.json_escape = False
+            elif char == '\\':
+                self.json_escape = True
+            elif char == '"':
+                self.json_in_string = False
+            return
+
+        if char == '"':
+            self.json_in_string = True
+        elif char in ('{', '['):
+            self.json_depth += 1
+        elif char in ('}', ']'):
+            self.json_depth -= 1
+
+        if self.json_depth > 0:
+            return
+
+        payload = ''.join(self.json_buffer)
+        self._reset_json_framing()
+        try:
+            data = json.loads(payload)
+            if 'JSON' not in self._detected_protocols:
+                print(f"Serial {self.connection_name} detected JSON")
+                self._detected_protocols.add('JSON')
+            self.process_json_telemetry_message(data)
+        except json.JSONDecodeError as e:
+            print(f"Serial {self.connection_name} JSON decode error: {e}")
+
+    def _reset_json_framing(self):
+        self.json_buffer = []
+        self.json_depth = 0
+        self.json_in_string = False
+        self.json_escape = False
+
     def process_mavlink_message(self, msg):
         """處理 MAVLink 訊息"""
         try:
@@ -263,9 +501,11 @@ class SerialMavlinkReceiver(threading.Thread):
             elif msg_type == "GLOBAL_POSITION_INT":
                 latitude = msg.lat / 1e7
                 longitude = msg.lon / 1e7
+                relative_alt = msg.relative_alt / 1000.0
                 self.signals.update_signal.emit('gps', drone_id, {
                     'lat': latitude,
-                    'lon': longitude
+                    'lon': longitude,
+                    'alt': relative_alt,
                 })
 
             elif msg_type == "GPS_RAW_INT":
@@ -317,15 +557,23 @@ class SerialMavlinkReceiver(threading.Thread):
     def stop(self):
         """停止接收器"""
         self.running = False
+        if self.serial_conn:
+            try:
+                self.serial_conn.close()
+            except Exception:
+                pass
 
-class WebSocketMavlinkReceiver(threading.Thread):
+class WebSocketMavlinkReceiver(threading.Thread, JsonTelemetryProcessor):
     """WebSocket MAVLink 接收器"""
     def __init__(self, url, signals, connection_name, monitor=None):
         super().__init__(daemon=True)
         self.url = url
         self.signals = signals
         self.connection_name = connection_name
-        self.monitor = monitor  # 保存 monitor 引用        self.socket_id = monitor.get_next_socket_id() if monitor else 0  # 一次性分配 socket_id        self.running = False
+        self.monitor = monitor  # 保存 monitor 引用
+        self.socket_id = monitor.get_next_socket_id() if monitor else 0  # 一次性分配 socket_id
+        self.source_type = 'WS'
+        self.running = False
         self.max_retries = 5
         self.base_delay = 1.0
         
@@ -353,7 +601,8 @@ class WebSocketMavlinkReceiver(threading.Thread):
                             
                         try:
                             data = json.loads(message)
-                            data['_connection_source'] = self.connection_name
+                            if isinstance(data, dict):
+                                data['_connection_source'] = self.connection_name
                             self.process_websocket_message(data)
                         except json.JSONDecodeError as e:
                             print(f"WebSocket {self.connection_name} JSON decode error: {e}")
@@ -383,57 +632,7 @@ class WebSocketMavlinkReceiver(threading.Thread):
     
     def process_websocket_message(self, data):
         """處理 WebSocket 訊息"""
-        try:
+        self.process_json_telemetry_message(data)
-            system_id = data.get('system_id')
-            if not system_id:
-                return
-            drone_id = f"s{self.socket_id}_{system_id}"
-
-            # 模式
-            if 'mode' in data:
-                # 先發送連接類型資訊
-                self.signals.update_signal.emit('connection_type', drone_id, {
-                    'type': 'WS'
-                })
-                self.signals.update_signal.emit('state', drone_id, {
-                    'mode': data['mode'],
-                })
-
-            # 電池
-            if 'battery' in data:
-                self.signals.update_signal.emit('battery', drone_id, {
-                    'percentage': data['battery']  
-                })
-
-            # 位置
-            if 'position' in data:
-                pos = data['position']
-                self.signals.update_signal.emit('gps', drone_id, {
-                    'lat': pos.get('lat', 0),
-                    'lon': pos.get('lon', 0)
-                })
-
-                # Local position - 設定 x, y 為 0.0
-                self.signals.update_signal.emit('local_pose', drone_id, {
-                    'x': 0.0,
-                    'y': 0.0,
-                    'z': 0.0
-                })
-                
-                # Altitude - 設定為 0.0
-                self.signals.update_signal.emit('altitude', drone_id, {
-                    'altitude': 0.0
-                })
-
-            # 航向
-            if 'heading' in data:
-                self.signals.update_signal.emit('hud', drone_id, {
-                    'heading': data['heading'],
-                    'groundspeed': 0.0
-                })
-            
-        except Exception as e:
-            print(f"WebSocket message processing error: {e}")
             
     def stop(self):
         """停止接收器"""
@@ -604,8 +803,21 @@ class DroneMonitor(Node):
             if not hasattr(self, subs_attr):
                 self.setup_drone(sys_id)
             else:
-                # 檢查既有訂閱是否包含 position_ned / attitude，如果不包含就添加（兼容舊訂閱）
+                # 檢查既有訂閱是否包含 position_gnss / position_ned / attitude，如果不包含就添加（兼容舊訂閱）
                 subs = getattr(self, subs_attr, {})
+                if isinstance(subs, dict) and 'position_gnss' not in subs and GnssRaw is not None:
+                    base_topic = f'/fc_network/vehicle/{sys_id}'
+                    try:
+                        position_gnss_sub = self.create_subscription(
+                            GnssRaw,
+                            f'{base_topic}/position_gnss',
+                            lambda msg, sid=sys_id: self.gps_callback(sid, msg),
+                            10
+                        )
+                        subs['position_gnss'] = position_gnss_sub
+                        setattr(self, subs_attr, subs)
+                    except Exception:
+                        pass
                 if isinstance(subs, dict) and 'position_ned' not in subs:
                     base_topic = f'/fc_network/vehicle/{sys_id}'
                     try:
@@ -661,12 +873,6 @@ class DroneMonitor(Node):
                 lambda msg, sid=sys_id: self.battery_callback(sid, msg),
                 10
             ),
-            'position': self.create_subscription(
-                NavSatFix,
-                f'{base_topic}/position',
-                lambda msg, sid=sys_id: self.gps_callback(sid, msg),
-                10
-            ),
             'summary': self.create_subscription(
                 String,
                 f'{base_topic}/summary',
@@ -687,6 +893,21 @@ class DroneMonitor(Node):
             )
         }
 
+        if GnssRaw is not None:
+            subs['position_gnss'] = self.create_subscription(
+                GnssRaw,
+                f'{base_topic}/position_gnss',
+                lambda msg, sid=sys_id: self.gps_callback(sid, msg),
+                10
+            )
+        else:
+            subs['position'] = self.create_subscription(
+                NavSatFix,
+                f'{base_topic}/position',
+                lambda msg, sid=sys_id: self.gps_callback(sid, msg),
+                10
+            )
+
         if AttitudeRaw is not None:
             subs['attitude'] = self.create_subscription(
                 AttitudeRaw,
@@ -990,20 +1211,27 @@ class DroneMonitor(Node):
         if actual_drone_id is None:
             return
 
-        self.latest_data[(actual_drone_id, 'gps')] = {
+        gps_data = {
             'lat': msg.latitude,
             'lon': msg.longitude,
             'alt': msg.altitude
         }
 
+        if hasattr(msg, 'fix_type'):
+            gps_data['fix_type'] = msg.fix_type
+        if hasattr(msg, 'satellites_visible'):
+            gps_data['satellites_visible'] = msg.satellites_visible
+        if hasattr(msg, 'eph'):
+            gps_data['eph'] = msg.eph
+        if hasattr(msg, 'epv'):
+            gps_data['epv'] = msg.epv
+        
+        self.latest_data[(actual_drone_id, 'gps')] = gps_data
+        
         # ================================================================================
         # 【新增】儲存 GPS 資料到 drone_gps 字典
         # ================================================================================
-        self.drone_gps[actual_drone_id] = {
+        self.drone_gps[actual_drone_id] = gps_data.copy()
-            'lat': msg.latitude,
-            'lon': msg.longitude,
-            'alt': msg.altitude
-        }
         # ================================================================================
 
     def local_vel_callback(self, drone_id, msg):
@@ -1101,10 +1329,10 @@ class DroneMonitor(Node):
         }
 
     def start_serial_connection(self, port='/dev/ttyUSB0', baudrate=57600):
-        """啟動串口 MAVLink 連接"""
+        """啟動串口遙測連接（自動辨識 MAVLink / JSON）"""
         connection_name = f"Serial_{port.replace('/', '_')}"
         receiver = SerialMavlinkReceiver(port, baudrate, self.signals, connection_name, self)
         receiver.start()
         self.serial_receivers.append(receiver)
-        _log("INFO", f"已啟動 Serial 連線: {port} @ {baudrate} baud")
+        _log("INFO", f"已啟動 Serial 連線: {port} @ {baudrate} baud (MAVLink/JSON)")
         return receiver

src/GUI/drone_panel.py

@@ -104,7 +104,7 @@ class DronePanel(QWidget):
     def _init_ui(self):
         """初始化UI"""
         self.setObjectName(f"panel_{self.drone_id}")
-        self.setFixedHeight(140)
+        self.setFixedHeight(160)
         _set_scaled_stylesheet(self, """
             background-color: #2A2A2A;
             border-radius: 8px;
@@ -197,7 +197,11 @@ class DronePanel(QWidget):
         altitude_row = self._create_altitude_row()
         info_layout.addWidget(altitude_row)
 
-        # 第四行：航向 + 速度
+        # 第四行：GNSS 定位狀態
+        gnss_row = self._create_gnss_row()
+        info_layout.addWidget(gnss_row)
+        
+        # 第五行：航向 + 速度
         nav_row = self._create_nav_row()
         info_layout.addWidget(nav_row)
         
@@ -340,6 +344,34 @@ class DronePanel(QWidget):
         
         return altitude_row
 
+    def _create_gnss_row(self):
+        """創建 GNSS 定位狀態行。"""
+        gnss_row = QWidget()
+        gnss_layout = QHBoxLayout(gnss_row)
+        gnss_layout.setContentsMargins(0, 0, 0, 0)
+
+        fix_title = QLabel("定位：")
+        _set_scaled_stylesheet(fix_title, "color: #888; min-width: 50px;")
+
+        self.fix_type_label = QLabel("--")
+        self.fix_type_label.setObjectName(f"{self.drone_id}_fix_type_label")
+        _set_scaled_stylesheet(self.fix_type_label, "color: #DDD;")
+
+        error_title = QLabel("誤差(H/V)：")
+        _set_scaled_stylesheet(error_title, "color: #888; margin-left: 8px;")
+
+        self.gnss_error_label = QLabel("--")
+        self.gnss_error_label.setObjectName(f"{self.drone_id}_gnss_error")
+        _set_scaled_stylesheet(self.gnss_error_label, "color: #DDD;")
+
+        gnss_layout.addWidget(fix_title)
+        gnss_layout.addWidget(self.fix_type_label)
+        gnss_layout.addWidget(error_title)
+        gnss_layout.addWidget(self.gnss_error_label)
+        gnss_layout.addStretch()
+
+        return gnss_row
+    
     def _create_position_row(self):
         """位置行已移除（位置座標不再顯示於面板）。"""
         return QWidget()

src/GUI/gui.py

@@ -14,6 +14,8 @@ import subprocess
 import time
 import traceback
 import re
+import threading
+from concurrent.futures import ThreadPoolExecutor
 
 
 def _log(level, message):
@@ -143,7 +145,9 @@ class ToggleSwitch(QWidget):
 
 
 class ControlStationUI(QMainWindow):
-    VERSION = '2.2.0'
+    planning_finished = pyqtSignal(object)
+
+    VERSION = '2.4.1'
     FONT_SCALE_MIN = 70
     FONT_SCALE_MAX = 180
     FONT_SCALE_DEFAULT = 100
@@ -152,6 +156,7 @@ class ControlStationUI(QMainWindow):
         super().__init__()
         self.setWindowTitle(f'GCS v{self.VERSION}')
         self.resize(1400, 900)
+        self._closing = False
 
         # 初始化消息隊列（用於線程安全的 GUI 更新）
         import queue
@@ -168,10 +173,16 @@ class ControlStationUI(QMainWindow):
         # 將執行器註冊到 DroneMonitor，以便動態創建的 CommandLongClient 能被添加
         self.monitor.executor = self.executor
         
-        # 定时处理ROS事件
+        # 在背景執行緒處理 ROS2 spin，避免佔用 Qt 主執行緒時間
-        self.timer = QTimer()
+        self.ros_thread_running = True
-        self.timer.timeout.connect(self.spin_ros)
+        self.ros_thread = threading.Thread(target=self._ros_spin_thread, daemon=True)
-        self.timer.start(10)
+        self.ros_thread.start()
+
+        # 任務規劃移到 worker thread，避免大量航點計算卡住 Qt 主執行緒
+        self.planner_pool = ThreadPoolExecutor(max_workers=2)
+        self._plan_request_counter = 0
+        self._latest_plan_request_by_group = {}
+        self.planning_finished.connect(self._on_plan_done)
         
         # 驅動 asyncio 事件循環的定時器（新增 - 關鍵！）
         # 這允許異步任務（如 arm_drone）能夠執行
@@ -179,11 +190,16 @@ class ControlStationUI(QMainWindow):
         self.asyncio_timer.timeout.connect(self._spin_asyncio)
         self.asyncio_timer.start(10)  # 每 10ms 驅動一次 asyncio
         
-        # 初始化 panel 和 map 更新（10Hz）
+        # 初始化 panel 更新（10Hz）
         self.panel_map_timer = QTimer()
         self.panel_map_timer.timeout.connect(self._update_panel_and_map)
         self.panel_map_timer.start(100)  # 10Hz
 
+        # Attitude 顯示器獨立高頻更新（30Hz），避免被 panel/table/map 節奏拖住
+        self.attitude_timer = QTimer()
+        self.attitude_timer.timeout.connect(self._update_attitude_only)
+        self.attitude_timer.start(33)
+        
         # 消息隊列處理定時器（來自線程的 GUI 更新）
         self.msg_queue_timer = QTimer()
         self.msg_queue_timer.timeout.connect(self._process_message_queue)
@@ -191,6 +207,9 @@ class ControlStationUI(QMainWindow):
         
         # 快取消息數據，以便在沒有新消息時使用上一次的值
         self._message_cache = {}
+        self._attitude_cache = {}
+        self._overview_cache = {}
+        self._map_dirty_drones = set()
         self.message_history = []
         self.max_message_history = 500
         
@@ -217,6 +236,17 @@ class ControlStationUI(QMainWindow):
         self.drone_headings = {}
         # 初始化地圖
         self.drone_map = DroneMap()
+
+        # 地圖更新獨立節流（10Hz），避免 WebEngine / JS map 拖慢 panel 更新
+        self.map_timer = QTimer()
+        self.map_timer.timeout.connect(self._update_map_only)
+        self.map_timer.start(100)
+
+        # Overview table 獨立批次更新（5Hz），避免每筆資料都重繪 Qt table
+        self.overview_timer = QTimer()
+        self.overview_timer.timeout.connect(self._flush_overview_table)
+        self.overview_timer.start(200)
+
         # 初始化連接列表
         self.udp_receivers = []
         self.udp_connections = []
@@ -391,7 +421,7 @@ class ControlStationUI(QMainWindow):
         self.right_vertical_splitter.setChildrenCollapsible(False)
         self.right_vertical_splitter.setStretchFactor(0, 0)
         self.right_vertical_splitter.setStretchFactor(1, 1)
-        self.right_vertical_splitter.setSizes([170, 700])
+        self.right_vertical_splitter.setSizes([180, 640])
         right_layout.addWidget(self.right_vertical_splitter)
 
         # 添加地圖
@@ -726,7 +756,7 @@ class ControlStationUI(QMainWindow):
             self.message_history = self.message_history[-self.max_message_history:]
 
         if hasattr(self, 'message_history_view'):
-            self.message_history_view.setPlainText("\n".join(self.message_history))
+            self.message_history_view.appendPlainText(entry)
             scrollbar = self.message_history_view.verticalScrollBar()
             scrollbar.setValue(scrollbar.maximum())
 
@@ -800,7 +830,7 @@ class ControlStationUI(QMainWindow):
             status_label.setToolTip("已停止")
             self.statusBar().showMessage(f"已停止 WebSocket 連接: {conn['url']}", 3000)
         else:
-            receiver = WebSocketMavlinkReceiver(conn['url'], self.monitor.signals, conn['name'])
+            receiver = WebSocketMavlinkReceiver(conn['url'], self.monitor.signals, conn['name'], self.monitor)
             receiver.start()
             self.monitor.ws_receivers.append(receiver)
             conn['receiver'] = receiver
@@ -832,7 +862,7 @@ class ControlStationUI(QMainWindow):
             status_label.setToolTip("已停止")
             self.statusBar().showMessage(f"已停止 UDP 連接: {conn['ip']}:{conn['port']}", 3000)
         else:
-            receiver = UDPMavlinkReceiver(conn['ip'], conn['port'], self.monitor.signals, conn['name'])
+            receiver = UDPMavlinkReceiver(conn['ip'], conn['port'], self.monitor.signals, conn['name'], self.monitor)
             receiver.start()
             self.udp_receivers.append(receiver)
             conn['receiver'] = receiver
@@ -1049,7 +1079,7 @@ class ControlStationUI(QMainWindow):
             self.group_tab_widget.show()
             if hasattr(self, 'right_vertical_splitter'):
                 total = sum(self.right_vertical_splitter.sizes()) or self.height()
-                group_height = getattr(self, '_last_group_panel_height', 170)
+                group_height = getattr(self, '_last_group_panel_height', 230)
                 group_height = min(max(group_height, 120), max(120, total - 120))
                 self.right_vertical_splitter.setSizes([group_height, max(1, total - group_height)])
             self.toggle_group_btn.setText("▼")
@@ -1580,6 +1610,8 @@ class ControlStationUI(QMainWindow):
             self._message_cache[drone_id] = {}
         
         self._message_cache[drone_id][msg_type] = data
+        if msg_type == 'attitude':
+            self._attitude_cache[drone_id] = data
 
 
     # ================================================================================
@@ -1733,6 +1765,141 @@ class ControlStationUI(QMainWindow):
         """取得群組的無人機 ID 列表（排序後）"""
         return sorted(group.selected_drone_ids, key=lambda x: (x.split('_')[0], int(x.split('_')[1])))
 
+    @staticmethod
+    def _run_plan_formation_mission(planner_config, drone_gps_positions,
+                                    target_gps, mission_type, params):
+        planner = FormationPlanner(**planner_config)
+        return planner.plan_formation_mission(
+            drone_gps_positions, target_gps, mission_type, params=params
+        )
+
+    def _submit_mission_plan(self, group, selected_drones, drone_gps_positions,
+                             target_gps, mission_type, params, context):
+        self._plan_request_counter += 1
+        request_id = self._plan_request_counter
+        self._latest_plan_request_by_group[group.group_id] = request_id
+
+        planner_config = {
+            'spacing': self.mission_planner.spacing,
+            'base_altitude': self.mission_planner.base_altitude,
+            'altitude_diff': self.mission_planner.altitude_diff,
+        }
+        params = dict(params)
+        future = self.planner_pool.submit(
+            self._run_plan_formation_mission,
+            planner_config,
+            list(drone_gps_positions),
+            tuple(target_gps),
+            mission_type,
+            params,
+        )
+        future.add_done_callback(
+            lambda f: self._emit_plan_result(
+                f, request_id, group.group_id, list(selected_drones),
+                list(drone_gps_positions), tuple(target_gps), context
+            )
+        )
+
+    def _emit_plan_result(self, future, request_id, group_id, selected_drones,
+                          drone_gps_positions, target_gps, context):
+        if getattr(self, '_closing', False):
+            return
+        try:
+            waypoints_per_drone, center_origin, rendezvous_indices = future.result()
+            payload = {
+                'ok': True,
+                'request_id': request_id,
+                'group_id': group_id,
+                'selected_drones': selected_drones,
+                'drone_gps_positions': drone_gps_positions,
+                'target_gps': target_gps,
+                'waypoints_per_drone': waypoints_per_drone,
+                'center_origin': center_origin,
+                'rendezvous_indices': rendezvous_indices,
+                'context': context,
+            }
+        except Exception as e:
+            payload = {
+                'ok': False,
+                'request_id': request_id,
+                'group_id': group_id,
+                'error': str(e),
+                'traceback': traceback.format_exc(),
+                'context': context,
+            }
+        self.planning_finished.emit(payload)
+
+    def _on_plan_done(self, payload):
+        if getattr(self, '_closing', False):
+            return
+        group_id = payload['group_id']
+        context = payload.get('context', {})
+        if self._latest_plan_request_by_group.get(group_id) != payload['request_id']:
+            return
+
+        group = self.mission_groups.get(group_id)
+        if not group:
+            return
+
+        if not payload['ok']:
+            self.statusBar().showMessage(
+                f"Group {group_id}: {context.get('failure_label', '規劃')}失敗: {payload['error']}", 5000
+            )
+            _log("ERROR", payload.get('traceback', ''))
+            return
+
+        selected_drones = payload['selected_drones']
+        drone_gps_positions = payload['drone_gps_positions']
+        target_gps = payload['target_gps']
+        waypoints_per_drone = payload['waypoints_per_drone']
+        center_origin = payload['center_origin']
+        rendezvous_indices = payload['rendezvous_indices']
+        expected_mission_type = context.get('expected_mission_type')
+
+        if set(group.selected_drone_ids) != set(selected_drones):
+            self.statusBar().showMessage(
+                f"Group {group_id}: 無人機分配已變更，忽略舊規劃結果", 3000
+            )
+            return
+        if expected_mission_type and group.mission_type != expected_mission_type:
+            self.statusBar().showMessage(
+                f"Group {group_id}: 任務模式已變更，忽略舊規劃結果", 3000
+            )
+            return
+
+        group.planned_waypoints = {
+            'drone_ids': selected_drones,
+            'waypoints': waypoints_per_drone,
+            'rendezvous_indices': rendezvous_indices,
+        }
+        group.center_origin = center_origin
+        self.show_planned_waypoints(group)
+
+        center_lat, center_lon, _ = center_origin
+        target_lat, target_lon = context['draw_target']
+        self.drone_map.draw_mission_plan_for_group(
+            group_id, group.color,
+            center_lat, center_lon, target_lat, target_lon
+        )
+
+        self._launch_verification(
+            context['verification_type'], drone_gps_positions, selected_drones,
+            waypoints_per_drone, center_origin,
+            target_gps=target_gps if context.get('use_target_gps') else None,
+            rect_corners=context.get('rect_corners'),
+            route_waypoints=context.get('route_waypoints'),
+        )
+
+        panel = self.group_panels.get(group_id)
+        if panel:
+            panel.update_status()
+            panel.update_mission_info(center_lat, center_lon, target_lat, target_lon)
+
+        total_wps = sum(len(wps) for wps in waypoints_per_drone)
+        self.statusBar().showMessage(
+            f"Group {group_id}: {context['success_label']}，{len(selected_drones)} 台，共 {total_wps} 個航點", 5000
+        )
+
     def handle_map_click(self, lat, lon):
         """處理地圖點擊事件 — 根據 active group 的任務類型規劃"""
         group = self._get_active_group()
@@ -1761,46 +1928,22 @@ class ControlStationUI(QMainWindow):
         self.statusBar().showMessage(
             f"Group {group.group_id}: 正在規劃 {group.mission_type} ({len(selected_drones)} 台)", 2000)
 
-        try:
+        drone_gps_positions = self._collect_drone_gps(selected_drones, base_alt)
-            drone_gps_positions = self._collect_drone_gps(selected_drones, base_alt)
+        if drone_gps_positions is None:
-            if drone_gps_positions is None:
+            return
-                return
 
-            waypoints_per_drone, center_origin, rendezvous_indices = self.mission_planner.plan_formation_mission(
+        context = {
-                drone_gps_positions, target_gps, mission_type, params=params
+            'success_label': f"{group.mission_type} 規劃完成",
-            )
+            'failure_label': '規劃',
-
+            'verification_type': group.mission_type,
-            group.planned_waypoints = {
+            'draw_target': (lat, lon),
-                'drone_ids': selected_drones,
+            'use_target_gps': True,
-                'waypoints': waypoints_per_drone,
+            'expected_mission_type': group.mission_type,
-                'rendezvous_indices': rendezvous_indices,
+        }
-            }
+        self._submit_mission_plan(
-            group.center_origin = center_origin
+            group, selected_drones, drone_gps_positions,
-            self.show_planned_waypoints(group)
+            target_gps, mission_type, params, context
-
+        )
-            center_lat, center_lon, _ = center_origin
-            self.drone_map.draw_mission_plan_for_group(
-                group.group_id, group.color,
-                center_lat, center_lon, lat, lon
-            )
-
-            self._launch_verification(
-                group.mission_type, drone_gps_positions, selected_drones,
-                waypoints_per_drone, center_origin, target_gps=target_gps
-            )
-
-            panel = self.group_panels.get(group.group_id)
-            if panel:
-                panel.update_status()
-                panel.update_mission_info(center_lat, center_lon, lat, lon)
-
-            total_wps = sum(len(wps) for wps in waypoints_per_drone)
-            self.statusBar().showMessage(
-                f"Group {group.group_id}: {group.mission_type} 規劃完成，{len(selected_drones)} 台，共 {total_wps} 個航點", 5000
-            )
-        except Exception as e:
-            self.statusBar().showMessage(f"Group {group.group_id}: 規劃失敗: {str(e)}", 5000)
-            traceback.print_exc()
 
     # ================================================================================
     # 任務規劃 — 矩形選取 (Grid Sweep)
@@ -1831,51 +1974,27 @@ class ControlStationUI(QMainWindow):
         base_alt = params.get('altitude', 10.0)
         self.statusBar().showMessage(
             f"Group {group.group_id}: 正在規劃 Grid Sweep ({len(selected_drones)} 台)", 2000)
-        try:
+        drone_gps_positions = self._collect_drone_gps(selected_drones, base_alt)
-            drone_gps_positions = self._collect_drone_gps(selected_drones, base_alt)
+        if drone_gps_positions is None:
-            if drone_gps_positions is None:
+            return
-                return
 
-            target_lat = sum(c[0] for c in rect_corners) / 4
+        target_lat = sum(c[0] for c in rect_corners) / 4
-            target_lon = sum(c[1] for c in rect_corners) / 4
+        target_lon = sum(c[1] for c in rect_corners) / 4
-            target_gps = (target_lat, target_lon, base_alt)
+        target_gps = (target_lat, target_lon, base_alt)
 
-            params['rect_corners'] = rect_corners
+        params['rect_corners'] = rect_corners
-            waypoints_per_drone, center_origin, rendezvous_indices = self.mission_planner.plan_formation_mission(
+        context = {
-                drone_gps_positions, target_gps, MissionType.GRID_SWEEP,
+            'success_label': 'Grid Sweep 規劃完成',
-                params=params
+            'failure_label': 'Grid Sweep 規劃',
-            )
+            'verification_type': 'grid_sweep',
-
+            'draw_target': (target_lat, target_lon),
-            group.planned_waypoints = {
+            'rect_corners': rect_corners,
-                'drone_ids': selected_drones,
+            'expected_mission_type': group.mission_type,
-                'waypoints': waypoints_per_drone,
+        }
-                'rendezvous_indices': rendezvous_indices,
+        self._submit_mission_plan(
-            }
+            group, selected_drones, drone_gps_positions,
-            group.center_origin = center_origin
+            target_gps, MissionType.GRID_SWEEP, params, context
-            self.show_planned_waypoints(group)
+        )
-
-            center_lat, center_lon, _ = center_origin
-            self.drone_map.draw_mission_plan_for_group(
-                group.group_id, group.color,
-                center_lat, center_lon, target_lat, target_lon
-            )
-            self._launch_verification(
-                'grid_sweep', drone_gps_positions, selected_drones,
-                waypoints_per_drone, center_origin, rect_corners=rect_corners
-            )
-
-            panel = self.group_panels.get(group.group_id)
-            if panel:
-                panel.update_status()
-                panel.update_mission_info(center_lat, center_lon, target_lat, target_lon)
-
-            total_wps = sum(len(wps) for wps in waypoints_per_drone)
-            self.statusBar().showMessage(
-                f"Group {group.group_id}: Grid Sweep 規劃完成，{len(selected_drones)} 台，共 {total_wps} 個航點", 5000
-            )
-        except Exception as e:
-            self.statusBar().showMessage(f"Group {group.group_id}: Grid Sweep 規劃失敗: {str(e)}", 5000)
-            traceback.print_exc()
 
     # ================================================================================
     # 任務規劃 — 路徑確認 (Leader-Follower)
@@ -1917,53 +2036,28 @@ class ControlStationUI(QMainWindow):
         self.statusBar().showMessage(
             f"Group {group.group_id}: 正在規劃跟隨模式 ({len(selected_drones)} 台)", 2000)
 
-        try:
+        drone_gps_positions = self._collect_drone_gps(selected_drones, base_alt)
-            drone_gps_positions = self._collect_drone_gps(selected_drones, base_alt)
+        if drone_gps_positions is None:
-            if drone_gps_positions is None:
+            return
-                return
 
-            target_lat = sum(p[0] for p in route_waypoints) / len(route_waypoints)
+        target_lat = sum(p[0] for p in route_waypoints) / len(route_waypoints)
-            target_lon = sum(p[1] for p in route_waypoints) / len(route_waypoints)
+        target_lon = sum(p[1] for p in route_waypoints) / len(route_waypoints)
-            target_gps = (target_lat, target_lon, base_alt)
+        target_gps = (target_lat, target_lon, base_alt)
 
-            params['route_waypoints'] = route_waypoints
+        params['route_waypoints'] = route_waypoints
-            waypoints_per_drone, center_origin, rendezvous_indices = self.mission_planner.plan_formation_mission(
+        context = {
-                drone_gps_positions, target_gps, MissionType.LEADER_FOLLOWER,
+            'success_label': '跟隨模式規劃完成',
-                params=params
+            'failure_label': '跟隨模式規劃',
-            )
+            'verification_type': 'LEADER_FOLLOWER',
-
+            'draw_target': (target_lat, target_lon),
-            group.planned_waypoints = {
+            'use_target_gps': True,
-                'drone_ids': selected_drones,
+            'route_waypoints': route_waypoints,
-                'waypoints': waypoints_per_drone,
+            'expected_mission_type': group.mission_type,
-                'rendezvous_indices': rendezvous_indices,
+        }
-            }
+        self._submit_mission_plan(
-            group.center_origin = center_origin
+            group, selected_drones, drone_gps_positions,
-            self.show_planned_waypoints(group)
+            target_gps, MissionType.LEADER_FOLLOWER, params, context
-
+        )
-            center_lat, center_lon, _ = center_origin
-            self.drone_map.draw_mission_plan_for_group(
-                group.group_id, group.color,
-                center_lat, center_lon, target_lat, target_lon
-            )
-
-            self._launch_verification(
-                'LEADER_FOLLOWER', drone_gps_positions, selected_drones,
-                waypoints_per_drone, center_origin,
-                target_gps=target_gps, route_waypoints=route_waypoints
-            )
-
-            panel = self.group_panels.get(group.group_id)
-            if panel:
-                panel.update_status()
-                panel.update_mission_info(center_lat, center_lon, target_lat, target_lon)
-
-            total_wps = sum(len(wps) for wps in waypoints_per_drone)
-            self.statusBar().showMessage(
-                f"Group {group.group_id}: 跟隨模式規劃完成，{len(selected_drones)} 台，共 {total_wps} 個航點", 5000
-            )
-        except Exception as e:
-            self.statusBar().showMessage(f"Group {group.group_id}: 跟隨模式規劃失敗: {str(e)}", 5000)
-            traceback.print_exc()
 
     # ================================================================================
     # 任務執行回呼
@@ -2059,11 +2153,51 @@ class ControlStationUI(QMainWindow):
         if isinstance(panel, DronePanel):
             panel.update_field(field, text, color)
 
+    @staticmethod
+    def _format_gnss_fix_type(fix_type):
+        fix_labels = {
+            0: "No GPS",
+            1: "No GPS",
+            2: "2D Fix",
+            3: "3D Fix",
+            4: "DGPS",
+            5: "RTK Float",
+            6: "RTK Fixed",
+        }
+        try:
+            return fix_labels.get(int(fix_type), f"Fix {fix_type}")
+        except (TypeError, ValueError):
+            return "--"
+
     def update_overview_table(self, drone_id=None, field=None, value=None):
         if not hasattr(self, 'overview_table') or self.overview_table is None: return
         self.overview_table.set_drones(self.drones)
         self.overview_table.update_table(drone_id, field, value)
 
+    def queue_overview_update(self, drone_id, field, value):
+        if not hasattr(self, '_overview_cache'):
+            self._overview_cache = {}
+        self._overview_cache.setdefault(drone_id, {})[field] = value
+
+    def _flush_overview_table(self):
+        if not hasattr(self, 'overview_table') or self.overview_table is None:
+            return
+        if not getattr(self, '_overview_cache', None):
+            return
+
+        pending_updates = self._overview_cache
+        self._overview_cache = {}
+
+        self.overview_table.set_drones(self.drones)
+        self.overview_table.setUpdatesEnabled(False)
+        try:
+            for drone_id, fields in pending_updates.items():
+                for field, value in fields.items():
+                    self.overview_table.update_table(drone_id, field, value)
+        finally:
+            self.overview_table.setUpdatesEnabled(True)
+            self.overview_table.viewport().update()
+    
     def get_socket_id(self, drone_id):
         import re
         match = re.match(r's(\d+)_(\d+)', drone_id)
@@ -2100,7 +2234,7 @@ class ControlStationUI(QMainWindow):
             self.drone_panel_layout.addWidget(self.socket_groups[socket_id])
 
     def _update_panel_and_map(self):
-        """30Hz 定時更新 panel 和 map，批量更新 UI 以避免過度重繪"""
+        """定時更新 panel / table，批量更新 UI 以避免過度重繪。"""
         if not hasattr(self, '_message_cache') or not self._message_cache:
             return
         
@@ -2115,20 +2249,17 @@ class ControlStationUI(QMainWindow):
             self._map_update_time = now
             self._map_update_count = 0
         
-        # ✅ 步驟 1: 暫停表格的即時重繪
-        if hasattr(self, 'overview_table') and self.overview_table:
-            self.overview_table.setUpdatesEnabled(False)
-        
         try:
             start_time = time.time()
+            pending_messages = self._message_cache
+            self._message_cache = {}
             
-            # ✅ 步驟 2: 遍歷快取中最新的資料來更新 UI
+            # ✅ 步驟 2: 只處理本批新資料，避免每個 tick 重跑舊資料造成週期性卡頓
-            for drone_id in list(self._message_cache.keys()):
+            for drone_id, cached_data in pending_messages.items():
                 if drone_id not in self.drones:
                     continue
                 
                 panel = self.drones[drone_id]
-                cached_data = self._message_cache[drone_id]
                 
                 # 處理所有快取的消息類型
                 for msg_type, data in cached_data.items():
@@ -2144,8 +2275,8 @@ class ControlStationUI(QMainWindow):
                             arm_text, arm_color = "--", '#AAAAAA'
                         self.update_field(panel, drone_id, 'mode', mode, mode_color)
                         self.update_field(panel, drone_id, 'armed', arm_text, arm_color)
-                        self.update_overview_table(drone_id, 'mode', mode)
+                        self.queue_overview_update(drone_id, 'mode', mode)
-                        self.update_overview_table(drone_id, 'armed', arm_text)
+                        self.queue_overview_update(drone_id, 'armed', arm_text)
                     
                     elif msg_type == 'battery':
                         voltage = data.get('voltage', 16)
@@ -2158,90 +2289,147 @@ class ControlStationUI(QMainWindow):
                         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.update_overview_table(drone_id, 'battery', f"{voltage:.2f}V")
+                        self.queue_overview_update(drone_id, 'battery', f"{voltage:.2f}V")
                     
                     elif msg_type == 'altitude':
                         altitude = data.get('altitude', 0)
                         text = f"{altitude:.1f} m"
                         self.update_field(panel, drone_id, 'altitude', text)
-                        self.update_overview_table(drone_id, 'altitude', text)
+                        self.queue_overview_update(drone_id, 'altitude', text)
                     
                     elif msg_type == 'local_pose':
                         x, y = data.get('x', 0), data.get('y', 0)
                         if not hasattr(self.monitor, 'drone_local'):
                             self.monitor.drone_local = {}
                         self.monitor.drone_local[drone_id] = {'x': x, 'y': y}
-                        self.update_overview_table(drone_id, 'local', f"{x:.1f}, {y:.1f}")
+                        self.queue_overview_update(drone_id, 'local', f"{x:.1f}, {y:.1f}")
                     
                     elif msg_type == 'loss_rate':
                         text = f"{data.get('loss_rate', 0):.1f}%"
                         self.update_field(panel, drone_id, 'loss_rate', text)
-                        self.update_overview_table(drone_id, 'loss_rate', text)
+                        self.queue_overview_update(drone_id, 'loss_rate', text)
                     
                     elif msg_type == 'ping':
                         text = f"{data.get('ping', 0):.1f} ms"
                         self.update_field(panel, drone_id, 'ping', text)
-                        self.update_overview_table(drone_id, 'ping', text)
+                        self.queue_overview_update(drone_id, 'ping', text)
                     
                     elif msg_type == 'velocity':
-                        self.update_overview_table(drone_id, 'velocity', f"{data['vx']:.1f}, {data['vy']:.1f}")
+                        self.queue_overview_update(drone_id, 'velocity', f"{data['vx']:.1f}, {data['vy']:.1f}")
                     
                     elif msg_type == 'attitude':
                         roll, pitch, yaw = data.get('roll', 0), data.get('pitch', 0), data.get('yaw', 0)
-                        self.update_overview_table(drone_id, 'roll', f"{roll:.1f}°")
+                        self.queue_overview_update(drone_id, 'roll', f"{roll:.1f}°")
-                        self.update_overview_table(drone_id, 'pitch', f"{pitch:.1f}°")
+                        self.queue_overview_update(drone_id, 'pitch', f"{pitch:.1f}°")
-                        self.update_overview_table(drone_id, 'yaw', f"{yaw:.1f}°")
+                        self.queue_overview_update(drone_id, 'yaw', f"{yaw:.1f}°")
-                        panel._last_roll = roll
-                        panel._last_pitch = pitch
-                        if hasattr(panel, 'update_attitude'):
-                            heading = self.drone_headings.get(drone_id, yaw)
-                            panel.update_attitude(heading, roll, pitch)
                     
                     elif msg_type == 'gps':
                         gps_data = data
                         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] = {'lat': lat, 'lon': lon, 'alt': alt}
+                        self.monitor.drone_gps[drone_id] = gps_data.copy()
-                        self.update_overview_table(drone_id, 'latitude', f"{lat:.6f}°")
+                        self.queue_overview_update(drone_id, 'latitude', f"{lat:.6f}°")
-                        self.update_overview_table(drone_id, 'longitude', f"{lon:.6f}°")
+                        self.queue_overview_update(drone_id, 'longitude', f"{lon:.6f}°")
+                        fix_type = gps_data.get('fix_type')
+                        satellites_visible = gps_data.get('satellites_visible')
+                        eph = gps_data.get('eph')
+                        epv = gps_data.get('epv')
+                        fix_text = self._format_gnss_fix_type(fix_type)
+                        self.update_field(panel, drone_id, 'fix_type_label', fix_text)
+                        if isinstance(eph, (int, float)) and isinstance(epv, (int, float)):
+                            error_text = f"{eph:.1f}/{epv:.1f}"
+                        else:
+                            error_text = "--"
+                        self.update_field(panel, drone_id, 'gnss_error', error_text)
+                        self.queue_overview_update(
+                            drone_id, 'fix_type',
+                            fix_text
+                        )
+                        self.queue_overview_update(
+                            drone_id, 'satellites_visible',
+                            str(satellites_visible) if satellites_visible is not None else "--"
+                        )
+                        self.queue_overview_update(
+                            drone_id, 'eph',
+                            f"{eph:.2f}" if isinstance(eph, (int, float)) else "--"
+                        )
+                        self.queue_overview_update(
+                            drone_id, 'epv',
+                            f"{epv:.2f}" if isinstance(epv, (int, float)) else "--"
+                        )
                     
                     elif msg_type == 'hud':
                         hud_data = data
                         heading = hud_data.get('heading', 0)
                         self.drone_headings[drone_id] = heading
+                        self._map_dirty_drones.add(drone_id)
                         groundspeed = hud_data.get('groundspeed', 0)
                         airspeed = hud_data.get('airspeed', 0)
                         throttle = hud_data.get('throttle', 0)
                         hud_alt = hud_data.get('alt', 0)
                         climb = hud_data.get('climb', 0)
                         
-                        self.update_overview_table(drone_id, 'heading', f"{heading:.1f}°")
+                        self.queue_overview_update(drone_id, 'heading', f"{heading:.1f}°")
-                        self.update_overview_table(drone_id, 'groundspeed', f"{groundspeed:.1f} m/s" if isinstance(groundspeed, (int, float)) else "--")
+                        self.queue_overview_update(drone_id, 'groundspeed', f"{groundspeed:.1f} m/s" if isinstance(groundspeed, (int, float)) else "--")
-                        self.update_overview_table(drone_id, 'airspeed', f"{airspeed:.1f} m/s" if isinstance(airspeed, (int, float)) else "--")
+                        self.queue_overview_update(drone_id, 'airspeed', f"{airspeed:.1f} m/s" if isinstance(airspeed, (int, float)) else "--")
-                        self.update_overview_table(drone_id, 'throttle', f"{throttle:.0f}%" if isinstance(throttle, (int, float)) else "--")
+                        self.queue_overview_update(drone_id, 'throttle', f"{throttle:.0f}%" if isinstance(throttle, (int, float)) else "--")
-                        self.update_overview_table(drone_id, 'hud_alt', f"{hud_alt:.1f} m" if isinstance(hud_alt, (int, float)) else "--")
+                        self.queue_overview_update(drone_id, 'hud_alt', f"{hud_alt:.1f} m" if isinstance(hud_alt, (int, float)) else "--")
-                        self.update_overview_table(drone_id, 'climb', f"{climb:.1f} m/s" if isinstance(climb, (int, float)) else "--")
+                        self.queue_overview_update(drone_id, 'climb', f"{climb:.1f} m/s" if isinstance(climb, (int, float)) else "--")
                         
                         self.update_field(panel, drone_id, 'heading', f"{heading:.1f}°")
                         self.update_field(panel, drone_id, 'groundspeed', f"{groundspeed:.1f} m/s" if isinstance(groundspeed, (int, float)) else "--")
                         self.update_field(panel, drone_id, 'speed', f"{groundspeed:.1f} m/s" if isinstance(groundspeed, (int, float)) else "--")
                         
-                        if drone_id in self.drone_positions:
-                            lat, lon = self.drone_positions[drone_id]
-                            self.drone_map.update_drone_position(drone_id, lat, lon, heading)
             
             elapsed = (time.time() - start_time) * 1000
             if elapsed > 33:
                 _log("WARN", f"UI 更新耗時 {elapsed:.1f}ms (目標 <33ms)")
+        except Exception as e:
+            _log("ERROR", f"Panel 更新錯誤: {e}")
+            traceback.print_exc()
 
-        finally:
+    def _update_map_only(self):
-            # ✅ 步驟 3: 恢復表格重繪（所有資料已填好，一次性重繪）
+        """獨立降頻更新地圖，避免地圖 JS 呼叫拖住 panel / table。"""
-            if hasattr(self, 'overview_table') and self.overview_table:
+        dirty_drones = getattr(self, '_map_dirty_drones', set())
-                self.overview_table.setUpdatesEnabled(True)
+        if not dirty_drones:
-                self.overview_table.viewport().update()
+            return
+
+        pending_drones = list(dirty_drones)
+        self._map_dirty_drones = set()
+
+        for drone_id in pending_drones:
+            pos = self.drone_positions.get(drone_id)
+            if not pos:
+                continue
+            heading = self.drone_headings.get(drone_id, 0)
+            lat, lon = pos
+            self.drone_map.update_drone_position(drone_id, lat, lon, heading)
+
+    def _update_attitude_only(self):
+        """高頻更新 drone panel 的 attitude 顯示器。"""
+        if not getattr(self, '_attitude_cache', None):
+            return
+
+        latest_attitudes = self._attitude_cache
+        self._attitude_cache = {}
+
+        for drone_id, data in latest_attitudes.items():
+            panel = self.drones.get(drone_id)
+            if not panel or not hasattr(panel, 'update_attitude'):
+                continue
+
+            roll = data.get('roll', 0)
+            pitch = data.get('pitch', 0)
+            yaw = data.get('yaw', 0)
+            heading = self.drone_headings.get(drone_id, yaw)
+
+            panel._last_roll = roll
+            panel._last_pitch = pitch
+            panel.update_attitude(heading, roll, pitch)
 
 
 
@@ -2275,28 +2463,41 @@ class ControlStationUI(QMainWindow):
             # 但仍然打印詳細的堆棧跟踪以便調試
             traceback.print_exc()
 
-    def spin_ros(self):
+    def _ros_spin_thread(self):
-        try:
+        while self.ros_thread_running and rclpy.ok():
-            # 仅在 ROS2 正常工作时才尝试 spin
+            try:
-            if rclpy.ok():
                 self.executor.spin_once(timeout_sec=0.01)
-                for (drone_id, msg_type), data in self.monitor.latest_data.items():
+
-                    self.monitor.signals.update_signal.emit(msg_type, drone_id, data)
+                latest_items = list(self.monitor.latest_data.items())
                 self.monitor.latest_data.clear()
-        except RuntimeError as e:
+
-            # ROS2 context 被破坏或不可用
+                for (drone_id, msg_type), data in latest_items:
-            if "Context" in str(e) or "context" in str(e).lower():
+                    self.monitor.signals.update_signal.emit(msg_type, drone_id, data)
-                _log("WARN", f"ROS2 context 錯誤（已忽略）: {e}")
+            except RuntimeError as e:
-            else:
+                if "Context" in str(e) or "context" in str(e).lower():
-                _log("ERROR", f"ROS spin error: {e}")
+                    _log("WARN", f"ROS2 context 錯誤（已忽略）: {e}")
+                    break
+                _log("ERROR", f"ROS spin thread error: {e}")
                 traceback.print_exc()
-        except Exception as e:
+            except Exception as e:
-            _log("ERROR", f"ROS spin error: {e}")
+                _log("ERROR", f"ROS spin thread error: {e}")
-            traceback.print_exc()
+                traceback.print_exc()
+
+    def _stop_ros_thread(self):
+        self.ros_thread_running = False
+        ros_thread = getattr(self, 'ros_thread', None)
+        if ros_thread and ros_thread.is_alive():
+            ros_thread.join(timeout=1.0)
 
     def closeEvent(self, event):
+        self._closing = True
         self._restore_stream_redirector()
+        self._stop_ros_thread()
         try:
+            try:
+                self.planner_pool.shutdown(wait=False, cancel_futures=True)
+            except TypeError:
+                self.planner_pool.shutdown(wait=False)
             for group in self.mission_groups.values():
                 if group.executor:
                     group.executor.stop()

src/GUI/map_layout.py

@@ -340,6 +340,7 @@ class DroneMap:
                 var markers = {};
                 var idLabels = {};
                 var trajectories = {};
+                const maxTrajectoryPoints = 300;
                 var trajectoryLines = {};
                 var focusedId = null;
                 var initialized = false;
@@ -702,11 +703,12 @@ class DroneMap:
                     const point = [lat, lon];
                     trajectories[id].push(point);
                     
-                    if (trajectories[id].length > 1000) {
+                    if (trajectories[id].length > maxTrajectoryPoints) {
                         trajectories[id].shift();
+                        trajectoryLines[id].setLatLngs(trajectories[id]);
+                    } else {
+                        trajectoryLines[id].addLatLng(point);
                     }
-                    
-                    trajectoryLines[id].setLatLngs([...trajectories[id]]);
                 }
 
                 function focusOn(id) {
@@ -878,7 +880,7 @@ class DroneMap:
         # 設置地圖更新計時器
         self.map_update_timer = QTimer()
         self.map_update_timer.timeout.connect(self.update_map_positions)
-        self.map_update_timer.start(200)  # 每 200ms 更新一次
+        self.map_update_timer.start(100)  # 每 100ms 更新一次（10Hz）
     
     def _on_map_loaded(self, ok: bool):
         """地圖加載完成回調"""

src/GUI/mission_planner.py

@@ -83,10 +83,11 @@ class FormationPlanner:
 
         center_lat = sum(pos[0] for pos in drone_gps_positions) / len(drone_gps_positions)
         center_lon = sum(pos[1] for pos in drone_gps_positions) / len(drone_gps_positions)
-        center_alt = sum(pos[2] for pos in drone_gps_positions) / len(drone_gps_positions)
 
-        self.current_origin = (center_lat, center_lon, center_alt)
+        # origin_alt = 0：送給飛控的高度透過 MAV_FRAME_GLOBAL_RELATIVE_ALT_INT
-        self.converter = CoordinateConverter(center_lat, center_lon, center_alt)
+        # 直接就是「相對 home 高度」，不需要加 GPS 橢球高度偏移
+        self.current_origin = (center_lat, center_lon, 0.0)
+        self.converter = CoordinateConverter(center_lat, center_lon, 0.0)
 
         drone_local = [self.converter.gps_to_local(*pos) for pos in drone_gps_positions]
         target_local = self.converter.gps_to_local(*target_gps)
@@ -206,29 +207,17 @@ class FormationPlanner:
 
     def _calculate_leader_follower(self, drone_positions, route_wps_local, params):
         """
-        路徑跟隨編隊 — 虛擬領隊 / 弧長參數化版 (virtual leader / arc-length)
+        路徑跟隨編隊 — 虛擬領隊 / 弧長參數化
 
-        核心想法：
+        center_path = 使用者畫的路徑點（直線段連接，不插弧），
-            - center_path 建好後計算每個 sample 的累積弧長 s
+        每架 drone 沿弧長 s 參數化做隊形偏移：
-            - 每架 drone 給 (lat_k, lon_k) 的隊形偏移
+            s_follower = s_leader - lon_k
-            - 把 leader 想成沿 s 參數化的點，follower k 的目標 s = s_leader - lon_k
+            position = lookup(s_follower) + lat_k × perp(tangent)
-            - follower k 第 i 個 waypoint = lookup(s_list[i] - lon_k) + lat_k × perp(tangent)
+        s 超出 path 範圍時外推（不 clip），確保每架 drone 起終點不重疊。
-            - 超過 path 起點/終點以 clip 處理，避免倒退或衝出
 
-        解決前版「空間偏移」的三個 bug：
+        Rank = 輸入順序（GUI 選取順序），drone_positions[0] = leader。
-            1. 起點暴衝：s<0 clip → follower 起點就是 start 加橫向偏移，不往後倒退
+        隊形 (俯視):
-            2. 弧段角度爆炸：lon 不再換算成弧度，直接沿 polyline lookup
+            D0 (lat=-L, lon=0)   ← leader
-            3. straight/arc 切換不連續：統一走 polyline 線性內插與 segment 切線
-
-        Rank 定序規則：
-            **以 drone_positions 的輸入順序為準**（= GUI 選取順序）。
-            drone_positions[0] = rank 0 = leader (lon=0)，
-            drone_positions[1] = rank 1 (lon=5)，依此類推。
-            刻意「不」用位置投影自動排序，避免浮點噪音造成 run-to-run
-            leader 漂移，以確保整個 mission 像軍隊縱隊那樣順序固定。
-
-        隊形 (俯視, 以路徑行進方向為前):
-            D0 (lat=-L, lon=0)   ← front-right (leader)
                 D1 (lat=+L, lon=5)
             D2 (lat=-L, lon=10)
                 D3 (lat=+L, lon=15)
@@ -237,29 +226,16 @@ class FormationPlanner:
         lateral_offset = params.get('lateral_offset', 3.0)
         longitudinal_spacing = params.get('longitudinal_spacing', 5.0)
         altitude = params.get('altitude', self.base_altitude)
-        min_inner_radius = params.get('min_inner_radius', 3.0)
-        arc_resolution = params.get('arc_resolution', 12)
-        sharp_turn_cos = params.get('sharp_turn_cos_threshold', 0.0)  # cos(90°)=0
 
-        # Step 1: 建立中心路徑（含圓弧、銳角單點；每點帶累積 s）
+        # Step 1: 建立中心路徑（直線段連接，每個中間航點設 barrier）
-        center_path, barrier_indices = self._build_center_path(
+        center_path, barrier_indices = self._build_center_path(route_wps_local)
-            route_wps_local,
-            formation_max_lateral=lateral_offset,
-            min_inner_radius=min_inner_radius,
-            arc_resolution=arc_resolution,
-            sharp_turn_cos_threshold=sharp_turn_cos,
-        )
 
         s_list = [pt['s'] for pt in center_path]
         s_max = s_list[-1]
         n_pts = len(center_path)
 
         def lookup(s_target):
-            """
+            """在 center path 上以弧長 s 回傳 (x, y, tangent_dir)。超出範圍時沿端點方向外推。"""
-            在 center path 上以弧長 s 回傳 (x, y, tangent_dir)。
-            s<0 → clip 到 start；s>s_max → clip 到 end。
-            tangent 方向取當前 polyline segment 的切線，避免對不連續的 dir 做插值。
-            """
             if n_pts == 1:
                 pt = center_path[0]
                 return pt['x'], pt['y'], pt['dir']
@@ -270,7 +246,8 @@ class FormationPlanner:
                 sdx, sdy = b['x'] - a['x'], b['y'] - a['y']
                 slen = math.hypot(sdx, sdy)
                 if slen > 1e-6:
-                    return a['x'], a['y'], (sdx / slen, sdy / slen)
+                    dx, dy = sdx / slen, sdy / slen
+                    return a['x'] + s_target * dx, a['y'] + s_target * dy, (dx, dy)
                 return a['x'], a['y'], a['dir']
 
             if s_target >= s_max:
@@ -279,7 +256,9 @@ class FormationPlanner:
                 sdx, sdy = b['x'] - a['x'], b['y'] - a['y']
                 slen = math.hypot(sdx, sdy)
                 if slen > 1e-6:
-                    return b['x'], b['y'], (sdx / slen, sdy / slen)
+                    dx, dy = sdx / slen, sdy / slen
+                    overshoot = s_target - s_max
+                    return b['x'] + overshoot * dx, b['y'] + overshoot * dy, (dx, dy)
                 return b['x'], b['y'], b['dir']
 
             # Binary search：s_list[lo] <= s_target < s_list[hi]
@@ -308,16 +287,6 @@ class FormationPlanner:
         # 避免浮點噪音在投影值相近時翻轉 leader，保證 run-to-run 穩定
         all_waypoints = [None] * N
 
-        # 預先算好路徑終點的切線（給收尾點用）
-        end = center_path[-1]
-        end_dx, end_dy = end['dir']
-        if n_pts >= 2:
-            a = center_path[-2]
-            sdx, sdy = end['x'] - a['x'], end['y'] - a['y']
-            slen = math.hypot(sdx, sdy)
-            if slen > 1e-6:
-                end_dx, end_dy = sdx / slen, sdy / slen
-
         for rank in range(N):
             lat_sign = -1 if rank % 2 == 0 else 1
             lat = lat_sign * lateral_offset
@@ -334,46 +303,16 @@ class FormationPlanner:
                     altitude,
                 ))
 
-            # 收尾：全員到 path 終點的 rank 位置（lon 歸零）
-            waypoints.append((
-                end['x'] + lat * (-end_dy),
-                end['y'] + lat * end_dx,
-                altitude,
-            ))
-
             all_waypoints[rank] = waypoints
 
-        # barrier 索引仍指向 center_path 內 (range [0, n_pts-1])，
-        # 不觸及末尾收尾點，直接沿用即可
         return all_waypoints, barrier_indices
 
-    def _build_center_path(self, waypoints,
+    def _build_center_path(self, waypoints):
-                           formation_max_lateral,
-                           min_inner_radius,
-                           arc_resolution,
-                           sharp_turn_cos_threshold):
         """
-        建立以「圓弧」連接直線段的中心路徑。
+        建立中心路徑 — 直接用路徑點連接，不插弧。
 
-        每個中間 waypoint 的處理：
+        每個中間航點設為 barrier（轉彎集合點），讓隊伍到齊後再推進。
-            1. 計算入向 u_in、出向 u_out 的夾角 β = acos(u_in·u_out)
+        飛控在 GUIDED 模式下自行處理直線飛行，不需要 GUI 預先平滑。
-            2. 若 cos β < sharp_turn_cos_threshold → 銳角：只插單點 (hover + 原地轉向)，
-               barrier 放在該點，讓隊伍整體停下再繼續
-            3. 否則 → 平滑弧：
-                  R_base = formation_max_lateral + min_inner_radius
-                  d = R_base × tan(β/2)，並以相鄰段長的 45% 為上限
-                  R_actual = d / tan(β/2)
-                  tangent points T_in = WP - u_in·d, T_out = WP + u_out·d
-                  arc center = T_in + R_actual·sign·n_left，其中 sign=+1 左轉/CCW，-1 右轉/CW
-                  arc 由 theta_in 到 theta_out 以 arc_resolution 等分
-               path 內容: [T_in(straight,u_in), arc_samples..., T_out(straight,u_out)]
-               barrier 放在 T_out (轉完彎後的集合點)
-
-        path 是一個 list[dict]，每個 dict 至少含:
-            {'seg': 'straight' | 'arc',
-             'x': float, 'y': float,
-             'dir': (dx, dy)}
-        'arc' 類型額外含: 'arc_center', 'arc_R', 'arc_sign', 'theta'
 
         Returns:
             (path, barrier_indices)
@@ -385,129 +324,35 @@ class FormationPlanner:
                 'seg': 'straight',
                 'x': waypoints[0][0], 'y': waypoints[0][1],
                 'dir': (0.0, 1.0),
+                's': 0.0,
             }], []
 
-        segments = []
-        for i in range(num_wps - 1):
-            dx = waypoints[i + 1][0] - waypoints[i][0]
-            dy = waypoints[i + 1][1] - waypoints[i][1]
-            length = math.hypot(dx, dy)
-            if length < 0.01:
-                segments.append(((0.0, 1.0), length))
-            else:
-                segments.append(((dx / length, dy / length), length))
-
-        R_base = formation_max_lateral + min_inner_radius
         path = []
         barrier_indices = []
 
-        # 起點
+        for i in range(num_wps):
-        path.append({
+            if i < num_wps - 1:
-            'seg': 'straight',
+                dx = waypoints[i + 1][0] - waypoints[i][0]
-            'x': waypoints[0][0], 'y': waypoints[0][1],
+                dy = waypoints[i + 1][1] - waypoints[i][1]
-            'dir': segments[0][0],
+            else:
-        })
+                dx = waypoints[i][0] - waypoints[i - 1][0]
+                dy = waypoints[i][1] - waypoints[i - 1][1]
 
-        for i in range(1, num_wps - 1):
+            length = math.hypot(dx, dy)
-            u_in, len_in = segments[i - 1]
+            if length < 0.01:
-            u_out, len_out = segments[i]
+                direction = (0.0, 1.0)
-            wp = waypoints[i]
+            else:
+                direction = (dx / length, dy / length)
 
-            dot = u_in[0] * u_out[0] + u_in[1] * u_out[1]
+            path.append({
-            dot = max(-1.0, min(1.0, dot))
+                'seg': 'straight',
+                'x': waypoints[i][0], 'y': waypoints[i][1],
+                'dir': direction,
+            })
 
-            # 銳角：hover + 原地轉向
+            if 0 < i < num_wps - 1:
-            if dot < sharp_turn_cos_threshold:
-                avg_dx = u_in[0] + u_out[0]
-                avg_dy = u_in[1] + u_out[1]
-                avg_len = math.hypot(avg_dx, avg_dy)
-                if avg_len > 0.01:
-                    avg_dir = (avg_dx / avg_len, avg_dy / avg_len)
-                else:
-                    avg_dir = u_in
-                path.append({
-                    'seg': 'straight',
-                    'x': wp[0], 'y': wp[1],
-                    'dir': avg_dir,
-                })
                 barrier_indices.append(len(path) - 1)
-                continue
 
-            # 平滑弧
-            beta = math.acos(dot)
-            if beta < 1e-4:
-                # 幾乎直線，不插弧
-                path.append({
-                    'seg': 'straight',
-                    'x': wp[0], 'y': wp[1],
-                    'dir': u_in,
-                })
-                continue
-
-            half_tan = math.tan(beta / 2.0)
-            d_ideal = R_base * half_tan
-            d_max = 0.45 * min(len_in, len_out)
-            d = min(d_ideal, d_max)
-            R_actual = d / half_tan
-
-            T_in = (wp[0] - u_in[0] * d, wp[1] - u_in[1] * d)
-            T_out = (wp[0] + u_out[0] * d, wp[1] + u_out[1] * d)
-
-            # +1 CCW (左轉) / -1 CW (右轉)
-            cross = u_in[0] * u_out[1] - u_in[1] * u_out[0]
-            sign = 1 if cross > 0 else -1
-
-            # 入向左法線
-            n_left = (-u_in[1], u_in[0])
-            center = (
-                T_in[0] + R_actual * sign * n_left[0],
-                T_in[1] + R_actual * sign * n_left[1],
-            )
-
-            theta_in = math.atan2(T_in[1] - center[1], T_in[0] - center[0])
-
-            # T_in (straight, 方向 u_in)
-            path.append({
-                'seg': 'straight',
-                'x': T_in[0], 'y': T_in[1],
-                'dir': u_in,
-            })
-
-            # 弧段採樣 k=1..arc_resolution-1（不含兩端）
-            for k in range(1, arc_resolution):
-                t = k / arc_resolution
-                theta = theta_in + sign * beta * t
-                px = center[0] + R_actual * math.cos(theta)
-                py = center[1] + R_actual * math.sin(theta)
-                # 切線: sign × (-sin θ, cos θ)
-                tx = sign * (-math.sin(theta))
-                ty = sign * math.cos(theta)
-                path.append({
-                    'seg': 'arc',
-                    'x': px, 'y': py,
-                    'dir': (tx, ty),
-                    'arc_center': center,
-                    'arc_R': R_actual,
-                    'arc_sign': sign,
-                    'theta': theta,
-                })
-
-            # T_out (straight, 方向 u_out)，barrier 放這
-            path.append({
-                'seg': 'straight',
-                'x': T_out[0], 'y': T_out[1],
-                'dir': u_out,
-            })
-            barrier_indices.append(len(path) - 1)
-
-        # 終點
-        path.append({
-            'seg': 'straight',
-            'x': waypoints[-1][0], 'y': waypoints[-1][1],
-            'dir': segments[-1][0],
-        })
-
-        # 後處理：為每個 sample 加上累積 polyline 弧長 s
         path[0]['s'] = 0.0
         for i in range(1, len(path)):
             prev = path[i - 1]

src/GUI/overview_table.py

@@ -6,8 +6,9 @@ class OverviewTable(QTableWidget):
     """總覽表格，顯示所有無人機的狀態資訊"""
     
     # 默認的資訊類型和映射
-    DEFAULT_INFO_TYPES = ["模式", "ARM", "電壓", "經度", "緯度", "高度", "XY位置", "XY速度", "地速", "航向",
+    DEFAULT_INFO_TYPES = ["模式", "ARM", "電壓", "經度", "緯度", "FixType", "衛星數", "EPH", "EPV",
-                          "空速", "油門", "海拔高度", "爬升率", "Roll", "Pitch", "Yaw", "丟包", "延遲"]
+                          "高度", "XY位置", "XY速度", "地速", "航向", "空速", "油門", "海拔高度",
+                          "爬升率", "Roll", "Pitch", "Yaw", "丟包", "延遲"]
     
     DEFAULT_INFO_TYPE_MAP = {
         "mode": 0,
@@ -15,20 +16,24 @@ class OverviewTable(QTableWidget):
         "battery": 2,
         "longitude": 3,
         "latitude": 4,
-        "altitude": 5,
+        "fix_type": 5,
-        "local": 6,
+        "satellites_visible": 6,
-        "velocity": 7,
+        "eph": 7,
-        "groundspeed": 8,
+        "epv": 8,
-        "heading": 9,
+        "altitude": 9,
-        "airspeed": 10,
+        "local": 10,
-        "throttle": 11,
+        "velocity": 11,
-        "hud_alt": 12,
+        "groundspeed": 12,
-        "climb": 13,
+        "heading": 13,
-        "roll": 14,
+        "airspeed": 14,
-        "pitch": 15,
+        "throttle": 15,
-        "yaw": 16,
+        "hud_alt": 16,
-        "loss_rate": 17,
+        "climb": 17,
-        "ping": 18
+        "roll": 18,
+        "pitch": 19,
+        "yaw": 20,
+        "loss_rate": 21,
+        "ping": 22
     }
     
     def __init__(self, info_types=None, info_type_map=None, parent=None):

src/fc_interfaces/CMakeLists.txt

@@ -9,13 +9,15 @@ endif()
 find_package(ament_cmake REQUIRED)
 find_package(rosidl_default_generators REQUIRED)
 find_package(std_msgs REQUIRED)
+find_package(builtin_interfaces REQUIRED)
 
 rosidl_generate_interfaces(${PROJECT_NAME}
   "msg/AttitudeRaw.msg"
+  "msg/GnssRaw.msg"
   "srv/MavPing.srv"
   "srv/MavCommandLong.srv"
   "srv/MavPositionTargetGlobalInt.srv"
-  DEPENDENCIES std_msgs
+  DEPENDENCIES std_msgs builtin_interfaces
   )
 
 ament_package()

src/fc_interfaces/msg/GnssRaw.msg

@@ -0,0 +1,8 @@
+builtin_interfaces/Time stamp
+float64 latitude
+float64 longitude
+float64 altitude
+uint8 fix_type
+uint8 satellites_visible
+float32 eph
+float32 epv

src/fc_interfaces/package.xml

@@ -17,6 +17,8 @@
 
   <build_depend>std_msgs</build_depend>
   <exec_depend>std_msgs</exec_depend>
+  <build_depend>builtin_interfaces</build_depend>
+  <exec_depend>builtin_interfaces</exec_depend>
 
   <export>
     <build_type>ament_cmake</build_type>

src/fc_network_adapter/fc_network_adapter/fc_network_adapter.md

@@ -176,13 +176,15 @@
     這裡支持了所有 ROS2 框架的對應功能 包涵 
     1. 將載具的串流更新的狀態更新到對應的 topic 
     2. 將地面站與載具互動 mavlink microservice 包裝起來提供 ros2 service 互動介面
+    3. 訂閱外部 RTCM 資料並轉發為 MAVLink GPS_RTCM_DATA 給所有載具
 
 ### **[Class]** fc_ros_manager
-    MAVLink ROS2 節點管理器 管理兩個獨立的 Node
+    MAVLink ROS2 節點管理器 管理三個獨立的 Node
-    會開一個執行緒 讓兩個 Node 都跑在裡面
+    會開一個執行緒 讓三個 Node 都跑在裡面
     然後用 spin_once 保持 Node 的活性
 - *self.status_publisher* 物件實例
 - *self.command_service* 物件實例
+- *self.rtcm_relay* 物件實例
 - *self.spin_thread* 執行緒
 ---
 - *start()* 啟動自己
@@ -210,6 +212,20 @@
 - *__init__()* 這邊登入要創建的 service 到 ros2 系統
 - *handle_XXX()* 這邊是實現 service 的具體方法
 
+### **[Class]** RtcmRelay
+    訂閱 /fc_network/rtcm_in (mavros_msgs/msg/RTCM) 並轉發為 MAVLink GPS_RTCM_DATA
+    topic 的 QoS：BEST_EFFORT / KEEP_LAST depth=1 / VOLATILE（低延遲優先，丟舊包不排隊）
+    處理順序：過期丟棄(1s) → blake2s hash 去重 → 最短轉發間隔節流 → 分片轉發
+    分片：GPS_RTCM_DATA 每片 180 bytes，超過自動分片，最多 4 片 (720 bytes)，超長直接丟棄
+    失敗語意：不做舊包重送，送失敗就放棄等下一包
+- *self.counters* 觀測用計數器 (received/forwarded/dropped_stale/dropped_duplicate/dropped_throttle/dropped_oversize/no_route/send_fail)
+- *self.clock_offset_sec* 與 RTCM 來源的時鐘偏移量（秒）
+---
+- *_rtcm_callback()* 收到 RTCM 封包時的處理流程
+- *_send_rtcm()* 對單一載具編碼並送出 GPS_RTCM_DATA（含分片邏輯）
+- *set_clock_offset()* 設定時鐘偏移量
+- *get_counters()* 取得觀測計數器
+
 
 # 開發記錄
 
@@ -225,6 +241,7 @@
 9. 載具狀態收集與彙整
 10. a. ros2 topic 應用開發介面 (基礎框架)
     b. ros2 service 應用開發介面 (基礎框架)
+11. RTCM 轉發 - 訂閱 RTCM topic 轉為 MAVLink GPS_RTCM_DATA 廣播給所有載具
 
 ### 待開發功能
 5-1. 建立 serial 連線 並可以對接收器下達AT指令

src/fc_network_adapter/fc_network_adapter/mainOrchestrator.py

@@ -1145,9 +1145,9 @@ class Orchestrator:
         self.fc_ros_manager = mros.ros2_manager
         self.fc_ros_manager.initialize()
         self.fc_ros_manager.status_publisher.rate_controller.topic_intervals = {
-            'position': 1.0,
+            'position_gnss': 0.2,
-            'position_ned': 1.0,
+            'position_ned': 0.5,
-            'attitude': 1.0,
+            'attitude': 0.1,
             'velocity': 0.0,
             'battery': 1.0,
             'vfr_hud': 1.0,

src/fc_network_adapter/fc_network_adapter/mavlinkObject.py

@@ -109,6 +109,7 @@ class mavlink_bridge:
         """初始化訊息處理器映射表，提高處理效率"""
         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
@@ -239,6 +240,14 @@ class mavlink_bridge:
         component.status.position.relative_altitude = msg.relative_alt / 1000.0  # 轉換為公尺
         component.status.position.timestamp = timestamp
 
+    def _handle_gps_raw_int(self, vehicle, component, msg, timestamp):
+        """處理 GPS_RAW_INT 訊息 (msg_id: 24)"""
+        component.status.gps.fix_type = msg.fix_type
+        component.status.gps.satellites_visible = msg.satellites_visible
+        component.status.gps.eph = None if msg.eph == 65535 else msg.eph
+        component.status.gps.epv = None if msg.epv == 65535 else msg.epv
+        component.status.gps.timestamp = timestamp
+
     def _handle_vfr_hud(self, vehicle, component, msg, timestamp):
         """處理 VFR_HUD 訊息 (msg_id: 74)"""
         component.status.vfr.airspeed = msg.airspeed
@@ -386,7 +395,9 @@ class mavlink_object:
             self.MAVLink = mav_common.MAVLink(self.mavlinkPipeline, srcSystem=254, srcComponent=191)
         
         # 記錄訊息過濾類型 (可選)
-        self.bridge_msg_types = set([0, 30, 32, 33, 74, 147])  # 0 HEARTBEAT, 30 ATTITUDE, 33 GLOBAL_POSITION_INT, 74 VFR_HUD, 147 BATTERY_STATUS, 32 LOCAL_POSITION_NED 
+        # 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])  
         self.return_msg_types = set([])
 
         # 轉發到別的 mavlink object 作為目標端口 的列表

src/fc_network_adapter/fc_network_adapter/mavlinkROS2Nodes.py

@@ -1,11 +1,13 @@
 """
 MAVLink ROS2 Nodes
-主要包含兩個獨立的 ROS2 Node :
+主要包含三個獨立的 ROS2 Node :
 1. VehicleStatusPublisher - 發布載具狀態到 ROS2 topics
     從 vehicle_registry 讀取狀態數據，頻率控制，模組化設計
 2. MavlinkCommandService - 提供 MAVLink 指令 service 介面
     以最基礎的 mavlink microservice 會實現目標 
     並不會包含額外的功能
+3. RtcmRelay - 訂閱 RTCM topic 並轉發為 MAVLink GPS_RTCM_DATA 給所有載具
+    過期丟棄、去重、節流、分片
 
 與一個節點管理器
 - fc_ros_manager
@@ -15,6 +17,7 @@ MAVLink ROS2 Nodes
 import os
 import time
 import math
+import hashlib
 import threading
 from typing import Dict, Optional
 
@@ -22,6 +25,7 @@ from typing import Dict, Optional
 import rclpy
 from rclpy.node import Node
 from rclpy.executors import MultiThreadedExecutor
+from rclpy.qos import QoSProfile, ReliabilityPolicy, HistoryPolicy, DurabilityPolicy
 
 # ROS2 Message imports
 import std_msgs.msg
@@ -61,7 +65,7 @@ class PublishRateController:
         # 注意 這邊是定義區 不要把參數寫在這裡 所以預設全部關閉
         # 以這個專案 請看 mainOrchestrator.py 的 Orchestrator 初始化階段
         self.topic_intervals = {
-            'position': 0.0,       # GNSS位置
+            'position_gnss': 0.0,  # GNSS位置
             'position_ned': 0.0,   # LOCAL_POSITION_NED (位置+速度)
             'attitude': 0.0,       # 姿態 (pitch yaw row 與其加速狀態)
             'velocity': 0.0,       # 速度 (已經包含在 vfr_hud 未來移除)
@@ -131,8 +135,8 @@ class VehicleStatusPublisher(Node):
         self.fc_publishers: Dict[tuple, any] = {}
         
         # 定時器：以較高頻率檢查 vehicle_registry 並發布
-        # 10Hz 檢查頻率，但通過 rate_controller 控制實際發布頻率
+        # 20Hz 檢查頻率，但通過 rate_controller 控制實際發布頻率
-        self.timer_period = 0.1  # 100ms
+        self.timer_period = 0.05  # 50ms
         self.timer = self.create_timer(self.timer_period, self.timer_callback)
         
         # 狀態標誌
@@ -171,7 +175,7 @@ class VehicleStatusPublisher(Node):
         # 例如：self._publish_ekf_status(sysid, status)
         # ═══════════════════════════════════════════════════════════════
         # 發布各種狀態（通過頻率控制器判斷是否發布）
-        self._publish_position(sysid, status)
+        self._publish_position_gnss(sysid, status)
         self._publish_position_ned(sysid, status)
         self._publish_attitude(sysid, status)
         self._publish_velocity(sysid, status)
@@ -202,9 +206,9 @@ class VehicleStatusPublisher(Node):
             logger.info(f"Created publisher: {topic_name}")
         return self.fc_publishers[key]
     
-    def _publish_position(self, sysid: int, status: mvv.ComponentStatus):
+    def _publish_position_gnss(self, sysid: int, status: mvv.ComponentStatus):
         """發布 GPS 位置"""
-        if not self.rate_controller.should_publish(sysid, 'position'):
+        if not self.rate_controller.should_publish(sysid, 'position_gnss'):
             return
         
         pos = status.position
@@ -212,22 +216,27 @@ class VehicleStatusPublisher(Node):
             return
         
         publisher = self._get_or_create_publisher(
-            sysid, 'position', sensor_msgs.msg.NavSatFix
+            sysid, 'position_gnss', fcmsg.GnssRaw
             )
         
         # 檢查是否有訂閱者
         if publisher.get_subscription_count() == 0:
             return
         
-        msg = sensor_msgs.msg.NavSatFix()
+        msg = fcmsg.GnssRaw()
-        msg.latitude = pos.latitude
+        msg.stamp = self.get_clock().now().to_msg()
-        msg.longitude = pos.longitude
+        msg.latitude = float(pos.latitude)
-        msg.altitude = pos.altitude if pos.altitude is not None else 0.0
+        msg.longitude = float(pos.longitude)
+        msg.altitude = float(pos.altitude) if pos.altitude is not None else 0.0
         
         # GPS 狀態資訊
         gps = status.gps
         if gps.fix_type is not None:
-            msg.status.status = gps.fix_type - 1  # MAVLink fix_type 轉 NavSatStatus
+            msg.fix_type = int(gps.fix_type)
+        if gps.satellites_visible is not None:
+            msg.satellites_visible = int(gps.satellites_visible)
+        msg.eph = float(gps.eph) / 100.0 if gps.eph is not None else float('nan')
+        msg.epv = float(gps.epv) / 100.0 if gps.epv is not None else float('nan')
         
         publisher.publish(msg)
 
@@ -431,7 +440,7 @@ class VehicleStatusPublisher(Node):
             # '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,
+            # 'gps_fix': status.gps.fix_type if status.gps.fix_type else 0,
             'connection_type': vehicle.connected_via.value,
             'last_update': component.packet_stats.last_msg_time if component.packet_stats.last_msg_time else 0.0,
         }
@@ -950,6 +959,179 @@ class MavlinkCommandService(Node):
         logger.info("MavlinkCommandService stopped")
 
 
+# ============================================================================
+#                          RtcmRelay Node
+# ============================================================================
+
+class RtcmRelay(Node):
+    """
+    RTCM 轉發節點 - 訂閱 RTCM topic 並轉發為 MAVLink GPS_RTCM_DATA 給所有載具
+
+    職責:
+    - 訂閱外部 RTCM 資料（mavros_msgs/msg/RTCM）
+    - 過期丟棄：封包 Header.stamp 超過 1 秒則丟棄
+    - 去重：與上一筆 payload blake2s hash 相同則丟棄
+    - 節流：最短轉發間隔保護（可參數化）
+    - 對所有在線載具直接編碼並送出 MAVLink GPS_RTCM_DATA（含分片）
+
+    MAVLink GPS_RTCM_DATA (msg_id=233) 每片 payload 上限 180 bytes
+    超過 180 bytes 需分片，最多 4 片 = 720 bytes
+    """
+
+    FRAGMENT_MAX_LEN = 180
+    MAX_TOTAL_LEN = 4 * 180  # 720 bytes
+    STALE_THRESHOLD_SEC = 1.0
+
+    def __init__(self, min_forward_interval_ms: float = 0.0, clock_offset_ms: float = 0.0):
+        super().__init__('rtcm_relay')
+
+        self.running = True
+        self.min_forward_interval_sec = min_forward_interval_ms / 1000.0
+        self.clock_offset_sec = clock_offset_ms / 1000.0
+
+        # 去重：上一筆 payload 的短 hash
+        self._last_hash: Optional[bytes] = None
+
+        # 節流：上次成功轉發的時間
+        self._last_forward_time: float = 0.0
+
+        # MAVLink GPS_RTCM_DATA 分片序號 (0~31 循環，同一筆 RTCM 的所有分片共用)
+        self._seq: int = 0
+
+        # 觀測 counters
+        self.counters = {
+            'received': 0,
+            'forwarded': 0,
+            'dropped_stale': 0,
+            'dropped_duplicate': 0,
+            'dropped_throttle': 0,
+            'dropped_oversize': 0,
+            'no_route': 0,
+            'send_fail': 0,
+        }
+
+        # QoS: 低延遲、弱可靠 — 寧可掉舊包也不排隊
+        rtcm_qos = QoSProfile(
+            history=HistoryPolicy.KEEP_LAST,
+            depth=1,
+            reliability=ReliabilityPolicy.BEST_EFFORT,
+            durability=DurabilityPolicy.VOLATILE,
+        )
+
+        self.subscription = self.create_subscription(
+            mavros_msgs.msg.RTCM,
+            '/fc_network/rtcm_input',
+            self._rtcm_callback,
+            rtcm_qos,
+        )
+
+        logger.info("RtcmRelay initialized")
+
+    # ── callback ──────────────────────────────────────────────────────
+
+    def _rtcm_callback(self, msg):
+        """處理順序：過期檢查 → 去重 → 節流 → 轉發"""
+        if not self.running:
+            return
+
+        self.counters['received'] += 1
+        data = bytes(msg.data)
+        now = time.time()
+
+        # 1) 過期丟棄
+        stamp_sec = msg.header.stamp.sec + msg.header.stamp.nanosec * 1e-9
+        age = (now + self.clock_offset_sec) - stamp_sec
+        if age > self.STALE_THRESHOLD_SEC:
+            self.counters['dropped_stale'] += 1
+            return
+
+        # 2) 去重：blake2s 短 hash 與上一筆比對
+        h = hashlib.blake2s(data, digest_size=8).digest()
+        if h == self._last_hash:
+            self.counters['dropped_duplicate'] += 1
+            return
+        self._last_hash = h
+
+        # 3) 節流：最短轉發間隔
+        if self.min_forward_interval_sec > 0:
+            if (now - self._last_forward_time) < self.min_forward_interval_sec:
+                self.counters['dropped_throttle'] += 1
+                return
+
+        # 4) 超長丟棄
+        if len(data) > self.MAX_TOTAL_LEN:
+            self.counters['dropped_oversize'] += 1
+            logger.warning(f"RTCM data too large ({len(data)} bytes), max {self.MAX_TOTAL_LEN}")
+            return
+
+        # 5) 取得本次的分片序號，然後遞增給下一筆
+        seq = self._seq
+        self._seq = (self._seq + 1) & 0x1F
+
+        # 6) 轉發給所有載具
+        forwarded_any = False
+        for sysid, vehicle in mvv.vehicle_registry.read_all():
+            socket_id = vehicle.custom_meta.get("socket_id")
+            if socket_id is None:
+                self.counters['no_route'] += 1
+                continue
+
+            mav_obj = mo.mavlink_object.mavlinkObjects.get(socket_id)
+            if mav_obj is None:
+                self.counters['no_route'] += 1
+                continue
+
+            try:
+                self._send_rtcm(mav_obj, data, seq)
+                forwarded_any = True
+            except Exception as e:
+                self.counters['send_fail'] += 1
+                logger.debug(f"RTCM send fail sysid={sysid}: {e}")
+
+        if forwarded_any:
+            self.counters['forwarded'] += 1
+            self._last_forward_time = now
+
+    # ── MAVLink 編碼與分片 ────────────────────────────────────────────
+
+    def _send_rtcm(self, mav_obj, data: bytes, seq: int):
+        """
+        對單一載具送出 GPS_RTCM_DATA 必要時做分片
+
+        GPS_RTCM_DATA flags 欄位:
+          bit 0     : 1=fragmented
+          bit 1-2   : fragment_id (0~3)
+          bit 3-7   : sequence number (0~31)
+        """
+        if len(data) <= self.FRAGMENT_MAX_LEN:
+            flags = (seq & 0x1F) << 3
+            padded = data + b'\x00' * (self.FRAGMENT_MAX_LEN - len(data))
+            mav_obj.MAVLink.gps_rtcm_data_send(flags, len(data), padded)
+        else:
+            fragments = [
+                data[i:i + self.FRAGMENT_MAX_LEN]
+                for i in range(0, len(data), self.FRAGMENT_MAX_LEN)
+            ]
+            for frag_id, frag_data in enumerate(fragments):
+                flags = 1 | ((frag_id & 0x03) << 1) | ((seq & 0x1F) << 3)
+                padded = frag_data + b'\x00' * (self.FRAGMENT_MAX_LEN - len(frag_data))
+                mav_obj.MAVLink.gps_rtcm_data_send(flags, len(frag_data), padded)
+
+    # ── 外部介面 ──────────────────────────────────────────────────────
+
+    def set_clock_offset(self, offset_ms: float):
+        """更新時鐘偏移量（毫秒），用於與 RTCM 來源的時間對齊"""
+        self.clock_offset_sec = offset_ms / 1000.0
+
+    def get_counters(self) -> dict:
+        return dict(self.counters)
+
+    def stop(self):
+        """停止轉發"""
+        self.running = False
+        logger.info("RtcmRelay stopped")
+
+
 # ============================================================================
 #                         ROS2 節點管理器
 # ============================================================================
@@ -963,9 +1145,10 @@ class fc_ros_manager:
     stop 是停下止 ROS2 nodes 的運行，但不銷毀節點實例，允許後續再次 start。
     shutdown 是完全關閉 ROS2 並銷毀節點實例。
 
-    管理兩個獨立的 ROS2 Node：
+    管理三個獨立的 ROS2 Node：
     - VehicleStatusPublisher
     - MavlinkCommandService
+    - RtcmRelay
     
     提供統一的啟動/停止介面給 mainOrchestrator
     """
@@ -974,9 +1157,10 @@ class fc_ros_manager:
         self.initialized = False
         self.running = False
         
-        # 两个 node 實例
+        # node 實例
         self.status_publisher: Optional[VehicleStatusPublisher] = None
         self.command_service: Optional[MavlinkCommandService] = None
+        self.rtcm_relay: Optional[RtcmRelay] = None
         
         # Executor & Thread
         self.spin_thread: Optional[threading.Thread] = None
@@ -996,11 +1180,13 @@ class fc_ros_manager:
             # 創建節點 node
             self.status_publisher = VehicleStatusPublisher()
             self.command_service = MavlinkCommandService()
+            self.rtcm_relay = RtcmRelay()
     
             # 創建執行者 MultiThreadedExecutor 並把 node 加入其中
             self.executor = MultiThreadedExecutor()
             self.executor.add_node(self.status_publisher)
             self.executor.add_node(self.command_service)
+            self.executor.add_node(self.rtcm_relay)
             
             self.initialized = True
             # logger.info("fc_ros_manager initialized")
@@ -1064,6 +1250,8 @@ class fc_ros_manager:
                 self.status_publisher.stop()
             if self.command_service:
                 self.command_service.stop()
+            if self.rtcm_relay:
+                self.rtcm_relay.stop()
             
             # 等待 spin 執行緒結束
             if self.spin_thread and self.spin_thread.is_alive():
@@ -1088,6 +1276,8 @@ class fc_ros_manager:
                     self.status_publisher.destroy_node()
                 if self.command_service:
                     self.command_service.destroy_node()
+                if self.rtcm_relay:
+                    self.rtcm_relay.destroy_node()
                 
                 # 關閉 ROS2
                 if rclpy.ok():
@@ -1105,6 +1295,7 @@ class fc_ros_manager:
             'running': self.running,
             'status_publisher_active': self.status_publisher is not None and self.status_publisher.running,
             'command_service_active': self.command_service is not None,
+            'rtcm_relay_active': self.rtcm_relay is not None and self.rtcm_relay.running,
         }
 
 
@@ -1135,6 +1326,11 @@ ros2_manager = fc_ros_manager()
 3. 新增 _publish_position_ned() 發布剛體座標的訊息
 4. 重製 _publish_attitude() 使其更簡單的去發布姿態訊息
 
+2026.05.11
+1. 新增 RtcmRelay node - 訂閱 mavros_msgs/RTCM topic 轉發為 MAVLink GPS_RTCM_DATA
+2. 實作過期丟棄(1s)、blake2s hash 去重、最短轉發間隔節流、分片(180 bytes/片)
+3. 接入 fc_ros_manager 的 initialize/start/stop/shutdown 生命週期
+
 TODO 
 1. service 部分會需要跟 mavlinkobject 大量互動 也許需要考慮對方的生命週期
 2. 還沒測試 如果有失效狀態讓 fc_ros_manager 中斷了 如何恢復的問題

src/fc_network_adapter/tests/test_vehicleStatusPublisher.py

@@ -17,6 +17,7 @@ import std_msgs.msg
 import sensor_msgs.msg
 import geometry_msgs.msg
 import mavros_msgs.msg
+import fc_interfaces.msg
 
 # 專案 imports
 from ..fc_network_adapter.mavlinkROS2Nodes import (
@@ -39,7 +40,7 @@ class TestSubscriber(Node):
         
         self.sysid = sysid
         self.received_messages = {
-            'position': [],
+            'position_gnss': [],
             'attitude': [],
             'velocity': [],
             'battery': [],
@@ -60,9 +61,9 @@ class TestSubscriber(Node):
         
         # Position
         self.create_subscription(
-            sensor_msgs.msg.NavSatFix,
+            fc_interfaces.msg.GnssRaw,
-            f'{base_topic}/position',
+            f'{base_topic}/position_gnss',
-            lambda msg: self._on_message('position', msg),
+            lambda msg: self._on_message('position_gnss', msg),
             10
         )
         
@@ -121,7 +122,7 @@ class TestSubscriber(Node):
     
     def _format_msg(self, topic_name: str, msg) -> str:
         """格式化消息以便顯示"""
-        if topic_name == 'position':
+        if topic_name == 'position_gnss':
             return f"lat={msg.latitude:.6f}, lon={msg.longitude:.6f}, alt={msg.altitude:.2f}m"
         elif topic_name == 'attitude':
             return f"quat=({msg.orientation.x:.3f}, {msg.orientation.y:.3f}, {msg.orientation.z:.3f}, {msg.orientation.w:.3f})"
@@ -264,7 +265,7 @@ def test_basic_publishing():
     # 檢查收到的消息
     print("\n--- 消息統計 ---")
     total_messages = 0
-    for topic in ['position', 'attitude', 'velocity', 'battery', 'vfr_hud', 'mode', 'summary']:
+    for topic in ['position_gnss', 'attitude', 'velocity', 'battery', 'vfr_hud', 'mode', 'summary']:
         count = test_sub.get_message_count(topic)
         total_messages += count
         print(f"  {topic:15s}: {count:3d} 條消息")
@@ -304,7 +305,7 @@ def test_frequency_control():
     # 修改頻率設定
     publisher_node = ros2_manager.status_publisher
     publisher_node.rate_controller.topic_intervals = {
-        'position': 1.5,   
+        'position_gnss': 1.5,   
         'attitude': 1.0,    
         'velocity': 1.0,     
         'battery': 1.0,   
@@ -329,7 +330,7 @@ def test_frequency_control():
     print("預期：position 約 2 條 (0.67Hz)，attitude/battery/velocity 約 3 條 (1Hz)，vfr_hud 約 6 條 (2Hz) mode/summary 不發布")
     
     print("2Hz Topics (預期 ~6 條):")
-    for topic in ['position', 'attitude', 'velocity', 'vfr_hud']:
+    for topic in ['position_gnss', 'attitude', 'velocity', 'vfr_hud']:
         count = test_sub.get_message_count(topic)
         print(f"  {topic:15s}: {count:3d} 條")
     for topic in ['battery', 'mode', 'summary']:

src/fc_network_module/fc_network_module/ntrip_client.py

@@ -0,0 +1,146 @@
+import socket
+import base64
+import threading
+import time
+
+import rclpy
+from rclpy.node import Node
+from rclpy.qos import QoSProfile, HistoryPolicy, ReliabilityPolicy, DurabilityPolicy
+from mavros_msgs.msg import RTCM
+
+
+class NtripClientNode(Node):
+    """連線 NTRIP caster，接收 RTCM3 資料流並發布至 ROS2 topic。"""
+
+    RECONNECT_BASE_SEC = 2.0
+    RECONNECT_MAX_SEC = 60.0
+
+    def __init__(self):
+        super().__init__('ntrip_client')
+
+        self.declare_parameter('host', 'rtk2go.com')
+        self.declare_parameter('port', 2101)
+        self.declare_parameter('mountpoint', '')
+        self.declare_parameter('username', '')
+        self.declare_parameter('password', '')
+
+        rtcm_qos = QoSProfile(
+            history=HistoryPolicy.KEEP_LAST,
+            depth=1,
+            reliability=ReliabilityPolicy.BEST_EFFORT,
+            durability=DurabilityPolicy.VOLATILE,
+        )
+        self.publisher_ = self.create_publisher(RTCM, '/fc_network/rtcm_input', rtcm_qos)
+
+        self._running = True
+        self._thread = threading.Thread(target=self._receive_loop, daemon=True)
+        self._thread.start()
+
+        self.get_logger().info('NtripClientNode 已啟動')
+
+    # ── NTRIP 連線 + RTCM 接收迴圈 ──────────────────────────────
+
+    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
+
+            if not mount:
+                self.get_logger().error('mountpoint 參數未設定，等待重試...')
+                time.sleep(backoff)
+                continue
+
+            sock = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
+            sock.settimeout(10)
+
+            try:
+                self.get_logger().info(f'正在連線 {host}:{port}/{mount} ...')
+                sock.connect((host, port))
+
+                auth = base64.b64encode(f'{user}:{pwd}'.encode()).decode()
+                request = (
+                    f'GET /{mount} 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'
+                )
+                sock.sendall(request.encode())
+
+                resp = sock.recv(4096)
+                if b'ICY 200 OK' not in resp:
+                    self.get_logger().error(
+                        f'NTRIP 握手失敗: {resp.decode(errors="ignore").strip()}'
+                    )
+                    raise ConnectionError('handshake failed')
+
+                self.get_logger().info(f'已連接至 {mount}，開始接收 RTCM 資料流')
+                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)
+            finally:
+                sock.close()
+
+    def _read_stream(self, sock: socket.socket):
+        buf = b''
+
+        while self._running:
+            try:
+                chunk = sock.recv(4096)
+            except socket.timeout:
+                continue
+            if not chunk:
+                break
+            buf += chunk
+
+            while len(buf) >= 6:
+                if buf[0] != 0xD3:
+                    buf = buf[1:]
+                    continue
+
+                payload_len = ((buf[1] & 0x03) << 8) | buf[2]
+                frame_len = payload_len + 6  # header(3) + payload + CRC(3)
+
+                if len(buf) < frame_len:
+                    break
+
+                packet = buf[:frame_len]
+                buf = buf[frame_len:]
+
+                self._publish_rtcm(packet)
+
+    def _publish_rtcm(self, raw_packet: bytes):
+        msg = RTCM()
+        msg.header.stamp = self.get_clock().now().to_msg()
+        msg.data = list(raw_packet)
+        self.publisher_.publish(msg)
+
+    def destroy_node(self):
+        self._running = False
+        self._thread.join(timeout=3.0)
+        super().destroy_node()
+
+
+def main(args=None):
+    rclpy.init(args=args)
+    node = NtripClientNode()
+    try:
+        rclpy.spin(node)
+    except KeyboardInterrupt:
+        pass
+    finally:
+        node.destroy_node()
+        rclpy.shutdown()
+
+if __name__ == '__main__':
+    main()

src/fc_network_module/package.xml

@@ -0,0 +1,22 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>fc_network_module</name>
+  <version>0.0.0</version>
+  <description>TODO: Package description</description>
+  <maintainer email="chiyu1468@hotmail.com">picars</maintainer>
+  <license>TODO: License declaration</license>
+
+  <depend>rclpy</depend>
+  <depend>mavros_msgs</depend>
+  <depend>std_msgs</depend>
+
+  <test_depend>ament_copyright</test_depend>
+  <test_depend>ament_flake8</test_depend>
+  <test_depend>ament_pep257</test_depend>
+  <test_depend>python3-pytest</test_depend>
+
+  <export>
+    <build_type>ament_python</build_type>
+  </export>
+</package>

src/fc_network_module/setup.cfg

@@ -0,0 +1,4 @@
+[develop]
+script_dir=$base/lib/fc_network_module
+[install]
+install_scripts=$base/lib/fc_network_module

src/fc_network_module/setup.py

@@ -0,0 +1,26 @@
+from setuptools import find_packages, setup
+
+package_name = 'fc_network_module'
+
+setup(
+    name=package_name,
+    version='0.0.0',
+    packages=find_packages(exclude=['test']),
+    data_files=[
+        ('share/ament_index/resource_index/packages',
+            ['resource/' + package_name]),
+        ('share/' + package_name, ['package.xml']),
+    ],
+    install_requires=['setuptools'],
+    zip_safe=True,
+    maintainer='picars',
+    maintainer_email='chiyu1468@hotmail.com',
+    description='TODO: Package description',
+    license='TODO: License declaration',
+    tests_require=['pytest'],
+    entry_points={
+        'console_scripts': [
+            'ntrip_client = fc_network_module.ntrip_client:main',
+        ],
+    },
+)

src/fc_network_module/test/test_copyright.py

@@ -0,0 +1,25 @@
+# Copyright 2015 Open Source Robotics Foundation, Inc.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#     http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+from ament_copyright.main import main
+import pytest
+
+
+# Remove the `skip` decorator once the source file(s) have a copyright header
+@pytest.mark.skip(reason='No copyright header has been placed in the generated source file.')
+@pytest.mark.copyright
+@pytest.mark.linter
+def test_copyright():
+    rc = main(argv=['.', 'test'])
+    assert rc == 0, 'Found errors'

src/fc_network_module/test/test_flake8.py

@@ -0,0 +1,25 @@
+# Copyright 2017 Open Source Robotics Foundation, Inc.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#     http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+from ament_flake8.main import main_with_errors
+import pytest
+
+
+@pytest.mark.flake8
+@pytest.mark.linter
+def test_flake8():
+    rc, errors = main_with_errors(argv=[])
+    assert rc == 0, \
+        'Found %d code style errors / warnings:\n' % len(errors) + \
+        '\n'.join(errors)

src/fc_network_module/test/test_pep257.py

@@ -0,0 +1,23 @@
+# Copyright 2015 Open Source Robotics Foundation, Inc.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#     http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+from ament_pep257.main import main
+import pytest
+
+
+@pytest.mark.linter
+@pytest.mark.pep257
+def test_pep257():
+    rc = main(argv=['.', 'test'])
+    assert rc == 0, 'Found code style errors / warnings'

src/unitdev02/unitdev02/devnote.txt

@@ -7,9 +7,17 @@
  - 不同 socket 上面有重複的 sysid 分開儲存 (不做 不允許sysid重複) 
 
 這一步
- 研究 ros2 service
+ 希望 ackEnum 可以擺到正確的位置
+ node 增加 GNSS 訊息 (2d fix 3d fix rtk float...)
+ RTK
+   跟 RTK2go 抓取列表
+   從特定 mount point 得到數據
+   做一個 ros2 service(action?) 接到數據並包裝
+ 
 
 下一步
+  NODE 重啟
+  可以 refresh node topic
 
 下下一步
  
@@ -18,6 +26,8 @@
  rssi 資訊提取s
 
 
+mavlink timeout - CommLONG 這個是 mavlink bus 沒有回應 可能是丟包了
+
 
 自己的常用指令
 python -m fc_network_adapter.tests.test_vehicleStatusPublisher
@@ -27,6 +37,8 @@ python -m fc_network_adapter.fc_network_adapter.mainOrchestrator
 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
+
 ros2 topic list
 ros2 topic echo /fc_network/vehicle/sys3/battery