diff --git a/src/unitdev01/unitdev01/gui.py b/src/unitdev01/unitdev01/gui.py
index 6636fa3..5bc7213 100644
--- a/src/unitdev01/unitdev01/gui.py
+++ b/src/unitdev01/unitdev01/gui.py
@@ -4,7 +4,7 @@ from rclpy.node import Node
 from PyQt6.QtWidgets import (QApplication, QMainWindow, QVBoxLayout, QHBoxLayout,
                             QWidget, QLabel, QSplitter, QScrollArea, 
                             QSizePolicy, QTabWidget, QTableWidget, QTableWidgetItem,
-                            QHeaderView, QPushButton, QCheckBox)
+                            QHeaderView, QPushButton, QCheckBox, QLineEdit)
 from PyQt6.QtCore import Qt, QTimer, QObject, pyqtSignal
 from PyQt6.QtWebEngineWidgets import QWebEngineView
 import math
@@ -18,6 +18,9 @@ from mavros_msgs.msg import State, VfrHud
 from mavros_msgs.srv import CommandBool, CommandTOL, SetMode
 import asyncio
 from functools import partial
+import threading
+import websockets
+import json
 
 class DroneSignals(QObject):
     update_signal = pyqtSignal(str, str, object)  # (msg_type, drone_id, data)
@@ -31,14 +34,99 @@ class DroneMonitor(Node):
         
         self.arm_clients = {}
         self.takeoff_clients = {}
+        self.setpoint_pubs = {}
         self.selected_drones = set()
+        self.latest_data = {}
+
+        # 啟動 WebSocket client 執行緒
+        threading.Thread(target=self.start_ws_client, daemon=True).start()
 
         # 主题检测定时器
         self.create_timer(1.0, self.scan_topics)
+
+    def start_ws_client(self):
+        asyncio.set_event_loop(asyncio.new_event_loop())
+        asyncio.get_event_loop().run_until_complete(self.ws_client_loop())
+
+    async def ws_client_loop(self):
+        retry_count = 0
+        max_retries = 5
+        base_delay = 1.0
+        local = "ws://0.0.0.0:8765"  # 本地 WebSocket 地址
+        remote = "ws://140.120.31.123:8765"
+        while retry_count < max_retries:
+            try:
+                async with websockets.connect(local) as websocket:
+                    print("WebSocket connected")
+                    retry_count = 0  # 重置重試計數
+                    
+                    async for message in websocket:
+                        try:
+                            data = json.loads(message)
+                            print(f"📡 Received: {data}")
+                            self.process_websocket_message(data)
+                        except json.JSONDecodeError as e:
+                            print(f"WebSocket JSON decode error: {e}")
+                        except Exception as e:
+                            print(f"WebSocket message processing error: {e}")
+                            
+            except websockets.exceptions.ConnectionClosedError:
+                print("WebSocket connection closed")
+                break
+            except Exception as e:
+                retry_count += 1
+                delay = base_delay * (2 ** min(retry_count, 4))  # 指數退避
+                print(f"WebSocket connection error: {e}, retrying in {delay}s (attempt {retry_count}/{max_retries})")
+                await asyncio.sleep(delay)
         
+        print("WebSocket client stopped after maximum retries")
+
+
+    def process_websocket_message(self, data):
+        try:
+            drone_id = data.get('system_id')
+            drone_id = f"s9_{drone_id}" if drone_id else None
+            if not drone_id:
+                return
+
+            # 模式
+            if 'mode' in data:
+                self.signals.update_signal.emit('state', drone_id, {
+                    'mode': data['mode'],
+                })
+
+            # 電池
+            if 'battery' in data:
+                # 這裡假設 battery 是百分比
+                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)
+                })
+
+                self.signals.update_signal.emit('local_pose', drone_id, {
+                    'x': round(pos.get('lat', 0), 6),
+                    'y': round(pos.get('lon', 0), 6),
+                })
+
+            # 航向
+            if 'heading' in data:
+                self.signals.update_signal.emit('hud', drone_id, {
+                    'heading': data['heading'],
+                })
+            
+        except Exception as e:
+            print(f"WebSocket message processing error: {e}")
+            
     def scan_topics(self):
         topics = self.get_topic_names_and_types()
-        drone_pattern = re.compile(r'/MavLinkBus/(s\d+)/(\w+)')
+        drone_pattern = re.compile(r'/MavLinkBus/(s\d+_\d+)/(\w+)')
         
         found_drones = set()
         for topic_name, _ in topics:
@@ -55,7 +143,6 @@ class DroneMonitor(Node):
     def setup_drone(self, drone_id):
         base_topic = f'/MavLinkBus/{drone_id}'
         
-
         # Add service clients
         self.arm_clients[drone_id] = self.create_client(
             CommandBool,
@@ -65,7 +152,14 @@ class DroneMonitor(Node):
             CommandTOL,
             f'{base_topic}/cmd/takeoff'
         )
-        
+
+        # Add setpoint publisher
+        self.setpoint_pubs[drone_id] = self.create_publisher(
+            Point,
+            f'{base_topic}/setpoint_position/local',
+            10
+        )
+
         subs = {
             'attitude': self.create_subscription(
                 Imu,
@@ -130,7 +224,6 @@ class DroneMonitor(Node):
         }
         
         setattr(self, f'drone_{drone_id}_subs', subs)
-        self.signals.update_signal.emit('new_drone', drone_id, None)
 
     async def arm_drone(self, drone_id, arm):
         if drone_id not in self.arm_clients:
@@ -171,7 +264,20 @@ class DroneMonitor(Node):
         except Exception as e:
             self.get_logger().error(f'Takeoff service call failed: {e}')
             return False
+
+    def send_setpoint(self, drone_id, x, y, z):
+        """Send setpoint position command"""
+        if drone_id not in self.setpoint_pubs:
+            return False
+            
+        msg = Point()
+        msg.x = float(x)
+        msg.y = float(y)
+        msg.z = float(z)
         
+        self.setpoint_pubs[drone_id].publish(msg)
+        return True
+    
     def quaternion_to_euler(self, q):
         sinr_cosp = 2 * (q.w * q.x + q.y * q.z)
         cosr_cosp = 1 - 2 * (q.x**2 + q.y**2)
@@ -190,71 +296,71 @@ class DroneMonitor(Node):
     def attitude_callback(self, drone_id, msg):
         if hasattr(msg, 'orientation'):
             roll, pitch, yaw = self.quaternion_to_euler(msg.orientation)
-            self.signals.update_signal.emit('attitude', drone_id, {
+            self.latest_data[(drone_id, 'attitude')] = {
                 'roll': roll,
                 'pitch': pitch,
                 'yaw': yaw,
                 'rates': (msg.angular_velocity.x, 
                          msg.angular_velocity.y,
                          msg.angular_velocity.z)
-            })
+            }
 
     def battery_callback(self, drone_id, msg):
-        self.signals.update_signal.emit('battery', drone_id, {
+        self.latest_data[(drone_id, 'battery')] = {
             'voltage': msg.voltage
-        })
+        }
 
     def state_callback(self, drone_id, msg):
-        self.signals.update_signal.emit('state', drone_id, {
+        self.latest_data[(drone_id, 'state')] = {
             'mode': msg.mode,
             'armed': msg.armed
-        })
+        }
 
     def gps_callback(self, drone_id, msg):
-        self.signals.update_signal.emit('gps', drone_id, {
+        self.latest_data[(drone_id, 'gps')] = {
             'lat': msg.latitude,
             'lon': msg.longitude,
             'alt': msg.altitude
-        })
+        }
 
     def local_vel_callback(self, drone_id, msg):
-        self.signals.update_signal.emit('velocity', drone_id, {
+        self.latest_data[(drone_id, 'velocity')] = {
             'vx': msg.x,
             'vy': msg.y,
             'vz': msg.z
-        })
+        }
 
     def altitude_callback(self, drone_id, msg):
-        self.signals.update_signal.emit('altitude', drone_id, {
+        self.latest_data[(drone_id, 'altitude')] = {
             'altitude': msg.data
-        })
+        }
 
     def local_pose_callback(self, drone_id, msg):
-        self.signals.update_signal.emit('local_pose', drone_id, {
+        self.latest_data[(drone_id, 'local_pose')] = {
             'x': msg.x,
             'y': msg.y,
             'z': msg.z
-        })
+        }
 
     def hud_callback(self, drone_id, msg):
-        self.signals.update_signal.emit('hud', drone_id, {
+        self.latest_data[(drone_id, 'hud')] = {
             'airspeed': msg.airspeed,
             'groundspeed': msg.groundspeed,
             'heading': msg.heading,
             'throttle': msg.throttle,
             'alt': msg.altitude,
             'climb': msg.climb
-        })
+        }
 
     def loss_rate_callback(self, drone_id, msg):
-        self.signals.update_signal.emit('loss_rate', drone_id, {
+        self.latest_data[(drone_id, 'loss_rate')] = {
             'loss_rate': msg.data
-        })
+        }
 
     def ping_callback(self, drone_id, msg):
-        self.signals.update_signal.emit('ping', drone_id, {
+        self.latest_data[(drone_id, 'ping')] = {
             'ping': msg.data
-        })
+        }
 
 class ControlStationUI(QMainWindow):
     def __init__(self):
@@ -271,8 +377,14 @@ class ControlStationUI(QMainWindow):
         self.executor = rclpy.executors.SingleThreadedExecutor()
         self.executor.add_node(self.monitor)
         
+        # 定时处理ROS事件
+        self.timer = QTimer()
+        self.timer.timeout.connect(self.spin_ros)
+        self.timer.start(10)
+        
         # 初始化UI
         self.drones = {}
+        self.socket_groups = {}  # 新增：用於儲存 socket 分組
         self.info_types = ["模式", "ARM", "電壓", "高度", "Local", "Velocity", "地速", "航向",
                            "Roll", "Pitch", "Yaw", "丟包", "延遲"]
         self.info_type_map = {
@@ -293,12 +405,15 @@ class ControlStationUI(QMainWindow):
         self.drone_positions = {}
         self.drone_headings = {}
         self.map_loaded = False
+        # 添加地圖更新節流定時器
+        self.map_update_timer = QTimer()
+        self.map_update_timer.timeout.connect(self.update_map_positions)
+        self.map_update_timer.start(200)  # 每 200ms 更新一次地圖
+        # 添加待更新的無人機位置緩存
+        self.pending_map_updates = {}
+        
         self.init_ui()
         
-        # 定时处理ROS事件
-        self.timer = QTimer()
-        self.timer.timeout.connect(self.spin_ros)
-        self.timer.start(50)
 
     def init_ui(self):
 
@@ -323,7 +438,7 @@ class ControlStationUI(QMainWindow):
         
         # 底部控制按鈕區域
         bottom_control = QWidget()
-        bottom_layout = QHBoxLayout(bottom_control)
+        bottom_layout = QVBoxLayout(bottom_control)
         
         select_all_btn = QPushButton("全選")
         select_all_btn.clicked.connect(self.handle_select_all)
@@ -367,10 +482,82 @@ class ControlStationUI(QMainWindow):
             QPushButton:hover { background-color: #555; }
         """)
         
-        bottom_layout.addWidget(select_all_btn)
-        bottom_layout.addWidget(arm_all_btn)
-        bottom_layout.addWidget(takeoff_all_btn)
-        bottom_layout.addStretch()
+        # Add coordinate inputs
+        coord_widget = QWidget()
+        coord_layout = QHBoxLayout(coord_widget)
+        coord_layout.setContentsMargins(0, 0, 0, 0)
+        
+        self.x_input = QLineEdit()
+        self.y_input = QLineEdit()
+        self.z_input = QLineEdit()
+        
+        for input_field in [self.x_input, self.y_input, self.z_input]:
+            input_field.setFixedWidth(60)
+            input_field.setStyleSheet("""
+                QLineEdit {
+                    background-color: #333;
+                    color: #DDD;
+                    border: 1px solid #555;
+                    border-radius: 4px;
+                    padding: 3px;
+                }
+            """)
+        
+        coord_layout.addWidget(QLabel("X:"))
+        coord_layout.addWidget(self.x_input)
+        coord_layout.addWidget(QLabel("Y:"))
+        coord_layout.addWidget(self.y_input)
+        coord_layout.addWidget(QLabel("Z:"))
+        coord_layout.addWidget(self.z_input)
+
+        # Add buttons to control layout
+        setpoint_btn = QPushButton("Setpoint")
+        setpoint_btn.clicked.connect(self.handle_setpoint_selected)
+        setpoint_btn.setStyleSheet("""
+            QPushButton {
+                background-color: #444;
+                color: #DDD;
+                border: none;
+                padding: 5px 10px;
+                border-radius: 4px;
+                min-width: 80px;
+            }
+            QPushButton:hover { background-color: #555; }
+        """)
+
+        # 上方按鈕區域
+        upper_buttons = QHBoxLayout()
+        select_all_btn = QPushButton("全選")
+        select_all_btn.clicked.connect(self.handle_select_all)
+        arm_all_btn = QPushButton("批次解鎖")
+        arm_all_btn.clicked.connect(self.handle_arm_selected)
+        takeoff_all_btn = QPushButton("批次起飛")
+        takeoff_all_btn.clicked.connect(self.handle_takeoff_selected)
+        for btn in [select_all_btn, arm_all_btn, takeoff_all_btn]:
+            btn.setStyleSheet("""
+                QPushButton {
+                    background-color: #444;
+                    color: #DDD;
+                    border: none;
+                    padding: 5px 10px;
+                    border-radius: 4px;
+                    min-width: 80px;
+                }
+                QPushButton:hover { background-color: #555; }
+            """)
+            upper_buttons.addWidget(btn)
+        upper_buttons.addStretch()
+        
+        # 下方座標輸入區域
+        lower_control = QHBoxLayout()
+        lower_control.addWidget(coord_widget)
+        lower_control.addWidget(setpoint_btn)
+        lower_control.addStretch()
+    
+
+        # 將兩個區域加入底部控制區
+        bottom_layout.addLayout(upper_buttons)
+        bottom_layout.addLayout(lower_control)
     
         # — 分頁 2：Overview Table
         self.overview_table = QTableWidget()
@@ -408,9 +595,9 @@ class ControlStationUI(QMainWindow):
             <body>
             <div id="map"></div>
             <script>
-                var map = L.map('map').setView([0, 0], 16);
+                var map = L.map('map').setView([0, 0], 24);
                 L.tileLayer('https://{s}.tile.openstreetmap.org/{z}/{x}/{y}.png', {
-                    maxZoom: 19
+                    maxZoom: 20
                 }).addTo(map);
 
                 var arrowIcon = L.icon({
@@ -419,7 +606,31 @@ class ControlStationUI(QMainWindow):
                     iconAnchor: [20, 20]
                 });
 
+                // 新增 ID 標籤的圖標
+                function createIdIcon(id) {
+                    return L.divIcon({
+                        className: 'drone-id',
+                        html: `<div style="
+                            position: relative;
+                            left: 2px;
+                            background-color: #00DDDD;  // 改為較深的綠色
+                            color: white;
+                            width: 16px;
+                            height: 16px;
+                            border-radius: 50%;
+                            display: flex;
+                            align-items: center;
+                            justify-content: center;
+                            font-weight: bold;
+                            font-size: 10px;
+                            ">${id.substring(1)}</div>`,
+                        iconSize: [20, 20],
+                        iconAnchor: [10, 10]
+                    });
+                }
+
                 var markers = {};                // 儲存所有無人機
+                var idLabels = {};    // 新增：儲存 ID 標籤
                 var focusedId = null;           // 目前被鎖定的 sysid
                 var initialized = false;        // 是否完成首次初始化
 
@@ -429,7 +640,6 @@ class ControlStationUI(QMainWindow):
                     focusedId = id;
                     var latlng = markers[id].getLatLng();
                     map.flyTo(latlng, map.getZoom());
-                    markers[id].openPopup();
                 }
 
                 // 🔁 定期重新鎖定 focusedId
@@ -446,17 +656,28 @@ class ControlStationUI(QMainWindow):
                         markers[id]
                             .setLatLng([lat, lon])
                             .setRotationAngle(heading);
+                        // 更新 ID 標籤位置
+                        idLabels[id].setLatLng([lat, lon]);
                     } else {
                         markers[id] = L.marker([lat, lon], {
                             icon: arrowIcon,
                             rotationAngle: heading,
                             rotationOrigin: 'center'
                         })
-                        .addTo(map)
-                        .bindPopup("UAV #" + id)
                         .on('click', function () {
-                            focusOn(id);  // 使用者點選無人機 → 鎖定
-                        });
+                            focusOn(id);
+                        })
+                        .addTo(map)
+
+                        // 新增 ID 標籤
+                        idLabels[id] = L.marker([lat, lon], {
+                            icon: createIdIcon(id),
+                            zIndexOffset: 1000  // 確保 ID 標籤在箭頭上方
+                        })
+                        .on('click', function() {
+                        focusOn(id);
+                        })
+                        .addTo(map);
 
                         // 🧭 第一次加入 → 若未初始化，則以 sysid 最小的初始化
                         if (!initialized || id < focusedId) {
@@ -497,9 +718,28 @@ class ControlStationUI(QMainWindow):
         else:
             print("⚠️ 地图页加载失败")
 
+    # 修改2: 添加地圖更新節流方法
+    def update_map_positions(self):
+        """批量更新地圖上的無人機位置"""
+        if not self.map_loaded or not self.pending_map_updates:
+            return
+        
+        # 批量執行所有待更新的位置
+        js_commands = []
+        for drone_id, (lat, lon, heading) in self.pending_map_updates.items():
+            js_commands.append(f"updateDrone({lat:.6f}, {lon:.6f}, '{drone_id}', {heading:.1f});")
+        
+        if js_commands:
+            combined_js = "\n".join(js_commands)
+            self.map_view.page().runJavaScript(combined_js)
+            
+        # 清空待更新緩存
+        self.pending_map_updates.clear()
+
     def create_drone_panel(self, drone_id):
         panel = QWidget()
         panel.setObjectName(f"panel_{drone_id}")
+        panel.setFixedHeight(120)  # 根據需要調整高度
         panel.setStyleSheet("""
             QWidget#panel_%s {
                 background-color: #2A2A2A;
@@ -515,9 +755,16 @@ class ControlStationUI(QMainWindow):
             }
         """ % drone_id)
         
-        layout = QVBoxLayout(panel)
-        layout.setContentsMargins(8, 8, 8, 8)
-        layout.setSpacing(4)
+        # 主佈局改為水平佈局
+        main_layout = QHBoxLayout(panel)
+        main_layout.setContentsMargins(8, 8, 8, 8)
+        main_layout.setSpacing(8)
+        
+        # 左側資訊區域
+        info_widget = QWidget()
+        info_layout = QVBoxLayout(info_widget)
+        info_layout.setContentsMargins(0, 0, 0, 0)
+        info_layout.setSpacing(4)
         
         # 顶部标题栏
         header = QWidget()
@@ -532,8 +779,9 @@ class ControlStationUI(QMainWindow):
         
         header_layout.insertWidget(0, checkbox)  # 插入到最前面
 
-        # ID显示
-        id_label = QLabel(drone_id)
+        # ID显示 - 修改這裡：將 s0_4 格式改為 s4 格式
+        display_id = 's' + drone_id.split('_')[1]
+        id_label = QLabel(display_id)
         id_label.setStyleSheet("""
             font-weight: bold; 
             font-size: 14px; 
@@ -541,29 +789,107 @@ class ControlStationUI(QMainWindow):
             min-width: 80px;
         """)
         
-        # 状态指示灯
-        status_light = QLabel("●")
-        status_light.setStyleSheet("color: #00FF00; font-size: 16px;")
-        
         header_layout.addWidget(id_label)
-        header_layout.addWidget(status_light)
         header_layout.addStretch()
         
-        layout.addWidget(header)
+        info_layout.addWidget(header)
+        
+        # 第一行：狀態 (模式 + ARM狀態)
+        status_row = QWidget()
+        status_layout = QHBoxLayout(status_row)
+        status_layout.setContentsMargins(0, 0, 0, 0)
         
-        # 数据字段（带标签）
-        self.create_data_row(layout, "模式:", f"{drone_id}_mode", "--")
-        self.create_data_row(layout, "ARM:", f"{drone_id}_armed", "--")
-        self.create_data_row(layout, "電池:", f"{drone_id}_battery", "--")
-        self.create_data_row(layout, "高度:", f"{drone_id}_altitude", "--")
-        self.create_data_row(layout, "Local:", f"{drone_id}_local", "--")
-        self.create_data_row(layout, "地速:", f"{drone_id}_groundspeed", "--")
-        self.create_data_row(layout, "航向:", f"{drone_id}_heading", "--")
+        status_title = QLabel("狀態：")
+        status_title.setStyleSheet("color: #888; min-width: 50px;")
         
-            # Add control buttons
+        mode_label = QLabel("--")
+        mode_label.setObjectName(f"{drone_id}_mode")
+        
+        armed_label = QLabel("--")
+        armed_label.setObjectName(f"{drone_id}_armed")
+        
+        status_layout.addWidget(status_title)
+        status_layout.addWidget(mode_label)
+        status_layout.addWidget(armed_label)
+        status_layout.addStretch()
+        
+        info_layout.addWidget(status_row)
+        
+        # 第二行：電池
+        battery_row = QWidget()
+        battery_layout = QHBoxLayout(battery_row)
+        battery_layout.setContentsMargins(0, 0, 0, 0)
+        
+        battery_title = QLabel("電池：")
+        battery_title.setStyleSheet("color: #888; min-width: 50px;")
+        
+        battery_label = QLabel("--")
+        battery_label.setObjectName(f"{drone_id}_battery")
+        
+        battery_layout.addWidget(battery_title)
+        battery_layout.addWidget(battery_label)
+        battery_layout.addStretch()
+        
+        info_layout.addWidget(battery_row)
+        
+        # 第三行：位置 + 高度
+        position_row = QWidget()
+        position_layout = QHBoxLayout(position_row)
+        position_layout.setContentsMargins(0, 0, 0, 0)
+        
+        position_title = QLabel("位置：")
+        position_title.setStyleSheet("color: #888; min-width: 50px;")
+        
+        local_label = QLabel("--")
+        local_label.setObjectName(f"{drone_id}_local")
+        
+        altitude_title = QLabel("高度：")
+        altitude_title.setStyleSheet("color: #888; margin-left: 10px;")
+        
+        altitude_label = QLabel("--")
+        altitude_label.setObjectName(f"{drone_id}_altitude")
+        
+        position_layout.addWidget(position_title)
+        position_layout.addWidget(local_label)
+        position_layout.addWidget(altitude_title)
+        position_layout.addWidget(altitude_label)
+        position_layout.addStretch()
+        
+        info_layout.addWidget(position_row)
+        
+        # 第四行：航向 + 速度
+        nav_row = QWidget()
+        nav_layout = QHBoxLayout(nav_row)
+        nav_layout.setContentsMargins(0, 0, 0, 0)
+        
+        heading_title = QLabel("航向：")
+        heading_title.setStyleSheet("color: #888; min-width: 50px;")
+        
+        heading_label = QLabel("--")
+        heading_label.setObjectName(f"{drone_id}_heading")
+        
+        speed_title = QLabel("速度：")
+        speed_title.setStyleSheet("color: #888; margin-left: 10px;")
+        
+        groundspeed_label = QLabel("--")
+        groundspeed_label.setObjectName(f"{drone_id}_groundspeed")
+        
+        nav_layout.addWidget(heading_title)
+        nav_layout.addWidget(heading_label)
+        nav_layout.addWidget(speed_title)
+        nav_layout.addWidget(groundspeed_label)
+        nav_layout.addStretch()
+        
+        info_layout.addWidget(nav_row)
+        
+        # 右側控制按鈕區域
         control_widget = QWidget()
-        control_layout = QHBoxLayout(control_widget)
+        control_layout = QVBoxLayout(control_widget)
         control_layout.setContentsMargins(0, 0, 0, 0)
+        control_layout.setSpacing(6)
+        
+        # 設置控制區域的固定寬度
+        control_widget.setFixedWidth(80)
         
         arm_btn = QPushButton("解鎖")
         arm_btn.setObjectName(f"{drone_id}_arm_btn")
@@ -573,8 +899,10 @@ class ControlStationUI(QMainWindow):
                 background-color: #444;
                 color: #DDD;
                 border: none;
-                padding: 5px 10px;
+                padding: 8px 6px;
                 border-radius: 4px;
+                font-size: 11px;
+                min-height: 24px;
             }
             QPushButton:hover {
                 background-color: #555;
@@ -589,42 +917,95 @@ class ControlStationUI(QMainWindow):
                 background-color: #444;
                 color: #DDD;
                 border: none;
-                padding: 5px 10px;
+                padding: 8px 6px;
                 border-radius: 4px;
+                font-size: 11px;
+                min-height: 24px;
             }
             QPushButton:hover {
                 background-color: #555;
             }
         """)
+
+        setpoint_btn = QPushButton("Setpoint")
+        setpoint_btn.setObjectName(f"{drone_id}_setpoint_btn")
+        setpoint_btn.clicked.connect(lambda: self.handle_single_setpoint(drone_id))
+        setpoint_btn.setStyleSheet("""
+            QPushButton {
+                background-color: #444;
+                color: #DDD;
+                border: none;
+                padding: 8px 6px;
+                border-radius: 4px;
+                font-size: 11px;
+                min-height: 24px;
+            }
+            QPushButton:hover { 
+                background-color: #555; 
+            }
+        """)
         
+        # 按鈕由上而下排列
         control_layout.addWidget(arm_btn)
         control_layout.addWidget(takeoff_btn)
-        control_layout.addStretch()
+        control_layout.addWidget(setpoint_btn)
+        control_layout.addStretch()  # 將按鈕推向頂部
         
-        layout.addWidget(control_widget)
+        # 將左側資訊區域和右側控制區域加入主佈局
+        main_layout.addWidget(info_widget)
+        main_layout.addWidget(control_widget)
         
         return panel
 
-    def handle_arm(self, drone_id):
-        loop = asyncio.get_event_loop()
-        arm_state = not self.monitor.get_arm_state(drone_id)  # Toggle arm state
-        future = self.monitor.arm_drone(drone_id, arm_state)
-        loop.create_task(self.handle_service_response(future, f"{'解鎖' if arm_state else '上鎖'} {drone_id}"))
-
-    def handle_takeoff(self, drone_id):
-        loop = asyncio.get_event_loop()
-        future = self.monitor.takeoff_drone(drone_id, 10.0)  # 10 meters default
-        loop.create_task(self.handle_service_response(future, f"起飛 {drone_id}"))
-
-    async def handle_service_response(self, future, action):
-        try:
-            result = await future
-            if result:
-                self.statusBar().showMessage(f"{action} 成功", 3000)
-            else:
-                self.statusBar().showMessage(f"{action} 失敗", 3000)
-        except Exception as e:
-            self.statusBar().showMessage(f"{action} 錯誤: {str(e)}", 3000)
+    def create_socket_group_panel(self, socket_id):
+        """創建 socket 分組面板"""
+        group_panel = QWidget()
+        group_panel.setObjectName(f"socket_group_{socket_id}")
+        group_panel.setStyleSheet(f"""
+            QWidget#socket_group_{socket_id} {{
+                background-color: #1E1E1E;
+                border: 2px solid #555;
+                border-radius: 12px;
+                margin: 8px;
+                padding: 12px;
+            }}
+            QLabel {{
+                color: #DDD;
+                font-size: 12px;
+                padding: 2px;
+            }}
+        """)
+        
+        layout = QVBoxLayout(group_panel)
+        layout.setContentsMargins(10, 10, 10, 10)
+        layout.setSpacing(6)
+        
+        # Socket 分組標題
+        title_label = QLabel(f"Socket {socket_id}")
+        title_label.setStyleSheet("""
+            font-weight: bold; 
+            font-size: 16px; 
+            color: #FFD700;
+            margin-bottom: 8px;
+            padding: 4px 8px;
+            background-color: #333;
+            border-radius: 6px;
+        """)
+        layout.addWidget(title_label)
+        
+        # 創建子容器用於放置該 socket 下的所有無人機面板
+        drones_container = QWidget()
+        drones_layout = QVBoxLayout(drones_container)
+        drones_layout.setContentsMargins(0, 0, 0, 0)
+        drones_layout.setSpacing(4)
+        
+        layout.addWidget(drones_container)
+        
+        # 儲存容器的引用以便後續添加無人機
+        group_panel.drones_container = drones_container
+        group_panel.drones_layout = drones_layout
+        
+        return group_panel
 
     def create_data_row(self, layout, title, object_name, default):
         row = QWidget()
@@ -646,9 +1027,59 @@ class ControlStationUI(QMainWindow):
         hbox.addWidget(value_label)
         layout.addWidget(row)
 
+    def handle_arm(self, drone_id):
+        loop = asyncio.get_event_loop()
+        arm_state = not self.monitor.get_arm_state(drone_id)  # Toggle arm state
+        future = self.monitor.arm_drone(drone_id, arm_state)
+        loop.create_task(self.handle_service_response(future, f"{'解鎖' if arm_state else '上鎖'} {drone_id}"))
+
+    def handle_takeoff(self, drone_id):
+        loop = asyncio.get_event_loop()
+        future = self.monitor.takeoff_drone(drone_id, 10.0)  # 10 meters default
+        loop.create_task(self.handle_service_response(future, f"起飛 {drone_id}"))
+
+    def handle_setpoint_selected(self):
+        """處理發送 setpoint 命令"""
+        try:
+            x = float(self.x_input.text() or '0')
+            y = float(self.y_input.text() or '0')
+            z = float(self.z_input.text() or '0')
+            
+            for drone_id in self.monitor.selected_drones:
+                if self.monitor.send_setpoint(drone_id, x, y, z):
+                    self.statusBar().showMessage(f"發送位置命令到 {drone_id}: ({x}, {y}, {z})", 3000)
+                else:
+                    self.statusBar().showMessage(f"發送位置命令失敗: {drone_id}", 3000)
+        except ValueError:
+            self.statusBar().showMessage("座標格式錯誤", 3000)
+
+    def handle_single_setpoint(self, drone_id):
+        """處理單個無人機的 setpoint 命令"""
+        try:
+            x = float(self.x_input.text() or '0')
+            y = float(self.y_input.text() or '0')
+            z = float(self.z_input.text() or '0')
+            
+            if self.monitor.send_setpoint(drone_id, x, y, z):
+                self.statusBar().showMessage(f"發送位置命令到 {drone_id}: ({x}, {y}, {z})", 3000)
+            else:
+                self.statusBar().showMessage(f"發送位置命令失敗: {drone_id}", 3000)
+        except ValueError:
+            self.statusBar().showMessage("座標格式錯誤", 3000)
+
+    async def handle_service_response(self, future, action):
+        try:
+            result = await future
+            if result:
+                self.statusBar().showMessage(f"{action} 成功", 3000)
+            else:
+                self.statusBar().showMessage(f"{action} 失敗", 3000)
+        except Exception as e:
+            self.statusBar().showMessage(f"{action} 錯誤: {str(e)}", 3000)
+
     def update_ui(self, msg_type, drone_id, data):
         # 檢查是否為新無人機，若是則加入無人機面板
-        if msg_type == 'new_drone':
+        if drone_id not in self.drones:
             self.add_drone(drone_id)
             return
 
@@ -659,10 +1090,17 @@ class ControlStationUI(QMainWindow):
         # 更新特定欄位並在總覽頁面更新
         if msg_type == 'state':
             mode = data.get('mode', 'UNKNOWN')
-            armed = data.get('armed', False)
+            armed = data.get('armed', None)
             mode_color = '#FF5555' if any(x in mode.upper() for x in ['RTL', '返航', 'EMERGENCY']) else '#55FF55'
-            arm_text = "ARMED" if armed else "DISARMED"
-            arm_color = '#55FF55' if armed else '#FF5555'
+            if armed is True:
+                arm_text = "ARMED"
+                arm_color = '#55FF55'
+            elif armed is False:
+                arm_text = "DISARMED"
+                arm_color = '#FF5555'
+            else:
+                arm_text = "--"
+                arm_color = '#AAAAAA'  # 未知狀態
 
             # 更新無人機面板欄位
             self.update_field(panel, drone_id, 'mode', mode, mode_color)
@@ -673,40 +1111,28 @@ class ControlStationUI(QMainWindow):
             self.update_overview_table(drone_id, 'armed', arm_text)
 
         elif msg_type == 'battery':
-            voltage = data.get('voltage', 0)
-            voltage = 14.8
+            voltage = data.get('voltage', 16)
             
             # 使用標準電壓判斷電池節數
-            cells_by_max = voltage / 4.2
-            cells_by_min = voltage / 3.7
-            
-            # 檢查是否為合理的電池組
-            is_valid_cells = abs(round(cells_by_max) - cells_by_min) < 0.5
-            num_cells = round(cells_by_max)
+            cell_max = 4.2
+            cells = round(voltage / 3.95)
+            max = cell_max * cells
+
+            # 計算電量百分比
+            percentage = voltage / max * 100
             
-            if not is_valid_cells or num_cells < 2:
-                # 電壓異常，顯示錯誤
-                text = f"{voltage:.1f}V (電壓異常!)"
-                voltage_color = '#FF0000'  # 紅色
+            # 根據百分比設置顏色
+            if percentage < 20:
+                voltage_color = '#FF6464'  # 紅色 (低電量)
+            elif percentage < 40:
+                voltage_color = '#FFA500'  # 橘色 (中低電量)
             else:
-                # 正常電壓計算
-                cell_voltage = voltage / num_cells
-                cell_max = 4.2
-                cell_min = 3.7
-                
-                # 計算電量百分比
-                percentage = max(0, min(100, ((cell_voltage - cell_min) / (cell_max - cell_min)) * 100))
-                
-                # 根據百分比設置顏色
-                if percentage < 20:
-                    voltage_color = '#FF6464'  # 紅色 (低電量)
-                elif percentage < 40:
-                    voltage_color = '#FFA500'  # 橘色 (中低電量)
-                else:
-                    voltage_color = '#FFFFFF'  # 白色 (正常電量)
-                
-                # 顯示總電壓、電池節數、單節電壓和百分比
-                text = f"{voltage:.1f}V ({cell_voltage:.2f}V / {percentage:.0f}%)"
+                voltage_color = '#FFFFFF'  # 白色 (正常電量)
+            
+            percentage = data.get('percentage', percentage)
+            
+            # 顯示總電壓、電池節數、單節電壓和百分比
+            text = f"{percentage:.0f}%"
             
             self.update_field(panel, drone_id, 'battery', text, voltage_color)
             self.update_overview_table(drone_id, 'battery', text)
@@ -730,11 +1156,14 @@ class ControlStationUI(QMainWindow):
             self.update_overview_table(drone_id, 'local', text)
 
         elif msg_type == 'hud':
-            heading = data.get('heading', 0)
+            heading = data.get('heading')
             self.drone_headings[drone_id] = heading
-            groundspeed = data.get('groundspeed', 0)
+            groundspeed = data.get('groundspeed')
             heading_text = f"{heading:.1f}°"
-            groundspeed_text = f"{groundspeed:.1f} m/s"
+            if isinstance(groundspeed, (int, float)):
+                groundspeed_text = f"{groundspeed:.1f} m/s"
+            else:
+                groundspeed_text = "--"
             self.update_field(panel, drone_id, 'heading', heading_text)
             self.update_field(panel, drone_id, 'groundspeed', groundspeed_text)
             self.update_overview_table(drone_id, 'heading', heading_text)
@@ -767,13 +1196,11 @@ class ControlStationUI(QMainWindow):
             self.update_overview_table(drone_id, 'pitch', pitch_text)
             self.update_overview_table(drone_id, 'yaw', yaw_text)
 
-        # 如果地圖已經載入，則更新地圖顯示無人機位置
-        if self.map_loaded and drone_id in self.drone_positions and drone_id in self.drone_headings:
+        # 新的代碼 - 將地圖更新放入緩存，等待批量處理：
+        if drone_id in self.drone_positions and drone_id in self.drone_headings:
             lat, lon = self.drone_positions[drone_id]
             heading = self.drone_headings[drone_id]
-            js = f"updateDrone({lat:.6f}, {lon:.6f}, '{drone_id}', {heading:.1f});"
-            self.map_view.page().runJavaScript(js)
-
+            self.pending_map_updates[drone_id] = (lat, lon, heading)
 
     def handle_drone_selection(self, drone_id, state):
         """處理個別無人機勾選狀態變化"""
@@ -783,7 +1210,7 @@ class ControlStationUI(QMainWindow):
             self.monitor.selected_drones.discard(drone_id)
 
     def handle_select_all(self):
-        """處理全選按鈕 - 再次點擊時取消全選"""
+        """處理全選按鈕 - 修改為支援分組結構"""
         # 檢查是否所有無人機都已被選中
         all_selected = all(
             self.drones[drone_id].findChild(QCheckBox, f"{drone_id}_checkbox").isChecked()
@@ -798,7 +1225,7 @@ class ControlStationUI(QMainWindow):
             checkbox = self.drones[drone_id].findChild(QCheckBox, f"{drone_id}_checkbox")
             if checkbox:
                 checkbox.setChecked(new_state)
-
+                
     def handle_arm_selected(self):
         """處理批次解鎖"""
         loop = asyncio.get_event_loop()
@@ -814,11 +1241,13 @@ class ControlStationUI(QMainWindow):
             loop.create_task(self.handle_service_response(future, f"批次起飛 {drone_id}"))
             
     def update_field(self, panel, drone_id, field, text, color=None):
-        """Update a specific field in the panel."""
+        """Update a specific field in the panel - 添加變更檢查"""
         if label := panel.findChild(QLabel, f"{drone_id}_{field}"):
-            label.setText(text)
-            if color:
-                label.setStyleSheet(f"color: {color};")
+            # 只有在文字真正改變時才更新
+            if label.text() != text:
+                label.setText(text)
+                if color:
+                    label.setStyleSheet(f"color: {color};")
 
     def update_overview_table(self, drone_id=None, field=None, value=None):
         # Ensure the widget is available
@@ -856,30 +1285,78 @@ class ControlStationUI(QMainWindow):
                     val_item = QTableWidgetItem(val)
                     val_item.setTextAlignment(Qt.AlignmentFlag.AlignCenter)
                     self.overview_table.setItem(row, col, val_item)
+    
+    def get_socket_id(self, drone_id):
+        """從 drone_id 提取 socket_id (例如 's9_1' -> '9')"""
+        import re
+        match = re.match(r's(\d+)_\d+', drone_id)
+        return match.group(1) if match else 'unknown'
 
     def add_drone(self, drone_id):
-        """只這一個 add_drone，不要再重複定義。"""
         if drone_id in self.drones:
             return
+            
+        # 獲取 socket_id
+        socket_id = self.get_socket_id(drone_id)
+        
+        # 創建無人機面板
         panel = self.create_drone_panel(drone_id)
         self.drones[drone_id] = panel
+        
+        # 如果該 socket 分組不存在，創建它
+        if socket_id not in self.socket_groups:
+            group_panel = self.create_socket_group_panel(socket_id)
+            self.socket_groups[socket_id] = group_panel
+        
+        # 將無人機面板添加到對應的 socket 分組中
+        group_panel = self.socket_groups[socket_id]
+        group_panel.drones_layout.addWidget(panel)
+        
+        # 重新排序並顯示所有 socket 分組
+        self.reorganize_socket_groups()
+        
+        # 更新總覽表
+        self.update_overview_table()
 
-        # 重新排序並顯示所有 panel
-        # 先清空
+    def reorganize_socket_groups(self):
+        """重新組織和排序 socket 分組"""
+        # 先清空主佈局
         while self.drone_panel_layout.count():
             w = self.drone_panel_layout.takeAt(0).widget()
             if w:
                 w.setParent(None)
-        # 再依照數字排序加入
-        for did in sorted(self.drones, key=lambda x: int(x[1:])):
-            self.drone_panel_layout.addWidget(self.drones[did])
-
-        # 更新總覽表
-        self.update_overview_table()
+        
+        # 對每個 socket 分組內的無人機進行排序
+        for socket_id, group_panel in self.socket_groups.items():
+            # 獲取該分組內的所有無人機
+            group_drones = [did for did in self.drones.keys() 
+                           if self.get_socket_id(did) == socket_id]
+            
+            # 清空分組佈局
+            while group_panel.drones_layout.count():
+                w = group_panel.drones_layout.takeAt(0).widget()
+                if w:
+                    w.setParent(None)
+            
+            # 對該分組內的無人機按數字排序
+            def sort_key(x):
+                parts = x[1:].split('_')
+                return (int(parts[0]), int(parts[1]))
+            
+            # 重新添加排序後的無人機面板
+            for did in sorted(group_drones, key=sort_key):
+                group_panel.drones_layout.addWidget(self.drones[did])
+        
+        # 按 socket_id 數字順序添加分組到主佈局
+        for socket_id in sorted(self.socket_groups.keys(), key=lambda x: int(x)):
+            self.drone_panel_layout.addWidget(self.socket_groups[socket_id])
 
     def spin_ros(self):
         try:
-            self.executor.spin_once(timeout_sec=0)
+            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)
+            self.monitor.latest_data.clear()
         except Exception as e:
             print(f"ROS spin error: {e}")