Force sync local master to remote ken

主專案整合

README.md

@@ -1,43 +1,46 @@
 這是天雷系統的專案
 
+
+
 ===
-專案核心框架
+## 功能簡介
+1. mavlink 多對多支援平台
+2. 不允許進到 ros 系統有相同 sysid
+3. 假設一台載具上所有 component 共用同一 socket
+
+===
+## 運行環境
+
+### 專案核心框架
 1. ROS2 Humble
 2. Python 3.8.10
 
+### 必要相依套件及版本
+- Python
+[Package] fc_network_adapter
+    1. pymavlink -> Version: 2.4.42
+    2. conda-forge 中的 pyserial-asyncio
+    3. importlib_metadata -> Version: 8.5.0
+    4. setuptools -> Version: 58.2.0 (版本太新不行)
+    5. pyserial-asyncio
+[Package] GUI
+    1. testresources
+    2. websockets
+    3. PyQt6
+    4. PyQt6-WebEngine
 
-===
+- ROS2
-必要相依套件 順便記錄我開發時的環境版本
-
-Python
-1. pymavlink -> Version: 2.4.42
-2. conda-forge 中的 pyserial-asyncio
-3. importlib_metadata -> Version: 8.5.0
-4. setuptools -> Version: 58.2.0 (版本太新不行)
-5. pyserial-asyncio
-
-
-ROS2
 1. source ~/ros2_humble/install/setup.bash
-2.
+2. geographic_info (https://github.com/ros-geographic-info/geographic_info.git) 已經作為 submodule 放在 external
+3. angles (https://github.com/ros/angles.git) 已經作為 submodule 放在 external
+4. mavros_msgs (https://github.com/mavlink/mavros) 這個專案中的一個資料夾 這邊手動複製的
 
-===
+### 開發用輔助專案
-功能簡介
-1. mavlink 多對多支援平台
-2. 不允許進到 ros 系統有相同 sysid
-3. 假設所有 component 共用同一 socket
-
-===
-開發用輔助專案
 1. Gazebo Garden
 2. Ardupilot
 
 ===
-依賴的 ROS 庫
+## 使用說明
-1. https://github.com/ros-geographic-info/geographic_info.git 記得要搞 ros2 版本的
-2. https://github.com/ros/angles.git
-3. mavros_msgs 是 https://github.com/mavlink/mavros 這個專案中的一個資料夾 這邊手動複製的
-
 Clone 專案後 請先執行這些指令
 ```bash
 # 1.同步 submodule
@@ -49,38 +52,53 @@ colcon build --packages-select angles geographic_msgs
 colcon build --packages-select mavros_msgs # 這個依賴前面的
 colcon build --packages-select fc_interfaces # 自己定義的
 
+```
+1. 
+主要專案 fc_network_adapter 請一律在 ~/AirTrapMine/src/ 路徑下 以模組化啟動程式 
+```bash
+# 記得先開啟 依賴 Package 到 overlay
+. ./install/local_setup.bash
+# 範例
+cd ~/AirTrapMine/src/ # 這是範例!!!
+python -m fc_network_adapter.fc_network_adapter.mainOrchestrator
+python -m fc_network_adapter.tests.demo_integration
+python -m someotherpkg.src.example_wholeMoving
 ```
 
+2. 
+GUI 介面
+在 ~/AirTrapMine/src/GUI 路徑下 直接啟動
+```bash
+cd ~/AirTrapMine/src/GUI
+python gui.py
+```
 
 ===
-Package 簡述
+## 資料夾說明
 1. unitdev 為各自協作者做開發時的測試區
     01 -> 晉凱(ken)
     02 -> 其宇(chiyu)
     03 -> 文鈞
     04 -> 倫渝
-2. fc_network_adapter 
+2. fc_network_adapter (核心)
     建立、維護與飛控韌體的連接 
     構築 mavlink 封包
     處理無線模組的通訊格式 (XBee)
     --同時處理與 Gazebo 的 ardupilot_plugin 溝通的 FDM/JSON 訊息 (移除)--
-3. fc_interfaces
+3. fc_interfaces (重要)
-    自定義的介面檔
+    自定義的 ROS2 介面檔
 4. fc_network_apps
     與 fc_network_adapter 銜接做高階功能包裝的應用小程式 利於開發GUI或其他應用
-
+    使用者的外層包裝
+5. someotherpkg
+    如何使用 fc_network_apps 的範例檔案
+6. GUI 
+    由 PyQt6 開發的互動式介面
 N. logs 是執行時期的記錄檔
 
 ===
-主要專案 fc_network_adapter 請一律在 ~/AirTrapMine/src/ 資料夾下 以模組化啟動程式
 
-例如 在 ~/AirTrapMine/src/ 資料夾下 
-```bash
-# 記得先開啟 依賴 Package 到 overlay
-. ./install/local_setup.bash
-# 範例
-python -m fc_network_adapter.fc_network_adapter.mainOrchestrator
-python -m fc_network_adapter.tests.demo_integration
 
-```
 
+簡述0328
+主要是透過test_transform這隻程式執行fdm_switch_example_one_with_remote_forwarding這個副函式，透過UDP將封包轉接到Matlab，並在Matlab進行演算法迭代後，打包JSON檔，並再次透過port口傳回封包，傳回來的封包會在close_loop這隻程式被解析，並提取其中有關velocity的資料儲存成變數，以mavlink的方式打入Ardupilot，以實現封閉迴路的構思。

src/GUI/communication.py

@@ -4,10 +4,14 @@ import math
 import re
 import threading
 from threading import Lock
+from concurrent.futures import ThreadPoolExecutor
 import asyncio
 import websockets
 import json
 import socket
+import sys
+import os
+import traceback
 from pymavlink import mavutil
 from geometry_msgs.msg import Point, Vector3
 from sensor_msgs.msg import BatteryState, NavSatFix, Imu
@@ -15,6 +19,23 @@ from std_msgs.msg import Float64, String
 from mavros_msgs.msg import State, VfrHud
 from mavros_msgs.srv import CommandBool, CommandTOL
 
+# 確保 src 目錄在 Python 路徑中（用於 fc_network_apps 導入）
+_src_path = os.path.dirname(os.path.dirname(os.path.abspath(__file__)))
+if _src_path not in sys.path:
+    sys.path.insert(0, _src_path)
+
+# 導入 fc_network_apps 的 longCommand（統一的 MAV_CMD_* API）
+try:
+    from fc_network_apps.longCommand import CommandLongClient
+except ImportError as e:
+    import traceback
+    print(f"⚠️  警告: 無法導入 CommandLongClient")
+    print(f"   错误: {e}")
+    print(f"   这通常表示 ROS2 的 fc_interfaces 包未被编译或未正确安装")
+    print(f"   完整堆栈跟踪:")
+    traceback.print_exc()
+    CommandLongClient = None
+
 class DroneSignals(QObject):
     update_signal = pyqtSignal(str, str, object)  # (msg_type, drone_id, data)
 
@@ -398,8 +419,13 @@ class WebSocketMavlinkReceiver(threading.Thread):
 
 class DroneMonitor(Node):
     # Subscribe to drone ROS2 topics
+    _instance = None  # Singleton pattern to prevent duplicate nodes
+    
     def __init__(self):
-        super().__init__('drone_monitor')
+        # Use a unique node name with timestamp to avoid conflicts on restart
+        import time
+        node_name = f'drone_monitor_{int(time.time() * 1000) % 100000}'
+        super().__init__(node_name)
         self.signals = DroneSignals()
         self.drone_topics = {}
         self.lock = Lock()
@@ -440,6 +466,17 @@ class DroneMonitor(Node):
         # ================================================================================
         self.serial_receivers = []
         
+        # ================================================================================
+        # 【新增】初始化 CommandLongClient（持久化，不會每次調用都創建/銷毀）
+        # ================================================================================
+        self.command_long_client = None
+        try:
+            self.command_long_client = CommandLongClient()
+        except Exception as e:
+            print(f"⚠️ 警告: 無法初始化 CommandLongClient: {e}")
+            self.command_long_client = None
+        # ================================================================================
+        
         # 主题检测定时器
         self.create_timer(1.0, self.scan_topics)
     
@@ -536,44 +573,145 @@ class DroneMonitor(Node):
         
         setattr(self, f'drone_{sys_id}_subs', subs)
 
+    # ================================================================================
+    # 【新增】模式名稱到 custom_mode 值的映射（基於 Copter 模式）
+    # ================================================================================
+    MODE_MAPPING = {
+        "STABILIZE": 0,
+        "ACRO": 1,
+        "ALT_HOLD": 2,
+        "AUTO": 3,
+        "GUIDED": 4,
+        "LOITER": 5,
+        "RTL": 6,
+        "CIRCLE": 7,
+        "POSITION": 8,
+        "LAND": 9,
+        "OF_LOITER": 10,
+        "DRIFT": 11,
+        "SPORT": 13,
+        "FLIP": 14,
+        "AUTOTUNE": 15,
+        "POSHOLD": 16,
+        "BRAKE": 17,
+        "THROW": 18,
+        "AVOID_ADSB": 19,
+        "GUIDED_NOGPS": 20,
+        "SMART_RTL": 21,
+    }
+    # ================================================================================
+
+    async def set_mode(self, drone_id, mode_name):
+        """使用 CommandLongClient 切換無人機飛行模式（使用非阻塞的 async 方法）"""
+        try:
+            # 解析 drone_id 提取 sysid
+            parts = drone_id.split('_')
+            if len(parts) < 2:
+                print(f"❌ [SET_MODE] 無效的 drone_id 格式: {drone_id}")
+                return False
+            sysid = int(parts[-1])
+            
+            # 獲取模式對應的 custom_mode 值
+            custom_mode = self.MODE_MAPPING.get(mode_name)
+            if custom_mode is None:
+                print(f"❌ [SET_MODE] 未知模式: {mode_name}")
+                return False
+            
+            print(f"\n📢 [SET_MODE] {drone_id} → {mode_name} (custom_mode={custom_mode})")
+            
+            if not self.command_long_client:
+                print(f"❌ [SET_MODE] CommandLongClient 未初始化")
+                return False
+            
+            # 直接調用 async 方法，無需線程池（避免嵌套執行器衝突）
+            result = await self.command_long_client.change_mode_async(
+                target_sysid=sysid,
+                custom_mode=float(custom_mode),
+                target_compid=0,
+                base_mode=1.0,
+                timeout_sec=2.0,
+            )
+            
+            if result and result.success:
+                print(f"✅ [SET_MODE] 模式切換成功")
+                return True
+            else:
+                print(f"❌ [SET_MODE] 模式切換失敗 (message={result.message if result else 'None'})")
+                return False
+        except Exception as e:
+            print(f"❌ [SET_MODE] 例外錯誤: {e}")
+            traceback.print_exc()
+            return False
+
     async def arm_drone(self, drone_id, arm):
-        if drone_id not in self.arm_clients:
+        """使用 CommandLongClient 執行 ARM/DISARM（使用非阻塞的 async 方法）"""
-            return False
-        
-        client = self.arm_clients[drone_id]
-        if not client.wait_for_service(timeout_sec=1.0):
-            return False
-            
-        request = CommandBool.Request()
-        request.value = arm
-        
-        future = client.call_async(request)
         try:
-            response = await future
+            # 解析 drone_id 提取 sysid
-            return response.success
+            parts = drone_id.split('_')
+            if len(parts) < 2:
+                print(f"❌ [ARM] 無效的 drone_id 格式: {drone_id}")
+                return False
+            sysid = int(parts[-1])
+            
+            action_name = "解鎖" if arm else "上鎖"
+            print(f"\n📢 [ARM] {drone_id} → {action_name}")
+            
+            if not self.command_long_client:
+                print(f"❌ [ARM] CommandLongClient 未初始化")
+                return False
+            
+            # 直接調用 async 方法，無需線程池（避免嵌套執行器衝突）
+            result = await self.command_long_client.arm_disarm_async(
+                target_sysid=sysid,
+                arm=arm,
+                target_compid=0,
+                timeout_sec=2.0,
+            )
+            
+            if result and result.success:
+                print(f"✅ [ARM] {action_name}成功")
+                return True
+            else:
+                print(f"❌ [ARM] {action_name}失敗 (message={result.message if result else 'None'})")
+                return False
         except Exception as e:
-            self.get_logger().error(f'Arm service call failed: {e}')
+            print(f"❌ [ARM] 例外錯誤: {e}")
+            traceback.print_exc()
             return False
 
-    async def takeoff_drone(self, drone_id, altitude=10.0):
+    async def takeoff_drone(self, drone_id, altitude):
-        if drone_id not in self.takeoff_clients:
+        """使用 CommandLongClient 執行無人機起飛（使用非阻塞的 async 方法）"""
-            return False
-            
-        client = self.takeoff_clients[drone_id]
-        if not client.wait_for_service(timeout_sec=1.0):
-            return False
-            
-        request = CommandTOL.Request()
-        request.altitude = altitude
-        request.min_pitch = 0.0
-        request.yaw = 0.0
-        
-        future = client.call_async(request)
         try:
-            response = await future
+            # 解析 drone_id 提取 sysid
-            return response.success
+            parts = drone_id.split('_')
+            if len(parts) < 2:
+                print(f"❌ [TAKEOFF] 無效的 drone_id 格式: {drone_id}")
+                return False
+            sysid = int(parts[-1])
+            
+            print(f"\n📢 [TAKEOFF] {drone_id} → 起飛 (高度={altitude}m)")
+            
+            if not self.command_long_client:
+                print(f"❌ [TAKEOFF] CommandLongClient 未初始化")
+                return False
+            
+            # 直接調用 async 方法，無需線程池（避免嵌套執行器衝突）
+            result = await self.command_long_client.takeoff_async(
+                target_sysid=sysid,
+                altitude_m=float(altitude),
+                target_compid=0,
+                timeout_sec=2.0,
+            )
+            
+            if result and result.success:
+                print(f"✅ [TAKEOFF] 起飛成功")
+                return True
+            else:
+                print(f"❌ [TAKEOFF] 起飛失敗 (message={result.message if result else 'None'})")
+                return False
         except Exception as e:
-            self.get_logger().error(f'Takeoff service call failed: {e}')
+            print(f"❌ [TAKEOFF] 例外錯誤: {e}")
+            traceback.print_exc()
             return False
 
     def send_setpoint(self, drone_id, x, y, z):

src/GUI/map_layout.py

@@ -178,35 +178,8 @@ class DroneMap:
             </head>
             <body>
             <div id="map"></div>
-            <div class="map-controls">
+            <div class="map-controls" id="map-controls-panel" style="display: none;">
-                <button class="selection-button" onclick="clearAllTrajectories()">清除軌跡</button>
+                <button class="confirm-route-button" id="confirm-route-btn" onclick="confirmRoute()">確認路徑</button>
-                <div style="border-top: 1px solid #ddd; padding-top: 5px; margin-top: 5px;">
-                    <label style="font-size: 12px; color: #555; font-weight: bold;">任務模式</label>
-                    <select id="mission-mode-select" onchange="onMissionModeChanged(this.value)" style="width: 100%; padding: 6px; border-radius: 4px; border: 1px solid #ccc; font-size: 12px; margin-top: 3px;">
-                        <option value="M_FORMATION">列隊飛行</option>
-                        <option value="CIRCLE_FORMATION">環狀包圍</option>
-                        <option value="LEADER_FOLLOWER">跟隨模式</option>
-                        <option value="GRID_SWEEP">柵狀偵查</option>
-                    </select>
-                </div>
-                <button class="confirm-route-button" id="confirm-route-btn" onclick="confirmRoute()" style="display: none;">確認路徑</button>
-                <button class="selection-button" id="select-polygon-btn" onclick="togglePolygonSelection()">多點選擇區域 (開發中)</button>
-            </div>
-            <div class="mission-info-panel">
-                <div class="selection-buttons">
-                    <button class="selection-button-blue" onclick="toggleSelectAllDrones()">全選無人機</button>
-                    <button class="selection-button-blue" id="select-drones-btn" onclick="toggleDroneSelection()">框選無人機</button>
-                </div>
-                <div class="mission-info-row">
-                    <span class="mission-info-label">中心點: </span>
-                    <span class="mission-info-value" id="center-position">未設定</span>
-                </div>
-                <div class="mission-info-row">
-                    <span class="mission-info-label">目標點: </span>
-                    <span class="mission-info-value" id="target-position">未設定</span>
-                </div>
-                <button class="mission-start-button" id="start-mission-btn" onclick="startMission()" disabled>開始任務</button>
-                <button class="mission-start-button" id="pause-mission-btn" onclick="pauseMission()">暫停任務</button>
             </div>
 
             <script>
@@ -375,6 +348,9 @@ class DroneMap:
                 var centerPosition = null;
                 var targetPosition = null;
 
+                // 多群組任務規劃 — 每個 group 各自的 layer group
+                var groupMissionLayers = {};  // group_id → L.layerGroup
+                
                 // 選擇工具變量
                 var selectionMode = null;  // 'drones', 'rect', 'polygon', 'route', null
                 var selectionLayer = L.layerGroup().addTo(map);
@@ -393,30 +369,8 @@ class DroneMap:
                 var routeLayer = L.layerGroup().addTo(map);
                 // ================================================================================
                 
-                // 更新任務信息面板
+                // 任務信息已移至 GroupPanel 顯示
-                function updateMissionInfo() {
+                function updateMissionInfo() {}
-                    const centerElem = document.getElementById('center-position');
-                    const targetElem = document.getElementById('target-position');
-                    const startBtn = document.getElementById('start-mission-btn');
-                    
-                    if (centerPosition) {
-                        centerElem.textContent = `${centerPosition.lat.toFixed(6)}°, ${centerPosition.lng.toFixed(6)}°`;
-                    } else {
-                        centerElem.textContent = '未設定';
-                    }
-                    
-                    if (targetPosition) {
-                        targetElem.textContent = `${targetPosition.lat.toFixed(6)}°, ${targetPosition.lng.toFixed(6)}°`;
-                    } else {
-                        targetElem.textContent = '未設定';
-                    }
-                    
-                    if (centerPosition && targetPosition) {
-                        startBtn.disabled = false;
-                    } else {
-                        startBtn.disabled = true;
-                    }
-                }
                 
                 // ================================================================================
                 // 選擇工具函數
@@ -429,14 +383,14 @@ class DroneMap:
                 }
                 
                 function toggleDroneSelection() {
-                    clearSelectionMode();
                     if (selectionMode === 'drones') {
                         selectionMode = null;
-                        document.getElementById('select-drones-btn').classList.remove('active');
+                        selectionLayer.clearLayers();
+                        tempRectangle = null;
                         map.dragging.enable();
                     } else {
+                        clearSelectionMode();
                         selectionMode = 'drones';
-                        document.getElementById('select-drones-btn').classList.add('active');
                         map.dragging.disable();
                     }
                 }
@@ -450,7 +404,7 @@ class DroneMap:
                             clearSelectionMode();
                             clearPolygonPoints();
                             selectionMode = null;
-                            document.getElementById('select-polygon-btn').classList.remove('active');
+                            // select-polygon-btn removed from map overlay
                         }
                     } else {
                         clearSelectionMode();
@@ -462,17 +416,12 @@ class DroneMap:
                 }
                 
                 function clearSelectionMode() {
-                    document.getElementById('select-drones-btn').classList.remove('active');
-                    document.getElementById('select-polygon-btn').classList.remove('active');
-                    
                     selectionLayer.clearLayers();
                     tempRectangle = null;
 
                     // 不清除 routeLayer（由 clearRoutePoints 單獨管理）
-                    
                     map.dragging.enable();
 
-                    // 如果離開 route 模式，重置 selectionMode
                     if (selectionMode !== 'route') {
                         selectionMode = null;
                     }
@@ -527,7 +476,7 @@ class DroneMap:
                     }
                     
                     selectionMode = null;
-                    document.getElementById('select-polygon-btn').classList.remove('active');
+                    // select-polygon-btn removed from map overlay
                     map.dragging.enable();
                 }
                 
@@ -554,6 +503,10 @@ class DroneMap:
                             }
                         });
                         console.log('框選無人機:', selectedDrones);
+                        // 通知 Python 完整的框選結果
+                        if (bridge && selectedDrones.length > 0) {
+                            bridge.droneBoxSelected(JSON.stringify(selectedDrones));
+                        }
                         
                     } else if (selectionMode === 'rect') {
                         var rectPoints = [
@@ -588,18 +541,7 @@ class DroneMap:
                     
                     // 重置狀態
                     selectionMode = null;
-                    document.getElementById('select-drones-btn').classList.remove('active');
                     map.dragging.enable();
-                    
-                    // 如果仍在 Grid Sweep 模式，重新進入框選
-                    var currentMode = document.getElementById('mission-mode-select').value;
-                    if (currentMode === 'GRID_SWEEP') {
-                        setTimeout(function() {
-                            selectionMode = 'rect';
-                            map.dragging.disable();
-                            console.log('Grid Sweep: 重新進入框選模式');
-                        }, 500);
-                    }
                 }
                 
                 // ================================================================================
@@ -709,8 +651,8 @@ class DroneMap:
                     clearSelectionMode();
                     clearRoutePoints();
 
-                    // 隱藏確認路徑按鈕
+                    // 預設隱藏控制面板
-                    document.getElementById('confirm-route-btn').style.display = 'none';
+                    document.getElementById('map-controls-panel').style.display = 'none';
 
                     if (mode === 'GRID_SWEEP') {
                         // 自動進入框選模式
@@ -718,9 +660,9 @@ class DroneMap:
                         map.dragging.disable();
                         console.log('Grid Sweep: 進入框選模式');
                     } else if (mode === 'LEADER_FOLLOWER') {
-                        // 進入路徑標記模式
+                        // 進入路徑標記模式 + 顯示確認路徑按鈕
                         selectionMode = 'route';
-                        document.getElementById('confirm-route-btn').style.display = 'block';
+                        document.getElementById('map-controls-panel').style.display = 'flex';
                         console.log('跟隨模式: 進入路徑標記模式，點擊地圖添加路徑點');
                     }
                     
@@ -839,76 +781,74 @@ class DroneMap:
                 // 任務規劃視覺化函式
                 // ================================================================================
                 function drawMissionPlan(centerLat, centerLon, targetLat, targetLon) {
-                    clearMissionPlan();
+                    drawMissionPlanForGroup('_default', '#FF4444', centerLat, centerLon, targetLat, targetLon);
+                }
+
+                function drawMissionPlanForGroup(groupId, color, centerLat, centerLon, targetLat, targetLon) {
+                    clearMissionPlanForGroup(groupId);
 
                     centerPosition = {lat: centerLat, lng: centerLon};
                     targetPosition = {lat: targetLat, lng: targetLon};
                     updateMissionInfo();
 
+                    var layerGroup = L.layerGroup().addTo(map);
+                    groupMissionLayers[groupId] = layerGroup;
+
                     var centerIcon = L.divIcon({
                         className: 'mission-center',
-                        html: `<div style="
+                        html: '<div style="' +
-                            background-color: #FF4444;
+                            'background-color: ' + color + ';' +
-                            color: white;
+                            'color: white;' +
-                            width: 20px;
+                            'width: 22px; height: 22px;' +
-                            height: 20px;
+                            'border-radius: 50%;' +
-                            border-radius: 50%;
+                            'display: flex; align-items: center; justify-content: center;' +
-                            display: flex;
+                            'font-weight: bold; font-size: 10px;' +
-                            align-items: center;
+                            'border: 2px solid white;' +
-                            justify-content: center;
+                            'box-shadow: 0 2px 5px rgba(0,0,0,0.3);' +
-                            font-weight: bold;
+                            '">' + groupId + '</div>',
-                            font-size: 12px;
+                        iconSize: [22, 22],
-                            border: 2px solid white;
+                        iconAnchor: [11, 11]
-                            box-shadow: 0 2px 5px rgba(0,0,0,0.3);
-                            ">C</div>`,
-                        iconSize: [20, 20],
-                        iconAnchor: [10, 10]
                     });
 
-                    centerMarker = L.marker([centerLat, centerLon], {
+                    L.marker([centerLat, centerLon], {
-                        icon: centerIcon,
+                        icon: centerIcon, zIndexOffset: 2000
-                        zIndexOffset: 2000
+                    }).addTo(layerGroup);
-                    }).addTo(missionPlanGroup);
 
                     var targetIcon = L.divIcon({
                         className: 'mission-target',
-                        html: `<div style="
+                        html: '<div style="' +
-                            background-color: #FFD700;
+                            'background-color: #FFD700;' +
-                            color: #FF4444;
+                            'color: ' + color + ';' +
-                            width: 20px;
+                            'width: 22px; height: 22px;' +
-                            height: 20px;
+                            'border-radius: 50%;' +
-                            border-radius: 50%;
+                            'display: flex; align-items: center; justify-content: center;' +
-                            display: flex;
+                            'font-weight: bold; font-size: 14px;' +
-                            align-items: center;
+                            'border: 2px solid white;' +
-                            justify-content: center;
+                            'box-shadow: 0 2px 5px rgba(0,0,0,0.3);' +
-                            font-weight: bold;
+                            '">★</div>',
-                            font-size: 14px;
+                        iconSize: [22, 22],
-                            border: 2px solid white;
+                        iconAnchor: [11, 11]
-                            box-shadow: 0 2px 5px rgba(0,0,0,0.3);
-                            ">★</div>`,
-                        iconSize: [20, 20],
-                        iconAnchor: [10, 10]
                     });
 
-                    targetMarker = L.marker([targetLat, targetLon], {
+                    L.marker([targetLat, targetLon], {
-                        icon: targetIcon,
+                        icon: targetIcon, zIndexOffset: 2000
-                        zIndexOffset: 2000
+                    }).addTo(layerGroup);
-                    }).addTo(missionPlanGroup);
 
-                    console.log('任務規劃已繪製: C(' + centerLat + ', ' + centerLon + ') -> T(' + targetLat + ', ' + targetLon + ')');
+                    console.log('Group ' + groupId + ' 任務規劃已繪製');
                 }
 
-                function clearMissionPlan() {
+                function clearMissionPlanForGroup(groupId) {
-                    if (centerMarker) {
+                    if (groupMissionLayers[groupId]) {
-                        missionPlanGroup.removeLayer(centerMarker);
+                        map.removeLayer(groupMissionLayers[groupId]);
-                        centerMarker = null;
+                        delete groupMissionLayers[groupId];
                     }
+                }
 
-                    if (targetMarker) {
+                function clearAllMissionPlans() {
-                        missionPlanGroup.removeLayer(targetMarker);
+                    for (var gid in groupMissionLayers) {
-                        targetMarker = null;
+                        map.removeLayer(groupMissionLayers[gid]);
                     }
-                    
+                    groupMissionLayers = {};
                     centerPosition = null;
                     targetPosition = null;
                     updateMissionInfo();
@@ -969,17 +909,45 @@ class DroneMap:
     # 任務規劃視覺化方法
     # ================================================================================
     def draw_mission_plan(self, center_lat, center_lon, target_lat, target_lon):
-        """在地圖上繪製任務規劃"""
+        """在地圖上繪製任務規劃（舊介面，相容用）"""
+        self.draw_mission_plan_for_group('_default', '#FF4444',
+                                          center_lat, center_lon, target_lat, target_lon)
+
+    def draw_mission_plan_for_group(self, group_id, color,
+                                     center_lat, center_lon, target_lat, target_lon):
+        """在地圖上繪製指定群組的任務規劃（帶顏色區分）"""
         if self.map_loaded:
-            js_code = f"drawMissionPlan({center_lat:.6f}, {center_lon:.6f}, {target_lat:.6f}, {target_lon:.6f});"
+            js_code = (
+                f"drawMissionPlanForGroup("
+                f"'{group_id}', '{color}', "
+                f"{center_lat:.6f}, {center_lon:.6f}, "
+                f"{target_lat:.6f}, {target_lon:.6f});"
+            )
             self.map_view.page().runJavaScript(js_code)
-            print(f"📍 地圖已繪製任務規劃: C({center_lat:.6f}, {center_lon:.6f}) -> T({target_lat:.6f}, {target_lon:.6f})")
+            print(f"📍 地圖已繪製 Group {group_id} 任務規劃: "
+                  f"C({center_lat:.6f}, {center_lon:.6f}) -> T({target_lat:.6f}, {target_lon:.6f})")
 
     def clear_mission_plan(self):
-        """清除地圖上的任務規劃標記"""
+        """清除地圖上所有任務規劃標記"""
         if self.map_loaded:
-            self.map_view.page().runJavaScript("clearMissionPlan();")
+            self.map_view.page().runJavaScript("clearAllMissionPlans();")
-            print("🗑️ 地圖已清除任務規劃")
+            print("🗑️ 地圖已清除所有任務規劃")
+
+    def clear_mission_plan_for_group(self, group_id):
+        """清除指定群組的任務規劃標記"""
+        if self.map_loaded:
+            self.map_view.page().runJavaScript(f"clearMissionPlanForGroup('{group_id}');")
+            print(f"🗑️ 地圖已清除 Group {group_id} 任務規劃")
+
+    def set_mission_mode(self, mode):
+        """從 Python 端切換地圖的任務模式（觸發框選/路徑標記等）"""
+        if self.map_loaded:
+            self.map_view.page().runJavaScript(f"onMissionModeChanged('{mode}');")
+
+    def toggle_drone_box_select(self):
+        """切換地圖上的無人機框選模式"""
+        if self.map_loaded:
+            self.map_view.page().runJavaScript("toggleDroneSelection();")
     # ================================================================================
     
     def get_widget(self):
@@ -1030,6 +998,10 @@ class DroneMap:
         """獲取路徑確認信號"""
         return self.bridge.route_confirmed
 
+    def get_drone_box_selected_signal(self):
+        """獲取框選無人機完成信號"""
+        return self.bridge.drone_box_selected
+    
 class MapBridge(QObject):
     """JavaScript 和 Python 之間的橋接類"""
     gps_signal = pyqtSignal(float, float)
@@ -1042,6 +1014,7 @@ class MapBridge(QObject):
     polygon_selected = pyqtSignal(str)
     mission_mode_changed = pyqtSignal(str)
     route_confirmed = pyqtSignal(str)  # 路徑確認 (JSON 字串)
+    drone_box_selected = pyqtSignal(str)  # 框選無人機完成 (JSON 字串)
 
     def __init__(self):
         super().__init__()
@@ -1103,3 +1076,9 @@ class MapBridge(QObject):
         """供 JavaScript 調用的方法 - 路徑確認"""
         self.route_confirmed.emit(points_json)
         print(f"📍 路徑已確認: {points_json}")
+
+    @pyqtSlot(str)
+    def droneBoxSelected(self, drone_ids_json):
+        """供 JavaScript 調用的方法 - 框選無人機完成"""
+        self.drone_box_selected.emit(drone_ids_json)
+        print(f"📦 框選無人機: {drone_ids_json}")

src/GUI/mission_group.py

@@ -0,0 +1,519 @@
+#!/usr/bin/env python3
+"""
+任務群組模組
+管理多任務群組的資料結構與無人機分配對話框
+"""
+from PyQt6.QtWidgets import (
+    QWidget, QVBoxLayout, QHBoxLayout, QLabel, QPushButton,
+    QComboBox, QDialog, QCheckBox, QScrollArea, QFrame, QLineEdit
+)
+from PyQt6.QtCore import Qt, pyqtSignal
+from mission_executor import MissionExecutor, MissionState
+
+
+# 群組顏色（循環使用）
+GROUP_COLORS = [
+    '#4A9EFF',  # 藍
+    '#FF8C42',  # 橘
+    '#56C87A',  # 綠
+    '#E85D75',  # 紅
+    '#B07CED',  # 紫
+    '#F5C542',  # 黃
+    '#42C9C9',  # 青
+    '#FF6B9D',  # 粉
+]
+
+
+class MissionGroup:
+    """單一任務群組的資料"""
+
+    def __init__(self, group_id, color):
+        self.group_id = group_id          # 'A', 'B', 'C', ...
+        self.color = color                # 顏色 hex
+        self.drone_ids = set()            # 已分配的無人機 ID
+        self.mission_type = 'M_FORMATION' # 預設任務類型
+        self.planned_waypoints = None     # 規劃結果 dict
+        self.executor = None              # MissionExecutor 實例（延遲建立）
+        self.center_origin = None         # 規劃原點
+
+    @property
+    def state(self):
+        if self.executor is None:
+            return MissionState.IDLE
+        return self.executor.state
+
+    @property
+    def display_name(self):
+        return f"Group {self.group_id}"
+
+
+class DroneAssignDialog(QDialog):
+    """無人機分配對話框"""
+
+    def __init__(self, parent, all_drone_ids, current_assigned, other_assigned):
+        """
+        Args:
+            parent: 父 widget
+            all_drone_ids: 所有可用無人機 ID 列表
+            current_assigned: 當前群組已分配的無人機 set
+            other_assigned: 其他群組已佔用的無人機 dict {drone_id: group_id}
+        """
+        super().__init__(parent)
+        self.setWindowTitle("分配無人機")
+        self.setMinimumWidth(280)
+        self.setStyleSheet("""
+            QDialog { background-color: #2B2B2B; }
+            QLabel { color: #DDD; }
+            QCheckBox { color: #DDD; spacing: 8px; padding: 4px; }
+            QCheckBox::indicator { width: 16px; height: 16px; }
+            QCheckBox:disabled { color: #666; }
+        """)
+
+        layout = QVBoxLayout(self)
+
+        title = QLabel("選擇要分配到此群組的無人機：")
+        title.setStyleSheet("font-size: 13px; font-weight: bold; padding-bottom: 6px;")
+        layout.addWidget(title)
+
+        # 滾動區域
+        scroll = QScrollArea()
+        scroll.setWidgetResizable(True)
+        scroll.setMaximumHeight(300)
+        scroll_widget = QWidget()
+        scroll_layout = QVBoxLayout(scroll_widget)
+        scroll_layout.setSpacing(2)
+
+        self.checkboxes = {}
+        sorted_ids = sorted(all_drone_ids, key=lambda x: (x.split('_')[0], int(x.split('_')[1])))
+
+        for drone_id in sorted_ids:
+            cb = QCheckBox(drone_id)
+
+            if drone_id in current_assigned:
+                cb.setChecked(True)
+            elif drone_id in other_assigned:
+                cb.setEnabled(False)
+                cb.setToolTip(f"已分配到 Group {other_assigned[drone_id]}")
+                cb.setText(f"{drone_id}  (Group {other_assigned[drone_id]})")
+
+            self.checkboxes[drone_id] = cb
+            scroll_layout.addWidget(cb)
+
+        scroll_layout.addStretch()
+        scroll.setWidget(scroll_widget)
+        layout.addWidget(scroll)
+
+        # 按鈕
+        btn_layout = QHBoxLayout()
+        ok_btn = QPushButton("確定")
+        ok_btn.setStyleSheet("""
+            QPushButton { background-color: #4A9EFF; color: white; border: none;
+                padding: 8px 20px; border-radius: 4px; font-weight: bold; }
+            QPushButton:hover { background-color: #3A8EEF; }
+        """)
+        ok_btn.clicked.connect(self.accept)
+
+        cancel_btn = QPushButton("取消")
+        cancel_btn.setStyleSheet("""
+            QPushButton { background-color: #555; color: #DDD; border: none;
+                padding: 8px 20px; border-radius: 4px; }
+            QPushButton:hover { background-color: #666; }
+        """)
+        cancel_btn.clicked.connect(self.reject)
+
+        btn_layout.addStretch()
+        btn_layout.addWidget(cancel_btn)
+        btn_layout.addWidget(ok_btn)
+        layout.addLayout(btn_layout)
+
+    def get_selected(self):
+        """回傳勾選的無人機 ID set"""
+        return {did for did, cb in self.checkboxes.items() if cb.isChecked() and cb.isEnabled()}
+
+
+class GroupPanel(QWidget):
+    """單一群組的 UI 面板（嵌入到 tab 中）— 三欄式佈局"""
+
+    # 信號
+    assign_drones_requested = pyqtSignal(str)          # group_id
+    mission_type_changed = pyqtSignal(str, str)        # group_id, mission_type
+    start_requested = pyqtSignal(str)                  # group_id
+    pause_requested = pyqtSignal(str)                  # group_id
+    stop_requested = pyqtSignal(str)                   # group_id
+    mode_change_requested = pyqtSignal(str, str)       # group_id, mode
+    arm_requested = pyqtSignal(str)                    # group_id
+    takeoff_requested = pyqtSignal(str, float)         # group_id, altitude
+    box_select_requested = pyqtSignal(str)             # group_id — 框選直接分配
+    select_all_requested = pyqtSignal(str)             # group_id — 全選直接分配
+    clear_group_requested = pyqtSignal(str)            # group_id — 清除分組
+    add_group_requested = pyqtSignal()                 # 新增群組
+    delete_group_requested = pyqtSignal(str)           # group_id — 刪除群組
+
+    BUTTON_STYLE = """
+        QPushButton {{ background-color: {bg}; color: {fg}; border: none;
+            padding: 5px 8px; border-radius: 4px; font-size: 11px; }}
+        QPushButton:hover {{ background-color: {hover}; }}
+        QPushButton:disabled {{ background-color: #444; color: #666; }}
+    """
+
+    def __init__(self, group: MissionGroup, parent=None):
+        super().__init__(parent)
+        self.group = group
+        self._is_all_selected = False  # 追蹤全選狀態
+        self.all_btn_ref = None  # 保存全選按鈕的參考
+        self._build_ui()
+
+    def _make_sep(self):
+        """建立垂直分隔線"""
+        sep = QFrame()
+        sep.setFrameShape(QFrame.Shape.VLine)
+        sep.setStyleSheet("color: #444;")
+        return sep
+
+    def _build_ui(self):
+        layout = QVBoxLayout(self)
+        layout.setContentsMargins(6, 4, 6, 4)
+        layout.setSpacing(0)
+
+        COMBO = ("QComboBox { background-color: #333; color: #DDD; "
+                 "border-radius: 3px; padding: 2px 6px; font-size: 11px; }")
+        BTN = self.BUTTON_STYLE
+        LBL = "color: #AAA; font-size: 11px;"
+        TITLE = "color: #DDD; font-size: 11px; font-weight: bold; padding-bottom: 2px;"
+
+        # ── 三欄主佈局 ──
+        cols = QHBoxLayout()
+        cols.setSpacing(6)
+
+        # ============================
+        # 左欄：控制指令
+        # ============================
+        left = QVBoxLayout()
+        left.setSpacing(3)
+
+        left_title = QLabel("控制指令")
+        left_title.setStyleSheet(TITLE)
+        left.addWidget(left_title)
+
+        # 模式切換
+        mode_row = QHBoxLayout()
+        mode_row.setSpacing(3)
+        self.mode_combo = QComboBox()
+        self.mode_combo.addItems([
+            "GUIDED", "AUTO", "LAND", "LOITER",
+            "STABILIZE", "ALT_HOLD", "RTL", "POSHOLD", "SMART_RTL"
+        ])
+        self.mode_combo.setStyleSheet(COMBO)
+        mode_btn = QPushButton("切換")
+        mode_btn.setStyleSheet(BTN.format(bg='#555', fg='#DDD', hover='#666'))
+        mode_btn.clicked.connect(
+            lambda: self.mode_change_requested.emit(
+                self.group.group_id, self.mode_combo.currentText()))
+        mode_row.addWidget(self.mode_combo, 1)
+        mode_row.addWidget(mode_btn)
+        left.addLayout(mode_row)
+
+        # 解鎖
+        arm_btn = QPushButton("解鎖")
+        arm_btn.setStyleSheet(BTN.format(bg='#555', fg='#DDD', hover='#666'))
+        arm_btn.clicked.connect(
+            lambda: self.arm_requested.emit(self.group.group_id))
+        left.addWidget(arm_btn)
+
+        # 起飛
+        takeoff_row = QHBoxLayout()
+        takeoff_row.setSpacing(3)
+        self.alt_input = QComboBox()
+        self.alt_input.setEditable(True)
+        self.alt_input.addItems(["5", "10", "15", "20"])
+        self.alt_input.setCurrentText("10")
+        self.alt_input.setStyleSheet(COMBO)
+        alt_lbl = QLabel("m")
+        alt_lbl.setStyleSheet(LBL)
+        takeoff_btn = QPushButton("起飛")
+        takeoff_btn.setStyleSheet(BTN.format(bg='#555', fg='#DDD', hover='#666'))
+        takeoff_btn.clicked.connect(self._on_takeoff)
+        takeoff_row.addWidget(self.alt_input, 1)
+        takeoff_row.addWidget(alt_lbl)
+        takeoff_row.addWidget(takeoff_btn)
+        left.addLayout(takeoff_row)
+
+        left.addStretch()
+
+        # ============================
+        # 中欄：任務規劃（左右分割）
+        # ============================
+        mid = QVBoxLayout()
+        mid.setSpacing(2)
+
+        mid_title = QLabel("任務規劃")
+        mid_title.setStyleSheet(TITLE)
+        mid.addWidget(mid_title)
+
+        mid_body = QHBoxLayout()
+        mid_body.setSpacing(4)
+
+        # 左側：類型 + 狀態 + 座標
+        mid_left = QVBoxLayout()
+        mid_left.setSpacing(2)
+        self.type_combo = QComboBox()
+        self.type_combo.addItems([
+            "M_FORMATION", "CIRCLE_FORMATION", "LEADER_FOLLOWER", "GRID_SWEEP"
+        ])
+        self.type_combo.setStyleSheet(COMBO)
+        self.type_combo.currentTextChanged.connect(
+            lambda t: self.mission_type_changed.emit(self.group.group_id, t))
+        mid_left.addWidget(self.type_combo)
+        self.status_label = QLabel("○ 未規劃")
+        self.status_label.setStyleSheet("color: #888; font-size: 11px;")
+        mid_left.addWidget(self.status_label)
+        self.center_label = QLabel("中心: --")
+        self.center_label.setStyleSheet("color: #AAA; font-size: 11px;")
+        mid_left.addWidget(self.center_label)
+        self.target_label = QLabel("目標: --")
+        self.target_label.setStyleSheet("color: #AAA; font-size: 11px;")
+        mid_left.addWidget(self.target_label)
+        mid_left.addStretch()
+
+        # 右側：執行 / 暫停 / 停止（垂直排列）
+        mid_right = QVBoxLayout()
+        mid_right.setSpacing(3)
+        self.start_btn = QPushButton("▶ 執行")
+        self.start_btn.setStyleSheet(BTN.format(bg='#2E7D32', fg='white', hover='#388E3C'))
+        self.start_btn.clicked.connect(
+            lambda: self.start_requested.emit(self.group.group_id))
+        self.pause_btn = QPushButton("⏸ 暫停")
+        self.pause_btn.setStyleSheet(BTN.format(bg='#F57F17', fg='white', hover='#F9A825'))
+        self.pause_btn.clicked.connect(
+            lambda: self.pause_requested.emit(self.group.group_id))
+        self.stop_btn = QPushButton("■ 停止")
+        self.stop_btn.setStyleSheet(BTN.format(bg='#C62828', fg='white', hover='#D32F2F'))
+        self.stop_btn.clicked.connect(
+            lambda: self.stop_requested.emit(self.group.group_id))
+        mid_right.addWidget(self.start_btn)
+        mid_right.addWidget(self.pause_btn)
+        mid_right.addWidget(self.stop_btn)
+        mid_right.addStretch()
+
+        mid_body.addLayout(mid_left, 1)
+        mid_body.addLayout(mid_right)
+        mid.addLayout(mid_body)
+
+        # ============================
+        # 選取與分組（3x2 按鈕）
+        # ============================
+        right = QVBoxLayout()
+        right.setSpacing(3)
+
+        right_title = QLabel("選取與分組")
+        right_title.setStyleSheet(TITLE)
+        right.addWidget(right_title)
+
+        self.drone_list_label = QLabel("尚未分配")
+        self.drone_list_label.setStyleSheet("color: #888; font-size: 11px;")
+        self.drone_list_label.setWordWrap(True)
+        right.addWidget(self.drone_list_label)
+
+        # 3x2 按鈕網格：第一行 框選 全選 新增群組
+        grid_r1 = QHBoxLayout()
+        grid_r1.setSpacing(3)
+        box_btn = QPushButton("框選")
+        box_btn.setStyleSheet(BTN.format(bg='#64B5F6', fg='white', hover='#42A5F5'))
+        box_btn.clicked.connect(
+            lambda: self.box_select_requested.emit(self.group.group_id))
+        all_btn = QPushButton("全選/取消")
+        all_btn.setStyleSheet(BTN.format(bg='#64B5F6', fg='white', hover='#42A5F5'))
+        all_btn.clicked.connect(self._on_all_select_clicked)
+        self.all_btn_ref = all_btn  # 保存按鈕參考（備用）
+        add_group_btn = QPushButton("+ 新增群組")
+        add_group_btn.setStyleSheet(BTN.format(bg='#4A9EFF', fg='white', hover='#3A8EEF'))
+        add_group_btn.clicked.connect(lambda: self.add_group_requested.emit())
+        grid_r1.addWidget(box_btn)
+        grid_r1.addWidget(all_btn)
+        grid_r1.addWidget(add_group_btn)
+        right.addLayout(grid_r1)
+
+        # 第二行 編輯分配 清除分組 刪除群組
+        grid_r2 = QHBoxLayout()
+        grid_r2.setSpacing(3)
+        assign_btn = QPushButton("編輯分配")
+        assign_btn.setStyleSheet(BTN.format(bg='#555', fg='#DDD', hover='#666'))
+        assign_btn.clicked.connect(
+            lambda: self.assign_drones_requested.emit(self.group.group_id))
+        clear_btn = QPushButton("清除分組")
+        clear_btn.setStyleSheet(BTN.format(bg='#777', fg='white', hover='#888'))
+        clear_btn.clicked.connect(
+            lambda: self.clear_group_requested.emit(self.group.group_id))
+        delete_group_btn = QPushButton("刪除群組")
+        delete_group_btn.setStyleSheet(BTN.format(bg='#EF5350', fg='white', hover='#E53935'))
+        delete_group_btn.clicked.connect(
+            lambda: self.delete_group_requested.emit(self.group.group_id))
+        grid_r2.addWidget(assign_btn)
+        grid_r2.addWidget(clear_btn)
+        grid_r2.addWidget(delete_group_btn)
+        right.addLayout(grid_r2)
+
+        right.addStretch()
+
+        # ============================
+        # 第四欄：任務參數
+        # ============================
+        param_col = QVBoxLayout()
+        param_col.setSpacing(2)
+
+        param_title = QLabel("任務參數")
+        param_title.setStyleSheet(TITLE)
+        param_col.addWidget(param_title)
+
+        INPUT = ("QLineEdit { background-color: #333; color: #DDD; "
+                 "border: 1px solid #555; border-radius: 3px; "
+                 "padding: 1px 4px; font-size: 11px; }")
+
+        # 每種任務類型的參數定義: (key, label, default_value)
+        self._param_defs = {
+            'M_FORMATION': [
+                ('spacing', '間距 (m)', '5.0'),
+                ('base_altitude', '基準高度 (m)', '10.0'),
+                ('altitude_diff', '高低差 (m)', '2.0'),
+            ],
+            'CIRCLE_FORMATION': [
+                ('radius', '半徑 (m)', '10.0'),
+                ('altitude', '高度 (m)', '10.0'),
+                ('start_angle', '起始角 (°)', '0'),
+            ],
+            'LEADER_FOLLOWER': [
+                ('lateral_offset', '橫向偏移 (m)', '3.0'),
+                ('longitudinal_spacing', '縱向間距 (m)', '5.0'),
+                ('altitude', '高度 (m)', '10.0'),
+            ],
+            'GRID_SWEEP': [
+                ('line_spacing', '掃描線距 (m)', '5.0'),
+                ('altitude', '高度 (m)', '10.0'),
+            ],
+        }
+
+        # 建立所有參數列的 widget（先全部建好，切換時顯示/隱藏）
+        self._param_widgets = {}  # key → (label_widget, input_widget)
+        self._param_rows = []     # 所有 row layout 對應的 container widget
+
+        for mission_type, defs in self._param_defs.items():
+            for key, label_text, default in defs:
+                if key in self._param_widgets:
+                    continue  # 同名參數只建一次
+                row_w = QWidget()
+                row_l = QHBoxLayout(row_w)
+                row_l.setContentsMargins(0, 0, 0, 0)
+                row_l.setSpacing(3)
+                lbl = QLabel(label_text)
+                lbl.setStyleSheet(LBL)
+                inp = QLineEdit(default)
+                inp.setStyleSheet(INPUT)
+                inp.setFixedWidth(50)
+                row_l.addWidget(lbl, 1)
+                row_l.addWidget(inp)
+                param_col.addWidget(row_w)
+                self._param_widgets[key] = (row_w, inp)
+                self._param_rows.append(row_w)
+
+        param_col.addStretch()
+
+        # 初始顯示對應的參數
+        self._update_param_visibility()
+
+        # 當任務類型切換時更新參數顯示
+        self.type_combo.currentTextChanged.connect(self._update_param_visibility)
+
+        # ── 組裝四欄：控制指令 > 任務規劃 > 任務參數 > 選取與分組 ──
+        # 使用伸展因子 0 讓列根據內容自動調整寬度，而不是均等分配
+        cols.addLayout(left, 0)
+        cols.addWidget(self._make_sep())
+        cols.addLayout(mid, 0)
+        cols.addWidget(self._make_sep())
+        cols.addLayout(param_col, 0)
+        cols.addWidget(self._make_sep())
+        cols.addLayout(right, 0)
+        cols.addStretch()  # 填充剩餘空間，使四列置左
+
+        layout.addLayout(cols)
+
+    def update_drone_list(self):
+        """更新無人機列表顯示"""
+        if not self.group.drone_ids:
+            self.drone_list_label.setText("尚未分配")
+            self.drone_list_label.setStyleSheet("color: #888; font-size: 11px;")
+        else:
+            sorted_ids = sorted(self.group.drone_ids,
+                                key=lambda x: (x.split('_')[0], int(x.split('_')[1])))
+            self.drone_list_label.setText("、".join(sorted_ids))
+            self.drone_list_label.setStyleSheet(
+                f"color: {self.group.color}; font-size: 11px; font-weight: bold;")
+
+    def update_status(self):
+        """更新任務狀態顯示"""
+        state = self.group.state
+        if self.group.planned_waypoints is None:
+            self.status_label.setText("○ 未規劃")
+            self.status_label.setStyleSheet("color: #888; font-size: 11px;")
+        elif state == MissionState.RUNNING:
+            self.status_label.setText("▶ 執行中")
+            self.status_label.setStyleSheet("color: #4CAF50; font-size: 11px; font-weight: bold;")
+        elif state == MissionState.PAUSED:
+            self.status_label.setText("⏸ 已暫停")
+            self.status_label.setStyleSheet("color: #FFA000; font-size: 11px; font-weight: bold;")
+        else:
+            n = len(self.group.drone_ids)
+            total_wps = sum(len(wps) for wps in self.group.planned_waypoints['waypoints'])
+            self.status_label.setText(f"● 已規劃 ({n}架/{total_wps}點)")
+            self.status_label.setStyleSheet(
+                f"color: {self.group.color}; font-size: 11px; font-weight: bold;")
+
+    def _on_all_select_clicked(self):
+        """全選按鈕點擊 - 發送信號給 gui.py 處理 toggle 邏輯"""
+        self.select_all_requested.emit(self.group.group_id)
+    
+    def set_all_select_state(self, is_selected):
+        """外部設置全選狀態（按鈕文本保持「全選/取消」）"""
+        self._is_all_selected = is_selected
+
+    def _update_param_visibility(self, _=None):
+        """根據當前任務類型，顯示/隱藏對應的參數列"""
+        mission_type = self.type_combo.currentText()
+        visible_keys = {d[0] for d in self._param_defs.get(mission_type, [])}
+        for key, (row_w, _inp) in self._param_widgets.items():
+            row_w.setVisible(key in visible_keys)
+
+    def get_mission_params(self):
+        """讀取當前顯示的參數值，回傳 dict"""
+        mission_type = self.type_combo.currentText()
+        params = {}
+        for key, _label, default in self._param_defs.get(mission_type, []):
+            if key in self._param_widgets:
+                _row_w, inp = self._param_widgets[key]
+                try:
+                    params[key] = float(inp.text())
+                except ValueError:
+                    params[key] = float(default)
+        return params
+
+    def update_mission_info(self, center_lat, center_lon, target_lat, target_lon):
+        """更新中心點 / 目標點顯示"""
+        info_style = f"color: {self.group.color}; font-size: 11px; font-weight: bold;"
+        self.center_label.setText(f"中心: {center_lat:.6f}°, {center_lon:.6f}°")
+        self.center_label.setStyleSheet(info_style)
+        self.target_label.setText(f"目標: {target_lat:.6f}°, {target_lon:.6f}°")
+        self.target_label.setStyleSheet(info_style)
+
+    def clear_mission_info(self):
+        """清除中心點 / 目標點顯示"""
+        self.center_label.setText("中心: --")
+        self.center_label.setStyleSheet("color: #AAA; font-size: 11px;")
+        self.target_label.setText("目標: --")
+        self.target_label.setStyleSheet("color: #AAA; font-size: 11px;")
+
+    def _on_takeoff(self):
+        try:
+            alt = float(self.alt_input.currentText())
+        except ValueError:
+            alt = 10.0
+        self.takeoff_requested.emit(self.group.group_id, alt)
+

src/GUI/overview_table.py

@@ -6,7 +6,7 @@ class OverviewTable(QTableWidget):
     """總覽表格，顯示所有無人機的狀態資訊"""
     
     # 默認的資訊類型和映射
-    DEFAULT_INFO_TYPES = ["模式", "ARM", "電壓", "經度", "緯度", "高度", "位置", "速度", "地速", "航向",
+    DEFAULT_INFO_TYPES = ["模式", "ARM", "電壓", "經度", "緯度", "高度", "XY位置", "XY速度", "地速", "航向",
                           "空速", "油門", "HUD ALT", "爬升率", "Roll", "Pitch", "Yaw", "丟包", "延遲"]
     
     DEFAULT_INFO_TYPE_MAP = {

src/fc_interfaces/CMakeLists.txt

@@ -12,6 +12,7 @@ find_package(rosidl_default_generators REQUIRED)
 rosidl_generate_interfaces(${PROJECT_NAME}
   "srv/MavPing.srv"
   "srv/MavCommandLong.srv"
+  "srv/MavPositionTargetGlobalInt.srv"
   )
 
 ament_package()

src/fc_interfaces/srv/MavPositionTargetGlobalInt.srv

@@ -0,0 +1,36 @@
+# Request
+uint8  target_sysid
+uint8  target_compid
+
+uint32 time_boot_ms        # ms since system boot, 一般用 0 讓飛控自己填
+uint8  coordinate_frame    # MAV_FRAME_*
+uint16 type_mask           # POSITION_TARGET_TYPEMASK bitmask
+int32  lat_int             # 1e7 * latitude (deg)
+int32  lon_int             # 1e7 * longitude (deg)
+float32 alt                # altitude (m)
+float32 vx                 # X velocity (m/s)
+float32 vy                 # Y velocity (m/s)
+float32 vz                 # Z velocity (m/s)
+float32 afx                # X acceleration or force (m/s^2)
+float32 afy                # Y acceleration or force (m/s^2)
+float32 afz                # Z acceleration or force (m/s^2)
+float32 yaw                # yaw (rad)
+float32 yaw_rate           # yaw rate (rad/s)
+float32 timeout_sec        # 等待 POSITION_TARGET_GLOBAL_INT 回應的時間上限
+---
+# Response
+string message
+uint8  ack_result  
+uint32 r_time_boot_ms  
+uint16 r_type_mask           
+int32  r_lat_int            
+int32  r_lon_int            
+float32 r_alt                
+float32 r_vx 
+float32 r_vy 
+float32 r_vz 
+float32 r_afx          
+float32 r_afy      
+float32 r_afz              
+float32 r_yaw        
+float32 r_yaw_rate     

src/fc_network_adapter/fc_network_adapter/ackEnum.py

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

src/fc_network_adapter/fc_network_adapter/fc_network_adapter.md

@@ -6,6 +6,7 @@
 # 開發此專案的注意事項
 - 預設 autopilot 的 component id = 1
 - 不允許 system id 重複
+- 預設同一載具僅會使用相同的 socket
 - 增加一個固定數值監控然後要到 ros2 topic 
   - mavlinkROS2Node.py 檔案內 
     - PublishRateController.topic_intervals 建立
@@ -126,6 +127,7 @@
 - *cls.mavlinkObjects* 資料結構 { socket_id(序號) : mavlink_object(物件實例) }
 - *self.mavlink_socket* 從 pymavlink 繼承的socket物件
 - *self.state* 描述這個 socket 物件的狀態
+- *self.MAVLink* pymavlink 的一個裝置物件 模擬自己是某一個裝置 用以對於該 mavlink bus 負責封包發送 
 ---
 - *process_data()* [async method] 核心方法
 - *remove_target_socket()* *add_target_socket()*
@@ -170,6 +172,45 @@
 ## mavlinkVehicleView.py
     這個檔案是作為載具的資訊暫存庫使用 會搭配 ROS2 的功能 再做利用
 
+## mavlinkROS2Nodes.py
+    這裡支持了所有 ROS2 框架的對應功能 包涵 
+    1. 將載具的串流更新的狀態更新到對應的 topic 
+    2. 將地面站與載具互動 mavlink microservice 包裝起來提供 ros2 service 互動介面
+
+### **[Class]** fc_ros_manager
+    MAVLink ROS2 節點管理器 管理兩個獨立的 Node
+    會開一個執行緒 讓兩個 Node 都跑在裡面
+    然後用 spin_once 保持 Node 的活性
+- *self.status_publisher* 物件實例
+- *self.command_service* 物件實例
+- *self.spin_thread* 執行緒
+---
+- *start()* 啟動自己
+- *stop()* 停下自己
+- *shutdown()* 關閉自己並清理
+
+### **[Class]** VehicleStatusPublisher
+    這整個組件都是自動的 目前沒有什麼需要 runtime 設定的地方
+- *self.fc_publishers* 儲存所有 publisher 的資料結構
+- *self.rate_controller* 儲存頻率參數的地方 也可以關掉某個 topic 的發佈
+---
+- *timer_callback()* 自動的抓出現有的 vehicle 中有的資訊 然後固定的將訊息丟到負責發佈的方法中
+- *_publish_vehicle_status()* 接 timer_callback 後去分配到各項發佈中
+- *_get_or_create_publisher()* 實際創建 topic 的地方
+- *_publish_XXX()* 各項發佈的方法
+
+### **[Class]** MavlinkCommandService
+    提供 ros2 service 讓 serial_object 去丟出 mavlink 封包
+    然後會從回應封包中挑出期待的回應 再傳給 ros2 request 
+    其中 PendingEntry 是很關鍵的東西
+    它的每個物件 代表需要等待的封包種類與必需包含的內容
+- *self._pending_by_sysid* 儲存 PendingEntry 的地方
+---
+- *return_router()* 檢查並消耗 return_packet_ring 然後將封包分配到 pending 去
+- *__init__()* 這邊登入要創建的 service 到 ros2 系統
+- *handle_XXX()* 這邊是實現 service 的具體方法
+
+
 # 開發記錄
 
 ## 已實現功能
@@ -182,9 +223,9 @@
 7. 終端機介面控制
 8. 基礎載具流量觀測
 9. 載具狀態收集與彙整
-10. a. ros2 topic 應用開發介面
+10. a. ros2 topic 應用開發介面 (基礎框架)
+    b. ros2 service 應用開發介面 (基礎框架)
 
 ### 待開發功能
 5-1. 建立 serial 連線 並可以對接收器下達AT指令
 5-2. 模組化 serial 連線機制 以利後期擴容其他模組
-10. a. ros2  應用開發介面

src/fc_network_adapter/fc_network_adapter/mainOrchestrator.py

@@ -29,7 +29,7 @@ from .utils import acquireSerial, acquirePort
 from .utils.acquirePort import find_available_port
 
 logger = setup_logger(os.path.basename(__file__))
-VERSION_NO = "v0.60"
+PROJECT_VER = "v0.61"
 
 class PanelState:
     def __init__(self):
@@ -304,7 +304,7 @@ class ControlPanel:
                 # help_line = start_line + len(current_menu.children) + 2
                 help_line = height - 2
                 stdscr.addstr(help_line, 2, "操作: ↑↓選擇 Enter確認 ←返回上層 →進入下層", curses.A_DIM)
-                stdscr.addstr(height-1 , width-12, f" {VERSION_NO} ", curses.A_DIM)
+                stdscr.addstr(height-1 , width-12, f" {PROJECT_VER} ", curses.A_DIM)
 
                 stdscr.refresh()
 
@@ -1141,7 +1141,8 @@ class Orchestrator:
         self.fc_ros_manager.initialize()
         self.fc_ros_manager.status_publisher.rate_controller.topic_intervals = {
             'position': 1.0,
-            'attitude': 0.0,    
+            'position_ned': 1.0,
+            'attitude': 1.0,
             'velocity': 0.0,
             'battery': 1.0,
             'vfr_hud': 1.0,
@@ -1339,6 +1340,7 @@ class Orchestrator:
                 self.occupied_ip_ports[ip].append(port)
             else:
                 self.occupied_ip_ports[ip] = [port]
+
         # 外放資訊部分
         elif self.panelState.udp_info_temp["direction"] == "outbound":
             connection_string = f"udpout:{self.panelState.udp_info_temp['IP']}:{self.panelState.udp_info_temp['Port']}"
@@ -1558,8 +1560,27 @@ class Orchestrator:
 
 def main():
 
-    stop_evt = threading.Event()
+    # =========== 各項模組的版本先驗 ===========
+    # 除非你有在做這幾項模組的改版 不然動到這邊的版本號 代表執行環境有很大的問題!!!!!!
+    version_check = True
+    if mo.MODULE_VER != "1.50":
+        print("Module Version Error! : mavlinkObkect")
+        version_check = False
+    if mros.MODULE_VER != "1.50":
+        print("Module Version Error! : mavlinkROS2Nodes")
+        version_check = False
+    if mvv.MODULE_VER != "1.00":
+        print("Module Version Error! : mavlinkVehicleView")
+        version_check = False
+    if sm.MODULE_VER != "0.60":
+        print("Module Version Error! : serialManager")
+        version_check = False
+    if version_check == False:
+        print("Environment Obstacle! Check YOUR Execution System Path First!!")
+        return
+    # ========================================
 
+    stop_evt = threading.Event()
     def signal_handler(signum, frame):
         """處理 Ctrl+C 信號 藉此訊號 會結束下面的 while 迴圈 並逐步關閉各模組"""
         stop_evt.set()
@@ -1593,4 +1614,7 @@ if __name__ == "__main__":
 4. 註解掉無效代碼 action == "UPDATE_VEHICLES_LIST" 區塊
 5. 系統納入 mavlink ROS2 Manager 模組
 
+2025.04.13
+1. 加入各項模組的版本先驗 檢驗失敗就直接離開
+
 '''

src/fc_network_adapter/fc_network_adapter/mavlinkObject.py

@@ -28,9 +28,6 @@ mavlink_bridge:
     專門處理 stream_bridge_ring 裡面的訊息流
     會把訊息流解開後 存放到 mavlinkVehicleView.py 定義的載具結構視圖
 
-
-
-
 '''
 
 # 基礎功能的 import
@@ -61,6 +58,8 @@ from .utils import RingBuffer, setup_logger
 # ====================== 分割線 =====================
 
 logger = setup_logger(os.path.basename(__file__))
+MODULE_VER = "1.50"
+
 
 stream_bridge_ring = RingBuffer(capacity=1024, buffer_id=255)
 return_packet_ring = RingBuffer(capacity=256, buffer_id=254)
@@ -386,7 +385,7 @@ class mavlink_object:
             self.MAVLink = mav_common.MAVLink(self.mavlinkPipeline, srcSystem=254, srcComponent=191)
         
         # 記錄訊息過濾類型 (可選)
-        self.bridge_msg_types = set([0, 30, 33, 74, 147])  # 0 HEARTBEAT, 30 ATTITUDE, 33 GLOBAL_POSITION_INT, 74 VFR_HUD, 147 BATTERY_STATUS
+        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 
         self.return_msg_types = set([])
 
         # 轉發到別的 mavlink object 作為目標端口 的列表

src/fc_network_adapter/fc_network_adapter/mavlinkROS2Nodes.py

@@ -1,10 +1,15 @@
 """
 MAVLink ROS2 Nodes
-包含兩個獨立的 ROS2 Node：
+主要包含兩個獨立的 ROS2 Node :
 1. VehicleStatusPublisher - 發布載具狀態到 ROS2 topics
+    從 vehicle_registry 讀取狀態數據，頻率控制，模組化設計
 2. MavlinkCommandService - 提供 MAVLink 指令 service 介面
+    以最基礎的 mavlink microservice 會實現目標 
+    並不會包含額外的功能
+
+與一個節點管理器
+- fc_ros_manager
 
-從 vehicle_registry 讀取狀態數據，頻率控制，模組化設計
 """
 
 import os
@@ -22,10 +27,12 @@ from rclpy.executors import MultiThreadedExecutor
 import std_msgs.msg
 import sensor_msgs.msg
 import geometry_msgs.msg
+import nav_msgs.msg
 import mavros_msgs.msg
 
 # ROS2 Service imports
 import fc_interfaces.srv as fcsrv
+from .ackEnum import serviceAckResult
 
 # 自定義 imports
 from . import mavlinkVehicleView as mvv
@@ -33,12 +40,13 @@ from . import mavlinkObject as mo
 from .utils import setup_logger
 
 logger = setup_logger(os.path.basename(__file__))
-
+MODULE_VER = "1.50"
 
 # ============================================================================
-#                          頻率控制器
+#                       VehicleStatusPublisher Node
 # ============================================================================
 
+# 頻率控制器
 class PublishRateController:
     """發布頻率控制器 - 按時間間隔控制發布頻率"""
     
@@ -49,14 +57,17 @@ class PublishRateController:
         # 例如：'ekf_status': 1.0,  # EKF 狀態
         # ═══════════════════════════════════════════════════════════════
         # 各 topic 的發布間隔（秒）
+        # 注意 這邊是定義區 不要把參數寫在這裡 所以預設全部關閉
+        # 以這個專案 請看 mainOrchestrator.py 的 Orchestrator 初始化階段
         self.topic_intervals = {
-            'position': 0.5,       # GPS位置
+            'position': 0.0,       # GNSS位置
-            'attitude': 0.5,       # 姿態
+            'position_ned': 0.0,   # LOCAL_POSITION_NED (位置+速度)
-            'velocity': 0.5,       # 速度
+            'attitude': 0.0,       # 姿態 (pitch yaw row 與其加速狀態)
-            'battery': 1.0,        # 電池
+            'velocity': 0.0,       # 速度 (已經包含在 vfr_hud 未來移除)
-            'vfr_hud': 0.5,        # VFR HUD
+            'battery': 0.0,        # 電池
-            'mode': 1.0,           # 飛行模式
+            'vfr_hud': 0.0,        # VFR HUD (地速 空速 絕對高度 爬升率 航向 油門)
-            'summary': 1.0,        # 載具摘要
+            'mode': 0.0,           # 飛行模式 (已經在 summary 裡 未來移除)
+            'summary': 0.0,        # 載具摘要 (sysid 飛行模式 解鎖上鎖 gps狀態)
             # 在這裡新增更多 topics...
         }
         # 記錄每個 topic 的最後發布時間 {(sysid, topic): timestamp}
@@ -97,11 +108,6 @@ class PublishRateController:
         """重置所有計時器"""
         self.last_publish_time.clear()
 
-
-# ============================================================================
-#                       VehicleStatusPublisher Node
-# ============================================================================
-
 class VehicleStatusPublisher(Node):
     """
     載具狀態發布者 - 從 vehicle_registry 讀取數據並發布到 ROS2 topics
@@ -138,10 +144,8 @@ class VehicleStatusPublisher(Node):
         if not self.running:
             return
 
-        # 從 vehicle_registry 獲取所有載具
+        # 高頻快路徑：讀取 registry 的 immutable 快照
-        all_vehicles = mvv.vehicle_registry.get_all()
+        for sysid, vehicle in mvv.vehicle_registry.read_all():
-        
-        for sysid, vehicle in all_vehicles.items():
             self._publish_vehicle_status(vehicle)
     
     def _publish_vehicle_status(self, vehicle: mvv.VehicleView):
@@ -167,6 +171,7 @@ class VehicleStatusPublisher(Node):
         # ═══════════════════════════════════════════════════════════════
         # 發布各種狀態（通過頻率控制器判斷是否發布）
         self._publish_position(sysid, status)
+        self._publish_position_ned(sysid, status)
         self._publish_attitude(sysid, status)
         self._publish_velocity(sysid, status)
         self._publish_battery(sysid, status)
@@ -223,6 +228,42 @@ class VehicleStatusPublisher(Node):
         
         publisher.publish(msg)
 
+    def _publish_position_ned(self, sysid: int, status: mvv.ComponentStatus):
+        """發布 LOCAL_POSITION_NED（NED 座標，含位置與速度）"""
+        if not self.rate_controller.should_publish(sysid, 'position_ned'):
+            return
+
+        local_pos = status.custom_status.get('local_position')
+        if not local_pos:
+            return
+
+        required_keys = ('x', 'y', 'z', 'vx', 'vy', 'vz')
+        if any(local_pos.get(k) is None for k in required_keys):
+            return
+
+        publisher = self._get_or_create_publisher(
+            sysid, 'position_ned', nav_msgs.msg.Odometry
+        )
+
+        if publisher.get_subscription_count() == 0:
+            return
+
+        msg = nav_msgs.msg.Odometry()
+        msg.header.stamp = self.get_clock().now().to_msg()
+        msg.header.frame_id = 'local_ned'
+        msg.child_frame_id = 'base_link_ned'
+
+        # 保持 MAVLink LOCAL_POSITION_NED 座標定義，不進行 ENU 轉換
+        msg.pose.pose.position.x = float(local_pos['x'])
+        msg.pose.pose.position.y = float(local_pos['y'])
+        msg.pose.pose.position.z = float(local_pos['z'])
+
+        msg.twist.twist.linear.x = float(local_pos['vx'])
+        msg.twist.twist.linear.y = float(local_pos['vy'])
+        msg.twist.twist.linear.z = float(local_pos['vz'])
+
+        publisher.publish(msg)
+    
     def _publish_attitude(self, sysid: int, status: mvv.ComponentStatus):
         """發布姿態（IMU）"""
         if not self.rate_controller.should_publish(sysid, 'attitude'):
@@ -231,7 +272,6 @@ class VehicleStatusPublisher(Node):
         att = status.attitude
         if att.roll is None:
             return
-        
         publisher = self._get_or_create_publisher(sysid, 'attitude', sensor_msgs.msg.Imu)
         
         if publisher.get_subscription_count() == 0:
@@ -471,12 +511,17 @@ class MavlinkCommandService(Node):
     - 作為 service server 等待 client 請求
     - 接收請求，組裝 MAVLink 封包
     - 調用 mavlinkObject 發送封包
-    - 處理 ACK 等待和超時（未來實現）
+    - 處理 ACK 等待和超時
     
     設計理念：回歸 MAVLink 純粹結構
     - 只負責將 ROS2 請求轉換為 MAVLink 封包
     - 不預設功能（如 ARM/DISARM) 保持模組化
     - 高層應用可透過此 service 實現各種功能
+
+    講白話一點就是
+    每次接到一個 service 請求 要整個系統丟某種指令給載具時
+    會做兩件事 1."丟出mavlink封包" 2."創造一個臨時信箱 Pending"
+
     """
 
     serviceString_prefix = '/fc_network/vehicle'
@@ -487,8 +532,8 @@ class MavlinkCommandService(Node):
         # 狀態標記
         self.running = True
         
-        # mavlinkObject 的引用（將由外部設置）
+        # # mavlinkObject 的引用（將由外部設置） 用不到
-        self.mavlink_analyzer = None
+        # self.mavlink_analyzer = None
 
         # pending 旗標物件的儲存庫
         self._pending_by_sysid = {} # sysid(int) : PendingEntry
@@ -521,6 +566,15 @@ class MavlinkCommandService(Node):
             self.handle_command_long
         )
 
+        # ═══════════════════════════════════════════════════════════════════
+        # Service : 發送 SET_POSITION_TARGET_GLOBAL_INT (86)
+        # ═══════════════════════════════════════════════════════════════════
+        self.srv_pos_global_int = self.create_service(
+            fcsrv.MavPositionTargetGlobalInt,
+            self.serviceString_prefix + '/pos_global_int',
+            self.handle_set_position_target_global_int
+        )
+
         logger.info("MavlinkCommandService initialized")
 
     def _index(self, target_sysid, target_compid):
@@ -542,7 +596,8 @@ class MavlinkCommandService(Node):
 
     def return_router(self):
         '''
-        這邊是給外部迴圈呼叫的 消耗 return_packet_ring 裡接收到的 mavlink 封包
+        在節點管理器哪邊被呼叫
+        消耗 return_packet_ring 裡接收到的 mavlink 封包
         分送到各自的 pending 中 
         藉由 event.set() 解開 service 中的 block 
         '''
@@ -568,6 +623,7 @@ class MavlinkCommandService(Node):
     def handle_mav_timesync_ping(self, request, response):
         '''
         用 timesync 封包驗證來回的時間
+        隸屬於 MavLink PING Protocol
         '''
         # 設定失效回應
         response.success = False
@@ -612,7 +668,7 @@ class MavlinkCommandService(Node):
         fail_skip = False
         # 5) 等待回應封包
         if not evt.wait(timeout_sec):
-            response.message = "waiting Timeout - TIMESYNC"
+            response.message = "mavlink timeout - TIMESYNC"
             fail_skip = True
 
         # 6) 處理回應封包
@@ -633,35 +689,31 @@ class MavlinkCommandService(Node):
 
         return response
 
-    def handle_add_two_ints(self, request, response):
-        """測試用 Service Handler 未來刪除"""
-        logger.info(f"Received add_two_ints request: {request.a} + {request.b}")
-        response.sum = request.a + request.b
-        logger.info(
-            f"[add_two_ints] thread_ident={threading.get_ident()} time={time.time()}"
-        )
-        time.sleep(1)
-
-        return response
-
     # ═══════════════════════════════════════════════════════════════════════
-    # Service Handler: 發送 COMMAND_LONG
+    # Service Handler: 發送 COMMAND_LONG (76)
     # ═══════════════════════════════════════════════════════════════════════
     def handle_command_long(self, request, response):
+        '''
+        用 command long 丟出  MAV_CMD 
+        隸屬於 MavLink Command Protocol
+        '''
         # 設定失效回應
         response.success = False
         response.message = "Unknown error"
+        response.ack_result = serviceAckResult.UNKNOWN
         timeout_sec = request.timeout_sec
 
         # 1) 確認是否已經有相同 sysid 的其他需求正在 pending
         if request.target_sysid in self._pending_by_sysid:
             response.message = f"sysid {request.target_sysid} already has pending request"
+            response.ack_result = serviceAckResult.MAVLINK_BUSY
             return response
 
         # 2) 找到 socket 標地
         socketObject = self._index(request.target_sysid, request.target_compid)
         if socketObject is None:
             response.message = "This system id not found."
+            response.ack_result = serviceAckResult.MAVLINK_DEV_NOTFOUND
             return response
 
         # 3) 接收封包系統 的設定
@@ -691,7 +743,8 @@ class MavlinkCommandService(Node):
         fail_skip = False
         # 5) 等待回應封包
         if not evt.wait(timeout_sec):
-            response.message = "waiting Timeout - CommLONG"
+            response.message = "mavlink timeout - CommLONG"
+            response.ack_result = serviceAckResult.MAVLINK_TIMEOUT
             fail_skip = True
 
         # 6) 處理回應封包
@@ -711,6 +764,95 @@ class MavlinkCommandService(Node):
 
         return response
 
+    # ═══════════════════════════════════════════════════════════════════════
+    # Service Handler: 發送 SET_POSITION_TARGET_GLOBAL_INT (86)
+    # ═══════════════════════════════════════════════════════════════════════
+    def handle_set_position_target_global_int(self, request, response):
+        '''
+        隸屬於 MavLink Offboard Control Protocol
+        '''
+        # 設定失效回應
+        response.message = "Unknown error"
+        response.ack_result = serviceAckResult.UNKNOWN
+        timeout_sec = request.timeout_sec
+
+        # 1) 確認是否已經有相同 sysid 的其他需求正在 pending
+        if request.target_sysid in self._pending_by_sysid:
+            response.message = f"sysid {request.target_sysid} already has pending request"
+            response.ack_result = serviceAckResult.MAVLINK_BUSY
+            return response
+
+        # 2) 找到 socket 標地
+        socketObject = self._index(request.target_sysid, request.target_compid)
+        if socketObject is None:
+            response.message = "This system id not found."
+            response.ack_result = serviceAckResult.MAVLINK_DEV_NOTFOUND
+            return response
+
+        # 3) 接收封包系統 的設定
+        # 在 socket 那邊先把要的封包種類導流進來
+        expect_recieve_msg_id = 87 # POSITION_TARGET_GLOBAL_INT 
+        socketObject.set_return_message_types(list(socketObject.return_msg_types) + [expect_recieve_msg_id])
+        evt = threading.Event()
+        # 設定封包檢驗 
+        def match_fn(compare_msg):
+            if compare_msg.get_msgId() != expect_recieve_msg_id : 
+                return False
+            return True
+
+        # 4) 送出封包
+        socketObject.MAVLink.set_position_target_global_int_send(
+            request.time_boot_ms, 
+            request.target_sysid,
+            request.target_compid,
+            request.coordinate_frame,
+            request.type_mask,
+            request.lat_int, request.lon_int, request.alt,
+            request.vx, request.vy, request.vz,
+            request.afx, request.afy, request.afz,
+            request.yaw, request.yaw_rate
+        )
+
+        time.sleep(0.01) # 因應 mavlink 的廣播式回應 後置回應監聽 並且故意延遲10ms 讓系統代謝掉舊的87封包
+        _pending = PendingEntry(event = evt, match_fn = match_fn)
+        self._pending_by_sysid[request.target_sysid] = _pending
+
+        fail_skip = False
+        # 5) 等待回應封包
+        if not evt.wait(timeout_sec):
+            response.message = "mavlink timeout - SET POSITION 86"
+            response.ack_result = MAVLINK_SEND_BUT_NO_EXP_STEAM
+            fail_skip = True
+
+        # 6) 處理回應封包
+        if not fail_skip:
+            ack_msg = _pending.result_mav_msg
+
+            response.message = ""
+            response.ack_result = 0
+            response.r_time_boot_ms = ack_msg.time_boot_ms 
+            response.r_type_mask = ack_msg.type_mask
+            response.r_lat_int = ack_msg.lat_int
+            response.r_lon_int = ack_msg.lon_int
+            response.r_alt = ack_msg.alt
+            response.r_vx = ack_msg.vx
+            response.r_vy = ack_msg.vy
+            response.r_vz = ack_msg.vz
+            response.r_afx = ack_msg.afx
+            response.r_afy = ack_msg.afy
+            response.r_afz = ack_msg.afz
+            response.r_yaw = ack_msg.yaw
+            response.r_yaw_rate = ack_msg.yaw_rate
+
+        # 7) 接收封包系統 的重置
+        msg_types = list(socketObject.return_msg_types)
+        if expect_recieve_msg_id in msg_types:
+            msg_types.remove(expect_recieve_msg_id)
+        socketObject.set_return_message_types(msg_types)
+        del evt
+        self._pending_by_sysid.pop(request.target_sysid, None)
+
+        return response
 
     # ═══════════════════════════════════════════════════════════════════════
     # Service Handler: 參數請求
@@ -791,6 +933,20 @@ class MavlinkCommandService(Node):
         response.message = "Param request sent (placeholder implementation)"
         return response
 
+
+    def handle_add_two_ints(self, request, response):
+        """測試用 Service Handler 未來刪除"""
+        logger.info(f"Received add_two_ints request: {request.a} + {request.b}")
+        response.sum = request.a + request.b
+        logger.info(
+            f"[add_two_ints] thread_ident={threading.get_ident()} time={time.time()}"
+        )
+        time.sleep(1)
+
+        return response
+
+
+
     def stop(self):
         """停止服務"""
         self.running = False
@@ -837,7 +993,8 @@ class fc_ros_manager:
         
         try:
             # 初始化 ROS2
-            rclpy.init()
+            if not rclpy.ok():
+                rclpy.init(args=None)
             
             # 創建節點 node
             self.status_publisher = VehicleStatusPublisher()
@@ -973,7 +1130,15 @@ ros2_manager = fc_ros_manager()
 5. 預留 MavlinkCommandService 結構（稍後實現）
 
 2026.03.27
-1. 完成 ros2 service 結構與 timesync command_long 協議
+1. 完成 ros2 service 結構與 timesync 與 command_long 協議
 
+2026.04.07
+1. 完成 ros2 的 MavPositionTargetGlobalInt 區域
+2. 優化 response.ack_result 回傳值的資訊
+
+
+TODO 
+1. service 部分會需要跟 mavlinkobject 大量互動 也許需要考慮對方的生命週期
+2. 還沒測試 如果有失效狀態讓 fc_ros_manager 中斷了 如何恢復的問題
 '''
 

src/fc_network_adapter/fc_network_adapter/mavlinkVehicleView.py

@@ -5,14 +5,14 @@ VehicleView - Pure State Container
 """
 
 import os
-from typing import Dict, Optional, Any
+from typing import Dict, Optional, Any, Tuple
 from dataclasses import dataclass, field
 from enum import Enum
 
 from .utils import setup_logger
 
 logger = setup_logger(os.path.basename(__file__))
-
+MODULE_VER = "1.00"
 
 # ====================== Enums =====================
 
@@ -402,11 +402,18 @@ class VehicleViewRegistry:
     
     def __init__(self):
         self._vehicles: Dict[int, VehicleView] = {}
+        # 高頻讀取用 snapshot（弱一致性）
+        self._vehicles_snapshot_items: Tuple[Tuple[int, VehicleView], ...] = tuple()
+
+    def _refresh_snapshot(self) -> None:
+        """重建快照（僅在 key 集合變動時觸發）"""
+        self._vehicles_snapshot_items = tuple(self._vehicles.items())
     
     def register(self, sysid: int) -> VehicleView:
         """註冊新的載具視圖"""
         if sysid not in self._vehicles:
             self._vehicles[sysid] = VehicleView(sysid)
+            self._refresh_snapshot()
             logger.info(f"Registered new VehicleView for system {sysid}")
         return self._vehicles[sysid]
     
@@ -418,6 +425,7 @@ class VehicleViewRegistry:
         """註銷載具視圖"""
         if sysid in self._vehicles:
             del self._vehicles[sysid]
+            self._refresh_snapshot()
             logger.info(f"Unregistered VehicleView for system {sysid}")
             return True
         return False
@@ -426,9 +434,21 @@ class VehicleViewRegistry:
         """取得所有載具視圖"""
         return self._vehicles.copy()
 
+    def read_all(self) -> Tuple[Tuple[int, VehicleView], ...]:
+        """
+        高效讀取所有載具視圖快照（弱一致性）
+
+        注意:
+        - 回傳 immutable tuple 快照，適合高頻迭代
+        - 僅在 register/unregister/clear 後更新，可能不是最新資料
+        - 適合高頻讀取且可接受弱一致性的情境
+        """
+        return self._vehicles_snapshot_items
+    
     def clear(self) -> None:
         """清空所有載具視圖"""
         self._vehicles.clear()
+        self._refresh_snapshot()
         logger.info("Cleared all VehicleViews")
     
     def __len__(self) -> int:
@@ -449,5 +469,8 @@ vehicle_registry = VehicleViewRegistry()
 2026.01.16
 1. 新增 重置指定組件的封包統計 功能
 
+2026.04.13
+1. 新增 VehicleViewRegistry.read_all 方法 讓 ROS2 Node 組件更有效率的讀取其中的資料
+
 '''
 

src/fc_network_adapter/fc_network_adapter/serialManager.py

@@ -26,6 +26,7 @@ from .utils import setup_logger
 # ====================== 分割線 =====================
 
 logger = setup_logger(os.path.basename(__file__))
+MODULE_VER = "0.60"
 
 # ====================== 分割線 =====================
 

src/fc_network_apps/__init__.py

@@ -1,14 +1,10 @@
-from .longCommand import CommandLongClient, ChangeModeResult
+from .longCommand import CommandLongClient, CommandLongResult
-from .changeMode import change_mode
+from .navigation import PositionTargetGlobalIntClient, PositionTargetGlobalIntResult
-from .arm_disarm import arm_disarm
+
-from .takeoff import takeoff
-from .land import land
 
 __all__ = [
     "CommandLongClient",
-    "ChangeModeResult",
+    "PositionTargetGlobalIntClient",
-    "change_mode",
+    "CommandLongResult",
-    "arm_disarm",
+    "PositionTargetGlobalIntResult",
-    "takeoff",
-    "land",
 ]

src/fc_network_apps/arm_disarm.py

@@ -1,93 +0,0 @@
-"""Simple wrapper for arm/disarm via fc_network ROS2 service (MAV_CMD_COMPONENT_ARM_DISARM)."""
-
-from __future__ import annotations
-
-from dataclasses import dataclass
-from typing import Optional
-
-import rclpy
-from rclpy.node import Node
-
-from fc_interfaces.srv import MavCommandLong
-
-COMMAND_COMPONENT_ARM_DISARM = 400
-DEFAULT_SERVICE_NAME = "/fc_network/vehicle/send_command_long"
-DEFAULT_TIMEOUT_SEC = 2.0
-
-
-@dataclass
-class ArmDisarmResult:
-    success: bool
-    message: str
-    ack_result: int
-
-
-def arm_disarm(
-    *,
-    target_sysid: int,
-    arm: bool,
-    target_compid: int = 0,
-    confirmation: int = 0,
-    param2: float = 0.0,
-    timeout_sec: float = DEFAULT_TIMEOUT_SEC,
-    service_name: str = DEFAULT_SERVICE_NAME,
-) -> ArmDisarmResult:
-    """One-shot MAV_CMD_COMPONENT_ARM_DISARM (400) wrapper.
-
-    param1: 1.0 to arm, 0.0 to disarm.
-    param2: usually 0. Some stacks use 21196 for force-arm (ArduPilot); pass via param2 if needed.
-    """
-    rclpy.init(args=None)
-    node: Optional[Node] = None
-    try:
-        node = Node("fc_arm_disarm_client_once")
-        client = node.create_client(MavCommandLong, service_name)
-
-        if not client.wait_for_service(timeout_sec=timeout_sec):
-            return ArmDisarmResult(
-                success=False,
-                message=f"Service not available: {service_name}",
-                ack_result=-1,
-            )
-
-        req = MavCommandLong.Request()
-        req.target_sysid = target_sysid
-        req.target_compid = target_compid
-        req.command = COMMAND_COMPONENT_ARM_DISARM
-        req.confirmation = confirmation
-        req.param1 = 1.0 if arm else 0.0
-        req.param2 = float(param2)
-        req.param3 = 0.0
-        req.param4 = 0.0
-        req.param5 = 0.0
-        req.param6 = 0.0
-        req.param7 = 0.0
-        req.timeout_sec = float(timeout_sec)
-
-        future = client.call_async(req)
-        rclpy.spin_until_future_complete(node, future, timeout_sec=timeout_sec + 1.0)
-        if not future.done() or future.result() is None:
-            return ArmDisarmResult(
-                success=False,
-                message="Service call timeout or no response.",
-                ack_result=-1,
-            )
-
-        response = future.result()
-        return ArmDisarmResult(
-            success=response.success,
-            message=response.message,
-            ack_result=response.ack_result,
-        )
-    finally:
-        if node is not None:
-            node.destroy_node()
-        rclpy.shutdown()
-
-
-if __name__ == "__main__":
-    r = arm_disarm(target_sysid=3, arm=True)
-    print(
-        f"arm_disarm success={r.success}, "
-        f"ack_result={r.ack_result}, message='{r.message}'"
-    )

src/fc_network_apps/changeMode.py

@@ -1,99 +0,0 @@
-"""Simple wrapper for mode change via fc_network ROS2 service.
-
-Reference CLI command:
-ros2 service call /fc_network/vehicle/send_command_long \
-  fc_interfaces/srv/MavCommandLong \
-  "{target_sysid: 3, target_compid: 0, command: 176, confirmation: 0, \
-    param1: 1, param2: 4, param3: 0, param4: 0, param5: 0, param6: 0, \
-    param7: 0, timeout_sec: 2}"
-"""
-
-from __future__ import annotations
-
-from dataclasses import dataclass
-from typing import Optional
-
-import rclpy
-from rclpy.node import Node
-
-from fc_interfaces.srv import MavCommandLong
-
-COMMAND_DO_SET_MODE = 176
-DEFAULT_SERVICE_NAME = "/fc_network/vehicle/send_command_long"
-DEFAULT_TIMEOUT_SEC = 2.0
-
-@dataclass
-class ChangeModeResult:
-    """Return value for mode change requests."""
-
-    success: bool
-    message: str
-    ack_result: int
-
-def change_mode(
-    *,
-    target_sysid: int,
-    custom_mode: float,
-    target_compid: int = 0,
-    base_mode: float = 1.0,
-    confirmation: int = 0,
-    timeout_sec: float = DEFAULT_TIMEOUT_SEC,
-    service_name: str = DEFAULT_SERVICE_NAME,
-) -> ChangeModeResult:
-
-    """One-shot helper for collaborators who want minimal code."""
-    rclpy.init(args=None)
-    node: Optional[Node] = None
-    try:
-        node = Node("fc_change_mode_client_once")
-        client = node.create_client(MavCommandLong, service_name)
-
-        if not client.wait_for_service(timeout_sec=timeout_sec):
-            return ChangeModeResult(
-                success=False,
-                message=f"Service not available: {service_name}",
-                ack_result=-1,
-            )
-
-        req = MavCommandLong.Request()
-        req.target_sysid = target_sysid
-        req.target_compid = target_compid
-        req.command = COMMAND_DO_SET_MODE
-        req.confirmation = confirmation
-        req.param1 = float(base_mode)
-        req.param2 = float(custom_mode)
-        req.param3 = 0.0
-        req.param4 = 0.0
-        req.param5 = 0.0
-        req.param6 = 0.0
-        req.param7 = 0.0
-        req.timeout_sec = float(timeout_sec)
-
-        future = client.call_async(req)
-        rclpy.spin_until_future_complete(node, future, timeout_sec=timeout_sec + 1.0)
-        if not future.done() or future.result() is None:
-            return ChangeModeResult(
-                success=False,
-                message="Service call timeout or no response.",
-                ack_result=-1,
-            )
-
-        response = future.result()
-        return ChangeModeResult(
-            success=response.success,
-            message=response.message,
-            ack_result=response.ack_result,
-        )
-    finally:
-        if node is not None:
-            node.destroy_node()
-        rclpy.shutdown()
-
-
-if __name__ == "__main__":
-    # Example values are aligned with your terminal command.
-    result = change_mode(target_sysid=3, custom_mode=4)
-    print(
-        f"change_mode success={result.success}, "
-        f"ack_result={result.ack_result}, message='{result.message}'"
-    )

src/fc_network_apps/land.py

@@ -1,90 +0,0 @@
-"""Simple wrapper for land via fc_network ROS2 service."""
-
-from __future__ import annotations
-
-from dataclasses import dataclass
-from typing import Optional
-
-import rclpy
-from rclpy.node import Node
-
-from fc_interfaces.srv import MavCommandLong
-
-COMMAND_NAV_LAND = 21
-DEFAULT_SERVICE_NAME = "/fc_network/vehicle/send_command_long"
-DEFAULT_TIMEOUT_SEC = 2.0
-
-
-@dataclass
-class LandResult:
-    success: bool
-    message: str
-    ack_result: int
-
-
-def land(
-    *,
-    target_sysid: int,
-    target_compid: int = 0,
-    yaw_deg: float = 0.0,
-    latitude: Optional[float] = None,
-    longitude: Optional[float] = None,
-    altitude_m: float = 0.0,
-    timeout_sec: float = DEFAULT_TIMEOUT_SEC,
-    service_name: str = DEFAULT_SERVICE_NAME,
-) -> LandResult:
-    """One-shot MAV_CMD_NAV_LAND wrapper."""
-    rclpy.init(args=None)
-    node: Optional[Node] = None
-    try:
-        node = Node("fc_land_client_once")
-        client = node.create_client(MavCommandLong, service_name)
-
-        if not client.wait_for_service(timeout_sec=timeout_sec):
-            return LandResult(
-                success=False,
-                message=f"Service not available: {service_name}",
-                ack_result=-1,
-            )
-
-        req = MavCommandLong.Request()
-        req.target_sysid = target_sysid
-        req.target_compid = target_compid
-        req.command = COMMAND_NAV_LAND
-        req.confirmation = 0
-        req.param1 = 0.0
-        req.param2 = 0.0
-        req.param3 = 0.0
-        req.param4 = float(yaw_deg)
-        req.param5 = float(latitude) if latitude is not None else 0.0
-        req.param6 = float(longitude) if longitude is not None else 0.0
-        req.param7 = float(altitude_m)
-        req.timeout_sec = float(timeout_sec)
-
-        future = client.call_async(req)
-        rclpy.spin_until_future_complete(node, future, timeout_sec=timeout_sec + 1.0)
-        if not future.done() or future.result() is None:
-            return LandResult(
-                success=False,
-                message="Service call timeout or no response.",
-                ack_result=-1,
-            )
-
-        response = future.result()
-        return LandResult(
-            success=response.success,
-            message=response.message,
-            ack_result=response.ack_result,
-        )
-    finally:
-        if node is not None:
-            node.destroy_node()
-        rclpy.shutdown()
-
-
-if __name__ == "__main__":
-    result = land(target_sysid=3)
-    print(
-        f"land success={result.success}, "
-        f"ack_result={result.ack_result}, message='{result.message}'"
-    )

src/fc_network_apps/longCommand.py

@@ -1,8 +1,10 @@
+"""ROS2 client for fc_network MavCommandLong service (COMMAND_LONG 系列指令)."""
 
 from __future__ import annotations
 
 from dataclasses import dataclass
 from typing import Optional
+import asyncio
 
 import rclpy
 from rclpy.node import Node
@@ -10,30 +12,78 @@ from rclpy.node import Node
 from fc_interfaces.srv import MavCommandLong
 
 COMMAND_DO_SET_MODE = 176
+COMMAND_NAV_LAND = 21
+COMMAND_NAV_TAKEOFF = 22
+COMMAND_COMPONENT_ARM_DISARM = 400
+
 DEFAULT_SERVICE_NAME = "/fc_network/vehicle/send_command_long"
 DEFAULT_TIMEOUT_SEC = 2.0
 
 
 @dataclass
-class ChangeModeResult:
+class CommandLongResult:
-    """Return value for mode change requests."""
-
     success: bool
     message: str
     ack_result: int
 
-
 class CommandLongClient(Node):
-    """Small ROS2 client dedicated to change flight mode."""
+    """ROS2 client : 對同一個 send_command_long service 發送各種 MAV_CMD_*。"""
 
     def __init__(self, service_name: str = DEFAULT_SERVICE_NAME) -> None:
-        rclpy.init()
+        if not rclpy.ok():
-        super().__init__("fc_change_mode_client")
+            rclpy.init(args=None)
+        super().__init__("fc_command_long_client")
         self._client = self.create_client(MavCommandLong, service_name)
 
     def wait_for_service(self, timeout_sec: float = 3.0) -> bool:
         return self._client.wait_for_service(timeout_sec=timeout_sec)
 
+    def _send_command_long(
+        self,
+        *,
+        target_sysid: int,
+        target_compid: int,
+        command: int,
+        confirmation: int,
+        param1: float,
+        param2: float,
+        param3: float,
+        param4: float,
+        param5: float,
+        param6: float,
+        param7: float,
+        timeout_sec: float,
+    ) -> CommandLongResult:
+        req = MavCommandLong.Request()
+        req.target_sysid = target_sysid
+        req.target_compid = target_compid
+        req.command = command
+        req.confirmation = confirmation
+        req.param1 = param1
+        req.param2 = param2
+        req.param3 = param3
+        req.param4 = param4
+        req.param5 = param5
+        req.param6 = param6
+        req.param7 = param7
+        req.timeout_sec = float(timeout_sec)
+
+        future = self._client.call_async(req)
+        rclpy.spin_until_future_complete(self, future, timeout_sec=timeout_sec + 1.0)
+        if not future.done() or future.result() is None:
+            return CommandLongResult(
+                success=False,
+                message="Service timeout.",
+                ack_result=-1,
+            )
+
+        response = future.result()
+        return CommandLongResult(
+            success=response.success,
+            message=response.message,
+            ack_result=response.ack_result,
+        )
+
     def change_mode(
         self,
         *,
@@ -42,34 +92,212 @@ class CommandLongClient(Node):
         target_compid: int = 0,
         base_mode: float = 1.0,
         confirmation: int = 0,
-        timeout_sec: float = DEFAULT_TIMEOUT_SEC ) -> ChangeModeResult:
+        timeout_sec: float = DEFAULT_TIMEOUT_SEC,
-
+    ) -> CommandLongResult:
-        req = MavCommandLong.Request()
+        return self._send_command_long(
-        req.target_sysid = target_sysid
+            target_sysid=target_sysid,
-        req.target_compid = target_compid
+            target_compid=target_compid,
-        req.command = COMMAND_DO_SET_MODE
+            command=COMMAND_DO_SET_MODE,
-        req.confirmation = confirmation
+            confirmation=confirmation,
-        req.param1 = float(base_mode)
+            param1=float(base_mode),
-        req.param2 = float(custom_mode)
+            param2=float(custom_mode),
-        req.param3 = 0.0
+            param3=0.0,
-        req.param4 = 0.0
+            param4=0.0,
-        req.param5 = 0.0
+            param5=0.0,
-        req.param6 = 0.0
+            param6=0.0,
-        req.param7 = 0.0
+            param7=0.0,
-        req.timeout_sec = float(timeout_sec)
+            timeout_sec=timeout_sec,
-
+        )
-        future = self._client.call_async(req)
+
-        rclpy.spin_until_future_complete(self, future, timeout_sec=timeout_sec + 1.0)
+    def takeoff(
-        if not future.done() or future.result() is None:
+        self,
-            return ChangeModeResult(
+        *,
-                success=False,
+        target_sysid: int,
-                message="Service call timeout or no response.",
+        altitude_m: float,
-                ack_result=-1,
+        target_compid: int = 0,
+        min_pitch_deg: float = 0.0,
+        yaw_deg: float = 0.0,
+        latitude: Optional[float] = None,
+        longitude: Optional[float] = None,
+        timeout_sec: float = DEFAULT_TIMEOUT_SEC,
+    ) -> CommandLongResult:
+        return self._send_command_long(
+            target_sysid=target_sysid,
+            target_compid=target_compid,
+            command=COMMAND_NAV_TAKEOFF,
+            confirmation=0,
+            param1=float(min_pitch_deg),
+            param2=0.0,
+            param3=0.0,
+            param4=float(yaw_deg),
+            param5=float(latitude) if latitude is not None else 0.0,
+            param6=float(longitude) if longitude is not None else 0.0,
+            param7=float(altitude_m),
+            timeout_sec=timeout_sec,
+        )
+
+    def land(
+        self,
+        *,
+        target_sysid: int,
+        target_compid: int = 0,
+        yaw_deg: float = 0.0,
+        latitude: Optional[float] = None,
+        longitude: Optional[float] = None,
+        altitude_m: float = 0.0,
+        timeout_sec: float = DEFAULT_TIMEOUT_SEC,
+    ) -> CommandLongResult:
+        return self._send_command_long(
+            target_sysid=target_sysid,
+            target_compid=target_compid,
+            command=COMMAND_NAV_LAND,
+            confirmation=0,
+            param1=0.0,
+            param2=0.0,
+            param3=0.0,
+            param4=float(yaw_deg),
+            param5=float(latitude) if latitude is not None else 0.0,
+            param6=float(longitude) if longitude is not None else 0.0,
+            param7=float(altitude_m),
+            timeout_sec=timeout_sec,
+        )
+
+    def arm_disarm(
+        self,
+        *,
+        target_sysid: int,
+        arm: bool,
+        target_compid: int = 0,
+        confirmation: int = 0,
+        param2: float = 0.0,
+        timeout_sec: float = DEFAULT_TIMEOUT_SEC,
+    ) -> CommandLongResult:
+        return self._send_command_long(
+            target_sysid=target_sysid,
+            target_compid=target_compid,
+            command=COMMAND_COMPONENT_ARM_DISARM,
+            confirmation=confirmation,
+            param1=1.0 if arm else 0.0,
+            param2=float(param2),
+            param3=0.0,
+            param4=0.0,
+            param5=0.0,
+            param6=0.0,
+            param7=0.0,
+            timeout_sec=timeout_sec,
+        )
+
+    # ============================================================================
+    # 【新增】非阻塞 async 包裝方法（用於 GUI 的非阻塞調用）
+    # 這些方法在 ThreadPoolExecutor 中運行同步版本，以避免阻塞事件循環
+    # ============================================================================
+
+    async def change_mode_async(
+        self,
+        *,
+        target_sysid: int,
+        custom_mode: float,
+        target_compid: int = 0,
+        base_mode: float = 1.0,
+        confirmation: int = 0,
+        timeout_sec: float = DEFAULT_TIMEOUT_SEC,
+    ) -> CommandLongResult:
+        """非阻塞 async 版本的 change_mode（在 ThreadPoolExecutor 中運行）"""
+        from concurrent.futures import ThreadPoolExecutor
+        loop = asyncio.get_event_loop()
+        executor = ThreadPoolExecutor(max_workers=1)
+        return await loop.run_in_executor(
+            executor,
+            lambda: self.change_mode(
+                target_sysid=target_sysid,
+                custom_mode=custom_mode,
+                target_compid=target_compid,
+                base_mode=base_mode,
+                confirmation=confirmation,
+                timeout_sec=timeout_sec,
             )
+        )
 
-        response = future.result()
+    async def arm_disarm_async(
-        return ChangeModeResult(
+        self,
-            success=response.success,
+        *,
-            message=response.message,
+        target_sysid: int,
-            ack_result=response.ack_result,
+        arm: bool,
+        target_compid: int = 0,
+        confirmation: int = 0,
+        param2: float = 0.0,
+        timeout_sec: float = DEFAULT_TIMEOUT_SEC,
+    ) -> CommandLongResult:
+        """非阻塞 async 版本的 arm_disarm（在 ThreadPoolExecutor 中運行）"""
+        from concurrent.futures import ThreadPoolExecutor
+        loop = asyncio.get_event_loop()
+        executor = ThreadPoolExecutor(max_workers=1)
+        return await loop.run_in_executor(
+            executor,
+            lambda: self.arm_disarm(
+                target_sysid=target_sysid,
+                arm=arm,
+                target_compid=target_compid,
+                confirmation=confirmation,
+                param2=param2,
+                timeout_sec=timeout_sec,
+            )
+        )
+
+    async def takeoff_async(
+        self,
+        *,
+        target_sysid: int,
+        altitude_m: float,
+        target_compid: int = 0,
+        min_pitch_deg: float = 0.0,
+        yaw_deg: float = 0.0,
+        latitude: Optional[float] = None,
+        longitude: Optional[float] = None,
+        timeout_sec: float = DEFAULT_TIMEOUT_SEC,
+    ) -> CommandLongResult:
+        """非阻塞 async 版本的 takeoff（在 ThreadPoolExecutor 中運行）"""
+        from concurrent.futures import ThreadPoolExecutor
+        loop = asyncio.get_event_loop()
+        executor = ThreadPoolExecutor(max_workers=1)
+        return await loop.run_in_executor(
+            executor,
+            lambda: self.takeoff(
+                target_sysid=target_sysid,
+                altitude_m=altitude_m,
+                target_compid=target_compid,
+                min_pitch_deg=min_pitch_deg,
+                yaw_deg=yaw_deg,
+                latitude=latitude,
+                longitude=longitude,
+                timeout_sec=timeout_sec,
+            )
+        )
+
+    async def land_async(
+        self,
+        *,
+        target_sysid: int,
+        target_compid: int = 0,
+        yaw_deg: float = 0.0,
+        latitude: Optional[float] = None,
+        longitude: Optional[float] = None,
+        altitude_m: float = 0.0,
+        timeout_sec: float = DEFAULT_TIMEOUT_SEC,
+    ) -> CommandLongResult:
+        """非阻塞 async 版本的 land（在 ThreadPoolExecutor 中運行）"""
+        from concurrent.futures import ThreadPoolExecutor
+        loop = asyncio.get_event_loop()
+        executor = ThreadPoolExecutor(max_workers=1)
+        return await loop.run_in_executor(
+            executor,
+            lambda: self.land(
+                target_sysid=target_sysid,
+                target_compid=target_compid,
+                yaw_deg=yaw_deg,
+                latitude=latitude,
+                longitude=longitude,
+                altitude_m=altitude_m,
+                timeout_sec=timeout_sec,
+            )
         )

src/fc_network_apps/navigation.py

@@ -0,0 +1,262 @@
+"""ROS2 client for fc_network MavPositionTargetGlobalInt service (SET_POSITION_TARGET_GLOBAL_INT / Offboard)."""
+
+from __future__ import annotations
+
+import math
+from dataclasses import dataclass
+
+import rclpy
+from rclpy.node import Node
+
+from fc_interfaces.srv import MavPositionTargetGlobalInt
+
+DEFAULT_SERVICE_NAME = "/fc_network/vehicle/pos_global_int"
+DEFAULT_TIMEOUT_SEC = 2.0
+
+# MAV_FRAME
+MAV_FRAME_GLOBAL_RELATIVE_ALT_INT = 6 # 使用 WGS84 GNSS 座標系 高度為相對於 Home 點高度
+
+# POSITION_TARGET_TYPEMASK
+# 1  1      Ignore position x
+# 2  2      Ignore position y
+# 3  4      Ignore position z
+# 4  8      Ignore velocity x
+# 5  16     Ignore velocity y
+# 6  32     Ignore velocity z
+# 7  64     Ignore acceleration x
+# 8  128	Ignore acceleration y
+# 9  256	Ignore acceleration z
+# 10 512	Use force instead of acceleration
+# 11 1024	Ignore yaw
+# 12 2048   Ignore yaw rate
+# 13+       Not Used 
+#                 6543210987654321
+POSITION_ONLY = 0b0000110111111000 # 忽略速度、加速度、偏航
+
+
+# 回傳 echo 與送出值比對時，浮點欄位允許的絕對誤差（MAVLink float 轉傳常有小差）
+_FLOAT_ECHO_ABS_TOL = 1e-5
+def _float_echo_equal(a: float, b: float) -> bool:
+    return math.isclose(float(a), float(b), rel_tol=0.0, abs_tol=_FLOAT_ECHO_ABS_TOL)
+
+
+def _echo_position_target_matches(
+    *,
+    type_mask: int,
+    lat_int: int,
+    lon_int: int,
+    alt: float,
+    vx: float,
+    vy: float,
+    vz: float,
+    afx: float,
+    afy: float,
+    afz: float,
+    yaw: float,
+    yaw_rate: float,
+    time_boot_ms: int,
+    resp,
+) -> tuple[bool, str]:
+    """
+    比對本次送出的軌跡欄位與 server 填回的 r_* 是否一致。
+    若不符，回傳 (False, 簡短說明)；相符則 (True, "")。
+    """
+
+    _tm_sent = int(type_mask) & 0xFFF
+    _tm_recv = int(resp.r_type_mask) & 0xFFF
+    if _tm_recv != _tm_sent:
+        return False, f"type_mask echo: sent {_tm_sent:012b} != ack {_tm_recv:012b}"
+
+    coordinate_tolerance = 5 * 100
+    if abs(resp.r_lat_int - lat_int) >= coordinate_tolerance:
+        return False, f"lat_int echo: sent {lat_int} != ack {resp.r_lat_int}"
+    if abs(resp.r_lon_int - lon_int) >= coordinate_tolerance:
+        return False, f"lon_int echo: sent {lon_int} != ack {resp.r_lon_int}"
+    # TODO 我給飛控相對高度 它回給我絕對高度 看來這個比較方法 還有很多改良點 但是之後吧
+    # if not _float_echo_equal(resp.r_alt, alt):
+    #     return False, f"alt echo: sent {alt} != ack {resp.r_alt}"
+    if not _float_echo_equal(resp.r_vx, vx):
+        return False, f"vx echo: sent {vx} != ack {resp.r_vx}"
+    if not _float_echo_equal(resp.r_vy, vy):
+        return False, f"vy echo: sent {vy} != ack {resp.r_vy}"
+    if not _float_echo_equal(resp.r_vz, vz):
+        return False, f"vz echo: sent {vz} != ack {resp.r_vz}"
+    if not _float_echo_equal(resp.r_afx, afx):
+        return False, f"afx echo: sent {afx} != ack {resp.r_afx}"
+    if not _float_echo_equal(resp.r_afy, afy):
+        return False, f"afy echo: sent {afy} != ack {resp.r_afy}"
+    if not _float_echo_equal(resp.r_afz, afz):
+        return False, f"afz echo: sent {afz} != ack {resp.r_afz}"
+    if not _float_echo_equal(resp.r_yaw, yaw):
+        return False, f"yaw echo: sent {yaw} != ack {resp.r_yaw}"
+    if not _float_echo_equal(resp.r_yaw_rate, yaw_rate):
+        return False, f"yaw_rate echo: sent {yaw_rate} != ack {resp.r_yaw_rate}"
+    # if int(resp.r_time_boot_ms) != int(time_boot_ms):
+    #     return False, f"time_boot_ms echo: sent {time_boot_ms} != r {resp.r_time_boot_ms}"
+    return True, ""
+
+
+@dataclass
+class PositionTargetGlobalIntResult:
+    """對應 MavPositionTargetGlobalInt.srv 的 Response；success 依「送出與 r_* echo 是否一致」。"""
+    success: bool
+    message: str
+    ack_result: int
+    r_time_boot_ms: int = 0
+    r_type_mask: int = 0
+    r_lat_int: int = 0
+    r_lon_int: int = 0
+    r_alt: float = 0.0
+    r_vx: float = 0.0
+    r_vy: float = 0.0
+    r_vz: float = 0.0
+    r_afx: float = 0.0
+    r_afy: float = 0.0
+    r_afz: float = 0.0
+    r_yaw: float = 0.0
+    r_yaw_rate: float = 0.0
+
+class PositionTargetGlobalIntClient(Node):
+    """
+    ROS2 client：對 pos_global_int service 發送 SET_POSITION_TARGET_GLOBAL_INT (msg 86)，
+    並等待 POSITION_TARGET_GLOBAL_INT (msg 87) 回應（由 server 端處理）。
+    """
+
+    def __init__(self, service_name: str = DEFAULT_SERVICE_NAME) -> None:
+        if not rclpy.ok():
+            rclpy.init(args=None)
+        super().__init__("fc_position_target_global_int_client")
+        self._client = self.create_client(MavPositionTargetGlobalInt, service_name)
+
+    def wait_for_service(self, timeout_sec: float = 3.0) -> bool:
+        return self._client.wait_for_service(timeout_sec=timeout_sec)
+
+    def _send_position_target_global_int(
+        self,
+        *,
+        target_sysid: int,
+        target_compid: int,
+        time_boot_ms: int,
+        coordinate_frame: int,
+        type_mask: int,
+        lat_int: int,
+        lon_int: int,
+        alt: float,
+        vx: float,
+        vy: float,
+        vz: float,
+        afx: float,
+        afy: float,
+        afz: float,
+        yaw: float,
+        yaw_rate: float,
+        timeout_sec: float,
+    ) -> PositionTargetGlobalIntResult:
+        req = MavPositionTargetGlobalInt.Request()
+        req.target_sysid = target_sysid
+        req.target_compid = target_compid
+        req.time_boot_ms = time_boot_ms
+        req.coordinate_frame = coordinate_frame
+        req.type_mask = type_mask
+        req.lat_int = lat_int
+        req.lon_int = lon_int
+        req.alt = float(alt)
+        req.vx = float(vx)
+        req.vy = float(vy)
+        req.vz = float(vz)
+        req.afx = float(afx)
+        req.afy = float(afy)
+        req.afz = float(afz)
+        req.yaw = float(yaw)
+        req.yaw_rate = float(yaw_rate)
+        req.timeout_sec = float(timeout_sec)
+
+        future = self._client.call_async(req)
+        rclpy.spin_until_future_complete(self, future, timeout_sec=float(timeout_sec) + 1.0)
+        if not future.done() or future.result() is None:
+            return PositionTargetGlobalIntResult(
+                success=False,
+                message="Service timeout.",
+                ack_result=-1,
+            )
+
+        resp = future.result()
+
+        if resp.ack_result != 0:
+            return PositionTargetGlobalIntResult(
+                success=False,
+                message=resp.message,
+                ack_result=resp.ack_result,
+            )
+
+        echo_ok, echo_detail = _echo_position_target_matches(
+            type_mask=type_mask,
+            lat_int=lat_int,
+            lon_int=lon_int,
+            alt=alt,
+            vx=vx,
+            vy=vy,
+            vz=vz,
+            afx=afx,
+            afy=afy,
+            afz=afz,
+            yaw=yaw,
+            yaw_rate=yaw_rate,
+            time_boot_ms=time_boot_ms,
+            resp=resp,
+        )
+        # out_message = resp.message
+        # if not echo_ok:
+        #     out_message = echo_detail if not out_message else f"{echo_detail} | server: {out_message}"
+
+        return PositionTargetGlobalIntResult(
+            success=echo_ok,
+            message=echo_detail,
+            ack_result=resp.ack_result,
+            r_time_boot_ms=int(resp.r_time_boot_ms),
+            r_type_mask=int(resp.r_type_mask),
+            r_lat_int=int(resp.r_lat_int),
+            r_lon_int=int(resp.r_lon_int),
+            r_alt=float(resp.r_alt),
+            r_vx=float(resp.r_vx),
+            r_vy=float(resp.r_vy),
+            r_vz=float(resp.r_vz),
+            r_afx=float(resp.r_afx),
+            r_afy=float(resp.r_afy),
+            r_afz=float(resp.r_afz),
+            r_yaw=float(resp.r_yaw),
+            r_yaw_rate=float(resp.r_yaw_rate),
+        )
+
+    def goto_position_only(
+        self,
+        *,
+        target_sysid: int,
+        target_compid: int = 0,
+        latitude_deg: float,
+        longitude_deg: float,
+        alt: float = 0.0,
+        timeout_sec: float,
+    ):
+        lat_int = int(round(latitude_deg * 1e7))
+        lon_int = int(round(longitude_deg * 1e7))
+        
+        return self._send_position_target_global_int(
+            target_sysid=target_sysid,
+            target_compid=target_compid,
+            time_boot_ms= 0,
+            coordinate_frame=MAV_FRAME_GLOBAL_RELATIVE_ALT_INT,
+            type_mask=POSITION_ONLY,
+            lat_int=lat_int,
+            lon_int=lon_int,
+            alt=alt,
+            vx=0,
+            vy=0,
+            vz=0,
+            afx=0,
+            afy=0,
+            afz=0,
+            yaw=0,
+            yaw_rate=0,
+            timeout_sec=timeout_sec,
+        )

src/fc_network_apps/takeoff.py

@@ -1,91 +0,0 @@
-"""Simple wrapper for takeoff via fc_network ROS2 service."""
-
-from __future__ import annotations
-
-from dataclasses import dataclass
-from typing import Optional
-
-import rclpy
-from rclpy.node import Node
-
-from fc_interfaces.srv import MavCommandLong
-
-COMMAND_NAV_TAKEOFF = 22
-DEFAULT_SERVICE_NAME = "/fc_network/vehicle/send_command_long"
-DEFAULT_TIMEOUT_SEC = 2.0
-
-
-@dataclass
-class TakeoffResult:
-    success: bool
-    message: str
-    ack_result: int
-
-
-def takeoff(
-    *,
-    target_sysid: int,
-    altitude_m: float,
-    target_compid: int = 0,
-    min_pitch_deg: float = 0.0,
-    yaw_deg: float = 0.0,
-    latitude: Optional[float] = None,
-    longitude: Optional[float] = None,
-    timeout_sec: float = DEFAULT_TIMEOUT_SEC,
-    service_name: str = DEFAULT_SERVICE_NAME,
-) -> TakeoffResult:
-    """One-shot MAV_CMD_NAV_TAKEOFF wrapper."""
-    rclpy.init(args=None)
-    node: Optional[Node] = None
-    try:
-        node = Node("fc_takeoff_client_once")
-        client = node.create_client(MavCommandLong, service_name)
-
-        if not client.wait_for_service(timeout_sec=timeout_sec):
-            return TakeoffResult(
-                success=False,
-                message=f"Service not available: {service_name}",
-                ack_result=-1,
-            )
-
-        req = MavCommandLong.Request()
-        req.target_sysid = target_sysid
-        req.target_compid = target_compid
-        req.command = COMMAND_NAV_TAKEOFF
-        req.confirmation = 0
-        req.param1 = float(min_pitch_deg)
-        req.param2 = 0.0
-        req.param3 = 0.0
-        req.param4 = float(yaw_deg)
-        req.param5 = float(latitude) if latitude is not None else 0.0
-        req.param6 = float(longitude) if longitude is not None else 0.0
-        req.param7 = float(altitude_m)
-        req.timeout_sec = float(timeout_sec)
-
-        future = client.call_async(req)
-        rclpy.spin_until_future_complete(node, future, timeout_sec=timeout_sec + 1.0)
-        if not future.done() or future.result() is None:
-            return TakeoffResult(
-                success=False,
-                message="Service call timeout or no response.",
-                ack_result=-1,
-            )
-
-        response = future.result()
-        return TakeoffResult(
-            success=response.success,
-            message=response.message,
-            ack_result=response.ack_result,
-        )
-    finally:
-        if node is not None:
-            node.destroy_node()
-        rclpy.shutdown()
-
-
-if __name__ == "__main__":
-    result = takeoff(target_sysid=3, altitude_m=10.0)
-    print(
-        f"takeoff success={result.success}, "
-        f"ack_result={result.ack_result}, message='{result.message}'"
-    )

src/someotherpkg/src/example2_change_mode.py

@@ -1,55 +0,0 @@
-
-
-from fc_network_apps import CommandLongClient
-import time
-
-def main():
-    # Equivalent to:
-    # ros2 service call /fc_network/vehicle/send_command_long ... param1:1 param2:4
-    commandAPI = CommandLongClient()
-    result = commandAPI.change_mode(
-        target_sysid=3,
-        target_compid=0,
-        base_mode=1.0,
-        custom_mode=4.0,
-        timeout_sec=2.0,
-    )
-
-    print("=== change mode result ===")
-    print(f"success   : {result.success}")
-    print(f"ack_result: {result.ack_result}")
-    print(f"message   : {result.message}")
-
-    time.sleep(1)
-
-    result = commandAPI.change_mode(
-        target_sysid=3,
-        target_compid=0,
-        base_mode=1.0,
-        custom_mode=3.0,
-        timeout_sec=2.0,
-    )
-
-    print("=== change mode result ===")
-    print(f"success   : {result.success}")
-    print(f"ack_result: {result.ack_result}")
-    print(f"message   : {result.message}")
-
-    time.sleep(1)
-    
-    result = commandAPI.change_mode(
-        target_sysid=3,
-        target_compid=0,
-        base_mode=1.0,
-        custom_mode=5.0,
-        timeout_sec=2.0,
-    )
-
-    print("=== change mode result ===")
-    print(f"success   : {result.success}")
-    print(f"ack_result: {result.ack_result}")
-    print(f"message   : {result.message}")
-
-
-if __name__ == "__main__":
-    main()

src/someotherpkg/src/example_arm_disarm.py

@@ -1,30 +0,0 @@
-"""Usage example for arm/disarm helper.
-
-Run from repo root with module mode:
-    python -m someotherpkg.src.example_arm_disarm
-"""
-
-from fc_network_apps import arm_disarm
-
-
-def main() -> None:
-    # MAV_CMD_COMPONENT_ARM_DISARM (command=400)
-    # param1: 1 = arm, 0 = disarm
-    result = arm_disarm(
-        target_sysid=3,
-        target_compid=0,
-        arm=True,
-        timeout_sec=2.0,
-    )
-
-    print("=== arm result ===")
-    print(f"success   : {result.success}")
-    print(f"ack_result: {result.ack_result}")
-    print(f"message   : {result.message}")
-
-    # To disarm instead:
-    # result = arm_disarm(target_sysid=3, target_compid=0, arm=False, timeout_sec=2.0)
-
-
-if __name__ == "__main__":
-    main()

src/someotherpkg/src/example_change_mode.py

@@ -1,29 +0,0 @@
-"""Usage example for collaborators.
-
-Run after sourcing your ROS2 workspace:
-    source install/local_setup.bash
-    python src/fc_network_apps/example_change_mode.py
-"""
-
-from fc_network_apps import change_mode
-
-
-def main() -> None:
-    # Equivalent to:
-    # ros2 service call /fc_network/vehicle/send_command_long ... param1:1 param2:4
-    result = change_mode(
-        target_sysid=3,
-        target_compid=0,
-        base_mode=1.0,
-        custom_mode=4.0,
-        timeout_sec=2.0,
-    )
-
-    print("=== change mode result ===")
-    print(f"success   : {result.success}")
-    print(f"ack_result: {result.ack_result}")
-    print(f"message   : {result.message}")
-
-
-if __name__ == "__main__":
-    main()

src/someotherpkg/src/example_land.py

@@ -1,27 +0,0 @@
-"""Usage example for land helper.
-
-Run from repo root with module mode:
-    python -m someotherpkg.src.example_land
-"""
-
-from fc_network_apps import land
-
-
-def main() -> None:
-    # MAV_CMD_NAV_LAND (command=21)
-    # This example asks vehicle sysid=3 to land.
-    result = land(
-        target_sysid=3,
-        target_compid=0,
-        yaw_deg=0.0,
-        timeout_sec=2.0,
-    )
-
-    print("=== land result ===")
-    print(f"success   : {result.success}")
-    print(f"ack_result: {result.ack_result}")
-    print(f"message   : {result.message}")
-
-
-if __name__ == "__main__":
-    main()

src/someotherpkg/src/example_takeoff.py

@@ -1,28 +0,0 @@
-"""Usage example for takeoff helper.
-
-Run from repo root with module mode:
-    python -m someotherpkg.src.example_takeoff
-"""
-
-from fc_network_apps import takeoff
-
-
-def main() -> None:
-    # MAV_CMD_NAV_TAKEOFF (command=22)
-    # This example asks vehicle sysid=3 to take off to 10 meters.
-    result = takeoff(
-        target_sysid=3,
-        target_compid=0,
-        altitude_m=10.0,
-        yaw_deg=0.0,
-        timeout_sec=2.0,
-    )
-
-    print("=== takeoff result ===")
-    print(f"success   : {result.success}")
-    print(f"ack_result: {result.ack_result}")
-    print(f"message   : {result.message}")
-
-
-if __name__ == "__main__":
-    main()

src/someotherpkg/src/example_wholeMoving.py

@@ -0,0 +1,111 @@
+
+
+import fc_network_apps as fap
+import time
+
+def main():
+    commandAPI = fap.CommandLongClient()
+    navAPI = fap.PositionTargetGlobalIntClient()
+
+    target_sysid = 3
+
+
+    print("=== change mode ===")
+    result = fap.CommandLongResult(success=False,message="",ack_result=-1)
+    while not result.success:
+        result.ack_result -= 1
+        result = commandAPI.change_mode(
+            target_sysid=target_sysid,
+            base_mode=1.0,
+            custom_mode=4.0,
+            timeout_sec=3.0,
+        )
+        print(f"success   : {result.success}")
+        print(f"ack_result: {result.ack_result}")
+        print(f"message   : {result.message}")
+        time.sleep(1)
+        if result.ack_result < -3:
+            return
+
+    print("=== arm vehicle ===")
+    result = fap.CommandLongResult(success=False,message="",ack_result=-1)
+    while not result.success:
+        result.ack_result -= 1
+        result = commandAPI.arm_disarm(
+            target_sysid=target_sysid,
+            arm = True,
+        )
+        print(f"success   : {result.success}")
+        print(f"ack_result: {result.ack_result}")
+        print(f"message   : {result.message}")
+        time.sleep(1)
+        if result.ack_result < -3:
+            return
+
+    print("=== takeoff ===")
+    result = fap.CommandLongResult(success=False,message="",ack_result=-1)
+    while not result.success:
+        result.ack_result -= 1
+        result = commandAPI.takeoff(
+            target_sysid=target_sysid,
+            altitude_m = 100.0,
+        )
+        print(f"success   : {result.success}")
+        print(f"ack_result: {result.ack_result}")
+        print(f"message   : {result.message}")
+        time.sleep(1)
+        if result.ack_result < -3:
+            return
+
+
+    time.sleep(15)
+
+    print("=== set_position_target_global_int_deg ===")
+    result_deg = navAPI.goto_position_only(
+        target_sysid=target_sysid,
+        latitude_deg=-35.376655,
+        longitude_deg=149.157011,
+        alt=150.0,
+        timeout_sec=3.0,
+    )
+    print(f"success   : {result_deg.success}")
+    print(f"ack_result: {result_deg.ack_result}")
+    print(f"message   : {result_deg.message}")
+    print(f"r_lat_int : {result_deg.r_lat_int}  r_lon_int: {result_deg.r_lon_int}")
+
+    time.sleep(180)
+
+    result_deg = navAPI.goto_position_only(
+        target_sysid=target_sysid,
+        latitude_deg=-35.3632621,
+        longitude_deg=149.1652375,
+        alt=100.0,
+        timeout_sec=3.0,
+    )
+    print("=== set_position_target_global_int_deg ===")
+    print(f"success   : {result_deg.success}")
+    print(f"ack_result: {result_deg.ack_result}")
+    print(f"message   : {result_deg.message}")
+    print(f"r_lat_int : {result_deg.r_lat_int}  r_lon_int: {result_deg.r_lon_int}")
+
+    time.sleep(180)
+
+    print("=== land ===")
+    result = fap.CommandLongResult(success=False,message="",ack_result=-1)
+    while not result.success:
+        result.ack_result -= 1
+        result = commandAPI.land(
+            target_sysid=3,
+            target_compid=0,
+        )
+        print(f"success   : {result.success}")
+        print(f"ack_result: {result.ack_result}")
+        print(f"message   : {result.message}")
+        time.sleep(1)
+        if result.ack_result < -3:
+            return
+
+if __name__ == "__main__":
+    main()
+
+

src/unitdev01/unitdev01/comm_panel.py

@@ -0,0 +1,398 @@
+#!/usr/bin/env python3
+from PyQt6.QtWidgets import (QWidget, QVBoxLayout, QHBoxLayout, QLabel, 
+                            QPushButton, QLineEdit)
+from PyQt6.QtCore import pyqtSignal
+
+class CommPanel(QWidget):
+    """通讯设置面板类"""
+    
+    # 定义信号
+    udp_connection_added = pyqtSignal(str, int)  # ip, port
+    udp_connection_toggled = pyqtSignal(dict, QPushButton, QLabel)  # conn, btn, status_label
+    udp_connection_removed = pyqtSignal(dict, QWidget)  # conn, panel
+    ws_connection_added = pyqtSignal(str)  # url
+    ws_connection_toggled = pyqtSignal(dict, QPushButton, QLabel)  # conn, btn, status_label
+    ws_connection_removed = pyqtSignal(dict, QWidget)  # conn, panel
+    status_message = pyqtSignal(str, int)  # message, timeout
+    
+    def __init__(self, parent=None):
+        super().__init__(parent)
+        self.udp_connections = []
+        self.ws_connections = []
+        self._init_ui()
+    
+    def _init_ui(self):
+        """初始化UI"""
+        layout = QVBoxLayout(self)
+        layout.setContentsMargins(10, 10, 10, 10)
+        layout.setSpacing(10)
+        
+        # ========== UDP MAVLink 區域 ==========
+        udp_title = QLabel("UDP")
+        udp_title.setStyleSheet("""
+            color: #DDD;
+            font-size: 14px;
+            font-weight: bold;
+            padding: 5px;
+            background-color: #333;
+            border-radius: 4px;
+        """)
+        layout.addWidget(udp_title)
+        
+        # UDP 連接列表容器
+        self.udp_list_widget = QWidget()
+        self.udp_list_layout = QVBoxLayout(self.udp_list_widget)
+        self.udp_list_layout.setContentsMargins(0, 0, 0, 0)
+        self.udp_list_layout.setSpacing(5)
+        layout.addWidget(self.udp_list_widget)
+        
+        # UDP 添加新連接區域
+        add_udp_widget = QWidget()
+        add_udp_layout = QHBoxLayout(add_udp_widget)
+        add_udp_layout.setContentsMargins(0, 0, 0, 0)
+        
+        self.udp_ip_input = QLineEdit()
+        self.udp_ip_input.setText("127.0.0.1")
+        self.udp_ip_input.setPlaceholderText("IP")
+        self.udp_ip_input.setStyleSheet("""
+            QLineEdit {
+                background-color: #333;
+                color: #DDD;
+                border: 1px solid #555;
+                border-radius: 4px;
+                padding: 5px;
+            }
+        """)
+        
+        self.udp_port_input = QLineEdit()
+        self.udp_port_input.setText("14540")
+        self.udp_port_input.setPlaceholderText("Port")
+        self.udp_port_input.setFixedWidth(80)
+        self.udp_port_input.setStyleSheet("""
+            QLineEdit {
+                background-color: #333;
+                color: #DDD;
+                border: 1px solid #555;
+                border-radius: 4px;
+                padding: 5px;
+            }
+        """)
+        
+        add_udp_btn = QPushButton("添加")
+        add_udp_btn.clicked.connect(self._handle_add_udp)
+        add_udp_btn.setStyleSheet("""
+            QPushButton {
+                background-color: #4CAF50;
+                color: white;
+                border: none;
+                padding: 6px 12px;
+                border-radius: 4px;
+                min-width: 30px;
+            }
+            QPushButton:hover { background-color: #45a049; }
+        """)
+        
+        add_udp_layout.addWidget(QLabel("IP:", styleSheet="color: #DDD;"))
+        add_udp_layout.addWidget(self.udp_ip_input)
+        add_udp_layout.addWidget(QLabel("Port:", styleSheet="color: #DDD;"))
+        add_udp_layout.addWidget(self.udp_port_input)
+        add_udp_layout.addWidget(add_udp_btn)
+        
+        layout.addWidget(add_udp_widget)
+        
+        # 分隔線
+        separator = QWidget()
+        separator.setFixedHeight(20)
+        layout.addWidget(separator)
+        
+        # ========== WebSocket 區域 ==========
+        ws_title = QLabel("WebSocket")
+        ws_title.setStyleSheet("""
+            color: #DDD;
+            font-size: 14px;
+            font-weight: bold;
+            padding: 5px;
+            background-color: #333;
+            border-radius: 4px;
+        """)
+        layout.addWidget(ws_title)
+        
+        # WebSocket 連接列表容器
+        self.ws_list_widget = QWidget()
+        self.ws_list_layout = QVBoxLayout(self.ws_list_widget)
+        self.ws_list_layout.setContentsMargins(0, 0, 0, 0)
+        self.ws_list_layout.setSpacing(5)
+        layout.addWidget(self.ws_list_widget)
+        
+        # WebSocket 添加新連接區域
+        add_ws_widget = QWidget()
+        add_ws_layout = QHBoxLayout(add_ws_widget)
+        add_ws_layout.setContentsMargins(0, 0, 0, 0)
+        
+        self.ws_url_input = QLineEdit()
+        self.ws_url_input.setPlaceholderText("host")
+        self.ws_url_input.setStyleSheet("""
+            QLineEdit {
+                background-color: #333;
+                color: #DDD;
+                border: 1px solid #555;
+                border-radius: 4px;
+                padding: 5px;
+            }
+        """)
+        
+        add_ws_btn = QPushButton("添加")
+        add_ws_btn.clicked.connect(self._handle_add_ws)
+        add_ws_btn.setStyleSheet("""
+            QPushButton {
+                background-color: #4CAF50;
+                color: white;
+                border: none;
+                padding: 6px 12px;
+                border-radius: 4px;
+                min-width: 30px;
+            }
+            QPushButton:hover { background-color: #45a049; }
+        """)
+        
+        add_ws_layout.addWidget(QLabel("URL:", styleSheet="color: #DDD;"))
+        add_ws_layout.addWidget(self.ws_url_input)
+        add_ws_layout.addWidget(add_ws_btn)
+        
+        layout.addWidget(add_ws_widget)
+        layout.addStretch()
+    
+    def _handle_add_udp(self):
+        """處理添加 UDP 連接"""
+        ip = self.udp_ip_input.text().strip()
+        port_text = self.udp_port_input.text().strip()
+        
+        if not ip or not port_text:
+            self.status_message.emit("請輸入 IP 和 Port", 3000)
+            return
+        
+        try:
+            port = int(port_text)
+            if port < 1 or port > 65535:
+                raise ValueError("Port 超出範圍")
+        except ValueError:
+            self.status_message.emit("Port 必須是 1-65535 的數字", 3000)
+            return
+        
+        # 檢查是否已存在相同連接
+        for conn in self.udp_connections:
+            if conn['ip'] == ip and conn['port'] == port:
+                self.status_message.emit("連接已存在", 3000)
+                return
+        
+        # 發送信號通知主窗口
+        self.udp_connection_added.emit(ip, port)
+        
+        # 清空輸入框
+        self.udp_ip_input.clear()
+        self.udp_port_input.clear()
+    
+    def _handle_add_ws(self):
+        """處理添加 WebSocket 連接"""
+        input_url = self.ws_url_input.text().strip()
+        
+        if not input_url:
+            self.status_message.emit("請輸入 WebSocket URL", 3000)
+            return
+        
+        # 自動添加 ws:// 前綴
+        if not input_url.startswith('ws://') and not input_url.startswith('wss://'):
+            url = f'ws://{input_url}'
+        else:
+            url = input_url
+        
+        # 基本 URL 格式驗證
+        try:
+            if '://' in url:
+                parts = url.split('://', 1)
+                if len(parts) == 2 and ':' not in parts[1]:
+                    self.status_message.emit("URL 格式錯誤，需要包含端口號 (例如: 127.0.0.1:8756)", 3000)
+                    return
+        except:
+            self.status_message.emit("URL 格式錯誤", 3000)
+            return
+        
+        # 檢查是否已存在相同連接
+        for conn in self.ws_connections:
+            if conn['url'] == url:
+                self.status_message.emit("連接已存在", 3000)
+                return
+        
+        # 發送信號通知主窗口
+        self.ws_connection_added.emit(url)
+        
+        # 清空輸入框
+        self.ws_url_input.clear()
+    
+    def create_udp_connection_panel(self, conn):
+        """創建 UDP 連接面板"""
+        panel = QWidget()
+        panel.setStyleSheet("""
+            QWidget {
+                background-color: #2A2A2A;
+                border-radius: 6px;
+                padding: 8px;
+                border: 1px solid #444;
+            }
+        """)
+        
+        layout = QHBoxLayout(panel)
+        layout.setContentsMargins(8, 8, 8, 8)
+        
+        # 連接資訊
+        info_label = QLabel(f"{conn['name']} - {conn['ip']}:{conn['port']}")
+        info_label.setStyleSheet("color: #DDD; font-size: 12px;")
+        
+        # 狀態指示器
+        status_label = QLabel("●")
+        if conn.get('enabled', False):
+            status_label.setStyleSheet("color: #4CAF50; font-size: 16px;")
+            status_label.setToolTip("運行中")
+        else:
+            status_label.setStyleSheet("color: #888; font-size: 16px;")
+            status_label.setToolTip("已停止")
+        
+        # 控制按鈕
+        toggle_btn = QPushButton("停止" if conn.get('enabled', False) else "啟動")
+        toggle_btn.setFixedWidth(60)
+        toggle_btn.clicked.connect(lambda: self.udp_connection_toggled.emit(conn, toggle_btn, status_label))
+        toggle_btn.setStyleSheet("""
+            QPushButton {
+                background-color: #444;
+                color: #DDD;
+                border: none;
+                padding: 4px 8px;
+                border-radius: 3px;
+                font-size: 11px;
+            }
+            QPushButton:hover { background-color: #555; }
+        """)
+        
+        remove_btn = QPushButton("移除")
+        remove_btn.setFixedWidth(60)
+        remove_btn.clicked.connect(lambda: self.udp_connection_removed.emit(conn, panel))
+        remove_btn.setStyleSheet("""
+            QPushButton {
+                background-color: #d32f2f;
+                color: white;
+                border: none;
+                padding: 4px 8px;
+                border-radius: 3px;
+                font-size: 11px;
+            }
+            QPushButton:hover { background-color: #b71c1c; }
+        """)
+        
+        layout.addWidget(status_label)
+        layout.addWidget(info_label)
+        layout.addStretch()
+        layout.addWidget(toggle_btn)
+        layout.addWidget(remove_btn)
+        
+        # 儲存引用
+        panel.connection = conn
+        panel.toggle_btn = toggle_btn
+        panel.status_label = status_label
+        
+        return panel
+    
+    def create_ws_connection_panel(self, conn):
+        """創建 WebSocket 連接面板"""
+        panel = QWidget()
+        panel.setStyleSheet("""
+            QWidget {
+                background-color: #2A2A2A;
+                border-radius: 6px;
+                padding: 8px;
+                border: 1px solid #444;
+            }
+        """)
+        
+        layout = QHBoxLayout(panel)
+        layout.setContentsMargins(8, 8, 8, 8)
+        
+        # 連接資訊
+        info_label = QLabel(f"{conn['name']} - {conn['url']}")
+        info_label.setStyleSheet("color: #DDD; font-size: 12px;")
+        
+        # 狀態指示器
+        status_label = QLabel("●")
+        if conn.get('enabled', False):
+            status_label.setStyleSheet("color: #4CAF50; font-size: 16px;")
+            status_label.setToolTip("運行中")
+        else:
+            status_label.setStyleSheet("color: #888; font-size: 16px;")
+            status_label.setToolTip("已停止")
+        
+        # 控制按鈕
+        toggle_btn = QPushButton("停止" if conn.get('enabled', False) else "啟動")
+        toggle_btn.setFixedWidth(60)
+        toggle_btn.clicked.connect(lambda: self.ws_connection_toggled.emit(conn, toggle_btn, status_label))
+        toggle_btn.setStyleSheet("""
+            QPushButton {
+                background-color: #444;
+                color: #DDD;
+                border: none;
+                padding: 4px 8px;
+                border-radius: 3px;
+                font-size: 11px;
+            }
+            QPushButton:hover { background-color: #555; }
+        """)
+        
+        remove_btn = QPushButton("移除")
+        remove_btn.setFixedWidth(60)
+        remove_btn.clicked.connect(lambda: self.ws_connection_removed.emit(conn, panel))
+        remove_btn.setStyleSheet("""
+            QPushButton {
+                background-color: #d32f2f;
+                color: white;
+                border: none;
+                padding: 4px 8px;
+                border-radius: 3px;
+                font-size: 11px;
+            }
+            QPushButton:hover { background-color: #b71c1c; }
+        """)
+        
+        layout.addWidget(status_label)
+        layout.addWidget(info_label)
+        layout.addStretch()
+        layout.addWidget(toggle_btn)
+        layout.addWidget(remove_btn)
+        
+        # 儲存引用
+        panel.connection = conn
+        panel.toggle_btn = toggle_btn
+        panel.status_label = status_label
+        
+        return panel
+    
+    def add_udp_panel(self, conn):
+        """添加 UDP 連接面板到列表"""
+        panel = self.create_udp_connection_panel(conn)
+        self.udp_list_layout.addWidget(panel)
+        self.udp_connections.append(conn)
+        return panel
+    
+    def add_ws_panel(self, conn):
+        """添加 WebSocket 連接面板到列表"""
+        panel = self.create_ws_connection_panel(conn)
+        self.ws_list_layout.addWidget(panel)
+        self.ws_connections.append(conn)
+        return panel
+    
+    def remove_udp_connection_from_list(self, conn):
+        """從列表中移除 UDP 連接"""
+        if conn in self.udp_connections:
+            self.udp_connections.remove(conn)
+    
+    def remove_ws_connection_from_list(self, conn):
+        """從列表中移除 WebSocket 連接"""
+        if conn in self.ws_connections:
+            self.ws_connections.remove(conn)

src/unitdev01/unitdev01/communication.py

@@ -0,0 +1,646 @@
+from rclpy.node import Node
+from PyQt6.QtCore import QObject, pyqtSignal
+import math
+import re
+import threading
+from threading import Lock
+import asyncio
+import websockets
+import json
+import socket
+from pymavlink import mavutil
+from geometry_msgs.msg import Point, Vector3
+from sensor_msgs.msg import BatteryState, NavSatFix, Imu
+from std_msgs.msg import Float64
+from mavros_msgs.msg import State, VfrHud
+from mavros_msgs.srv import CommandBool, CommandTOL
+
+class DroneSignals(QObject):
+    update_signal = pyqtSignal(str, str, object)  # (msg_type, drone_id, data)
+
+class UDPMavlinkReceiver(threading.Thread):
+    """UDP MAVLink 接收器"""
+    def __init__(self, ip, port, signals, connection_name):
+        super().__init__(daemon=True)
+        self.ip = ip
+        self.port = port
+        self.signals = signals
+        self.connection_name = connection_name
+        self.running = False
+        self.sock = None
+        
+    def run(self):
+        """執行 UDP 接收循環"""
+        self.running = True
+        try:
+            print(f"UDP MAVLink receiver started on {self.ip}:{self.port}")
+            
+            # 創建 MAVLink 連接
+            mav = mavutil.mavlink_connection(f'udpin:{self.ip}:{self.port}')
+            while self.running:
+                try:
+                    msg = mav.recv_match(blocking=True, timeout=1.0)
+                    if msg is None:
+                        continue
+                    
+                    self.process_mavlink_message(msg)
+                    
+                except socket.timeout:
+                    continue
+                except Exception as e:
+                    print(f"Error receiving MAVLink message: {e}")
+                    
+        except Exception as e:
+            print(f"UDP receiver error: {e}")
+        finally:
+            if self.sock:
+                self.sock.close()
+                
+    def process_mavlink_message(self, msg):
+        """處理 MAVLink 訊息"""
+        try:
+            msg_type = msg.get_type()
+            system_id = msg.get_srcSystem()
+            drone_id = f"s8_{system_id}"  # 使用 s8_ 前綴表示 UDP 來源
+            
+            if msg_type == "HEARTBEAT":
+                mode = mavutil.mode_string_v10(msg)
+                armed = bool(msg.base_mode & 128)
+                self.signals.update_signal.emit('state', drone_id, {
+                    'mode': mode,
+                    'armed': armed
+                })
+
+            elif msg_type == "BATTERY_STATUS":
+                voltage = msg.voltages[0] / 1000
+                self.signals.update_signal.emit('battery', drone_id, {
+                    'voltage': voltage
+                })
+
+            elif msg_type == "GLOBAL_POSITION_INT":
+                latitude = msg.lat / 1e7
+                longitude = msg.lon / 1e7
+                self.signals.update_signal.emit('gps', drone_id, {
+                    'lat': latitude,
+                    'lon': longitude
+                })
+
+            elif msg_type == "GPS_RAW_INT":
+                fix_type = msg.fix_type
+
+            elif msg_type == "LOCAL_POSITION_NED":
+                x = msg.y
+                y = msg.x
+                z = -msg.z
+                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
+                })
+                self.signals.update_signal.emit('velocity', drone_id, {
+                    'vx': msg.vx,
+                    'vy': msg.vy,
+                    'vz': msg.vz
+                })
+
+            elif msg_type == "ATTITUDE":
+                pitch = math.degrees(msg.pitch)
+                self.signals.update_signal.emit('attitude', drone_id, {
+                    'pitch': pitch,
+                    'roll': 0,
+                    'yaw': 0,
+                    'rates': (0, 0, 0)
+                })
+
+            elif msg_type == "VFR_HUD":
+                groundspeed = msg.groundspeed
+                heading = msg.heading
+                self.signals.update_signal.emit('hud', drone_id, {
+                    'heading': heading,
+                    'groundspeed': groundspeed
+                })
+                
+        except Exception as e:
+            print(f"Error processing MAVLink message: {e}")
+            
+    def stop(self):
+        """停止接收器"""
+        self.running = False
+
+class SerialMavlinkReceiver(threading.Thread):
+    """串口 MAVLink 接收器"""
+    def __init__(self, port, baudrate, signals, connection_name):
+        super().__init__(daemon=True)
+        self.port = port
+        self.baudrate = baudrate
+        self.signals = signals
+        self.connection_name = connection_name
+        self.running = False
+        self.mav = None
+        
+    def run(self):
+        """執行串口接收循環"""
+        self.running = True
+        try:
+            print(f"Serial MAVLink receiver started on {self.port} at {self.baudrate} baud")
+            
+            # 創建 MAVLink 串口連接
+            self.mav = mavutil.mavlink_connection(
+                self.port,
+                baud=self.baudrate,
+                source_system=255
+            )
+            
+            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)
+                    if msg is None:
+                        continue
+                    
+                    self.process_mavlink_message(msg)
+                    
+                except Exception as e:
+                    if self.running:
+                        print(f"Error receiving MAVLink message from serial: {e}")
+                    
+        except Exception as e:
+            print(f"Serial receiver error: {e}")
+        finally:
+            if self.mav:
+                try:
+                    self.mav.close()
+                except:
+                    pass
+                
+    def process_mavlink_message(self, msg):
+        """處理 MAVLink 訊息"""
+        try:
+            msg_type = msg.get_type()
+            system_id = msg.get_srcSystem()
+            drone_id = f"s5_{system_id}"  # 使用 serial_ 前綴表示串口來源
+            
+            if msg_type == "HEARTBEAT":
+                mode = mavutil.mode_string_v10(msg)
+                armed = bool(msg.base_mode & 128)
+                self.signals.update_signal.emit('state', drone_id, {
+                    'mode': mode,
+                    'armed': armed
+                })
+
+            elif msg_type == "BATTERY_STATUS":
+                voltage = msg.voltages[0] / 1000
+                self.signals.update_signal.emit('battery', drone_id, {
+                    'voltage': voltage
+                })
+
+            elif msg_type == "GLOBAL_POSITION_INT":
+                latitude = msg.lat / 1e7
+                longitude = msg.lon / 1e7
+                self.signals.update_signal.emit('gps', drone_id, {
+                    'lat': latitude,
+                    'lon': longitude
+                })
+
+            elif msg_type == "GPS_RAW_INT":
+                fix_type = msg.fix_type
+
+            elif msg_type == "LOCAL_POSITION_NED":
+                x = msg.y
+                y = msg.x
+                z = -msg.z
+                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
+                })
+                self.signals.update_signal.emit('velocity', drone_id, {
+                    'vx': msg.vx,
+                    'vy': msg.vy,
+                    'vz': msg.vz
+                })
+
+            elif msg_type == "ATTITUDE":
+                pitch = math.degrees(msg.pitch)
+                self.signals.update_signal.emit('attitude', drone_id, {
+                    'pitch': pitch,
+                    'roll': 0,
+                    'yaw': 0,
+                    'rates': (0, 0, 0)
+                })
+
+            elif msg_type == "VFR_HUD":
+                groundspeed = msg.groundspeed
+                heading = msg.heading
+                self.signals.update_signal.emit('hud', drone_id, {
+                    'heading': heading,
+                    'groundspeed': groundspeed
+                })
+                
+        except Exception as e:
+            print(f"Error processing MAVLink message from serial: {e}")
+            
+    def stop(self):
+        """停止接收器"""
+        self.running = False
+
+class WebSocketMavlinkReceiver(threading.Thread):
+    """WebSocket MAVLink 接收器"""
+    def __init__(self, url, signals, connection_name):
+        super().__init__(daemon=True)
+        self.url = url
+        self.signals = signals
+        self.connection_name = connection_name
+        self.running = False
+        self.max_retries = 5
+        self.base_delay = 1.0
+        
+    def run(self):
+        """執行 WebSocket 接收循環"""
+        self.running = True
+        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):
+        """WebSocket 連接的主循環"""
+        retry_count = 0
+        
+        print(f"Starting WebSocket client for {self.connection_name} at {self.url}")
+        
+        while self.running and retry_count < self.max_retries:
+            try:
+                async with websockets.connect(self.url) as websocket:
+                    print(f"WebSocket {self.connection_name} connected to {self.url}")
+                    retry_count = 0  # 重置重試計數
+                    
+                    async for message in websocket:
+                        if not self.running:
+                            break
+                            
+                        try:
+                            data = json.loads(message)
+                            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}")
+                        except Exception as e:
+                            print(f"WebSocket {self.connection_name} message processing error: {e}")
+                            
+            except websockets.exceptions.ConnectionClosedError:
+                print(f"WebSocket {self.connection_name} connection closed")
+                if self.running:
+                    retry_count += 1
+                    if retry_count < self.max_retries:
+                        delay = self.base_delay * (2 ** min(retry_count, 4))
+                        print(f"Reconnecting in {delay}s...")
+                        await asyncio.sleep(delay)
+                else:
+                    break
+            except Exception as e:
+                retry_count += 1
+                if retry_count < self.max_retries and self.running:
+                    delay = self.base_delay * (2 ** min(retry_count, 4))
+                    print(f"WebSocket {self.connection_name} connection error: {e}, retrying in {delay}s (attempt {retry_count}/{self.max_retries})")
+                    await asyncio.sleep(delay)
+                else:
+                    break
+        
+        print(f"WebSocket client {self.connection_name} stopped")
+    
+    def process_websocket_message(self, data):
+        """處理 WebSocket 訊息"""
+        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:
+                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):
+        """停止接收器"""
+        self.running = False
+
+class DroneMonitor(Node):
+    def __init__(self):
+        super().__init__('drone_monitor')
+        self.signals = DroneSignals()
+        self.drone_topics = {}
+        self.lock = Lock()
+        
+        self.arm_clients = {}
+        self.takeoff_clients = {}
+        self.setpoint_pubs = {}
+        self.selected_drones = set()
+        self.latest_data = {}
+
+        # 定義需要過濾的模式
+        self.filtered_modes = ['Mode(0x000000c0)']
+
+        # WebSocket 接收器列表
+        self.ws_receivers = []
+        
+        # 串口接收器列表
+        self.serial_receivers = []
+        
+        # 主题检测定时器
+        self.create_timer(1.0, self.scan_topics)
+       
+    def scan_topics(self):
+        topics = self.get_topic_names_and_types()
+        drone_pattern = re.compile(r'/MavLinkBus/(s\d+_\d+)/(\w+)')
+        
+        found_drones = set()
+        for topic_name, _ in topics:
+            if match := drone_pattern.match(topic_name):
+                drone_id, topic_type = match.groups()
+                found_drones.add(drone_id)
+                with self.lock:
+                    self.drone_topics.setdefault(drone_id, set()).add(topic_type)
+        
+        for drone_id in found_drones:
+            if not hasattr(self, f'drone_{drone_id}_subs'):
+                self.setup_drone(drone_id)
+    
+    def setup_drone(self, drone_id):
+        base_topic = f'/MavLinkBus/{drone_id}'
+        
+        # Add service clients
+        self.arm_clients[drone_id] = self.create_client(
+            CommandBool,
+            f'{base_topic}/cmd/arming'
+        )
+        self.takeoff_clients[drone_id] = self.create_client(
+            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,
+                f'{base_topic}/attitude',
+                lambda msg, did=drone_id: self.attitude_callback(did, msg),
+                10
+            ),
+            'battery': self.create_subscription(
+                BatteryState,
+                f'{base_topic}/battery',
+                lambda msg, did=drone_id: self.battery_callback(did, msg),
+                10
+            ),
+            'global': self.create_subscription(
+                NavSatFix,
+                f'{base_topic}/global_position/global',
+                lambda msg, did=drone_id: self.gps_callback(did, msg),
+                10
+            ),
+            'rel_alt': self.create_subscription(
+                Float64,
+                f'{base_topic}/global_position/rel_alt',
+                lambda msg, did=drone_id: self.altitude_callback(did, msg),
+                10
+            ),
+            'local_pose': self.create_subscription(
+                Point,
+                f'{base_topic}/local_position/pose',
+                lambda msg, did=drone_id: self.local_pose_callback(did, msg),
+                10
+            ),
+            'local_vel': self.create_subscription(
+                Vector3,
+                f'{base_topic}/local_position/velocity',
+                lambda msg, did=drone_id: self.local_vel_callback(did, msg),
+                10
+            ),
+            'loss_rate': self.create_subscription(
+                Float64,
+                f'{base_topic}/packet_loss_rate',
+                lambda msg, did=drone_id: self.loss_rate_callback(did, msg),
+                10
+            ),
+            'state': self.create_subscription(
+                State,
+                f'{base_topic}/state',
+                lambda msg, did=drone_id: self.state_callback(did, msg),
+                10
+            ),
+            'ping': self.create_subscription(
+                Float64,
+                f'{base_topic}/ping',
+                lambda msg, did=drone_id: self.ping_callback(did, msg),
+                10
+            ),
+            'vfr_hud': self.create_subscription(
+                VfrHud,
+                f'{base_topic}/vfr_hud',
+                lambda msg, did=drone_id: self.hud_callback(did, msg),
+                10
+            )
+        }
+        
+        setattr(self, f'drone_{drone_id}_subs', subs)
+
+    async def arm_drone(self, drone_id, arm):
+        if drone_id not in self.arm_clients:
+            return False
+        
+        client = self.arm_clients[drone_id]
+        if not client.wait_for_service(timeout_sec=1.0):
+            return False
+            
+        request = CommandBool.Request()
+        request.value = arm
+        
+        future = client.call_async(request)
+        try:
+            response = await future
+            return response.success
+        except Exception as e:
+            self.get_logger().error(f'Arm service call failed: {e}')
+            return False
+
+    async def takeoff_drone(self, drone_id, altitude=10.0):
+        if drone_id not in self.takeoff_clients:
+            return False
+            
+        client = self.takeoff_clients[drone_id]
+        if not client.wait_for_service(timeout_sec=1.0):
+            return False
+            
+        request = CommandTOL.Request()
+        request.altitude = altitude
+        request.min_pitch = 0.0
+        request.yaw = 0.0
+        
+        future = client.call_async(request)
+        try:
+            response = await future
+            return response.success
+        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)
+        roll = math.atan2(sinr_cosp, cosr_cosp)
+        
+        sinp = 2 * (q.w * q.y - q.z * q.x)
+        pitch = math.asin(sinp) if abs(sinp) < 1 else math.copysign(math.pi/2, sinp)
+        
+        siny_cosp = 2 * (q.w * q.z + q.x * q.y)
+        cosy_cosp = 1 - 2 * (q.y**2 + q.z**2)
+        yaw = math.atan2(siny_cosp, cosy_cosp)
+        
+        return math.degrees(roll), math.degrees(pitch), math.degrees(yaw)
+
+    # callbacks
+    def attitude_callback(self, drone_id, msg):
+        if hasattr(msg, 'orientation'):
+            roll, pitch, yaw = self.quaternion_to_euler(msg.orientation)
+            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.latest_data[(drone_id, 'battery')] = {
+            'voltage': msg.voltage
+        }
+
+    def state_callback(self, drone_id, msg):
+        mode = msg.mode
+        if mode in self.filtered_modes:
+            return
+        self.latest_data[(drone_id, 'state')] = {
+            'mode': msg.mode,
+            'armed': msg.armed
+        }
+
+    def gps_callback(self, drone_id, msg):
+        self.latest_data[(drone_id, 'gps')] = {
+            'lat': msg.latitude,
+            'lon': msg.longitude,
+            'alt': msg.altitude
+        }
+
+    def local_vel_callback(self, drone_id, msg):
+        self.latest_data[(drone_id, 'velocity')] = {
+            'vx': msg.x,
+            'vy': msg.y,
+            'vz': msg.z
+        }
+
+    def altitude_callback(self, drone_id, msg):
+        self.latest_data[(drone_id, 'altitude')] = {
+            'altitude': msg.data
+        }
+
+    def local_pose_callback(self, drone_id, msg):
+        self.latest_data[(drone_id, 'local_pose')] = {
+            'x': msg.x,
+            'y': msg.y,
+            'z': msg.z
+        }
+
+    def hud_callback(self, drone_id, msg):
+        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.latest_data[(drone_id, 'loss_rate')] = {
+            'loss_rate': msg.data
+        }
+
+    def ping_callback(self, drone_id, msg):
+        self.latest_data[(drone_id, 'ping')] = {
+            'ping': msg.data
+        }
+
+    def start_serial_connection(self, port='/dev/ttyUSB0', baudrate=57600):
+        """啟動串口 MAVLink 連接"""
+        connection_name = f"Serial_{port.replace('/', '_')}"
+        receiver = SerialMavlinkReceiver(port, baudrate, self.signals, connection_name)
+        receiver.start()
+        self.serial_receivers.append(receiver)
+        print(f"Started serial connection on {port} at {baudrate} baud")
+        return receiver

src/unitdev01/unitdev01/drone_panel.py

@@ -0,0 +1,378 @@
+#!/usr/bin/env python3
+from PyQt6.QtWidgets import (QWidget, QVBoxLayout, QHBoxLayout, QLabel, 
+                            QPushButton, QSizePolicy, QCheckBox)
+from PyQt6.QtCore import pyqtSignal
+
+class DronePanel(QWidget):
+    """單個無人機面板類別"""
+    
+    # 定義信號
+    mode_change_requested = pyqtSignal(str)  # drone_id
+    arm_requested = pyqtSignal(str)  # drone_id
+    takeoff_requested = pyqtSignal(str)  # drone_id
+    setpoint_requested = pyqtSignal(str)  # drone_id
+    selection_changed = pyqtSignal(str, int)  # drone_id, state
+    
+    def __init__(self, drone_id, parent=None):
+        super().__init__(parent)
+        self.drone_id = drone_id
+        self.display_id = 's' + drone_id.split('_')[1]
+        self._init_ui()
+    
+    def _init_ui(self):
+        """初始化UI"""
+        self.setObjectName(f"panel_{self.drone_id}")
+        self.setFixedHeight(140)
+        self.setStyleSheet("""
+            background-color: #2A2A2A;
+            border-radius: 8px;
+        """)
+        
+        # 主佈局
+        main_layout = QHBoxLayout(self)
+        main_layout.setContentsMargins(8, 8, 8, 8)
+        main_layout.setSpacing(0)
+        
+        # 創建內容容器（包含 info 和 control）
+        content_widget = QWidget()
+        content_widget.setStyleSheet("background-color: #333; border-radius: 6px;")
+        content_layout = QHBoxLayout(content_widget)
+        content_layout.setContentsMargins(8, 8, 8, 8)
+        content_layout.setSpacing(8)
+        
+        # 左側資訊區域
+        info_widget = self._create_info_section()
+        
+        # 右側控制按鈕區域
+        control_widget = self._create_control_section()
+        
+        # 將 info 和 control 加入內容容器
+        content_layout.addWidget(info_widget)
+        content_layout.addWidget(control_widget)
+        
+        # 將內容容器加入主佈局
+        main_layout.addWidget(content_widget)
+    
+    def _create_info_section(self):
+        """創建資訊顯示區域"""
+        info_widget = QWidget()
+        info_layout = QVBoxLayout(info_widget)
+        info_layout.setContentsMargins(0, 0, 0, 0)
+        info_layout.setSpacing(4)
+        
+        # 頂部標題欄
+        header = QWidget()
+        header_layout = QHBoxLayout(header)
+        header_layout.setContentsMargins(0, 0, 0, 0)
+        
+        # 勾選框
+        self.checkbox = QCheckBox()
+        self.checkbox.setObjectName(f"{self.drone_id}_checkbox")
+        self.checkbox.setStyleSheet("QCheckBox { color: #DDD; }")
+        self.checkbox.stateChanged.connect(
+            lambda state: self.selection_changed.emit(self.drone_id, state)
+        )
+        
+        # ID 顯示
+        id_label = QLabel(self.display_id)
+        id_label.setStyleSheet("""
+            font-weight: bold; 
+            font-size: 14px; 
+            color: #7FFFD4;
+            min-width: 80px;
+        """)
+        
+        header_layout.addWidget(self.checkbox)
+        header_layout.addWidget(id_label)
+        header_layout.addStretch()
+        
+        info_layout.addWidget(header)
+        
+        # 第一行：狀態 (模式 + ARM狀態)
+        status_row = self._create_status_row()
+        info_layout.addWidget(status_row)
+        
+        # 第二行：電池
+        battery_row = self._create_battery_row()
+        info_layout.addWidget(battery_row)
+        
+        # 第三行：位置 + 高度
+        position_row = self._create_position_row()
+        info_layout.addWidget(position_row)
+        
+        # 第四行：航向 + 速度
+        nav_row = self._create_nav_row()
+        info_layout.addWidget(nav_row)
+        
+        return info_widget
+    
+    def _create_status_row(self):
+        """創建狀態行"""
+        status_row = QWidget()
+        status_layout = QHBoxLayout(status_row)
+        status_layout.setContentsMargins(0, 0, 0, 0)
+        
+        status_title = QLabel("狀態：")
+        status_title.setStyleSheet("color: #888; min-width: 50px;")
+        
+        self.mode_label = QLabel("--")
+        self.mode_label.setObjectName(f"{self.drone_id}_mode")
+        self.mode_label.setStyleSheet("color: #DDD;")
+        
+        self.armed_label = QLabel("--")
+        self.armed_label.setObjectName(f"{self.drone_id}_armed")
+        self.armed_label.setStyleSheet("color: #DDD;")
+        
+        status_layout.addWidget(status_title)
+        status_layout.addWidget(self.mode_label)
+        status_layout.addWidget(self.armed_label)
+        status_layout.addStretch()
+        
+        return status_row
+    
+    def _create_battery_row(self):
+        """創建電池行"""
+        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;")
+        
+        self.battery_label = QLabel("--")
+        self.battery_label.setObjectName(f"{self.drone_id}_battery")
+        self.battery_label.setStyleSheet("color: #DDD;")
+        
+        battery_layout.addWidget(battery_title)
+        battery_layout.addWidget(self.battery_label)
+        battery_layout.addStretch()
+        
+        return battery_row
+    
+    def _create_position_row(self):
+        """創建位置行"""
+        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;")
+        
+        self.local_label = QLabel("--")
+        self.local_label.setObjectName(f"{self.drone_id}_local")
+        self.local_label.setStyleSheet("color: #DDD;")
+        
+        altitude_title = QLabel("高度：")
+        altitude_title.setStyleSheet("color: #888; margin-left: 10px;")
+        
+        self.altitude_label = QLabel("--")
+        self.altitude_label.setObjectName(f"{self.drone_id}_altitude")
+        self.altitude_label.setStyleSheet("color: #DDD;")
+        
+        position_layout.addWidget(position_title)
+        position_layout.addWidget(self.local_label)
+        position_layout.addWidget(altitude_title)
+        position_layout.addWidget(self.altitude_label)
+        position_layout.addStretch()
+        
+        return position_row
+    
+    def _create_nav_row(self):
+        """創建導航行"""
+        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;")
+        
+        self.heading_label = QLabel("--")
+        self.heading_label.setObjectName(f"{self.drone_id}_heading")
+        self.heading_label.setStyleSheet("color: #DDD;")
+        
+        speed_title = QLabel("速度：")
+        speed_title.setStyleSheet("color: #888; margin-left: 10px;")
+        
+        self.groundspeed_label = QLabel("--")
+        self.groundspeed_label.setObjectName(f"{self.drone_id}_groundspeed")
+        self.groundspeed_label.setStyleSheet("color: #DDD;")
+        
+        nav_layout.addWidget(heading_title)
+        nav_layout.addWidget(self.heading_label)
+        nav_layout.addWidget(speed_title)
+        nav_layout.addWidget(self.groundspeed_label)
+        nav_layout.addStretch()
+        
+        return nav_row
+    
+    def _create_control_section(self):
+        """創建控制按鈕區域"""
+        control_widget = QWidget()
+        control_layout = QVBoxLayout(control_widget)
+        control_layout.setContentsMargins(0, 0, 0, 0)
+        control_layout.setSpacing(6)
+        
+        control_widget.setFixedWidth(80)
+        
+        btn_style = """
+	        QPushButton {
+                background-color: #444;
+                color: #DDD;
+                border: none;
+                border-radius: 4px;
+                font-size: 11px;
+            }
+            QPushButton:hover {
+                background-color: #555;
+            }
+        """
+        # 模式切換按鈕
+        mode_btn = QPushButton("切換模式")
+        mode_btn.setStyleSheet(btn_style)
+        mode_btn.setSizePolicy(QSizePolicy.Policy.Preferred, QSizePolicy.Policy.Expanding)
+        mode_btn.clicked.connect(lambda: self.mode_change_requested.emit(self.drone_id))
+        
+        # 解鎖按鈕
+        arm_btn = QPushButton("解鎖")
+        arm_btn.setStyleSheet(btn_style)
+        arm_btn.setSizePolicy(QSizePolicy.Policy.Preferred, QSizePolicy.Policy.Expanding)
+        arm_btn.clicked.connect(lambda: self.arm_requested.emit(self.drone_id))
+        
+        # 起飛按鈕
+        takeoff_btn = QPushButton("起飛")
+        takeoff_btn.setStyleSheet(btn_style)
+        takeoff_btn.setSizePolicy(QSizePolicy.Policy.Preferred, QSizePolicy.Policy.Expanding)
+        takeoff_btn.clicked.connect(lambda: self.takeoff_requested.emit(self.drone_id))
+        
+        # Setpoint 按鈕
+        setpoint_btn = QPushButton("Setpoint")
+        setpoint_btn.setStyleSheet(btn_style)
+        setpoint_btn.setSizePolicy(QSizePolicy.Policy.Preferred, QSizePolicy.Policy.Expanding)
+        setpoint_btn.clicked.connect(lambda: self.setpoint_requested.emit(self.drone_id))
+        
+        control_layout.addWidget(mode_btn)
+        control_layout.addWidget(arm_btn)
+        control_layout.addWidget(takeoff_btn)
+        control_layout.addWidget(setpoint_btn)
+        
+        return control_widget
+    
+    def update_field(self, field, text, color=None):
+        """更新指定欄位的值"""
+        label = self.findChild(QLabel, f"{self.drone_id}_{field}")
+        if label and label.text() != text:
+            label.setText(text)
+            if color:
+                label.setStyleSheet(f"color: {color};")
+    
+    def get_checkbox(self):
+        """獲取勾選框"""
+        return self.checkbox
+    
+    def set_checked(self, checked):
+        """設置勾選狀態"""
+        self.checkbox.setChecked(checked)
+    
+    def is_checked(self):
+        """獲取勾選狀態"""
+        return self.checkbox.isChecked()
+    
+class SocketGroupPanel(QWidget):
+    # 定義信號
+    group_selection_changed = pyqtSignal(str, int)  # socket_id, state
+    
+    def __init__(self, socket_id, color='#AAAAAA', parent=None):
+        super().__init__(parent)
+        self.socket_id = socket_id
+        self.color = color
+        self._init_ui()
+    
+    def _init_ui(self):
+        """初始化UI"""
+        self.setObjectName(f"socket_group_{self.socket_id}")
+        self.setStyleSheet("""
+            background-color: #1E1E1E;
+            border-radius: 12px;
+        """)
+        
+        layout = QVBoxLayout(self)
+        layout.setContentsMargins(10, 10, 10, 10)
+        layout.setSpacing(6)
+        
+        # Socket 分組標題行 - 包含勾選框
+        title_row = QWidget()
+        title_layout = QHBoxLayout(title_row)
+        title_layout.setContentsMargins(0, 0, 0, 0)
+        
+        # 分組勾選框
+        self.group_checkbox = QCheckBox()
+        self.group_checkbox.setObjectName(f"socket_{self.socket_id}_checkbox")
+        self.group_checkbox.setStyleSheet(f"""
+            QCheckBox {{ color: #DDD; }}
+            QCheckBox::indicator {{
+                width: 14px;
+                height: 14px;
+                border: 2px solid #888888;
+                border-radius: 3px;
+                background: transparent;
+            }}
+            QCheckBox::indicator:checked {{
+                background-color: {self.color};
+                border: 2px solid #888888;
+            }}
+            QCheckBox::indicator:indeterminate {{
+                background-color: #666;
+                border: 2px solid #888888;
+            }}
+        """)
+        self.group_checkbox.stateChanged.connect(
+            lambda state: self.group_selection_changed.emit(self.socket_id, state)
+        )
+        
+        # Socket 分組標題
+        title_label = QLabel(f"Socket {self.socket_id}")
+        title_label.setStyleSheet(f"""
+            font-weight: bold; 
+            font-size: 16px; 
+            color: {self.color};
+            margin-bottom: 8px;
+            padding: 4px 8px;
+            border-radius: 6px;
+        """)
+        
+        title_layout.addWidget(self.group_checkbox)
+        title_layout.addWidget(title_label)
+        title_layout.addStretch()
+        
+        layout.addWidget(title_row)
+        
+        # 創建子容器用於放置該 socket 下的所有無人機面板
+        self.drones_container = QWidget()
+        self.drones_layout = QVBoxLayout(self.drones_container)
+        self.drones_layout.setContentsMargins(0, 0, 0, 0)
+        self.drones_layout.setSpacing(4)
+        
+        layout.addWidget(self.drones_container)
+    
+    def add_drone_panel(self, panel):
+        """添加無人機面板到分組"""
+        self.drones_layout.addWidget(panel)
+    
+    def clear_drones(self):
+        """清空所有無人機面板"""
+        while self.drones_layout.count():
+            item = self.drones_layout.takeAt(0)
+            if item.widget():
+                item.widget().setParent(None)
+    
+    def get_checkbox(self):
+        """獲取分組勾選框"""
+        return self.group_checkbox
+    
+    def set_checked(self, checked):
+        """設置分組勾選狀態"""
+        self.group_checkbox.setChecked(checked)
+    
+    def set_check_state(self, state):
+        """設置分組勾選狀態（支持半選）"""
+        self.group_checkbox.setCheckState(state)

src/unitdev01/unitdev01/map_layout.py

@@ -0,0 +1,292 @@
+#!/usr/bin/env python3
+from PyQt6.QtWebEngineWidgets import QWebEngineView
+from PyQt6.QtCore import QTimer, pyqtSignal, QObject, pyqtSlot
+from PyQt6.QtWebChannel import QWebChannel
+
+class DroneMap:
+    """無人機地圖類別 - 負責管理 Leaflet 地圖顯示"""
+    
+    def __init__(self):
+        """初始化地圖"""
+        self.map_view = QWebEngineView()
+        self.map_loaded = False
+        self.pending_map_updates = {}
+        
+        # 創建橋接對象
+        self.bridge = MapBridge()
+        
+        # 設置 QWebChannel
+        self.channel = QWebChannel()
+        self.channel.registerObject('bridge', self.bridge)
+        self.map_view.page().setWebChannel(self.channel)
+        
+        # 設置地圖 HTML
+        inline_html = '''
+            <!DOCTYPE html>
+            <html>
+            <head>
+            <meta charset="utf-8" />
+            <link rel="stylesheet" href="https://unpkg.com/leaflet/dist/leaflet.css" />
+            <script src="https://unpkg.com/leaflet/dist/leaflet.js"></script>
+            <script src="https://unpkg.com/leaflet-rotatedmarker/leaflet.rotatedMarker.js"></script>
+            <script src="qrc:///qtwebchannel/qwebchannel.js"></script>
+            <style>
+                html, body, #map { height: 100%; margin: 0; }
+                .map-controls {
+                    position: absolute;
+                    top: 10px;
+                    right: 10px;
+                    z-index: 1000;
+                }
+                .control-button {
+                    padding: 8px 12px;
+                    background-color: #f44336;
+                    color: white;
+                    border: none;
+                    border-radius: 4px;
+                    cursor: pointer;
+                    font-size: 12px;
+                    box-shadow: 0 2px 5px rgba(0,0,0,0.2);
+                }
+                .control-button:hover {
+                    background-color: #d32f2f;
+                }
+            </style>
+            </head>
+            <body>
+            <div id="map"></div>
+            <div class="map-controls">
+                <button class="control-button" onclick="clearAllTrajectories()">清除軌跡</button>
+            </div>
+
+            <script>
+                var bridge;
+                new QWebChannel(qt.webChannelTransport, function(channel) {
+                    bridge = channel.objects.bridge;
+                });
+
+                var map = L.map('map').setView([0, 0], 19); 
+                L.tileLayer('https://{s}.tile.openstreetmap.org/{z}/{x}/{y}.png', {
+                    maxZoom: 19,
+                    attribution: '© OpenStreetMap contributors'
+                }).addTo(map);
+
+                // 地圖點擊事件
+                map.on('click', function(e) {
+                    if (bridge) {
+                        bridge.emitGpsSignal(e.latlng.lat, e.latlng.lng);
+                        console.log('點擊位置:', e.latlng.lat, e.latlng.lng);
+                    }
+                });
+
+                var arrowIcon = L.icon({
+                    iconUrl: 'https://cdn-icons-png.flaticon.com/512/399/399308.png',
+                    iconSize: [40, 40],
+                    iconAnchor: [20, 20]
+                });
+
+                function getColorBySocketId(id) {
+                    if (id.startsWith("s0_")) return "#00BFFF";  // 天藍色
+                    if (id.startsWith("s1_")) return "#FFD700";  // 金色
+                    if (id.startsWith("s2_")) return "#FF6969";  // 淺紅色
+                    if (id.startsWith("s3_")) return "#FF69B4";  // 熱粉紅
+                    if (id.startsWith("s4_")) return "#00FA9A";  // 中春綠
+                    if (id.startsWith("s5_")) return "#9370DB";  // 中紫色 (串口)
+                    if (id.startsWith("s6_")) return "#FFA500";  // 橙色
+                    if (id.startsWith("s7_")) return "#20B2AA";  // 淺海綠
+                    if (id.startsWith("s8_")) return "#7CFC00";  // 草綠色
+                    if (id.startsWith("s9_")) return "#FF8C00";  // 深橙色
+                    return "#AAAAAA";  // 灰色 (預設)
+                }
+
+                function createIdIcon(id) {
+                    const color = getColorBySocketId(id);
+                    const sysid = id.split('_')[1];
+                    return L.divIcon({
+                        className: 'drone-id',
+                        html: `<div style="
+                            position: relative;
+                            left: 2px;
+                            background-color: ${color};
+                            color: black;
+                            width: 16px;
+                            height: 16px;
+                            border-radius: 50%;
+                            display: flex;
+                            align-items: center;
+                            justify-content: center;
+                            font-weight: bold;
+                            font-size: 10px;
+                            ">${sysid}</div>`,
+                        iconSize: [20, 20],
+                        iconAnchor: [10, 10]
+                    });
+                }
+
+                var markers = {};
+                var idLabels = {};
+                var trajectories = {};
+                var trajectoryLines = {};
+                var focusedId = null;
+                var initialized = false;
+                var trajectoryGroup = L.layerGroup().addTo(map);
+
+                function initTrajectory(id) {
+                    if (!trajectories[id]) {
+                        trajectories[id] = [];
+                        const color = getColorBySocketId(id);
+                        trajectoryLines[id] = L.polyline([], {
+                            color: color,
+                            weight: 3,
+                            opacity: 0.7,
+                            smoothFactor: 1
+                        }).addTo(trajectoryGroup);
+                    }
+                }
+
+                function addTrajectoryPoint(id, lat, lon) {
+                    initTrajectory(id);
+                    const point = [lat, lon];
+                    trajectories[id].push(point);
+                    
+                    if (trajectories[id].length > 1000) {
+                        trajectories[id].shift();
+                    }
+                    
+                    trajectoryLines[id].setLatLngs([...trajectories[id]]);
+                }
+
+                function focusOn(id) {
+                    if (!markers[id]) return;
+                    focusedId = id;
+                    var latlng = markers[id].getLatLng();
+                    map.flyTo(latlng, map.getZoom());
+                }
+
+                setInterval(() => {
+                    if (focusedId && markers[focusedId]) {
+                        var latlng = markers[focusedId].getLatLng();
+                        map.panTo(latlng);
+                    }
+                }, 1000);
+
+                function updateDrone(lat, lon, id, heading) {
+                    if (markers[id]) {
+                        const lastPos = markers[id].getLatLng();
+                        const distance = lastPos.distanceTo([lat, lon]);
+                        if (distance > 1) {
+                            addTrajectoryPoint(id, lat, lon);
+                        }
+                        
+                        markers[id]
+                            .setLatLng([lat, lon])
+                            .setRotationAngle(heading);
+                        idLabels[id].setLatLng([lat, lon]);
+                    } else {
+                        initTrajectory(id);
+                        addTrajectoryPoint(id, lat, lon);
+                        
+                        markers[id] = L.marker([lat, lon], {
+                            icon: arrowIcon,
+                            rotationAngle: heading,
+                            rotationOrigin: 'center'
+                        })
+                        .on('click', function () {
+                            focusOn(id);
+                        })
+                        .addTo(map);
+
+                        idLabels[id] = L.marker([lat, lon], {
+                            icon: createIdIcon(id),
+                            zIndexOffset: 1000
+                        })
+                        .on('click', function() {
+                            focusOn(id);
+                        })
+                        .addTo(map);
+
+                        if (!initialized || id < focusedId) {
+                            focusOn(id);
+                            initialized = true;
+                        }
+                    }
+                }
+
+                function clearAllTrajectories() {
+                    trajectories = {};
+                    Object.values(trajectoryLines).forEach(line => {
+                        trajectoryGroup.removeLayer(line);
+                    });
+                    trajectoryLines = {};
+                    console.log('所有軌跡已清除');
+                }
+            </script>
+            </body>
+            </html>
+        '''
+        
+        self.map_view.setHtml(inline_html)
+        self.map_view.loadFinished.connect(self._on_map_loaded)
+        
+        # 設置地圖更新計時器
+        self.map_update_timer = QTimer()
+        self.map_update_timer.timeout.connect(self.update_map_positions)
+        self.map_update_timer.start(200)  # 每 200ms 更新一次
+    
+    def _on_map_loaded(self, ok: bool):
+        """地圖加載完成回調"""
+        if ok:
+            self.map_loaded = True
+        else:
+            print("⚠️ 地圖加載失敗")
+    
+    def update_drone_position(self, drone_id, lat, lon, heading):
+        """更新無人機位置（加入待處理隊列）"""
+        self.pending_map_updates[drone_id] = (lat, lon, heading)
+    
+    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 clear_trajectories(self):
+        """清除所有軌跡"""
+        if self.map_loaded:
+            self.map_view.page().runJavaScript("clearAllTrajectories();")
+    
+    def focus_on_drone(self, drone_id):
+        """聚焦到指定無人機"""
+        if self.map_loaded:
+            self.map_view.page().runJavaScript(f"focusOn('{drone_id}');")
+    
+    def get_widget(self):
+        """獲取地圖 widget"""
+        return self.map_view
+    
+    def get_gps_signal(self):
+        """獲取 GPS 信號"""
+        return self.bridge.gps_signal
+    
+class MapBridge(QObject):
+    """JavaScript 和 Python 之間的橋接類"""
+    gps_signal = pyqtSignal(float, float)  # lat, lon
+    
+    def __init__(self):
+        super().__init__()
+    
+    @pyqtSlot(float, float)
+    def emitGpsSignal(self, lat, lon):
+        """供 JavaScript 調用的方法"""
+        self.gps_signal.emit(lat, lon)

src/unitdev01/unitdev01/mavlink.py

@@ -21,13 +21,15 @@ class MAVLinkWorker(QThread):
     param_signal = pyqtSignal(str, int)
     kbps_signal = pyqtSignal(str, float)
 
-    def __init__(self, connection_string="udp:127.0.0.1:14550", usb='/dev/ttyUSB0'):
+    def __init__(self, connection_string="udp:127.0.0.1:14560   ", usb='/dev/ttyUSB0', acm ='/dev/ttyACM0'):
         super().__init__()
-        self.namespaces = ['UAV1', 'UAV2', 'UAV3']
+        self.sysids = [1, 5, 10]
-        self.connection = mavutil.mavlink_connection(usb, baud=57600)
+        self.namespaces = {}
+        self.namespaces = [f"UAV{sysid}" for sysid in self.sysids]
+        
+        self.connection = mavutil.mavlink_connection(connection_string, baud=115200)
         self.connection.wait_heartbeat()
-        for sysid in self.namespaces:
+        for sysid in self.sysids:
-            sysid = int(sysid.replace('UAV', ''))
             self.set_sr_params(sysid)
         self.running = True
 
@@ -42,8 +44,8 @@ class MAVLinkWorker(QThread):
         self.total_bytes_received = {}
         self.throughput_KBps = {}
 
-        for namespace in self.namespaces:
+        for sysid in self.sysids:
-            self.request_param(namespace, "SR1_EXTRA1")
+            self.request_param(sysid, "SR1_EXTRA1")
         self.connection.mav.timesync_send( 
             0, #tc1
             int(time.time() * 1e9) #ts1
@@ -52,7 +54,7 @@ class MAVLinkWorker(QThread):
     def run(self):
         while self.running:
             try:
-                msg = self.connection.recv_msg()
+                msg = self.connection.recv_msg() # Debugging line to see received messages
                 current_time = time.time()
                 if not msg:
                     continue
@@ -64,7 +66,6 @@ class MAVLinkWorker(QThread):
                 if msg_type =="BAD_DATA":
                     continue
                 
-
                 if sysid not in self.total_bytes_received:
                     self.total_bytes_received[sysid] = 0
                     self.throughput_KBps[sysid] = 0
@@ -162,7 +163,7 @@ class MAVLinkWorker(QThread):
                 elif msg_type == 'PARAM_VALUE':
                     param_name = "SR1_EXTRA1"
                     if msg.param_id == param_name:
-                        self.param_signal.emit(namespace, msg.param_value)
+                        self.param_signal.emit(namespace, int(msg.param_value))
 
             except Exception as e:
                 print(f"Error reading message: {e}")
@@ -171,8 +172,11 @@ class MAVLinkWorker(QThread):
         self.running = False
         self.connection.close()
 
+    def get_sysid(self, namespace):
+        return int(namespace.replace('UAV', ''))
+    
     def set_mode(self, namespace, mode):
-        sysid = int(namespace.replace('UAV', ''))
+        sysid = self.get_sysid(namespace)
         if mode == 'STABILIZE':
             mode = 0
         elif mode == 'AUTO':
@@ -188,7 +192,7 @@ class MAVLinkWorker(QThread):
         )
 
     def arm(self, namespace, arm):
-        sysid = int(namespace.replace('UAV', ''))
+        sysid = self.get_sysid(namespace)
         self.connection.mav.command_long_send(
             sysid,
             1,  # Component ID
@@ -199,7 +203,7 @@ class MAVLinkWorker(QThread):
         )
 
     def takeoff(self, namespace, altitude):
-        sysid = int(namespace.replace('UAV', ''))
+        sysid = self.get_sysid(namespace)
         self.connection.mav.command_long_send(
             sysid,
             1,  # Component ID
@@ -210,7 +214,7 @@ class MAVLinkWorker(QThread):
         )
 
     def land(self, namespace):
-        sysid = int(namespace.replace('UAV', ''))
+        sysid = self.get_sysid(namespace)
         self.connection.mav.command_long_send(
             sysid,
             1,  # Component ID
@@ -221,7 +225,7 @@ class MAVLinkWorker(QThread):
         )
 
     def set_local_position(self, namespace, x, y, z):
-        sysid = int(namespace.replace('UAV', ''))
+        sysid = self.get_sysid(namespace)
         self.connection.mav.set_position_target_local_ned_send(
             0, sysid, 1,  # time_boot_ms, sysid, compid
             mavutil.mavlink.MAV_FRAME_LOCAL_NED,  # Frame
@@ -233,7 +237,7 @@ class MAVLinkWorker(QThread):
         )
     
     def reboot_drone(self, namespace):
-        sysid = int(namespace.replace('UAV', ''))
+        sysid = self.get_sysid(namespace)
         self.connection.mav.command_long_send(
             sysid,
             1,  # target_component (一般為1)
@@ -244,7 +248,7 @@ class MAVLinkWorker(QThread):
         )
 
     def set_param(self, namespace, param_name, value):
-        sysid = int(namespace.replace('UAV', ''))
+        sysid = self.get_sysid(namespace)
         try:
             self.connection.mav.param_set_send(
                 sysid,
@@ -256,8 +260,7 @@ class MAVLinkWorker(QThread):
         except Exception as e:
             print(f"Failed to set parameter {param_name}: {e}")
 
-    def request_param(self, namespace, param_name):
+    def request_param(self, sysid, param_name):
-        sysid = int(namespace.replace('UAV', ''))
         try:
             self.connection.mav.param_request_read_send(
                 sysid,  # 系統 ID
@@ -268,10 +271,35 @@ class MAVLinkWorker(QThread):
         except Exception as e:
             print(f"Failed to set parameter {param_name}: {e}")
 
+     
+    def set_sr_params(self, sysid):
+        """ 直接設置 MAVLink 訊息頻率 """
+        freqs = [0, 1, 4]
+        params = {
+            "SR1_ADSB": freqs[0],
+            "SR1_EXT_STAT": freqs[1],
+            "SR1_EXTRA1": freqs[2],
+            "SR1_EXTRA2": freqs[1],
+            "SR1_EXTRA3": freqs[1],
+            "SR1_POSITION": freqs[2],
+            "SR1_RAW_SENS": freqs[1],
+            "SR1_RC_CHAN": freqs[1]
+        }
+        for param, value in params.items():
+            self.set_param(f"UAV{sysid}", param, value)
+        
     '''    
     def set_sr_params(self, sysid):
         """ 直接設置 MAVLink 訊息頻率 """
-        freqs = [0, 2, 4]
+        params = {
+            "SERIAL1_BAUD": 38
+        }
+        for param, value in params.items():
+            self.set_param(f"UAV{sysid}", param, value)
+           
+    def set_sr_params(self, sysid):
+        """ 直接設置 MAVLink 訊息頻率 """
+        freqs = [0, 3, 3]
         params = {
             "SR1_ADSB": freqs[0],
             "SR1_EXT_STAT": freqs[1],
@@ -285,26 +313,15 @@ class MAVLinkWorker(QThread):
         for param, value in params.items():
             self.set_param(f"UAV{sysid}", param, value)
     '''
-    def set_sr_params(self, sysid):
+    def init_drone(self, namespace):
-        """ 直接設置 MAVLink 訊息頻率 """
+        sysid = self.get_sysid(namespace)
-        freqs = [0, 1, 1]
+        self.set_local_position(sysid, 0, 0, 0)
-        params = {
-            "SR1_ADSB": freqs[0],
-            "SR1_EXT_STAT": freqs[0],
-            "SR1_EXTRA1": freqs[2],
-            "SR1_EXTRA2": freqs[2],
-            "SR1_EXTRA3": freqs[0],
-            "SR1_POSITION": freqs[1],
-            "SR1_RAW_SENS": freqs[1],
-            "SR1_RC_CHAN": freqs[0]
-        }
-        for param, value in params.items():
-            self.set_param(f"UAV{sysid}", param, value)
 
 class DroneControlApp(QMainWindow):
     def __init__(self):
         super().__init__()
         self.workers = MAVLinkWorker()
+        self.sysids = self.workers.sysids
         self.namespaces = self.workers.namespaces
         self.initUI()
         
@@ -438,6 +455,15 @@ class DroneControlApp(QMainWindow):
         self.rebootButton.clicked.connect(self.reboot_drone)
         main_layout.addWidget(self.rebootButton)
 
+        mission_layout = QHBoxLayout()
+        self.initButton = QPushButton("重設位置")
+        self.initButton.clicked.connect(self.init_drone)
+        self.startButton = QPushButton("開始任務")
+        self.startButton.clicked.connect(self.start_mission)
+        mission_layout.addWidget(self.initButton)
+        mission_layout.addWidget(self.startButton)
+        main_layout.addLayout(mission_layout)
+
         # 設置主佈局
         central_widget = QWidget(self)
         central_widget.setLayout(main_layout)
@@ -563,6 +589,24 @@ class DroneControlApp(QMainWindow):
         except ValueError:
             QtWidgets.QMessageBox.warning(self, "錯誤", "請輸入有效的數值！")
 
+    def init_drone(self):
+        try:
+            for namespace in self.namespaces:
+                if self.uav_checkboxes[namespace].isChecked():
+                    self.workers.init_drone(namespace)
+                    print(f"重設位置：{namespace}")
+        except Exception as e:
+            QtWidgets.QMessageBox.warning(self, "錯誤", f"重設位置失敗：{e}")
+
+    def start_mission(self):
+        try:
+            for namespace in self.namespaces:
+                if self.uav_checkboxes[namespace].isChecked():
+                    self.workers.start_mission(namespace)
+                    print(f"開始任務：{namespace}")
+        except Exception as e:
+            QtWidgets.QMessageBox.warning(self, "錯誤", f"開始任務失敗：{e}")
+
 def main():
     app = QtWidgets.QApplication(sys.argv)
     window = DroneControlApp()

src/unitdev01/unitdev01/xbee.py

@@ -1,619 +0,0 @@
-import sys
-import time
-import math
-import serial
-import struct
-from PyQt5 import QtWidgets
-from PyQt5.QtCore import QThread, pyqtSignal
-from PyQt5.QtWidgets import QVBoxLayout, QHBoxLayout, QLabel, QLineEdit, QPushButton, QCheckBox, QWidget, QMainWindow, QComboBox
-from pymavlink import mavutil
-from pymavlink.dialects.v20 import ardupilotmega as mavlink2
-
-class PacketCapture:
-    def __init__(self):
-        self.data = bytearray()
-
-    def write(self, b):
-        self.data.extend(b)
-        return len(b)
-  
-class MAVLinkWorker(QThread):
-    state_signal = pyqtSignal(str, str)
-    battery_signal = pyqtSignal(str, float)
-    gps_signal = pyqtSignal(str, float, float)
-    gps_status_signal = pyqtSignal(str, int, int)
-    local_position_signal = pyqtSignal(str, float, float, float)
-    imu_signal = pyqtSignal(str, float)
-    hdg_signal = pyqtSignal(str, float)
-    groundspeed_signal = pyqtSignal(str, float)
-    ping_signal = pyqtSignal(str, float)
-    loss_signal = pyqtSignal(str, float)
-    frequency_signal = pyqtSignal(str, float)
-    param_signal = pyqtSignal(str, int)
-    kbps_signal = pyqtSignal(str, float)
-
-    def __init__(self):
-        super().__init__()
-        self.namespaces = ['UAV1', 'UAV2', 'UAV3']
-        self.connection = mavutil.mavlink_connection('/dev/ttyUSB0', baud=57600)
-        self.connection.wait_heartbeat()
-        for sysid in self.namespaces:
-            sysid = int(sysid.replace('UAV', ''))
-            self.set_sr_params(sysid)
-        self.running = True
-
-        # 用於計算頻率丟包
-        self.message_count = {}
-        self.frequency = {}
-        self.start_time = {}
-        self.seq_numbers = {}
-        self.packet_loss_count = {}
-        self.total_packet_count = {}
-        self.loss_percentage = {}
-        self.total_bytes_received = {}
-        self.throughput_KBps = {}
-
-        for namespace in self.namespaces:
-            self.request_param(namespace, "SR1_EXTRA1")
-        self.connection.mav.timesync_send( 
-            0, #tc1
-            int(time.time() * 1e9) #ts1
-        )
-
-    def run(self):
-        while self.running:
-            try:
-                msg = self.connection.recv_msg()
-                current_time = time.time()
-                if not msg:
-                    continue
-                sysid = msg.get_srcSystem()
-                if sysid == 0:
-                    continue
-                namespace = f"UAV{sysid}"
-                msg_type = msg.get_type()
-                if msg_type =="BAD_DATA":
-                    continue
-                
-                if sysid not in self.total_bytes_received:
-                    self.total_bytes_received[sysid] = 0
-                    self.throughput_KBps[sysid] = 0
-
-                # 計算訊息大小
-                msg_bytes = msg.get_msgbuf()  # 取得封包的 bytes
-                if msg_bytes:
-                    self.total_bytes_received[sysid] += len(msg_bytes)
-                
-                # Packet loss calculation
-                if sysid not in self.seq_numbers:
-                    self.seq_numbers[sysid] = msg.get_seq()  # Initialize sequence number tracking
-                    self.packet_loss_count[sysid] = 0
-                    self.total_packet_count[sysid] = 1
-                else:
-                    current_seq = msg.get_seq()
-                    expected_seq = (self.seq_numbers[sysid] + 1) % 256
-                    self.total_packet_count[sysid] += 1
-
-                    if current_seq != expected_seq: # Packet(s) lost
-                        lost_packets = (current_seq - expected_seq) % 256
-                        self.packet_loss_count[sysid] += lost_packets
-                        self.total_packet_count[sysid] += lost_packets
-
-                    self.seq_numbers[sysid] = current_seq
-                    self.loss_percentage[sysid] = (self.packet_loss_count[sysid] / self.total_packet_count[sysid]) * 100
-                    self.loss_signal.emit(namespace, self.loss_percentage[sysid])
-
-                # Frequency calculation
-                if sysid not in self.message_count:
-                    self.message_count[sysid] = 0
-                    self.start_time[sysid] = current_time
-
-                self.message_count[sysid] += 1
-
-                # 每隔一段時間更新
-                elapsed_time = current_time - self.start_time[sysid]
-                if elapsed_time >= 1:
-                    # 每秒頻率
-                    self.frequency[sysid] = self.message_count[sysid] / elapsed_time
-                    self.frequency_signal.emit(namespace, self.frequency[sysid])
-
-                    # 發送 timesync request
-                    self.connection.mav.timesync_send( 
-                        0, #tc1
-                        int(current_time * 1e9) #ts1
-                    )
-                    #吞吐量
-                    self.throughput_KBps[sysid] = (self.total_bytes_received[sysid] / (1024)) / elapsed_time 
-                    self.kbps_signal.emit(namespace, self.throughput_KBps[sysid])
-
-                    self.start_time[sysid] = current_time
-                    self.message_count[sysid] = 0
-                    self.total_bytes_received[sysid] = 0
-
-                if msg_type == "HEARTBEAT":
-                    mode = mavutil.mode_string_v10(msg)
-                    self.state_signal.emit(namespace, mode)
-
-                elif msg_type == "BATTERY_STATUS":
-                    voltage = msg.voltages[0] / 1000
-                    self.battery_signal.emit(namespace, voltage)
-
-                elif msg_type == "GLOBAL_POSITION_INT":
-                    latitude = msg.lat / 1e7
-                    longitude = msg.lon / 1e7
-                    self.gps_signal.emit(namespace, latitude, longitude)
-
-                elif msg_type == "GPS_RAW_INT":
-                    fix_type = msg.fix_type
-                    satellites_visible = msg.satellites_visible
-                    self.gps_status_signal.emit(namespace, fix_type, satellites_visible)
-
-                elif msg_type == "LOCAL_POSITION_NED":
-                    x = msg.y
-                    y = msg.x
-                    z = -msg.z
-                    self.local_position_signal.emit(namespace, x, y, z)
-
-                elif msg_type == "ATTITUDE":
-                    pitch = math.degrees(msg.pitch)
-                    self.imu_signal.emit(namespace, pitch)
-
-                elif msg_type == "VFR_HUD":
-                    groundspeed = msg.groundspeed
-                    heading = msg.heading
-                    self.hdg_signal.emit(namespace, heading)
-                    self.groundspeed_signal.emit(namespace, groundspeed)
-
-                elif msg_type == "TIMESYNC":
-                    round_trip_time = (int(current_time * 1e9) - msg.ts1) / 1e6
-                    if(round_trip_time<1e6):
-                        self.ping_signal.emit(namespace, round_trip_time)
-                
-                elif msg_type == 'PARAM_VALUE':
-                    param_name = "SR1_EXTRA1"
-                    if msg.param_id == param_name:
-                        self.param_signal.emit(namespace, msg.param_value)
-
-            except Exception as e:
-                print(f"Error reading message: {e}")
-
-    def stop(self):
-        self.running = False
-        self.connection.close()
-
-    def build_api_tx_frame(self, data: bytes, frame_id=0x01):
-        frame_type = 0x10
-        dest_addr64 = b'\x00\x00\x00\x00\x00\x00\xFF\xFF'  # Broadcast
-        dest_addr16 = b'\xFF\xFE'
-        broadcast_radius = 0x00
-        options = 0x00
-
-        frame = struct.pack(">B", frame_type) + struct.pack(">B", frame_id)
-        frame += dest_addr64 + dest_addr16
-        frame += struct.pack(">BB", broadcast_radius, options) + data
-        checksum = 0xFF - (sum(frame) & 0xFF)
-        return b'\x7E' + struct.pack(">H", len(frame)) + frame + struct.pack("B", checksum)
-
-    def send_command(self, msg):
-        # Create the packet and send via serial port
-        PORT = "/dev/ttyUSB0"
-        BAUD = 57600
-        ser = serial.Serial(PORT, BAUD)
-        capture = PacketCapture()
-        mav = mavlink2.MAVLink(capture, srcSystem=1, srcComponent=1)
-        mav.version = 2
-        mav.send(msg)
-        api_frame = self.build_api_tx_frame(capture.data)
-        ser.write(api_frame)
-        print("RAW HEX:", capture.data.hex())
-
-    def set_mode(self, namespace, mode):
-        sysid = int(namespace.replace('UAV', ''))
-        if mode == 'STABILIZE':
-            mode = 0
-        elif mode == 'AUTO':
-            mode = 3
-        elif mode == 'GUIDED':
-            mode = 4
-        elif mode == 'LOITER':
-            mode = 5
-
-        msg = self.connection.mav.command_long_encode(
-            target_system=sysid,
-            target_component=1,
-            command=mavutil.mavlink.MAV_CMD_DO_SET_MODE,
-            confirmation=0,
-            param1=1,  # Custom mode enabled
-            param2=mode,
-            param3=0, param4=0, param5=0, param6=0, param7=0
-        )
-        self.send_command(msg)
-
-    def arm(self, namespace, arm):
-        sysid = int(namespace.replace('UAV', ''))
-        msg = self.connection.mav.command_long_encode(
-            target_system=sysid,
-            target_component=1,
-            command=mavutil.mavlink.MAV_CMD_COMPONENT_ARM_DISARM,
-            confirmation=0,
-            param1=1 if arm else 0,  # 1 to arm, 0 to disarm
-            param2=0, param3=0, param4=0, param5=0, param6=0, param7=0
-        )
-        self.send_command(msg)
-
-    def takeoff(self, namespace, altitude):
-        sysid = int(namespace.replace('UAV', ''))
-        msg = self.connection.mav.command_long_encode(
-            target_system=sysid,
-            target_component=1,
-            command=mavutil.mavlink.MAV_CMD_NAV_TAKEOFF,
-            confirmation=0,
-            param1=0, param2=0, param3=0, param4=0, param5=0, param6=0,
-            param7=altitude
-        )
-        self.send_command(msg)
-
-    def land(self, namespace):
-        sysid = int(namespace.replace('UAV', ''))
-        msg = self.connection.mav.command_long_encode(
-            target_system=sysid,
-            target_component=1,
-            command=mavutil.mavlink.MAV_CMD_NAV_LAND,
-            confirmation=0,
-            param1=0, param2=0, param3=0, param4=0, param5=0, param6=0,
-            param7=0
-        )
-        self.send_command(msg)
-
-    def set_local_position(self, namespace, x, y, z):
-        sysid = int(namespace.replace('UAV', ''))
-        msg = self.connection.mav.set_position_target_local_ned_encode(
-            0, sysid, 1,  # time_boot_ms, sysid, compid
-            mavutil.mavlink.MAV_FRAME_LOCAL_NED,  # Frame
-            0b110111111000,  # Mask
-            y, x, -z,  # Position
-            0, 0, 0,  # Velocity
-            0, 0, 0,  # Acceleration
-            0, 0  # Yaw, yaw_rate
-        )
-        self.send_command(msg)
-
-    def reboot_drone(self, namespace):
-        sysid = int(namespace.replace('UAV', ''))
-        self.connection.mav.command_long_send(
-            sysid,
-            1,  # target_component (一般為1)
-            mavutil.mavlink.MAV_CMD_PREFLIGHT_REBOOT_SHUTDOWN,  # Reboot 命令
-            0,  # confirmation
-            1,  # 第一個參數 (1 = reboot，0 = 無操作，2 = 關機)
-            0, 0, 0, 0, 0, 0  # 其餘參數保留
-        )
-
-    def set_param(self, namespace, param_name, value):
-        sysid = int(namespace.replace('UAV', ''))
-        try:
-            self.connection.mav.param_set_send(
-                sysid,
-                1,
-                param_name.encode('utf-8'),
-                float(value),
-                mavutil.mavlink.MAV_PARAM_TYPE_REAL32
-            )
-        except Exception as e:
-            print(f"Failed to set parameter {param_name}: {e}")
-
-    def request_param(self, namespace, param_name):
-        sysid = int(namespace.replace('UAV', ''))
-        try:
-            self.connection.mav.param_request_read_send(
-                sysid,  # 系統 ID
-                1,  # 組件 ID
-                param_name.encode('utf-8'),  # 參數名稱
-                -1  # 參數索引，-1 表示根據名稱請求
-            )
-        except Exception as e:
-            print(f"Failed to set parameter {param_name}: {e}")
-
-    '''
-    def set_sr_params(self, sysid):
-        """ 直接設置 MAVLink 訊息頻率 """
-        freqs = [0, 2, 4]
-        params = {
-            "SR1_ADSB": freqs[0],
-            "SR1_EXT_STAT": freqs[1],
-            "SR1_EXTRA1": freqs[2],
-            "SR1_EXTRA2": freqs[2],
-            "SR1_EXTRA3": freqs[1],
-            "SR1_POSITION": freqs[1],
-            "SR1_RAW_SENS": freqs[1],
-            "SR1_RC_CHAN": freqs[1]
-        }
-        for param, value in params.items():
-            self.set_param(f"UAV{sysid}", param, value)
-    '''
-    def set_sr_params(self, sysid):
-        """ 直接設置 MAVLink 訊息頻率 """
-        freqs = [0, 1, 1]
-        params = {
-            "SR1_ADSB": freqs[0],
-            "SR1_EXT_STAT": freqs[0],
-            "SR1_EXTRA1": freqs[2],
-            "SR1_EXTRA2": freqs[2],
-            "SR1_EXTRA3": freqs[0],
-            "SR1_POSITION": freqs[1],
-            "SR1_RAW_SENS": freqs[1],
-            "SR1_RC_CHAN": freqs[0]
-        }
-        for param, value in params.items():
-            self.set_param(f"UAV{sysid}", param, value)
-
-class DroneControlApp(QMainWindow):
-    def __init__(self):
-        super().__init__()
-        self.workers = MAVLinkWorker()
-        self.namespaces = self.workers.namespaces
-        self.initUI()
-        
-        # Connect signals to update the UI
-        self.workers.state_signal.connect(self.update_state)
-        self.workers.battery_signal.connect(self.update_battery)
-        self.workers.gps_signal.connect(self.update_gps)
-        self.workers.gps_status_signal.connect(self.update_gps_status)
-        self.workers.local_position_signal.connect(self.update_local_position)
-        self.workers.imu_signal.connect(self.update_imu)
-        self.workers.hdg_signal.connect(self.update_hdg)
-        self.workers.groundspeed_signal.connect(self.update_groundspeed)
-        self.workers.ping_signal.connect(self.update_ping)
-        self.workers.loss_signal.connect(self.update_loss)
-        self.workers.frequency_signal.connect(self.update_frequency)
-        self.workers.param_signal.connect(self.update_param)
-        self.workers.kbps_signal.connect(self.update_kbps)
-        self.workers.start()
-            
-    def initUI(self):
-        self.setWindowTitle("多無人機控制介面")
-        self.setGeometry(100, 100, 800, 600)
-
-        # 主佈局
-        main_layout = QVBoxLayout()
-
-        # 無人機選擇區域
-        uav_layout = QHBoxLayout()
-        self.uav_checkboxes = {}
-        for namespace in self.namespaces:
-            checkbox = QCheckBox(namespace)
-            checkbox.setChecked(True)  # 預設勾選
-            self.uav_checkboxes[namespace] = checkbox
-            uav_layout.addWidget(checkbox)
-        main_layout.addLayout(uav_layout)
-
-        # 顯示所有無人機資訊，從左到右顯示
-        uav_layout = QHBoxLayout()
-        
-        # 逐個顯示 UAV 的資訊
-        self.uav_labels = {}
-        for namespace in self.namespaces:
-            uav_info_layout = QVBoxLayout()  # 每台 UAV 的資訊垂直排列
-
-            # 狀態顯示
-            self.uav_labels[namespace] = {
-                "status": QLabel("狀態：等待數據..."),
-                "battery": QLabel("電壓：等待數據..."),
-                "local_position": QLabel("Local Position：等待數據..."),
-                "gps": QLabel("GPS位置：等待數據...\n\n"),
-                "fix": QLabel("Fix Type：等待數據..."),
-                "satellites": QLabel("衛星數量：等待數據..."),
-                "groundspeed": QLabel("地面速度：等待數據..."),
-                "pitch": QLabel("Pitch：等待數據..."),
-                "heading": QLabel("Heading：等待數據..."),
-                "ping": QLabel("Ping：等待數據..."),
-                "loss": QLabel("丟包：等待數據..."),
-                "frequency": QLabel("頻率：等待數據..."),
-                "kbps": QLabel("吞吐量：等待數據..."),
-                "param": QLabel("SR1_EXTRA1：等待數據...")
-            }
-
-            # 把每個資訊添加到該 UAV 的垂直佈局中
-            for label in self.uav_labels[namespace].values():
-                uav_info_layout.addWidget(label)
-
-            # 將該 UAV 的資訊佈局加到主佈局中（從左到右排列）
-            uav_layout.addLayout(uav_info_layout)
-
-        main_layout.addLayout(uav_layout)
-
-        # SR1_EXTRA1參數設置
-        param_layout = QHBoxLayout()
-        self.paramInput = QLineEdit()
-        self.paramInput.setPlaceholderText("輸入SR1_EXTRA1的新值")
-        self.setParamButton = QPushButton("設置SR1_EXTRA1")
-        self.setParamButton.clicked.connect(self.set_SR1_EXTRA1)
-        param_layout.addWidget(self.paramInput)
-        param_layout.addWidget(self.setParamButton)
-        main_layout.addLayout(param_layout)
-
-        # 模式切換區域
-        mode_layout = QHBoxLayout()
-        self.modeComboBox = QComboBox()
-        self.modeComboBox.addItems(["GUIDED", "STABILIZE", "AUTO", "LOITER"])
-        self.modeButton = QPushButton("切換模式")
-        self.modeButton.clicked.connect(self.change_mode)
-        mode_layout.addWidget(QLabel("選擇模式："))
-        mode_layout.addWidget(self.modeComboBox)
-        mode_layout.addWidget(self.modeButton)
-        main_layout.addLayout(mode_layout)
-
-        # 解鎖按鈕
-        fly_layout = QHBoxLayout()
-        self.armButton = QPushButton("解鎖")
-        self.armButton.clicked.connect(self.arm_drone)
-
-        # 起飛按鈕
-        self.takeoffButton = QPushButton("起飛")
-        self.takeoffButton.clicked.connect(self.takeoff_drone)
-
-        # 降落按鈕
-        self.landButton = QPushButton("降落")
-        self.landButton.clicked.connect(self.land_drone)
-        fly_layout.addWidget(self.armButton)
-        fly_layout.addWidget(self.takeoffButton)
-        fly_layout.addWidget(self.landButton)
-        main_layout.addLayout(fly_layout)
-
-        # XYZ 設置欄位
-        xyz_layout = QHBoxLayout()
-        self.positionX = QLineEdit()
-        self.positionX.setPlaceholderText("X")
-        self.positionY = QLineEdit()
-        self.positionY.setPlaceholderText("Y")
-        self.positionZ = QLineEdit()
-        self.positionZ.setPlaceholderText("Z")
-        self.setPositionButton = QPushButton("設置位置")
-        self.setPositionButton.clicked.connect(self.set_local_position)
-        xyz_layout.addWidget(QLabel("輸入位置："))
-        xyz_layout.addWidget(self.positionX)
-        xyz_layout.addWidget(self.positionY)
-        xyz_layout.addWidget(self.positionZ)
-        xyz_layout.addWidget(self.setPositionButton)
-        main_layout.addLayout(xyz_layout)
-
-        # 設置 XYZ 的按鈕
-
-        #設置重啟按鈕
-        self.rebootButton = QPushButton("重啟")
-        self.rebootButton.clicked.connect(self.reboot_drone)
-        main_layout.addWidget(self.rebootButton)
-
-        # 設置主佈局
-        central_widget = QWidget(self)
-        central_widget.setLayout(main_layout)
-        self.setCentralWidget(central_widget)
-        self.show()
-
-    def closeEvent(self, event):
-        try:
-            self.workers.stop()
-        finally:
-            event.accept()
-
-    def update_state(self, namespace, mode):
-        self.uav_labels[namespace]["status"].setText(f"狀態：{mode}")
-
-    def update_battery(self, namespace, voltage):
-        self.uav_labels[namespace]["battery"].setText(f"電壓：{voltage:.2f} V")
-
-    def update_gps(self, namespace, latitude, longitude):
-        self.uav_labels[namespace]["gps"].setText(f"GPS位置： \n    Lat:{latitude:.6f}° \n    Lon:{longitude:.6f}°")
-
-    def update_gps_status(self, namespace, fix_type, satellites_visible):
-        fix_type_str = {
-            0: "No Fix",
-            1: "No Fix",
-            2: "2D Fix",
-            3: "3D Fix",
-            4: "RTK Float",
-            5: "RTK Fix"
-        }.get(fix_type, "Unknown")
-        self.uav_labels[namespace]["fix"].setText(f"Fix Type：{fix_type_str}")
-        self.uav_labels[namespace]["satellites"].setText(f"衛星數量：{satellites_visible}")
-
-    def update_local_position(self, namespace, x, y, z):
-        self.uav_labels[namespace]["local_position"].setText(f"Local：({x:.2f}, {y:.2f}, {z:.2f})")
-
-    def update_imu(self, namespace, pitch):
-        self.uav_labels[namespace]["pitch"].setText(f"Pitch：{pitch:.2f}°")
-
-    def update_hdg(self, namespace, heading):
-        self.uav_labels[namespace]["heading"].setText(f"Heading：{heading:.2f}°")
-
-    def update_groundspeed(self, namespace, groundspeed):
-        self.uav_labels[namespace]["groundspeed"].setText(f"地面速度：{groundspeed:.2f} m/s")
-    
-    def update_ping(self, namespace, ping):
-        self.uav_labels[namespace]["ping"].setText(f"Ping：{ping:.2f} ms")
-
-    def update_loss(self, namespace, loss):
-        self.uav_labels[namespace]["loss"].setText(f"丟包：{loss:.2f}%")
-
-    def update_frequency(self, namespace, frequency):
-        self.uav_labels[namespace]["frequency"].setText(f"頻率：{frequency:.2f} Hz")
-
-    def update_kbps(self, namespace, kbps):
-        self.uav_labels[namespace]["kbps"].setText(f"吞吐量：{kbps:.2f} KB/s")
-
-    def update_param(self, namespace, value):
-        self.uav_labels[namespace]["param"].setText(f"SR1_EXTRA1:{value}")
-
-    def change_mode(self):
-        try:
-            selected_mode = self.modeComboBox.currentText()
-            for namespace in self.namespaces:
-                if self.uav_checkboxes[namespace].isChecked():
-                    self.workers.set_mode(namespace, selected_mode)
-        except Exception as e:
-            QtWidgets.QMessageBox.warning(self, "錯誤", f"模式切換失敗：{str(e)}")
-
-    def arm_drone(self):
-        try:
-            for namespace in self.namespaces:
-                if self.uav_checkboxes[namespace].isChecked():
-                    self.workers.arm(namespace, True)
-        except Exception as e:
-            QtWidgets.QMessageBox.warning(self, "錯誤", f"解鎖失敗：{e}")
-
-    def takeoff_drone(self):
-        try:
-            z_text = self.positionZ.text().strip()
-            z = float(z_text) if z_text else 5.0
-            h = 2
-            for i, namespace in enumerate(self.namespaces):
-                if self.uav_checkboxes[namespace].isChecked():
-                    self.workers.takeoff(namespace, z + h * i)
-            
-        except ValueError:
-            QtWidgets.QMessageBox.warning(self, "錯誤", "請輸入有效的數值！")
-
-    def land_drone(self):
-        for namespace in self.namespaces:
-            if self.uav_checkboxes[namespace].isChecked():
-                self.workers.land(namespace)
-
-    def set_local_position(self):
-        try:
-            x = float(self.positionX.text().strip())
-            y = float(self.positionY.text().strip())
-            z = float(self.positionZ.text().strip())
-            h = 2
-            for i, namespace in enumerate(self.namespaces):
-                if self.uav_checkboxes[namespace].isChecked():
-                    self.workers.set_local_position(namespace, x, y, z + h * i)
-            
-        except ValueError:
-            QtWidgets.QMessageBox.warning(self, "錯誤", "請輸入有效的數值！")
-
-    def reboot_drone(self):
-        try:
-            for namespace in self.namespaces:
-                if self.uav_checkboxes[namespace].isChecked():
-                    self.workers.reboot_drone(namespace)
-        except Exception as e:
-            QtWidgets.QMessageBox.warning(self, "錯誤", f"重啟失敗：{e}")
-
-    def set_SR1_EXTRA1(self):
-        param_value = self.paramInput.text().strip()
-        try:
-            for namespace in self.namespaces:
-                if self.uav_checkboxes[namespace].isChecked():
-                    self.workers.set_param(namespace, "SR1_EXTRA1", param_value)
-                    self.workers.request_param(namespace, "SR1_EXTRA1")
-        except ValueError:
-            QtWidgets.QMessageBox.warning(self, "錯誤", "請輸入有效的數值！")
-
-def main():
-    app = QtWidgets.QApplication(sys.argv)
-    window = DroneControlApp()
-    window.show()
-    sys.exit(app.exec_())
-
-if __name__ == '__main__':
-    main()

src/unitdev02/unitdev02/devnote.txt

@@ -24,25 +24,34 @@ python -m fc_network_adapter.tests.test_vehicleStatusPublisher
 python -m fc_network_adapter.tests.test_ringBuffer
 python -m fc_network_adapter.fc_network_adapter.mainOrchestrator
 
+python -m someotherpkg.src.example_takeoff_land
 python -m someotherpkg.src.example_change_mode
 
 ros2 topic list
-ros2 topic echo 
+ros2 topic echo /fc_network/vehicle/sys3/battery
 
 
 /home/picars/ardupilot/build/sitl/bin/arducopter -S --model + --speedup 1 --slave 0 --defaults /home/picars/ardupilot/Tools/autotest/default_params/copter.parm --sim-address=127.0.0.1 -I3 --sysid 7
 
 mavproxy.py --master=tcp:127.0.0.1:5790 --out=udp:127.0.0.1:14560
 
-
 ros2 service call /mavlink/add_two_ints example_interfaces/srv/AddTwoInts "{a: 5,b: 8}"
 ros2 service call /fc_network/vehicle/mav_ping fc_interfaces/srv/MavPing "{target_sysid: 3, target_compid: 0, ping_seq: 1}"
 
 ros2 service call /fc_network/vehicle/send_command_long fc_interfaces/srv/MavCommandLong "{target_sysid: 3, target_compid: 0, command: 176, confirmation: 0, param1: 1, param2: 4, param3: 0,param4: 0,param5: 0,param6: 0,param7: 0, timeout_sec: 2}"
+ros2 service call /fc_network/vehicle/send_command_long fc_interfaces/srv/MavCommandLong "{target_sysid: 3, target_compid: 0, command: 176, confirmation: 0, param1: 1, param2: 4, timeout_sec: 2}"
 
-MAV_CMD_DO_SET_MODE (176)
+ros2 service call /fc_network/vehicle/pos_global_int fc_interfaces/srv/MavPositionTargetGlobalInt "{target_sysid: 3, target_compid: 1, coordinate_frame: 6, type_mask: 3576, lat_int: -35376655, lon_int: 149157011, alt: 20.0, timeout_sec: 5.0}"
 
 
 sudo tcpdump -i lo 'udp dst port 14561' -X
 sudo tcpdump -i lo 'udp dst port 14550' -X -vv
 sudo tcpdump -i lo -X udp port 14550
+
+幾個要討論的
+1. 專案文件中 .mat 檔案是幹嘛的?
+2. GUI 的 print [Panel/Map Update] 11 Hz 希望關閉
+3. readme 那串文字來源? 用途?
+4. GUI介面 加一下海拔高 跟 相對高 速度跟位置 變更為 XY速度 XY位置
+5. pitch yaw roll 出來了 弄一下
+6.

src/unitdev03/0328/close_loop.py

@@ -0,0 +1,102 @@
+#!/usr/bin/env python3
+from pymavlink import mavutil
+import socket
+import re
+import time
+import math
+
+def main():
+    # 建立MAVLink連接
+    print("建立MAVLink連接...")
+    connection_string = "udp:127.0.0.1:14550"
+    master = mavutil.mavlink_connection(connection_string)
+    master.wait_heartbeat()
+    print("MAVLink連接已建立")
+    
+    # 切換到GUIDED模式
+    master.mav.set_mode_send(
+        master.target_system,
+        mavutil.mavlink.MAV_MODE_FLAG_CUSTOM_MODE_ENABLED,
+        4  # GUIDED模式ID
+    )
+    print("已切換到GUIDED模式")
+    
+    # 解鎖無人機
+    master.mav.command_long_send(
+        master.target_system,
+        master.target_component,
+        mavutil.mavlink.MAV_CMD_COMPONENT_ARM_DISARM,
+        0,
+        1, 0, 0, 0, 0, 0, 0  # 1表示解鎖
+    )
+    print("無人機已解鎖")
+    
+    # 設置UDP服務器
+    server_socket = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
+    server_socket.bind(('0.0.0.0', 5000))
+    server_socket.settimeout(0.1)  # 非阻塞操作
+    print("UDP服務器已啟動，監聽端口5000")
+    
+    # 主循環，持續處理數據
+    while True:
+        try:
+            # 接收UDP數據
+            try:
+                data, _ = server_socket.recvfrom(1024)
+                decoded_data = data.decode('utf-8')
+                
+                # 提取velocity數據
+                vel_match = re.search(r'"next_velocity":\[([-\d\.\,]+)\]', decoded_data)
+                
+                if vel_match:
+                    velocity_str = vel_match.group(1)
+                    velocity = [float(x) for x in velocity_str.split(',')]
+                    vx, vy, vz = velocity
+                    
+                    # 獲取當前位置
+                    pos_msg = master.recv_match(type='LOCAL_POSITION_NED', blocking=True, timeout=0.5)
+                    if pos_msg:
+                        x, y, z = pos_msg.x, pos_msg.y, pos_msg.z
+                        
+                        # 設置type_mask - 僅使用速度控制
+                        type_mask = (
+                            mavutil.mavlink.POSITION_TARGET_TYPEMASK_X_IGNORE |
+                            mavutil.mavlink.POSITION_TARGET_TYPEMASK_Y_IGNORE |
+                            mavutil.mavlink.POSITION_TARGET_TYPEMASK_Z_IGNORE |
+                            mavutil.mavlink.POSITION_TARGET_TYPEMASK_AX_IGNORE |
+                            mavutil.mavlink.POSITION_TARGET_TYPEMASK_AY_IGNORE |
+                            mavutil.mavlink.POSITION_TARGET_TYPEMASK_AZ_IGNORE |
+                            mavutil.mavlink.POSITION_TARGET_TYPEMASK_YAW_IGNORE |
+                            mavutil.mavlink.POSITION_TARGET_TYPEMASK_YAW_RATE_IGNORE
+                        )
+                        
+                        # 發送位置目標命令
+                        print(f"發送速度指令: vx={vx}, vy={vy}, vz={vz}")
+                        master.mav.set_position_target_local_ned_send(
+                            0,
+                            master.target_system,
+                            master.target_component,
+                            mavutil.mavlink.MAV_FRAME_LOCAL_NED,
+                            type_mask,
+                            0, 0, 0,   
+                            vx, vy, vz,  # 速度
+                            0, 0, 0,  # 加速度
+                            0, 0  # 航向
+                        )
+                        print("success")
+                        # 要再去偵錯沒有data進來的狀況
+            except socket.timeout:
+                pass
+            
+            time.sleep(0.05)  # 控制循環速率
+            
+        except KeyboardInterrupt:
+            print("程序終止")
+            server_socket.close()
+            break
+        except Exception as e:
+            print(f"錯誤: {e}")
+            time.sleep(1)
+
+if __name__ == "__main__":
+    main()

src/unitdev03/0328/pymavlink_02.py

@@ -0,0 +1,497 @@
+#!/usr/bin/env python3
+from pymavlink import mavutil
+import time
+import math
+import threading
+import sys
+
+class DroneController:
+    def __init__(self, connection_string="udp:127.0.0.1:14550", drone_id=0):
+        self.master = mavutil.mavlink_connection(connection_string)
+        self.drone_id = drone_id
+        self.connection_string = connection_string
+        self.master.wait_heartbeat()
+        print(f"無人機 #{self.drone_id} 已建立連接!")
+
+    def set_mode(self, mode):
+        if mode == 'STABILIZE':
+            mode_id = 0
+        elif mode == 'AUTO':
+            mode_id = 3
+        elif mode == 'GUIDED':
+            mode_id = 4
+        elif mode == 'LOITER':
+            mode_id = 5
+        elif mode == 'RTL':
+            mode_id = 6
+        elif mode == 'LAND':
+            mode_id = 9
+        else:
+            print(f"不支援的模式: {mode}")
+            return False
+
+        self.master.mav.set_mode_send(
+            self.master.target_system,
+            mavutil.mavlink.MAV_MODE_FLAG_CUSTOM_MODE_ENABLED,
+            mode_id
+        )
+        
+        for _ in range(5):
+            msg = self.master.recv_match(type='HEARTBEAT', blocking=True, timeout=1)
+            if msg and msg.custom_mode == mode_id:
+                return True
+        return False
+
+    def arm(self, arm_state=True):
+        self.master.mav.command_long_send(
+            self.master.target_system,
+            self.master.target_component,
+            mavutil.mavlink.MAV_CMD_COMPONENT_ARM_DISARM,
+            0,
+            1 if arm_state else 0, 0, 0, 0, 0, 0, 0
+        )
+        
+        for _ in range(5):
+            msg = self.master.recv_match(type='HEARTBEAT', blocking=True, timeout=1)
+            if msg:
+                armed = msg.base_mode & mavutil.mavlink.MAV_MODE_FLAG_SAFETY_ARMED
+                if (arm_state and armed) or (not arm_state and not armed):
+                    return True
+        return False
+
+    def takeoff(self, altitude):
+        self.master.mav.command_long_send(
+            self.master.target_system,
+            self.master.target_component,
+            mavutil.mavlink.MAV_CMD_NAV_TAKEOFF,
+            0,
+            0, 0, 0, 0, 0, 0,
+            altitude
+        )
+        
+        timeout = time.time() + 30
+        while time.time() < timeout:
+            msg = self.master.recv_match(type='GLOBAL_POSITION_INT', blocking=True, timeout=1)
+            if msg:
+                current_alt = msg.relative_alt / 1000.0
+                if abs(current_alt - altitude) < 0.5:
+                    return True
+            time.sleep(0.1)
+        return False
+
+    def goto_position_local(self, x, y, z, vx=0, vy=0, vz=0, yaw=0, yaw_rate=0):
+        z = -z
+        
+        type_mask = (
+            mavutil.mavlink.POSITION_TARGET_TYPEMASK_AX_IGNORE |
+            mavutil.mavlink.POSITION_TARGET_TYPEMASK_AY_IGNORE |
+            mavutil.mavlink.POSITION_TARGET_TYPEMASK_AZ_IGNORE
+        )
+        
+        print(f"無人機 #{self.drone_id} 發送位置指令: x={x}, y={y}, z={-z}, vx={vx}, vy={vy}, vz={vz}")
+        
+        self.master.mav.set_position_target_local_ned_send(
+            0,
+            self.master.target_system,
+            self.master.target_component,
+            mavutil.mavlink.MAV_FRAME_LOCAL_NED,
+            type_mask,
+            x, y, z,
+            vx, vy, vz,
+            0, 0, 0,
+            yaw, yaw_rate
+        )
+
+        print(f"無人機 #{self.drone_id} 開始監控位置變化...")
+        start_time = time.time()
+        last_progress_time = start_time
+        min_dist = float('inf')
+        
+        while time.time() - start_time < 30:
+            msg = self.master.recv_match(type='LOCAL_POSITION_NED', blocking=True, timeout=1)
+            if msg:
+                dist = math.sqrt((msg.x - x)**2 + (msg.y - y)**2 + (msg.z - z)**2)
+                min_dist = min(min_dist, dist)
+                
+                current_time = time.time()
+                if current_time - last_progress_time >= 5:
+                    if dist > min_dist + 0.1:
+                        print(f"無人機 #{self.drone_id} 偏離目標，重新調整")
+                        self.master.mav.set_position_target_local_ned_send(
+                            0, self.master.target_system, self.master.target_component,
+                            mavutil.mavlink.MAV_FRAME_LOCAL_NED,
+                            type_mask,
+                            x, y, z, vx, vy, vz, 0, 0, 0, yaw, yaw_rate
+                        )
+                    last_progress_time = current_time
+                
+                print(f"無人機 #{self.drone_id} 當前位置: x={msg.x:.2f}, y={msg.y:.2f}, z={-msg.z:.2f}, 距離目標: {dist:.2f}m")
+                
+                if dist < 0.5:
+                    print(f"無人機 #{self.drone_id} 到達目標位置")
+                    return True
+                    
+                vel_msg = self.master.recv_match(type='LOCAL_POSITION_NED', blocking=False)
+                if vel_msg:
+                    vel_total = math.sqrt(vel_msg.vx**2 + vel_msg.vy**2 + vel_msg.vz**2)
+                    if dist < 1.0 and vel_total < 0.1:
+                        print(f"無人機 #{self.drone_id} 已穩定懸停在目標位置附近")
+                        return True
+                        
+            time.sleep(0.1)
+        
+        print(f"無人機 #{self.drone_id} 移動超時")
+        return False
+
+    def get_current_state(self):
+        state = {}
+        
+        # Mode and armed status
+        hb_msg = self.master.recv_match(type='HEARTBEAT', blocking=True, timeout=0.5)
+        if hb_msg:
+            mode_id = hb_msg.custom_mode
+            mode_name = {
+                0: "STABILIZE",
+                3: "AUTO",
+                4: "GUIDED",
+                5: "LOITER",
+                6: "RTL",
+                9: "LAND"
+            }.get(mode_id, f"Unknown({mode_id})")
+            
+            state['mode'] = mode_name
+            state['armed'] = bool(hb_msg.base_mode & mavutil.mavlink.MAV_MODE_FLAG_SAFETY_ARMED)
+        
+        # Position
+        pos_msg = self.master.recv_match(type='LOCAL_POSITION_NED', blocking=True, timeout=1.0)
+        if pos_msg:
+            state['position'] = {
+                'x': round(pos_msg.x, 2),
+                'y': round(pos_msg.y, 2),
+                'z': round(-pos_msg.z, 2)
+            }
+        
+        # GPS
+        gps_msg = self.master.recv_match(type='GLOBAL_POSITION_INT', blocking=True, timeout=1.0)
+        if gps_msg:
+            state['gps_position'] = {
+                'lat': gps_msg.lat / 1e7,
+                'lon': gps_msg.lon / 1e7,
+                'alt': gps_msg.alt / 1000,
+                'relative_alt': gps_msg.relative_alt / 1000
+            }
+        
+        return state
+
+class MultiDroneController:
+    def __init__(self, num_drones=5):
+        self.drones = []
+        self.num_drones = num_drones
+        
+        for i in range(num_drones):
+            port = 14550 + i
+            connection_string = f"udp:127.0.0.1:{port}"
+            drone = DroneController(connection_string, i)
+            self.drones.append(drone)
+            print(f"已初始化無人機 #{i} 在端口 {port}")
+    
+    def get_drone(self, drone_id):
+        if 0 <= drone_id < len(self.drones):
+            return self.drones[drone_id]
+        return None
+    
+    def all_drones_command(self, command, *args):
+        results = []
+        threads = []
+        
+        for drone in self.drones:
+            thread = threading.Thread(
+                target=lambda d, cmd, args, results: results.append((d.drone_id, getattr(d, cmd)(*args))),
+                args=(drone, command, args, results)
+            )
+            threads.append(thread)
+            thread.start()
+            
+        for thread in threads:
+            thread.join()
+            
+        for drone_id, success in results:
+            print(f"無人機 #{drone_id} 執行 {command} 結果: {'成功' if success else '失敗'}")
+            
+        return all(success for _, success in results)
+
+def print_menu():
+    menu = [
+        "\n=== 多機無人機控制界面 ===",
+        "1. 選擇操作的無人機",
+        "2. 對所有無人機執行命令",
+        "3. 查看所有無人機狀態",
+        "4. 退出",
+        "0. 顯示此清單",
+        "=========================="
+    ]
+    for item in menu:
+        print(item)
+
+def print_drone_menu():
+    menu = [
+        "\n=== 單機無人機控制界面 ===",
+        "1. 切換至 GUIDED 模式",
+        "2. 切換至 LAND 模式",
+        "3. 解鎖",
+        "4. 上鎖",
+        "5. 起飛",
+        "6. 飛到指定位置",
+        "7. 切換至 RTL 模式(返航)",
+        "8. 查看當前狀態",
+        "9. 返回主菜單",
+        "0. 顯示此清單",
+        "=========================="
+    ]
+    for item in menu:
+        print(item)
+
+def print_all_drones_menu():
+    menu = [
+        "\n=== 所有無人機控制界面 ===",
+        "1. 全部切換至 GUIDED 模式",
+        "2. 全部切換至 LAND 模式",
+        "3. 全部解鎖",
+        "4. 全部上鎖",
+        "5. 全部起飛",
+        "6. 全部返航",
+        "7. 返回主菜單",
+        "0. 顯示此清單",
+        "=========================="
+    ]
+    for item in menu:
+        print(item)
+
+def control_single_drone(drone):
+    print_drone_menu()
+    current_drone_id = drone.drone_id
+    
+    while True:
+        try:
+            choice = input(f"\n無人機 #{current_drone_id} - 請輸入指令編號: ")
+
+            if choice == '0':
+                print_drone_menu()
+                
+            elif choice == '1':
+                if drone.set_mode("GUIDED"):
+                    print(f"無人機 #{current_drone_id} 已切換至 GUIDED 模式")
+                else:
+                    print(f"無人機 #{current_drone_id} 切換模式失敗")
+                    
+            elif choice == '2':
+                if drone.set_mode("LAND"):
+                    print(f"無人機 #{current_drone_id} 已切換至 LAND 模式")
+                else:
+                    print(f"無人機 #{current_drone_id} 切換模式失敗")
+                    
+            elif choice == '3':
+                if drone.arm(True):
+                    print(f"無人機 #{current_drone_id} 已解鎖")
+                else:
+                    print(f"無人機 #{current_drone_id} 解鎖失敗")
+                    
+            elif choice == '4':
+                if drone.arm(False):
+                    print(f"無人機 #{current_drone_id} 已上鎖")
+                else:
+                    print(f"無人機 #{current_drone_id} 上鎖失敗")
+                    
+            elif choice == '5':
+                altitude = float(input("請輸入起飛高度(公尺): "))
+                if drone.takeoff(altitude):
+                    print(f"無人機 #{current_drone_id} 已到達目標高度: {altitude}公尺")
+                else:
+                    print(f"無人機 #{current_drone_id} 起飛失敗")
+                    
+            elif choice == '6':
+                x = float(input("請輸入X座標(公尺): "))
+                y = float(input("請輸入Y座標(公尺): "))
+                z = float(input("請輸入Z座標(公尺,高度為正值): "))
+                vx = float(input("請輸入X方向速度(公尺/秒，可選): ") or "0")
+                vy = float(input("請輸入Y方向速度(公尺/秒，可選): ") or "0")
+                if drone.goto_position_local(x, y, z, vx, vy):
+                    print(f"無人機 #{current_drone_id} 已到達目標位置")
+                else:
+                    print(f"無人機 #{current_drone_id} 移動失敗")
+                    
+            elif choice == '7':
+                if drone.set_mode("RTL"):
+                    print(f"無人機 #{current_drone_id} 已切換至 RTL 模式，正在返航")
+                else:
+                    print(f"無人機 #{current_drone_id} 切換至 RTL 模式失敗")
+                    
+            elif choice == '8':
+                state = drone.get_current_state()
+                if state:
+                    print(f"\n=== 無人機 #{current_drone_id} 狀態 ===")
+                    print(f"模式: {state.get('mode', '未知')}")
+                    print(f"解鎖狀態: {'已解鎖' if state.get('armed', False) else '已上鎖'}")
+                    
+                    if 'gps_position' in state:
+                        gps = state['gps_position']
+                        print(f"GPS位置: 緯度={gps['lat']:.6f}, 經度={gps['lon']:.6f}, 高度={gps['alt']:.2f}m")
+                        
+                    if 'position' in state:
+                        pos = state['position']
+                        print(f"本地位置: X={pos['x']:.2f}m, Y={pos['y']:.2f}m, Z={pos['z']:.2f}m")
+                    print("=============================")
+                else:
+                    print(f"無法獲取無人機 #{current_drone_id} 狀態")
+                    
+            elif choice == '9':
+                print("返回主菜單")
+                break
+                
+            else:
+                print("無效的指令，請重新輸入")
+                
+        except ValueError as e:
+            print(f"輸入錯誤: {e}")
+        except Exception as e:
+            print(f"發生錯誤: {e}")
+
+def control_all_drones(controller):
+    print_all_drones_menu()
+    
+    while True:
+        try:
+            choice = input("\n請輸入指令編號: ")
+
+            if choice == '0':
+                print_all_drones_menu()
+                
+            elif choice == '1':
+                if controller.all_drones_command("set_mode", "GUIDED"):
+                    print("所有無人機已切換至 GUIDED 模式")
+                else:
+                    print("部分無人機切換模式失敗")
+                    
+            elif choice == '2':
+                if controller.all_drones_command("set_mode", "LAND"):
+                    print("所有無人機已切換至 LAND 模式")
+                else:
+                    print("部分無人機切換模式失敗")
+                    
+            elif choice == '3':
+                if controller.all_drones_command("arm", True):
+                    print("所有無人機已解鎖")
+                else:
+                    print("部分無人機解鎖失敗")
+                    
+            elif choice == '4':
+                if controller.all_drones_command("arm", False):
+                    print("所有無人機已上鎖")
+                else:
+                    print("部分無人機上鎖失敗")
+                    
+            elif choice == '5':
+                altitude = float(input("請輸入所有無人機起飛高度(公尺): "))
+                if controller.all_drones_command("takeoff", altitude):
+                    print(f"所有無人機已到達目標高度: {altitude}公尺")
+                else:
+                    print("部分無人機起飛失敗")
+                    
+            elif choice == '6':
+                if controller.all_drones_command("set_mode", "RTL"):
+                    print("所有無人機已切換至 RTL 模式，正在返航")
+                else:
+                    print("部分無人機切換至 RTL 模式失敗")
+                    
+            elif choice == '7':
+                print("返回主菜單")
+                break
+                
+            else:
+                print("無效的指令，請重新輸入")
+                
+        except ValueError as e:
+            print(f"輸入錯誤: {e}")
+        except Exception as e:
+            print(f"發生錯誤: {e}")
+
+def display_all_drone_states(controller):
+    """顯示所有無人機狀態"""
+    print("\n=== 所有無人機狀態 ===")
+    
+    for drone in controller.drones:
+        max_attempts = 5
+        state = None
+        
+        for attempt in range(max_attempts):
+            state = drone.get_current_state()
+            if state and 'position' in state:
+                break
+            time.sleep(0.2)
+            
+        print(f"\n--- 無人機 #{drone.drone_id} ---")
+        print(f"連接: {drone.connection_string}")
+        
+        if not state:
+            print("狀態: 無法獲取完整數據")
+            continue
+            
+        print(f"飛行模式: {state.get('mode', '未知')}")
+        print(f"解鎖狀態: {'已解鎖' if state.get('armed', False) else '已上鎖'}")
+        
+        if 'gps_position' in state:
+            gps = state['gps_position']
+            print(f"GPS位置: 緯度={gps['lat']:.6f}, 經度={gps['lon']:.6f}, 高度={gps['alt']:.2f}m")
+            print(f"相對高度: {gps['relative_alt']:.2f}m")
+            
+        if 'position' in state:
+            pos = state['position']
+            print(f"本地位置: X={pos['x']:.2f}m, Y={pos['y']:.2f}m, Z={pos['z']:.2f}m")
+            
+        print("-----------------------")
+    
+    print("=========================")
+
+def main():
+    controller = MultiDroneController(5)
+    print_menu()
+
+    while True:
+        try:
+            choice = input("\n請輸入指令編號: ")
+
+            if choice == '0':
+                print_menu()
+                
+            elif choice == '1':
+                drone_id = int(input("請輸入要操作的無人機ID (0-4): "))
+                drone = controller.get_drone(drone_id)
+                if drone:
+                    control_single_drone(drone)
+                else:
+                    print(f"無效的無人機ID: {drone_id}")
+                    
+            elif choice == '2':
+                control_all_drones(controller)
+                    
+            elif choice == '3':
+                display_all_drone_states(controller)
+                    
+            elif choice == '4':
+                print("程式結束")
+                break
+                
+            else:
+                print("無效的指令，請重新輸入")
+                
+        except ValueError as e:
+            print(f"輸入錯誤: {e}")
+        except Exception as e:
+            print(f"發生錯誤: {e}")
+
+if __name__ == "__main__":
+    try:
+        main()
+    except KeyboardInterrupt:
+        print("\n程式被使用者中斷")
+        sys.exit(0)                

src/unitdev03/0328/test_transform.py

@@ -0,0 +1,331 @@
+import rclpy
+from pymavlink import mavutil
+from time import sleep
+import time
+
+import socket
+import struct
+
+# used at fdm_switch_example_two
+import threading
+import queue
+
+import json
+
+def mavlink_analyzer_simple(count = 500):
+    # rclpy.init()
+    # node = rclpy.create_node('mavlink_analyzer_simple')
+ 
+    print('Inintializing connection')
+
+    connection_string="udp:127.0.0.1:14550"
+    connection = mavutil.mavlink_connection(connection_string)
+
+    print('Catch messages')
+    msg_count = {}
+
+    start_time = time.time()
+
+    for _ in range(count):
+        msg = connection.recv_msg()
+        # msg = connection.recv_match(blocking=True)
+        if msg:
+            msg_type = msg.get_type()
+            if msg_type in msg_count:
+                msg_count[msg_type] += 1
+            else:
+                msg_count[msg_type] = 1
+
+            if msg.get_type() == 'SERVO_OUTPUT_RAW':
+                print(msg)
+                pass
+
+        else:
+            print('No message yet, 1 second for data to fill')
+            sleep(1)
+
+    print('Messages Type received:')
+    print(msg_count)
+
+    end_time = time.time()
+    print('Time elapsed: ', end_time - start_time)
+
+    print('Closing connection')
+    connection.close()
+
+
+def fdm_parser_example(data=None):
+    if data is None:
+        data = bytes.fromhex('1a48e903b1340500e803e803e803e803000000000000000000000000000000000000000000000000')
+    # 1a48e903b1340500e803e803e803e803000000000000000000000000000000000000000000000000
+
+
+    parse_format = 'HHI16H'
+
+    if len(data) != struct.calcsize(parse_format):
+        print("got packet of len %u, expected %u" % (len(data), struct.calcsize(parse_format)))
+        exit(1)
+
+    decoded = struct.unpack(parse_format,data)
+
+    magic = decoded[0]
+    frame_rate_hz = decoded[1]
+    frame_count = decoded[2]
+    pwm = decoded[3:]
+
+
+    print("magic: %u, frame_rate_hz: %u, frame_count: %u, pwm: %s" % (magic, frame_rate_hz, frame_count, pwm))
+
+def fdm_switch_example_one():
+
+    '''
+    這個範例在 SITL 與 Gazebo 之間 做一個簡單的UDP轉發器 
+    沒有轉換數據格式
+    '''
+    # make a link get fdm from SITL, as a server
+    sock_s2g = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
+    sock_s2g.bind(('', 9002))
+    sock_s2g.settimeout(0.1)
+
+    # set a target link pass JSON to Gazebo, as a client
+    sock_g2s = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
+    server_address = ('127.0.0.1', 19002)
+
+    packet_count = 0
+    start_time = time.time()
+
+    while True:
+        try:
+            # 接到SITL的數據 並轉發給Gazebo
+            data_s2g, addr_s2g = sock_s2g.recvfrom(1024)
+            sock_g2s.sendto(data_s2g, server_address)
+            # 得到Gazebo的回應 並轉發給SITL
+            data_g2s, addr_g2s = sock_g2s.recvfrom(1024)
+            sock_s2g.sendto(data_g2s, addr_s2g)
+
+            # 解析並保存接收到的數據到 JSON 檔案 (SITL -> Gazebo)
+            try:
+                # 解析從 SITL 到 Gazebo 的數據
+                decoded = struct.unpack('HHI16H', data_s2g)
+                s2g_json = {
+                    "magic": decoded[0],
+                    "frame_rate_hz": decoded[1],
+                    "frame_count": decoded[2],
+                    "pwm": list(decoded[3:])
+                }
+                
+                # 解析從 Gazebo 到 SITL 的數據
+                # 注意：假設從 Gazebo 返回的數據是字串形式的 JSON
+                try:
+                    g2s_data_str = data_g2s.decode('utf-8')
+                    # 保存原始數據，適合後續分析
+                    with open('gazebo_response_raw.txt', 'a') as f:
+                        f.write(g2s_data_str + '\n')
+                except:
+                    # 如果不是 UTF-8 格式，可能是二進制數據
+                    g2s_data_str = str(data_g2s)
+                
+                # 合併數據到一個 JSON 對象
+                combined_data = {
+                    "sitl_to_gazebo": s2g_json,
+                    "gazebo_to_sitl": g2s_data_str,
+                    "timestamp": time.time()
+                }
+                
+                # 寫入 JSON 檔案
+                with open('fdm_one_data.json', 'w') as f:
+                    json.dump(combined_data, f, indent=2)
+                    
+            except Exception as e:
+                print(f"JSON 處理錯誤: {e}")
+
+            packet_count += 1
+        except socket.timeout:
+            print(f'no value')
+            #print(f'address:{addr_s2g}')
+            #pass
+
+        current_time = time.time()
+        elapsed_time = current_time - start_time
+
+        if elapsed_time >= 1.0:
+            #print(f"Packets received in the last second: {packet_count}")
+            print(f'JSON Pack:{data_g2s}')
+            packet_count = 0
+            start_time = current_time
+            fdm_parser_example(data_s2g)
+
+
+def fdm_switch_example_two():
+    '''
+    這個範例在 SITL 與 Gazebo 之間 做一個簡單的UDP轉發器 
+    有轉換數據格式
+
+    time_usec
+    servo1_raw 
+    servo16_raw
+    '''
+    # read info from SITL via MAVLink
+    connection_string="udp:127.0.0.1:14550" # uart mavlink  # MATLAB
+    connection = mavutil.mavlink_connection(connection_string)
+
+    # set a target link pass JSON to Gazebo, as a client
+    sock_g2s = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
+    server_address = ('127.0.0.1', 19002)
+
+    data_queue = queue.Queue()
+    servo_out = [0] * 16
+    data_queue.put(servo_out)
+
+    def send_data_from_queue(sock, server_address, q, frame_rate_fdm=600, frame_count_fdm=1):
+        interval = 1.0 / frame_rate_fdm
+        while True:
+            start_time = time.time()
+            try:
+                data = q.get(timeout=0.1)
+                if data is None:
+                    break
+                last_data = data
+            except queue.Empty:
+                data = last_data
+            data = struct.pack('HHI16H', 0x481a, frame_rate_fdm, frame_count_fdm, *servo_out)
+            sock.sendto(data, server_address)
+
+            # 新增：寫入 JSON 檔案
+            data_json = {
+                "magic": 0x481a,
+                "frame_rate_hz": frame_rate_fdm,
+                "frame_count": frame_count_fdm,
+                "pwm": list(servo_out)
+            }
+            with open('fdm_data.json', 'w') as f:
+                json.dump(data_json, f)
+
+            frame_count_fdm += 1
+            fdm_parser_example(data) # debug
+
+            elapsed_time = time.time() - start_time
+            sleep_time = interval - elapsed_time
+            if sleep_time > 0:
+                sleep(sleep_time)
+
+    Running = True
+    thread = threading.Thread(target=send_data_from_queue, args=(sock_g2s, server_address, data_queue))
+    thread.start()
+
+    while Running:
+        msgb1 = None
+        msg = connection.recv_msg()
+        while msg :
+            if msg.get_type() == 'SERVO_OUTPUT_RAW':
+                msgb1 = msg
+                # break # 這裡不需要break，因為我要最後一個 servo_output_raw 的值
+            msg = connection.recv_msg()
+
+        if msgb1:
+            for i in range(16):
+                servo_out[i] = getattr(msgb1, f'servo{i+1}_raw')
+                data_queue.put(servo_out)
+            msgb1 = None
+            # Running = False # debug
+        else:
+            # print('No message yet, 10 ms for data to fill')
+            sleep(0.01)
+
+        # Example of putting data into the queue
+        # data_queue.put(data)
+        # To stop the thread, you can put None into the queue
+        # data_queue.put(None)
+
+
+def fdm_switch_example_one_with_remote_forwarding():
+    '''
+    這個範例在 SITL 與 Gazebo 之間 做一個簡單的UDP轉發器 
+    並且將JSON數據轉發到指定的遠端IP
+    '''
+    # make a link get fdm from SITL, as a server
+    sock_s2g = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
+    sock_s2g.bind(('', 9002))
+    sock_s2g.settimeout(0.1)
+
+    # set a target link pass JSON to Gazebo, as a client
+    sock_g2s = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
+    server_address = ('127.0.0.1', 19002)
+
+    # 建立與遠端IP的socket連接
+    remote_socket = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
+    remote_address = ('140.120.31.130', 9003)  # 遠端IP與埠號，您可以根據需要更改埠號
+    print(f'Setting up forwarding to remote address: {remote_address}')
+
+    packet_count = 0
+    start_time = time.time()
+
+    while True:
+        try:
+            # 接到SITL的數據 並轉發給Gazebo
+            data_s2g, addr_s2g = sock_s2g.recvfrom(1024)
+            sock_g2s.sendto(data_s2g, server_address)
+            
+            # 得到Gazebo的回應 並轉發給SITL
+            data_g2s, addr_g2s = sock_g2s.recvfrom(1024)
+            sock_s2g.sendto(data_g2s, addr_s2g)
+
+            # 解析並保存接收到的數據到 JSON 檔案 (SITL -> Gazebo)
+            try:
+                # 解析從 SITL 到 Gazebo 的數據
+                decoded = struct.unpack('HHI16H', data_s2g)
+                s2g_json = {
+                    "magic": decoded[0],
+                    "frame_rate_hz": decoded[1],
+                    "frame_count": decoded[2],
+                    "pwm": list(decoded[3:])
+                }
+                
+                # 解析從 Gazebo 到 SITL 的數據
+                try:
+                    g2s_data_str = data_g2s.decode('utf-8')
+                    # 保存原始數據，適合後續分析
+                    with open('gazebo_response_raw.txt', 'a') as f:
+                        f.write(g2s_data_str + '\n')
+                except:
+                    # 如果不是 UTF-8 格式，可能是二進制數據
+                    g2s_data_str = str(data_g2s)
+                
+                # 合併數據到一個 JSON 對象
+                combined_data = {
+                    "sitl_to_gazebo": s2g_json,
+                    "gazebo_to_sitl": g2s_data_str,
+                    "timestamp": time.time()
+                }
+                
+                # 寫入 JSON 檔案
+                with open('fdm_one_data.json', 'w') as f:
+                    json.dump(combined_data, f, indent=2)
+                
+                # 將JSON數據轉發到遠端IP
+                remote_socket.sendto(json.dumps(combined_data).encode(), remote_address)
+                    
+            except Exception as e:
+                print(f"JSON 處理錯誤: {e}")
+
+            packet_count += 1
+        except socket.timeout:
+            print(f'no value')
+            #pass
+
+        current_time = time.time()
+        elapsed_time = current_time - start_time
+
+        if elapsed_time >= 1.0:
+            print(f'JSON Pack:{data_g2s}')
+            print(f'Forwarded data to remote IP: 140.120.31.130')
+            packet_count = 0
+            start_time = current_time
+            fdm_parser_example(data_s2g)
+
+print('Start')
+#fdm_switch_example_two()
+#mavlink_analyzer_simple()
+#fdm_switch_example_one()
+fdm_switch_example_one_with_remote_forwarding()
+#fdm_parser_example()

src/unitdev03/0328/testserver.py

@@ -0,0 +1,55 @@
+import socket
+import re
+
+def start_udp_server(host='0.0.0.0', port=5000, buffer_size=1024):
+    server_socket = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
+    server_socket.bind((host, port))
+    
+    print(f"UDP Server running on {host}:{port}")
+    print("Waiting for data...")
+    
+    position = None
+    velocity = None
+    
+    try:
+        while True:
+            data, client_address = server_socket.recvfrom(buffer_size)
+            
+            print(f"\n----- 接收到的資料 -----")
+            print(f"從 {client_address} 接收:")
+            
+            try:
+                decoded_data = data.decode('utf-8')
+                print(f"文本數據: {decoded_data}")
+                
+                # 直接使用正則表達式提取數據
+                pos_match = re.search(r'"position":\[([-\d\.\,]+)\]', decoded_data)
+                vel_match = re.search(r'"velocity":\[([-\d\.\,]+)\]', decoded_data)
+                
+                if pos_match:
+                    position_str = pos_match.group(1)
+                    position = [float(x) for x in position_str.split(',')]
+                    print(f"Position: {position}")
+                
+                if vel_match:
+                    velocity_str = vel_match.group(1)
+                    velocity = [float(x) for x in velocity_str.split(',')]
+                    print(f"Velocity: {velocity}")
+                
+                # 這裡可以對 position 和 velocity 進行進一步操作
+                
+            except UnicodeDecodeError:
+                print(f"二進制數據: {data.hex()}")
+            except Exception as e:
+                print(f"處理數據時出錯: {e}")
+            
+            print(f"數據大小: {len(data)} 字節")
+            print("-----------------------")
+            
+    except KeyboardInterrupt:
+        print("Server shutting down...")
+    finally:
+        server_socket.close()
+
+if __name__ == "__main__":
+    start_udp_server(port=5000)

src/unitdev03/0519/close_loop_multi_1.py

@@ -0,0 +1,297 @@
+#!/usr/bin/env python3
+from pymavlink import mavutil
+import socket
+import re
+import time
+import math
+import threading
+import json
+import os
+import datetime
+import queue
+import argparse
+from select import select
+
+class Drone:
+    def __init__(self, instance_id, mavlink_port, udp_port):
+        self.instance_id = instance_id
+        self.mavlink_port = mavlink_port
+        self.udp_port = udp_port
+        self.running = True
+        self.connection = None
+        
+        # 創建數據緩存
+        self.latest_position = None
+        
+        # 創建日誌佇列
+        self.log_queue = queue.Queue()
+        
+        # 創建日誌文件目錄
+        self.log_dir = "drone_logs"
+        if not os.path.exists(self.log_dir):
+            os.makedirs(self.log_dir)
+        # 創建一個含時間戳的日誌文件名
+        timestamp = datetime.datetime.now().strftime("%Y%m%d_%H%M%S")
+        self.log_file = os.path.join(self.log_dir, f"drone_{self.instance_id}_{timestamp}.log")
+        
+        # 通訊統計
+        self.packet_counter = 0
+        self.start_time = time.time()
+        self.packets_per_second = 0
+
+    def connect(self):
+        """僅建立MAVLink連接"""
+        print(f"Drone {self.instance_id}: 建立MAVLink連接 (Port {self.mavlink_port})...")
+        connection_string = f"udp:127.0.0.1:{self.mavlink_port}"
+        self.connection = mavutil.mavlink_connection(connection_string)
+        self.connection.wait_heartbeat()
+        print(f"Drone {self.instance_id}: MAVLink連接已建立")
+    
+    def set_guided_mode(self):
+        """切換到GUIDED模式"""
+        self.connection.mav.set_mode_send(
+            self.connection.target_system,
+            mavutil.mavlink.MAV_MODE_FLAG_CUSTOM_MODE_ENABLED,
+            4  # GUIDED模式ID
+        )
+        print(f"Drone {self.instance_id}: 已切換到GUIDED模式")
+    
+    def arm(self):
+        """解鎖無人機"""
+        self.connection.mav.command_long_send(
+            self.connection.target_system,
+            self.connection.target_component,
+            mavutil.mavlink.MAV_CMD_COMPONENT_ARM_DISARM,
+            0, 1, 0, 0, 0, 0, 0, 0  # 1表示解鎖
+        )
+        print(f"Drone {self.instance_id}: 無人機已解鎖")
+    
+    def takeoff(self, altitude=3.0):
+        """起飛到指定高度"""
+        self.connection.mav.command_long_send(
+            self.connection.target_system,
+            self.connection.target_component,
+            mavutil.mavlink.MAV_CMD_NAV_TAKEOFF,
+            0, 0, 0, 0, 0, 0, 0, altitude
+        )
+        print(f"Drone {self.instance_id}: 起飛命令已發送，目標高度{altitude}公尺")
+    
+    def mavlink_listener(self):
+        """事件驅動方式監聽MAVLink訊息"""
+        # 獲取底層socket
+        mavlink_socket = self.connection.port.fileno()
+        
+        while self.running:
+            # 使用select等待數據，無須sleep
+            readable, _, _ = select([mavlink_socket], [], [], 0.001)
+            if readable:
+                msg = self.connection.recv_msg()
+                if msg and msg.get_type() == 'LOCAL_POSITION_NED':
+                    self.latest_position = msg
+    
+    def log_worker(self):
+        """事件驅動方式處理日誌寫入"""
+        while self.running:
+            if not self.log_queue.empty():
+                try:
+                    # 批量處理日誌
+                    batch = []
+                    while not self.log_queue.empty() and len(batch) < 100:
+                        try:
+                            entry = self.log_queue.get_nowait()
+                            batch.append(entry)
+                        except queue.Empty:
+                            break
+                    
+                    # 寫入日誌
+                    if batch:
+                        with open(self.log_file, 'a', encoding='utf-8') as f:
+                            for entry in batch:
+                                f.write(json.dumps(entry, ensure_ascii=False) + "\n")
+                except Exception as e:
+                    print(f"Drone {self.instance_id}: 日誌寫入錯誤: {e}")
+            
+            # 每秒計算一次統計數據
+            now = time.time()
+            if now - self.start_time >= 1.0:
+                self.packets_per_second = self.packet_counter
+                print(f"Drone {self.instance_id} - 每秒封包數: {self.packets_per_second}")
+                self.packet_counter = 0
+                self.start_time = now
+            
+            # 使用select等待短暫時間而非sleep
+            select([], [], [], 0.01)  # 10ms的微小等待
+    
+    def log_packet(self, packet_data, packet_num):
+        """將數據加入日誌佇列"""
+        try:
+            timestamp = datetime.datetime.now().strftime("%Y-%m-%d %H:%M:%S.%f")[:-3]
+            log_entry = {
+                "timestamp": timestamp,
+                "packet_number": packet_num,
+                "data": packet_data
+            }
+            self.log_queue.put(log_entry)
+        except Exception as e:
+            print(f"Drone {self.instance_id}: 佇列添加錯誤: {e}")
+    
+    def run(self):
+        """使用事件驅動方式處理UDP通訊"""
+        # 啟動背景線程
+        mavlink_thread = threading.Thread(target=self.mavlink_listener)
+        mavlink_thread.daemon = True
+        mavlink_thread.start()
+        
+        log_thread = threading.Thread(target=self.log_worker)
+        log_thread.daemon = True
+        log_thread.start()
+        
+        # 設置UDP服務器
+        server_socket = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
+        server_socket.setsockopt(socket.SOL_SOCKET, socket.SO_RCVBUF, 262144)  # 增大接收緩衝區
+        server_socket.bind(('0.0.0.0', self.udp_port))
+        server_socket.setblocking(0)  # 非阻塞模式
+        
+        print(f"Drone {self.instance_id}: UDP服務器已啟動，監聽端口{self.udp_port}")
+        print(f"Drone {self.instance_id}: 日誌將保存至 {self.log_file}")
+        
+        # 主事件循環
+        while self.running:
+            # 使用select監聽UDP
+            readable, _, _ = select([server_socket], [], [], 0.001)
+            
+            if readable:
+                try:
+                    data, _ = server_socket.recvfrom(4096)
+                    decoded_data = data.decode('utf-8')
+                    
+                    # 增加計數器
+                    self.packet_counter += 1
+                    total_packets = self.packet_counter
+                    
+                    try:
+                        # 解析JSON
+                        json_data = json.loads(decoded_data)
+                        
+                        # 寫入日誌
+                        self.log_packet(json_data, total_packets)
+                        
+                        # 提取速度數據
+                        vel_match = re.search(r'"next_velocity":\[([-\d\.\,]+)\]', decoded_data)
+                        if vel_match:
+                            velocity_str = vel_match.group(1)
+                            velocity = [float(x) for x in velocity_str.split(',')]
+                            vx, vy, vz = velocity
+                            
+                            # 設置type_mask - 僅使用速度控制
+                            type_mask = (
+                                mavutil.mavlink.POSITION_TARGET_TYPEMASK_X_IGNORE |
+                                mavutil.mavlink.POSITION_TARGET_TYPEMASK_Y_IGNORE |
+                                mavutil.mavlink.POSITION_TARGET_TYPEMASK_Z_IGNORE |
+                                mavutil.mavlink.POSITION_TARGET_TYPEMASK_AX_IGNORE |
+                                mavutil.mavlink.POSITION_TARGET_TYPEMASK_AY_IGNORE |
+                                mavutil.mavlink.POSITION_TARGET_TYPEMASK_AZ_IGNORE |
+                                mavutil.mavlink.POSITION_TARGET_TYPEMASK_YAW_IGNORE |
+                                mavutil.mavlink.POSITION_TARGET_TYPEMASK_YAW_RATE_IGNORE
+                            )
+                            
+                            # 直接發送速度指令，不需等待位置數據
+                            self.connection.mav.set_position_target_local_ned_send(
+                                0,
+                                self.connection.target_system,
+                                self.connection.target_component,
+                                mavutil.mavlink.MAV_FRAME_LOCAL_NED,
+                                type_mask,
+                                0, 0, 0,  # 位置
+                                vx, vy, -vz,  # 速度
+                                0, 0, 0,  # 加速度
+                                0, 0  # 航向
+                            )
+                            print(f"Drone {self.instance_id}: 發送速度指令: vx={vx}, vy={vy}, vz={-vz}")
+                    
+                    except json.JSONDecodeError as je:
+                        print(f"Drone {self.instance_id}: JSON解析錯誤: {je}")
+                        self.log_packet({"raw_data": decoded_data}, total_packets)
+                
+                except Exception as e:
+                    print(f"Drone {self.instance_id}: 處理錯誤: {e}")
+        
+        # 清理資源
+        server_socket.close()
+        print(f"Drone {self.instance_id}: 線程結束")
+
+def main():
+    # 解析命令行參數
+    parser = argparse.ArgumentParser(description='多無人機控制程序')
+    parser.add_argument('--drones', type=str, default='1,2,3',
+                        help='要控制的無人機ID，用逗號分隔 (例如: 1,2,3)')
+    args = parser.parse_args()
+    
+    # 所有可用的無人機配置
+    all_drone_configs = [
+        (1, 14550, 5003),
+        (2, 14560, 5013),
+        (3, 14570, 5023)
+    ]
+    
+    # 解析要使用的無人機ID
+    drone_ids = [int(id_str) for id_str in args.drones.split(',')]
+    drone_configs = [config for config in all_drone_configs if config[0] in drone_ids]
+    
+    drones = []
+    threads = []
+    
+    try:
+        # 創建所有無人機實例
+        for instance_id, mavlink_port, udp_port in drone_configs:
+            drone = Drone(instance_id, mavlink_port, udp_port)
+            drones.append(drone)
+        
+        # 步驟1: 所有無人機連接
+        for drone in drones:
+            drone.connect()
+        
+        # 適當延遲確保連接穩定
+        time.sleep(1)
+        
+        # 步驟2: 所有無人機同時切換到GUIDED模式
+        for drone in drones:
+            drone.set_guided_mode()
+        
+        time.sleep(1)
+        
+        # 步驟3: 所有無人機同時解鎖
+        for drone in drones:
+            drone.arm()
+        
+        time.sleep(1)
+        
+        # 步驟4: 所有無人機同時起飛
+        for drone in drones:
+            drone.takeoff()
+        
+        # 啟動監聽線程
+        for drone in drones:
+            thread = threading.Thread(target=drone.run)
+            thread.daemon = True
+            threads.append(thread)
+            thread.start()
+        
+        print(f"已啟動 {len(drones)} 台無人機 (ID: {args.drones})，按Ctrl+C結束程序")
+        
+        # 主線程事件循環
+        while True:
+            select([], [], [], 0.1)  # 使用select代替time.sleep
+            
+    except KeyboardInterrupt:
+        print("正在停止所有無人機...")
+        for drone in drones:
+            drone.running = False
+        
+        for thread in threads:
+            thread.join(timeout=2)
+        
+        print("程序已結束")
+
+if __name__ == "__main__":
+    main()

src/unitdev03/0519/enemy_status.py

@@ -0,0 +1,340 @@
+#!/usr/bin/env python3
+
+from pymavlink import mavutil
+import time
+import sys
+import json
+import socket
+import threading
+import os
+from datetime import datetime
+
+# 固定初始位置設定 (5,5,0)
+INITIAL_POSITION = [5, 5, 0]
+
+# MAVLink 連接設定
+connection_string = 'udp:127.0.0.1:14553'
+
+# 網路傳輸設定
+target_ip = '140.120.31.130'
+target_port = 9053
+
+# 建立 UDP socket 用於傳送數據
+sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
+
+# 封包編號計數器
+packet_counter = 0
+
+# 共享變數 - 存儲最新位置
+current_position = None
+initial_position = INITIAL_POSITION.copy()
+position_lock = threading.Lock()
+
+# 連接到無人機
+print(f"嘗試連接到 MAVLink: {connection_string}")
+try:
+    connection = mavutil.mavlink_connection(connection_string, autoreconnect=True)
+    connection.wait_heartbeat()
+    print(f"成功連接到系統 {connection.target_system}!")
+except Exception as e:
+    print(f"MAVLink 連接失敗: {e}")
+    sys.exit(1)
+
+print(f"使用指定初始位置: 北={initial_position[0]}, 東={initial_position[1]}, 下={initial_position[2]}")
+
+# 請求位置數據流
+connection.mav.request_data_stream_send(
+    connection.target_system,
+    connection.target_component,
+    mavutil.mavlink.MAV_DATA_STREAM_POSITION,
+    10,
+    1
+)
+
+# 日誌記錄函數
+def start_logging():
+    """創建並開啟日誌文件"""
+    log_dir = "logs"
+    if not os.path.exists(log_dir):
+        os.makedirs(log_dir)
+    
+    timestamp = datetime.now().strftime("%Y%m%d_%H%M%S")
+    log_file = os.path.join(log_dir, f"drone_log_{timestamp}.csv")
+    
+    file = open(log_file, "w")
+    file.write("packet_id,timestamp,x,y,z,vx,vy,vz\n")  # CSV標頭
+    print(f"日誌記錄已開始: {log_file}")
+    return file
+
+# 控制函數
+def set_guided_mode():
+    """設置為 GUIDED 模式"""
+    connection.mav.command_long_send(
+        connection.target_system,
+        connection.target_component,
+        mavutil.mavlink.MAV_CMD_DO_SET_MODE,
+        0,
+        mavutil.mavlink.MAV_MODE_FLAG_CUSTOM_MODE_ENABLED,
+        4,
+        0, 0, 0, 0, 0
+    )
+    ack = connection.recv_match(type='COMMAND_ACK', blocking=True, timeout=3)
+    return ack and ack.result == mavutil.mavlink.MAV_RESULT_ACCEPTED
+
+def arm_throttle():
+    """解鎖油門"""
+    connection.mav.command_long_send(
+        connection.target_system,
+        connection.target_component,
+        mavutil.mavlink.MAV_CMD_COMPONENT_ARM_DISARM,
+        0, 1, 0, 0, 0, 0, 0, 0
+    )
+    ack = connection.recv_match(type='COMMAND_ACK', blocking=True, timeout=3)
+    return ack and ack.result == mavutil.mavlink.MAV_RESULT_ACCEPTED
+
+def takeoff(altitude):
+    """起飛到指定高度"""
+    connection.mav.command_long_send(
+        connection.target_system,
+        connection.target_component,
+        mavutil.mavlink.MAV_CMD_NAV_TAKEOFF,
+        0, 0, 0, 0, 0, 0, 0, altitude
+    )
+    ack = connection.recv_match(type='COMMAND_ACK', blocking=True, timeout=3)
+    return ack and ack.result == mavutil.mavlink.MAV_RESULT_ACCEPTED
+
+def set_speed_limit(speed_type, speed):
+    """
+    設置速度限制
+    
+    Parameters:
+    -----------
+    speed_type : int
+        速度類型 (0=空速, 1=地速, 2=爬升速度, 3=下降速度)
+    speed : float
+        速度值 (米/秒)
+    """
+    connection.mav.command_long_send(
+        connection.target_system,
+        connection.target_component,
+        mavutil.mavlink.MAV_CMD_DO_CHANGE_SPEED,
+        0,  # 確認
+        speed_type,  # 參數1: 速度類型
+        speed,       # 參數2: 速度 (m/s)
+        -1,          # 參數3: 油門 (-1 表示不變)
+        0, 0, 0, 0   # 參數4-7: 未使用
+    )
+    ack = connection.recv_match(type='COMMAND_ACK', blocking=True, timeout=3)
+    success = ack and ack.result == mavutil.mavlink.MAV_RESULT_ACCEPTED
+    if success:
+        print(f"已設置速度限制為 {speed:.2f} m/s")
+    else:
+        print("設置速度限制失敗")
+    return success
+
+def goto_position(north, east, down):
+    """前往指定位置 (世界座標系)"""
+    # 將世界座標轉換為相對座標
+    rel_north = north - initial_position[0]
+    rel_east = east - initial_position[1]
+    rel_down = down - initial_position[2]
+    
+    connection.mav.set_position_target_local_ned_send(
+        0,
+        connection.target_system,
+        connection.target_component,
+        mavutil.mavlink.MAV_FRAME_LOCAL_NED,
+        0b0000111111111000,
+        rel_north, rel_east, rel_down,
+        0, 0, 0,
+        0, 0, 0,
+        0, 0
+    )
+    # print(f"相對位置: 北={rel_north:.2f}, 東={rel_east:.2f}, 下={rel_down:.2f}")
+
+def get_current_position():
+    """獲取存儲的當前位置"""
+    with position_lock:
+        if current_position is None:
+            return None
+        return current_position.copy()
+
+def control_interface():
+    """命令行控制界面"""
+    print("無人機控制介面已啟動，輸入命令：")
+    print("  arm - 解鎖油門")
+    print("  takeoff [高度] - 起飛")
+    print("  goto [北] [東] [高度] - 前往位置 (世界座標系)")
+    print("  pos - 顯示當前位置 (相對座標和世界座標)")
+    print("  init - 顯示初始位置")
+    print("  exit - 退出控制")
+    
+    while True:
+        try:
+            cmd = input("命令> ").strip().split()
+            if not cmd:
+                continue
+                
+            if cmd[0] == "arm":
+                print("嘗試解鎖油門...")
+                if set_guided_mode() and arm_throttle():
+                    print("成功解鎖油門！")
+                else:
+                    print("解鎖失敗")
+                    
+            elif cmd[0] == "takeoff":
+                alt = float(cmd[1]) if len(cmd) > 1 else 3.0
+                print(f"嘗試起飛到 {alt} 米高...")
+                if takeoff(alt):
+                    print("起飛命令已發送")
+                else:
+                    print("起飛失敗")
+                    
+            elif cmd[0] == "goto":
+                if len(cmd) >= 4:
+                    north = float(cmd[1])
+                    east = float(cmd[2])
+                    down = -float(cmd[3])  # 高度是負的下
+                    print(f"前往世界位置: 北:{north} 東:{east} 下:{-down} 米")
+                    goto_position(north, east, down)
+                else:
+                    print("用法: goto [北] [東] [高度]")
+            
+            elif cmd[0] == "pos":
+                pos = get_current_position()
+                if pos:
+                    rel_x, rel_y, rel_z = pos
+                    # 計算世界座標
+                    world_x = initial_position[0] + rel_x
+                    world_y = initial_position[1] + rel_y
+                    world_z = initial_position[2] + rel_z
+                    # NED 轉 ENU
+                    enu_x = rel_y
+                    enu_y = rel_x
+                    enu_z = -rel_z
+                    
+                    print("\n當前位置:")
+                    #print(f"  相對座標 (NED): 北={rel_x:.2f}m, 東={rel_y:.2f}m, 下={rel_z:.2f}m")
+                    print(f"  世界座標 : 北={world_x:.2f}m, 東={world_y:.2f}m, 下={-world_z:.2f}m")
+                    #print(f"  相對座標 (ENU): 東={enu_x:.2f}m, 北={enu_y:.2f}m, 上={enu_z:.2f}m")
+                    #print(f"  高度: {-rel_z:.2f}m")
+                else:
+                    print("尚未獲取到位置信息")
+
+            elif cmd[0] == "speed":
+                if len(cmd) > 1:
+                    speed = float(cmd[1])
+                    set_speed_limit(1, speed)  # 1 = 地速
+                else:
+                    print("用法: speed [速度m/s]")
+                    
+            elif cmd[0] == "climbspeed":
+                if len(cmd) > 1:
+                    speed = float(cmd[1])
+                    set_speed_limit(2, speed)  # 2 = 爬升速度
+                    set_speed_limit(3, speed)  # 3 = 下降速度
+                else:
+                    print("用法: climbspeed [速度m/s]")                    
+            
+            elif cmd[0] == "init":
+                print(f"初始位置 (NED): 北={initial_position[0]:.2f}m, 東={initial_position[1]:.2f}m, 下={initial_position[2]:.2f}m")
+                    
+            elif cmd[0] == "exit":
+                print("退出控制介面")
+                break
+                
+            else:
+                print(f"未知命令: {cmd[0]}")
+        except Exception as e:
+            print(f"命令執行錯誤: {e}")
+
+# 數據傳輸線程函數
+def data_sending_thread():
+    global packet_counter, current_position
+    
+    # 開啟日誌文件
+    log_file = start_logging()
+    
+    try:
+        print(f"開始接收數據並傳送到 {target_ip}:{target_port}...")
+        while True:
+            msg = connection.recv_match(type='LOCAL_POSITION_NED', blocking=True, timeout=1)
+            if msg is not None:
+                # 更新當前位置（線程安全）
+                with position_lock:
+                    current_position = [msg.x, msg.y, msg.z]
+                
+                # 增加封包編號
+                packet_counter += 1
+
+                # 計算世界座標
+                world_x = initial_position[0] + msg.x
+                world_y = initial_position[1] + msg.y
+                world_z = initial_position[2] + msg.z
+
+                # NED 轉換為 ENU 並使用世界座標
+                enu_position = {
+                    "x": world_x,  # 東 (從 NED 的 x)
+                    "y": world_y,  # 北 (從 NED 的 y)
+                    "z": -world_z  # 上 (從 NED 的 -z)
+                }
+                
+                enu_velocity = {
+                    "vx": msg.vy,  # 東向速度
+                    "vy": msg.vx,  # 北向速度
+                    "vz": -msg.vz  # 上向速度
+                }
+                
+                # 創建JSON對象 (使用世界座標)
+                json_data = {
+                    "packet_id": packet_counter,
+                    "timestamp": msg.time_boot_ms,
+                    "enemy_position": enu_position,
+                    "enemy_velocity": enu_velocity
+                }
+                
+                # 寫入日誌文件 (CSV格式)
+                try:
+                    log_file.write(f"{packet_counter},{msg.time_boot_ms},{enu_position['x']},{enu_position['y']},{enu_position['z']},{enu_velocity['vx']},{enu_velocity['vy']},{enu_velocity['vz']}\n")
+                    log_file.flush()  # 確保數據即時寫入文件
+                except Exception as e:
+                    print(f"寫入日誌失敗: {e}")
+                
+                json_str = json.dumps(json_data)
+                
+                try:
+                    sock.sendto(json_str.encode('utf-8'), (target_ip, target_port))
+                except Exception as e:
+                    print(f"傳送數據失敗: {e}")
+                
+                time.sleep(0.1)
+    except Exception as e:
+        print(f"數據傳輸線程錯誤: {e}")
+    finally:
+        # 確保關閉日誌文件
+        log_file.close()
+        print("日誌文件已關閉")
+
+# 創建並啟動數據發送線程
+sender_thread = threading.Thread(target=data_sending_thread)
+sender_thread.daemon = True
+sender_thread.start()
+
+# 啟動命令行控制界面
+try:
+    control_interface()
+except KeyboardInterrupt:
+    print("\n程序已被用戶中斷")
+finally:
+    if connection:
+        connection.mav.request_data_stream_send(
+            connection.target_system,
+            connection.target_component,
+            mavutil.mavlink.MAV_DATA_STREAM_ALL,
+            0, 0
+        )
+        print("數據流已停止")
+    sock.close()
+    print("網絡連接已關閉")
+    print(f"總共傳送了 {packet_counter} 個封包")
+    print("日誌文件已儲存")

src/unitdev03/0519/test_transform.py

@@ -0,0 +1,663 @@
+import rclpy
+from pymavlink import mavutil
+from time import sleep
+import time
+
+import socket
+import struct
+
+# used at fdm_switch_example_two
+import threading
+import queue
+
+import json
+
+def mavlink_analyzer_simple(count = 500):
+    # rclpy.init()
+    # node = rclpy.create_node('mavlink_analyzer_simple')
+ 
+    print('Inintializing connection')
+
+    connection_string="udp:127.0.0.1:14550"
+    connection = mavutil.mavlink_connection(connection_string)
+
+    print('Catch messages')
+    msg_count = {}
+
+    start_time = time.time()
+
+    for _ in range(count):
+        msg = connection.recv_msg()
+        # msg = connection.recv_match(blocking=True)
+        if msg:
+            msg_type = msg.get_type()
+            if msg_type in msg_count:
+                msg_count[msg_type] += 1
+            else:
+                msg_count[msg_type] = 1
+
+            if msg.get_type() == 'SERVO_OUTPUT_RAW':
+                print(msg)
+                pass
+
+        else:
+            print('No message yet, 1 second for data to fill')
+            sleep(1)
+
+    print('Messages Type received:')
+    print(msg_count)
+
+    end_time = time.time()
+    print('Time elapsed: ', end_time - start_time)
+
+    print('Closing connection')
+    connection.close()
+
+
+def fdm_parser_example(data=None):
+    if data is None:
+        data = bytes.fromhex('1a48e903b1340500e803e803e803e803000000000000000000000000000000000000000000000000')
+    # 1a48e903b1340500e803e803e803e803000000000000000000000000000000000000000000000000
+
+
+    parse_format = 'HHI16H'
+
+    if len(data) != struct.calcsize(parse_format):
+        print("got packet of len %u, expected %u" % (len(data), struct.calcsize(parse_format)))
+        exit(1)
+
+    decoded = struct.unpack(parse_format,data)
+
+    magic = decoded[0]
+    frame_rate_hz = decoded[1]
+    frame_count = decoded[2]
+    pwm = decoded[3:]
+
+
+    print("magic: %u, frame_rate_hz: %u, frame_count: %u, pwm: %s" % (magic, frame_rate_hz, frame_count, pwm))
+
+def fdm_switch_example_one():
+
+    '''
+    這個範例在 SITL 與 Gazebo 之間 做一個簡單的UDP轉發器 
+    沒有轉換數據格式
+    '''
+    # make a link get fdm from SITL, as a server
+    sock_s2g = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
+    sock_s2g.bind(('', 9012))
+    sock_s2g.settimeout(0.1)
+
+    # set a target link pass JSON to Gazebo, as a client
+    sock_g2s = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
+    server_address = ('127.0.0.1', 19012)
+
+    packet_count = 0
+    start_time = time.time()
+
+    while True:
+        try:
+            # 接到SITL的數據 並轉發給Gazebo
+            data_s2g, addr_s2g = sock_s2g.recvfrom(1024)
+            sock_g2s.sendto(data_s2g, server_address)
+            # 得到Gazebo的回應 並轉發給SITL
+            data_g2s, addr_g2s = sock_g2s.recvfrom(1024)
+            sock_s2g.sendto(data_g2s, addr_s2g)
+
+            # 解析並保存接收到的數據到 JSON 檔案 (SITL -> Gazebo)
+            try:
+                # 解析從 SITL 到 Gazebo 的數據
+                decoded = struct.unpack('HHI16H', data_s2g)
+                s2g_json = {
+                    "magic": decoded[0],
+                    "frame_rate_hz": decoded[1],
+                    "frame_count": decoded[2],
+                    "pwm": list(decoded[3:])
+                }
+                
+                # 解析從 Gazebo 到 SITL 的數據
+                # 注意：假設從 Gazebo 返回的數據是字串形式的 JSON
+                try:
+                    g2s_data_str = data_g2s.decode('utf-8')
+                    # 保存原始數據，適合後續分析
+                    with open('gazebo_response_raw.txt', 'a') as f:
+                        f.write(g2s_data_str + '\n')
+                except:
+                    # 如果不是 UTF-8 格式，可能是二進制數據
+                    g2s_data_str = str(data_g2s)
+                
+                # 合併數據到一個 JSON 對象
+                combined_data = {
+                    "sitl_to_gazebo": s2g_json,
+                    "gazebo_to_sitl": g2s_data_str,
+                    "timestamp": time.time()
+                }
+                
+                # 寫入 JSON 檔案
+                with open('fdm_one_data.json', 'w') as f:
+                    json.dump(combined_data, f, indent=2)
+                    
+            except Exception as e:
+                print(f"JSON 處理錯誤: {e}")
+
+            packet_count += 1
+        except socket.timeout:
+            print(f'no value')
+            #print(f'address:{addr_s2g}')
+            #pass
+
+        current_time = time.time()
+        elapsed_time = current_time - start_time
+
+        if elapsed_time >= 1.0:
+            #print(f"Packets received in the last second: {packet_count}")
+            print(f'JSON Pack:{data_g2s}')
+            packet_count = 0
+            start_time = current_time
+            fdm_parser_example(data_s2g)
+
+
+def fdm_switch_example_two():
+    '''
+    這個範例在 SITL 與 Gazebo 之間 做一個簡單的UDP轉發器 
+    有轉換數據格式
+
+    time_usec
+    servo1_raw 
+    servo16_raw
+    '''
+    # read info from SITL via MAVLink
+    connection_string="udp:127.0.0.1:14550" # uart mavlink  # MATLAB
+    connection = mavutil.mavlink_connection(connection_string)
+
+    # set a target link pass JSON to Gazebo, as a client
+    sock_g2s = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
+    server_address = ('127.0.0.1', 19012)
+
+    data_queue = queue.Queue()
+    servo_out = [0] * 16
+    data_queue.put(servo_out)
+
+    def send_data_from_queue(sock, server_address, q, frame_rate_fdm=600, frame_count_fdm=1):
+        interval = 1.0 / frame_rate_fdm
+        while True:
+            start_time = time.time()
+            try:
+                data = q.get(timeout=0.1)
+                if data is None:
+                    break
+                last_data = data
+            except queue.Empty:
+                data = last_data
+            data = struct.pack('HHI16H', 0x481a, frame_rate_fdm, frame_count_fdm, *servo_out)
+            sock.sendto(data, server_address)
+
+            # 新增：寫入 JSON 檔案
+            data_json = {
+                "magic": 0x481a,
+                "frame_rate_hz": frame_rate_fdm,
+                "frame_count": frame_count_fdm,
+                "pwm": list(servo_out)
+            }
+            with open('fdm_data.json', 'w') as f:
+                json.dump(data_json, f)
+
+            frame_count_fdm += 1
+            fdm_parser_example(data) # debug
+
+            elapsed_time = time.time() - start_time
+            sleep_time = interval - elapsed_time
+            if sleep_time > 0:
+                sleep(sleep_time)
+
+    Running = True
+    thread = threading.Thread(target=send_data_from_queue, args=(sock_g2s, server_address, data_queue))
+    thread.start()
+
+    while Running:
+        msgb1 = None
+        msg = connection.recv_msg()
+        while msg :
+            if msg.get_type() == 'SERVO_OUTPUT_RAW':
+                msgb1 = msg
+                # break # 這裡不需要break，因為我要最後一個 servo_output_raw 的值
+            msg = connection.recv_msg()
+
+        if msgb1:
+            for i in range(16):
+                servo_out[i] = getattr(msgb1, f'servo{i+1}_raw')
+                data_queue.put(servo_out)
+            msgb1 = None
+            # Running = False # debug
+        else:
+            # print('No message yet, 10 ms for data to fill')
+            sleep(0.01)
+
+        # Example of putting data into the queue
+        # data_queue.put(data)
+        # To stop the thread, you can put None into the queue
+        # data_queue.put(None)
+
+
+def fdm_switch_example_one_with_remote_forwarding():
+    '''
+    這個範例在 SITL 與 Gazebo 之間 做一個簡單的UDP轉發器 
+    並且將JSON數據轉發到指定的遠端IP
+    '''
+    # make a link get fdm from SITL, as a server
+    sock_s2g = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
+    sock_s2g.bind(('', 9012))
+    sock_s2g.settimeout(0.1)
+
+    # set a target link pass JSON to Gazebo, as a client
+    sock_g2s = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
+    server_address = ('127.0.0.1', 19012)
+
+    # 建立與遠端IP的socket連接
+    remote_socket = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
+    remote_address = ('140.120.31.130', 9003)  # 遠端IP與埠號，您可以根據需要更改埠號
+    print(f'Setting up forwarding to remote address: {remote_address}')
+
+    packet_count = 0
+    start_time = time.time()
+
+    while True:
+        try:
+            # 接到SITL的數據 並轉發給Gazebo
+            data_s2g, addr_s2g = sock_s2g.recvfrom(1024)
+            sock_g2s.sendto(data_s2g, server_address)
+            
+            # 得到Gazebo的回應 並轉發給SITL
+            data_g2s, addr_g2s = sock_g2s.recvfrom(1024)
+            sock_s2g.sendto(data_g2s, addr_s2g)
+
+            # 解析並保存接收到的數據到 JSON 檔案 (SITL -> Gazebo)
+            try:
+                # 解析從 SITL 到 Gazebo 的數據
+                decoded = struct.unpack('HHI16H', data_s2g)
+                s2g_json = {
+                    "magic": decoded[0],
+                    "frame_rate_hz": decoded[1],
+                    "frame_count": decoded[2],
+                    "pwm": list(decoded[3:])
+                }
+                
+                # 解析從 Gazebo 到 SITL 的數據
+                try:
+                    g2s_data_str = data_g2s.decode('utf-8')
+                    # 保存原始數據，適合後續分析
+                    with open('gazebo_response_raw.txt', 'a') as f:
+                        f.write(g2s_data_str + '\n')
+                except:
+                    # 如果不是 UTF-8 格式，可能是二進制數據
+                    g2s_data_str = str(data_g2s)
+                
+                # 合併數據到一個 JSON 對象
+                combined_data = {
+                    "sitl_to_gazebo": s2g_json,
+                    "gazebo_to_sitl": g2s_data_str,
+                    "timestamp": time.time()
+                }
+                
+                # 寫入 JSON 檔案
+                with open('fdm_one_data.json', 'w') as f:
+                    json.dump(combined_data, f, indent=2)
+                
+                # 將JSON數據轉發到遠端IP
+                remote_socket.sendto(json.dumps(combined_data).encode(), remote_address)
+                    
+            except Exception as e:
+                print(f"JSON 處理錯誤: {e}")
+
+            packet_count += 1
+        except socket.timeout:
+            print(f'no value')
+            #pass
+
+        current_time = time.time()
+        elapsed_time = current_time - start_time
+
+        if elapsed_time >= 1.0:
+            print(f'JSON Pack:{data_g2s}')
+            print(f'Forwarded data to remote IP: 140.120.31.130')
+            packet_count = 0
+            start_time = current_time
+            fdm_parser_example(data_s2g)
+
+
+def fdm_switch_multiple(num_systems=3):
+    '''
+    Create multiple UDP forwarders between SITL and Gazebo using threading
+    Each forwarder uses its own set of ports following the pattern:
+    - SITL ports: 9002, 9012, 9022, ...
+    - Gazebo ports: 19002, 19012, 19022, ...
+    
+    Parameters:
+    num_systems (int): Number of forwarding pairs to create
+    '''
+    # Flag to control thread execution
+    running = True
+    
+    def forwarder_thread(system_id):
+        sitl_port = 9002 + (system_id * 10)
+        gazebo_port = 19002 + (system_id * 10)
+        
+        # Create SITL socket (server)
+        sitl_socket = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
+        sitl_socket.bind(('', sitl_port))
+        sitl_socket.settimeout(0.1)
+        
+        # Create Gazebo socket (client)
+        gazebo_socket = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
+        gazebo_socket.settimeout(0.1)
+        gazebo_address = ('127.0.0.1', gazebo_port)
+        
+        print(f"System {system_id}: Forwarding {sitl_port} → {gazebo_port}")
+        
+        packet_count = 0
+        start_time = time.time()
+        last_data_s2g = None
+        
+        try:
+            while running:
+                try:
+                    # Receive from SITL and forward to Gazebo
+                    data_s2g, addr_s2g = sitl_socket.recvfrom(1024)
+                    last_data_s2g = data_s2g
+                    gazebo_socket.sendto(data_s2g, gazebo_address)
+                    
+                    try:
+                        # Get response from Gazebo and send back to SITL
+                        data_g2s, addr_g2s = gazebo_socket.recvfrom(1024)
+                        sitl_socket.sendto(data_g2s, addr_s2g)
+                        
+                        # Process and store data
+                        try:
+                            # Parse SITL to Gazebo data
+                            decoded = struct.unpack('HHI16H', data_s2g)
+                            s2g_json = {
+                                "magic": decoded[0],
+                                "frame_rate_hz": decoded[1],
+                                "frame_count": decoded[2],
+                                "pwm": list(decoded[3:])
+                            }
+                            
+                            # Try to parse Gazebo to SITL data
+                            try:
+                                g2s_data_str = data_g2s.decode('utf-8')
+                                if packet_count % 50 == 0:
+                                    with open(f'gazebo_response_raw_{system_id}.txt', 'a') as f:
+                                        f.write(g2s_data_str + '\n')
+                            except:
+                                g2s_data_str = str(data_g2s)
+                            
+                            # Combine data
+                            combined_data = {
+                                "sitl_to_gazebo": s2g_json,
+                                "gazebo_to_sitl": g2s_data_str,
+                                "timestamp": time.time(),
+                                "system_id": system_id,
+                                "sitl_port": sitl_port,
+                                "gazebo_port": gazebo_port
+                            }
+                            
+                            # Save to JSON file (once per second)
+                            current_time = time.time()
+                            elapsed_time = current_time - start_time
+                            if elapsed_time >= 1.0:
+                                with open(f'fdm_data_system_{system_id}.json', 'w') as f:
+                                    json.dump(combined_data, f, indent=2)
+                                
+                        except Exception as e:
+                            print(f"System {system_id} - JSON processing error: {e}")
+                        
+                        packet_count += 1
+                        
+                    except socket.timeout:
+                        # No response from Gazebo, continue
+                        pass
+                    
+                except socket.timeout:
+                    # No data from SITL, continue
+                    pass
+                
+                # Print stats periodically
+                current_time = time.time()
+                elapsed_time = current_time - start_time
+                
+                if elapsed_time >= 1.0:
+                    # print(f"System {system_id} - Packets: {packet_count} in the last second")
+                    if packet_count > 0 and last_data_s2g:
+                        print(f'System {system_id} - JSON Pack: {data_g2s}')
+                        fdm_parser_example(last_data_s2g)
+                    packet_count = 0
+                    start_time = current_time
+        
+        except Exception as e:
+            print(f"System {system_id} - Error in forwarder thread: {e}")
+        finally:
+            # Clean up resources
+            sitl_socket.close()
+            gazebo_socket.close()
+            print(f"System {system_id} - Forwarder thread terminated")
+    
+    # Create and start threads for each system
+    threads = []
+    for i in range(num_systems):
+        thread = threading.Thread(target=forwarder_thread, args=(i,))
+        thread.daemon = True
+        threads.append(thread)
+        thread.start()
+    
+    # Keep the main thread running
+    try:
+        while True:
+            time.sleep(1)
+    except KeyboardInterrupt:
+        print("\nShutting down all forwarders...")
+        running = False
+        time.sleep(1)
+        print("Done.")
+        return
+
+
+def fdm_switch_multiple_with_remote(num_systems=3, remote_ip='140.120.31.130'):
+    '''
+    Create multiple UDP forwarders between SITL and Gazebo using threading
+    Also forwards the combined data to a remote IP address
+    
+    Each forwarder uses its own set of ports following the pattern:
+    - SITL ports: 9002, 9012, 9022, ...
+    - Gazebo ports: 19002, 19012, 19022, ...
+    - Remote ports: 9003, 9013, 9023, ... (for the remote forwarding)
+    
+    Parameters:
+    num_systems (int): Number of forwarding pairs to create
+    remote_ip (str): IP address for remote forwarding
+    '''
+    # Flag to control thread execution
+    running = True
+
+    # Add shared counters for total packets
+    total_packets_sent = 0
+    total_packets_lock = threading.Lock()  # For thread safety    
+    
+    def forwarder_thread(system_id):
+        nonlocal total_packets_sent
+        sitl_port = 9002 + (system_id * 10)
+        gazebo_port = 19002 + (system_id * 10)
+        remote_port = 9003 + (system_id * 10)
+        
+        # Create SITL socket (server)
+        sitl_socket = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
+        sitl_socket.bind(('', sitl_port))
+        sitl_socket.settimeout(0.1)
+        
+        # Create Gazebo socket (client)
+        gazebo_socket = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
+        gazebo_socket.settimeout(0.1)
+        gazebo_address = ('127.0.0.1', gazebo_port)
+        
+        # Create remote forwarding socket
+        remote_socket = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
+        remote_address = (remote_ip, remote_port)
+        
+        print(f"System {system_id}: SITL:{sitl_port} → Gazebo:{gazebo_port}, Remote:{remote_ip}:{remote_port}")
+        
+        packet_count = 0
+        start_time = time.time()
+        last_data_s2g = None
+        forward_counter = 0  # 添加轉發計數器
+        system_packets_sent = 0  # Local counter for this system
+
+        try:
+            while running:
+                try:
+                    # Receive from SITL and forward to Gazebo
+                    data_s2g, addr_s2g = sitl_socket.recvfrom(1024)
+                    last_data_s2g = data_s2g
+                    gazebo_socket.sendto(data_s2g, gazebo_address)
+                    
+                    try:
+                        # Get response from Gazebo and send back to SITL
+                        data_g2s, addr_g2s = gazebo_socket.recvfrom(1024)
+                        sitl_socket.sendto(data_g2s, addr_s2g)
+                        
+                        # Process and store data
+                        try:
+                            # Parse SITL to Gazebo data
+                            decoded = struct.unpack('HHI16H', data_s2g)
+                            s2g_json = {
+                                "magic": decoded[0],
+                                "frame_rate_hz": decoded[1],
+                                "frame_count": decoded[2],
+                                "pwm": list(decoded[3:])
+                            }
+                            
+                            # Try to parse Gazebo to SITL data
+                            try:
+                                g2s_data_str = data_g2s.decode('utf-8')
+                                if packet_count % 50 == 0:
+                                    with open(f'gazebo_response_raw_{system_id}.txt', 'a') as f:
+                                        f.write(g2s_data_str + '\n')
+                            except:
+                                g2s_data_str = str(data_g2s)
+                            
+                            # Combine data
+                            combined_data = {
+                                "sitl_to_gazebo": s2g_json,
+                                "gazebo_to_sitl": g2s_data_str,
+                                "timestamp": time.time(),
+                                "system_id": system_id,
+                                "sitl_port": sitl_port,
+                                "gazebo_port": gazebo_port
+                            }
+                            
+                            '''
+                            # 每n筆才轉發一次
+                            forward_counter += 1
+                            if forward_counter >= 18:
+                                # Forward data to remote server
+                                remote_socket.sendto(json.dumps(combined_data).encode(), remote_address)
+                                #print(f"System {system_id} - Send data to remote (第{packet_count}個包)")
+                                forward_counter = 0  # 重置計數器
+                            '''
+
+                            # 使用模運算代替計數器重置
+                            forward_counter += 1
+                            if packet_count % 1 == 0:
+                                # Forward data to remote server
+                                remote_socket.sendto(json.dumps(combined_data).encode(), remote_address) 
+                                system_packets_sent += 1
+                                with total_packets_lock:
+                                    total_packets_sent += 1                                
+                                # Debug用，看每秒傳了多少筆出去
+                                #print(f"System {system_id} - Send data to remote (第{packet_count}個包)")               
+                            
+                            # Save to JSON file (once per second)
+                            current_time = time.time()
+                            elapsed_time = current_time - start_time
+                            if elapsed_time >= 1.0:
+                                with open(f'fdm_data_system_{system_id}.json', 'w') as f:
+                                    json.dump(combined_data, f, indent=2)
+                                
+                        except Exception as e:
+                            print(f"System {system_id} - JSON processing error: {e}")
+                        
+                        packet_count += 1
+                        
+                    except socket.timeout:
+                        # No response from Gazebo, continue
+                        pass
+                    
+                except socket.timeout:
+                    # No data from SITL, continue
+                    pass
+                
+                # Print stats periodically
+                current_time = time.time()
+                elapsed_time = current_time - start_time
+                
+                if elapsed_time >= 1.0:
+                    print(f"System {system_id} - Packets: {packet_count} in the last second")
+                    if packet_count > 0 and last_data_s2g:
+                        fdm_parser_example(last_data_s2g)
+                    packet_count = 0
+                    start_time = current_time
+        
+        except Exception as e:
+            print(f"System {system_id} - Error in forwarder thread: {e}")
+        finally:
+            # Clean up resources
+            sitl_socket.close()
+            gazebo_socket.close()
+            remote_socket.close()
+            print(f"System {system_id} - Forwarder thread terminated")
+
+    # Create statistics display thread
+    def stats_display_thread():
+        start_time = time.time()
+        while running:
+            time.sleep(5)  # Display stats every 5 seconds
+            elapsed = time.time() - start_time
+            with total_packets_lock:
+                rate = total_packets_sent / elapsed if elapsed > 0 else 0
+                print(f"\n===== STATISTICS =====")
+                print(f"Total packets sent: {total_packets_sent}")
+                print(f"Average rate: {rate:.2f} packets/sec")
+                print(f"Running time: {elapsed:.1f} seconds")
+                print(f"======================\n")
+
+    # Create and start threads for each system
+    threads = []
+    for i in range(num_systems):
+        thread = threading.Thread(target=forwarder_thread, args=(i,))
+        thread.daemon = True
+        threads.append(thread)
+        thread.start()
+
+    # Start stats display thread
+    stats_thread = threading.Thread(target=stats_display_thread)
+    stats_thread.daemon = True
+    stats_thread.start()        
+    
+    # Keep the main thread running
+    try:
+        while True:
+            time.sleep(1)
+    except KeyboardInterrupt:
+        print("\nShutting down all forwarders...")
+        running = False
+        time.sleep(1)
+        print(f"Final total packets sent: {total_packets_sent}")
+        print("Done.")
+        return
+
+
+print('Start')
+#fdm_switch_example_two()
+#mavlink_analyzer_simple()
+#fdm_switch_example_one()
+#fdm_switch_example_one_with_remote_forwarding()
+#fdm_switch_multiple(1)  # Create 3 forwarding pairs
+fdm_switch_multiple_with_remote(3)  # Create 1 forwarding pair with remote forwarding
+#fdm_parser_example()