學長建議修改內容

1. serialManager.py - AT command 支援與架構重整
2. mainOrchestrator.py - serial 介面功能開啟

.gitignore

@@ -26,3 +26,9 @@ Makefile
 **/*.pyc
 **/*.pyo
 **/.cursor/
+CLAUDE.md
+
+# 本地筆記與硬體 dump
+my_env_notes.md
+notes/
+**/eeprom.bin

README.md

@@ -1,43 +1,44 @@
 這是天雷系統的專案
 
 ===
-專案核心框架
+## 功能簡介
+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
 
-===
-必要相依套件 順便記錄我開發時的環境版本
-
-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
+- 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,45 +50,48 @@ 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
 ```
 
-
-=== 
-Package 簡述
-<<<<<<< HEAD
+2. 
+GUI 介面
+在 ~/AirTrapMine/src/GUI 路徑下 直接啟動
+```bash
+cd ~/AirTrapMine/src/GUI
+python gui.py
+```
 
 ===
-簡述0328
-主要是透過test_transform這隻程式執行fdm_switch_example_one_with_remote_forwarding這個副函式，透過UDP將封包轉接到Matlab，並在Matlab進行演算法迭代後，打包JSON檔，並再次透過port口傳回封包，傳回來的封包會在close_loop這隻程式被解析，並提取其中有關velocity的資料儲存成變數，以mavlink的方式打入Ardupilot，以實現封閉迴路的構思。
-=======
+## 資料夾說明
 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
-
-```
-
->>>>>>> chiyu

serial_udp_update.py

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

src/GUI/command_sender.py

@@ -3,10 +3,16 @@
 指令發送模組
 負責將目標位置轉成具體的通訊指令發送到飛控
 
-現階段: MavlinkSender (直接 pymavlink 發送)
-未來:   替換為 ROS2Sender (發到 ROS2 topic，由 fc_network_adapter 轉發)
+目前使用 Ros2CommandSender，經 fc_network_adapter 的
+/fc_network/vehicle/pos_global_int service 發送。
+
+MavlinkSender 保留作為直連 SITL 的 fallback 工具，但已不是預設路徑。
 """
+import asyncio
+import traceback
 from abc import ABC, abstractmethod
+
+from PyQt6.QtCore import QObject, pyqtSignal
 from pymavlink import mavutil
 
 
@@ -14,11 +20,12 @@ class CommandSender(ABC):
     """指令發送抽象介面"""
 
     @abstractmethod
-    def send_position_global(self, sysid, lat, lon, alt):
+    def send_position_global(self, drone_id, sysid, lat, lon, alt):
         """
         發送全球座標位置指令
 
         Args:
+            drone_id: GUI 用的 drone 識別字串 (如 's0_11')
             sysid: 目標無人機的 MAVLink system ID
             lat: 緯度 (度)
             lon: 經度 (度)
@@ -58,16 +65,8 @@ class MavlinkSender(CommandSender):
         self.mav = mavutil.mavlink_connection(connection_string)
         print(f"MavlinkSender 已建立連線: {connection_string}")
 
-    def send_position_global(self, sysid, lat, lon, alt):
-        """
-        發送 SET_POSITION_TARGET_GLOBAL_INT
-
-        注意:
-            - coordinate_frame 使用 MAV_FRAME_GLOBAL_RELATIVE_ALT_INT
-              高度是相對起飛點的高度 (公尺)
-            - 如果 FormationPlanner 產出的 alt 是 AMSL 絕對高度，
-              需要在外部先減去起飛點高度再傳入
-        """
+    def send_position_global(self, drone_id, sysid, lat, lon, alt):
+        """發送 SET_POSITION_TARGET_GLOBAL_INT。drone_id 未使用，保留為介面相容。"""
         self.mav.mav.set_position_target_global_int_send(
             0,                          # time_boot_ms (不使用)
             sysid,                      # target_system
@@ -87,4 +86,80 @@ class MavlinkSender(CommandSender):
         if self.mav:
             self.mav.close()
             self.mav = None
-            print("MavlinkSender 已關閉")
+            print("MavlinkSender 已關閉")
+
+
+class Ros2CommandSender(QObject):
+    """
+    透過 fc_network_apps.PositionTargetGlobalIntClient 送 goto 指令。
+
+    設計重點:
+        - send_position_global 是同步介面，立刻回傳。結果透過 send_result signal 回報
+        - 每台 drone 維護自己的 asyncio.Task pipeline，新的目標會 cancel 掉舊的 in-flight
+        - client 由 DroneMonitor.get_or_create_position_client 管理，per-drone 獨立節點
+    """
+
+    # (drone_id, sysid, success, message)
+    send_result = pyqtSignal(str, int, bool, str)
+
+    DEFAULT_TIMEOUT_SEC = 2.0
+
+    def __init__(self, monitor, timeout_sec: float = DEFAULT_TIMEOUT_SEC):
+        super().__init__()
+        self._monitor = monitor
+        self._timeout_sec = timeout_sec
+        self._pending = {}  # drone_id -> asyncio.Task
+
+    def send_position_global(self, drone_id, sysid, lat, lon, alt):
+        """同步介面。立刻回，結果透過 send_result signal 回報。"""
+        # 取消該機上一個未完成的 task，避免舊指令覆蓋新指令
+        old = self._pending.get(drone_id)
+        if old is not None and not old.done():
+            old.cancel()
+
+        try:
+            loop = asyncio.get_event_loop()
+        except RuntimeError:
+            loop = asyncio.new_event_loop()
+            asyncio.set_event_loop(loop)
+
+        task = loop.create_task(
+            self._do_send(drone_id, int(sysid), float(lat), float(lon), float(alt))
+        )
+        self._pending[drone_id] = task
+
+    async def _do_send(self, drone_id, sysid, lat, lon, alt):
+        try:
+            client = self._monitor.get_or_create_position_client(drone_id)
+            if client is None:
+                self.send_result.emit(
+                    drone_id, sysid, False,
+                    "PositionTargetGlobalIntClient 無法建立"
+                )
+                return
+
+            result = await client.goto_position_only_async(
+                target_sysid=sysid,
+                target_compid=0,
+                latitude_deg=lat,
+                longitude_deg=lon,
+                alt=alt,
+                timeout_sec=self._timeout_sec,
+            )
+            self.send_result.emit(
+                drone_id, sysid, bool(result.success), str(result.message or "")
+            )
+        except asyncio.CancelledError:
+            # 被新的目標取代，正常狀況，不用回報
+            pass
+        except Exception as e:
+            traceback.print_exc()
+            self.send_result.emit(drone_id, sysid, False, f"exception: {e}")
+
+    def close(self):
+        """取消所有未完成的 task。PositionTargetGlobalIntClient 由 DroneMonitor 負責 destroy。"""
+        for task in self._pending.values():
+            if not task.done():
+                task.cancel()
+        self._pending.clear()
+        print("Ros2CommandSender 已關閉")

src/GUI/communication.py

@@ -4,17 +4,49 @@ 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 geometry_msgs.msg import Point, Vector3, Vector3Stamped, PoseWithCovarianceStamped
 from sensor_msgs.msg import BatteryState, NavSatFix, Imu
 from std_msgs.msg import Float64, String
 from mavros_msgs.msg import State, VfrHud
+from nav_msgs.msg import Odometry
 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
+
+# 導入 fc_network_apps 的 navigation（PositionTargetGlobalInt / Offboard goto）
+try:
+    from fc_network_apps.navigation import PositionTargetGlobalIntClient
+except ImportError as e:
+    import traceback
+    print(f"⚠️  警告: 無法導入 PositionTargetGlobalIntClient")
+    print(f"   错误: {e}")
+    traceback.print_exc()
+    PositionTargetGlobalIntClient = None
+
 class DroneSignals(QObject):
     update_signal = pyqtSignal(str, str, object)  # (msg_type, drone_id, data)
 
@@ -398,8 +430,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 +477,23 @@ class DroneMonitor(Node):
         # ================================================================================
         self.serial_receivers = []
         
+        # ================================================================================
+        # 【新增】初始化 CommandLongClient 字典（為每個 drone 維護獨立的 client）
+        # ================================================================================
+        # 改為為每個 drone 創建獨立的 client，避免多機並行時的競態條件
+        self.command_long_clients = {}  # {drone_id: CommandLongClient}
+        self.client_lock = Lock()  # 保護 clients 字典的訪問
+        self.client_counter = 0  # 用於生成唯一的 client 節點名稱
+        self.executor = None  # 將在 gui.py 中設置，用於添加新的 clients
+        # ================================================================================
+
+        # ================================================================================
+        # PositionTargetGlobalIntClient 字典（per-drone，用於 Offboard goto）
+        # ================================================================================
+        self.position_target_clients = {}  # {drone_id: PositionTargetGlobalIntClient}
+        self.pos_client_counter = 0
+        # ================================================================================
+        
         # 主题检测定时器
         self.create_timer(1.0, self.scan_topics)
     
@@ -464,6 +518,58 @@ class DroneMonitor(Node):
             
             return self.socket_id_mapping[original_socket_id]
        
+    def get_or_create_client(self, drone_id):
+        """為每個 drone 獲取或創建獨立的 CommandLongClient，避免競態條件"""
+        with self.client_lock:
+            if drone_id not in self.command_long_clients:
+                try:
+                    # 生成唯一的 client 節點名稱
+                    self.client_counter += 1
+                    unique_name = f"cmd_long_client_{drone_id}_{self.client_counter}"
+                    client = CommandLongClient(node_name=unique_name)
+                    self.command_long_clients[drone_id] = client
+                    print(f"   ✓ 為 {drone_id} 創建新的 CommandLongClient (node={unique_name})")
+                    
+                    # 將新 client 添加到主執行器（這樣它的回調才能被處理）
+                    if self.executor:
+                        self.executor.add_node(client)
+                        print(f"   ✓ 將 {drone_id} 的 client 添加到主執行器")
+                    
+                except TypeError:
+                    # 舊版 CommandLongClient 不支持 node_name 參數，使用預設
+                    client = CommandLongClient()
+                    self.command_long_clients[drone_id] = client
+                    print(f"   ✓ 為 {drone_id} 創建新的 CommandLongClient (使用預設名稱)")
+                    
+                    if self.executor:
+                        self.executor.add_node(client)
+                        print(f"   ✓ 將 {drone_id} 的 client 添加到主執行器")
+                        
+                except Exception as e:
+                    print(f"⚠️  無法為 {drone_id} 創建 CommandLongClient: {e}")
+                    return None
+            return self.command_long_clients[drone_id]
+
+    def get_or_create_position_client(self, drone_id):
+        """為每個 drone 獲取或創建獨立的 PositionTargetGlobalIntClient。"""
+        if PositionTargetGlobalIntClient is None:
+            return None
+        with self.client_lock:
+            if drone_id not in self.position_target_clients:
+                try:
+                    self.pos_client_counter += 1
+                    unique_name = f"pos_target_client_{drone_id}_{self.pos_client_counter}"
+                    client = PositionTargetGlobalIntClient(node_name=unique_name)
+                    self.position_target_clients[drone_id] = client
+                    print(f"   ✓ 為 {drone_id} 創建新的 PositionTargetGlobalIntClient (node={unique_name})")
+                    if self.executor:
+                        self.executor.add_node(client)
+                        print(f"   ✓ 將 {drone_id} 的 position client 添加到主執行器")
+                except Exception as e:
+                    print(f"⚠️  無法為 {drone_id} 創建 PositionTargetGlobalIntClient: {e}")
+                    return None
+            return self.position_target_clients[drone_id]
+
     def scan_topics(self):
         topics = self.get_topic_names_and_types()
         drone_pattern = re.compile(r'/fc_network/vehicle/(sys\d+)/(\w+)')
@@ -483,8 +589,25 @@ class DroneMonitor(Node):
                 # 暂时所有 ROS2 topic 共享同一个 socket_id = 0
                 self.sys_to_socket_id[sys_id] = 0
             
-            if not hasattr(self, f'drone_{sys_id}_subs'):
+            subs_attr = f'drone_{sys_id}_subs'
+            if not hasattr(self, subs_attr):
                 self.setup_drone(sys_id)
+            else:
+                # 檢查既有訂閱是否包含 position_ned，如果不包含就添加（兼容舊訂閱）
+                subs = getattr(self, subs_attr, {})
+                if isinstance(subs, dict) and 'position_ned' not in subs:
+                    base_topic = f'/fc_network/vehicle/{sys_id}'
+                    try:
+                        pos_ned_sub = self.create_subscription(
+                            Odometry,
+                            f'{base_topic}/position_ned',
+                            lambda msg, sid=sys_id: self.position_ned_callback(sid, msg),
+                            10
+                        )
+                        subs['position_ned'] = pos_ned_sub
+                        setattr(self, subs_attr, subs)  # 明確保存更新後的字典
+                    except Exception as e:
+                        pass
     
     def setup_drone(self, sys_id):
         # sys_id 格式： sys11, sys12, ...
@@ -531,49 +654,162 @@ class DroneMonitor(Node):
                 f'{base_topic}/vfr_hud',
                 lambda msg, sid=sys_id: self.hud_callback(sid, msg),
                 10
+            ),
+            'position_ned': self.create_subscription(
+                Odometry,
+                f'{base_topic}/position_ned',
+                lambda msg, sid=sys_id: self.position_ned_callback(sid, msg),
+                10
             )
         }
         
         setattr(self, f'drone_{sys_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)
+    # ================================================================================
+    # 【新增】模式名稱到 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:
-            response = await future
-            return response.success
+            # 解析 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})")
+            
+            # 獲取或創建該 drone 專用的 client（避免多機並行時的競態條件）
+            client = self.get_or_create_client(drone_id)
+            if not client:
+                print(f"❌ [SET_MODE] CommandLongClient 無法初始化")
+                return False
+            
+            # 直接調用 async 方法，無需線程池（避免嵌套執行器衝突）
+            result = await client.change_mode_async(
+                target_sysid=sysid,
+                custom_mode=float(custom_mode),
+                target_compid=0,
+                base_mode=1.0,
+                timeout_sec=5.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:
-            self.get_logger().error(f'Arm service call failed: {e}')
+            print(f"❌ [SET_MODE] 例外錯誤: {e}")
+            traceback.print_exc()
             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)
+    async def arm_drone(self, drone_id, arm):
+        """使用 CommandLongClient 執行 ARM/DISARM（使用非阻塞的 async 方法）"""
         try:
-            response = await future
-            return response.success
+            # 解析 drone_id 提取 sysid
+            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}")
+            
+            # 獲取或創建該 drone 專用的 client（避免多機並行時的競態條件）
+            client = self.get_or_create_client(drone_id)
+            if not client:
+                print(f"❌ [ARM] CommandLongClient 無法初始化")
+                return False
+            
+            # 直接調用 async 方法，無需線程池（避免嵌套執行器衝突）
+            result = await client.arm_disarm_async(
+                target_sysid=sysid,
+                arm=arm,
+                target_compid=0,
+                timeout_sec=5.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'Takeoff service call failed: {e}')
+            print(f"❌ [ARM] 例外錯誤: {e}")
+            traceback.print_exc()
+            return False
+
+    async def takeoff_drone(self, drone_id, altitude):
+        """使用 CommandLongClient 執行無人機起飛（使用非阻塞的 async 方法）"""
+        try:
+            # 解析 drone_id 提取 sysid
+            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)")
+            
+            # 獲取或創建該 drone 專用的 client（避免多機並行時的競態條件）
+            client = self.get_or_create_client(drone_id)
+            if not client:
+                print(f"❌ [TAKEOFF] CommandLongClient 無法初始化")
+                return False
+            
+            # 直接調用 async 方法，無需線程池（避免嵌套執行器衝突）
+            result = await client.takeoff_async(
+                target_sysid=sysid,
+                altitude_m=float(altitude),
+                target_compid=0,
+                timeout_sec=5.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:
+            print(f"❌ [TAKEOFF] 例外錯誤: {e}")
+            traceback.print_exc()
             return False
 
     def send_setpoint(self, drone_id, x, y, z):
@@ -667,6 +903,7 @@ class DroneMonitor(Node):
                 sys_num = sys_id.replace('sys', '')
                 actual_drone_id = f's{assigned_socket_id}_{sys_num}'
                 self.sys_to_actual_id[sys_id] = actual_drone_id
+                print(f"[DEBUG] summary_callback: 已創建映射 {sys_id} -> {actual_drone_id} (使用 sys_num)")
             
             # 先發送連接類型資訊
             self.signals.update_signal.emit('connection_type', actual_drone_id, {
@@ -747,6 +984,45 @@ class DroneMonitor(Node):
             'climb': msg.climb
         }
 
+    def position_ned_callback(self, sys_id, msg):
+        """處理 position_ned topic (nav_msgs/msg/Odometry)
+        NED 座標系中的位置（在 msg.pose.pose.position）：
+        - x: 北向位移 (m)
+        - y: 東向位移 (m)
+        - z: 向下位移 (m) - 負值表示向下，轉換為高度需要 * (-1)
+        """
+        try:
+            # 使用映射獲取實際的 drone_id
+            actual_drone_id = self.sys_to_actual_id.get(sys_id, None)
+            # 如果還沒有從 summary 獲取到映射，則不處理
+            if actual_drone_id is None:
+                return
+            
+            # 從 Odometry 消息中提取位置數據 (msg.pose.pose.position)
+            # Odometry 結構：header, child_frame_id, pose(包含PoseWithCovariance), twist
+            x = msg.pose.pose.position.y  # NED 座標系中交換 x/y（與 local_pose 對齐）
+            y = msg.pose.pose.position.x
+            z = -msg.pose.pose.position.z  # 將向下的 NED z 轉換為向上的高度（z 為負表示向下）
+            
+            # 儲存高度信息
+            self.latest_data[(actual_drone_id, 'altitude')] = {
+                'altitude': z
+            }
+            
+            # 儲存本地位置信息
+            self.latest_data[(actual_drone_id, 'local_pose')] = {
+                'x': x,
+                'y': y,
+                'z': z
+            }
+            
+            # 發送信號給 GUI 更新高度顯示
+            self.signals.update_signal.emit('altitude', actual_drone_id, {
+                'altitude': z
+            })
+        except Exception as e:
+            pass
+
     def loss_rate_callback(self, drone_id, msg):
         self.latest_data[(drone_id, 'loss_rate')] = {
             'loss_rate': msg.data

src/GUI/mission_executor.py

@@ -6,11 +6,15 @@
 設計:
     - 每架無人機持有一個航點序列，逐點推進
     - 各自到達就各自切換到下一個航點
-    - 用 QTimer 驅動，在 Qt 主線程執行
-    - 暫停 = 停止發送 setpoint，飛控自動懸停
-    - 相容舊的 2 階段任務與新的多航點任務 (Grid Sweep)
+    - 事件驅動發送：航點切換時送一次，收到 send_result 才決定重送/前進
+    - 失敗策略 (b)：單機重試 MAX_RETRY 次仍失敗 → 該機 fallback LOITER，其他機繼續
+    - Rendezvous barrier：在指定 wp index 等所有活著的機到齊才一起推進，有 timeout 保護
+    - 用 QTimer 驅動到達判定，在 Qt 主線程執行
+    - 暫停 = 停止到達判定 + 停止新指令
 """
+import asyncio
 import math
+import time
 from enum import Enum
 from PyQt6.QtCore import QObject, QTimer, pyqtSignal
 
@@ -22,22 +26,31 @@ class MissionState(Enum):
     PAUSED = "paused"
 
 
+class TaskStatus(Enum):
+    """單機任務狀態"""
+    NORMAL = "normal"
+    RETRYING = "retrying"
+    WAITING_AT_BARRIER = "waiting_at_barrier"
+    FALLBACK_LOITER = "fallback_loiter"
+
+
 class DroneTask:
     """單架無人機的任務資料"""
-    __slots__ = ('drone_id', 'sysid', 'waypoints', 'wp_index', 'done')
+    __slots__ = (
+        'drone_id', 'sysid', 'waypoints', 'wp_index', 'done',
+        'sent_current_wp', 'fail_count', 'status', 'waiting_since',
+    )
 
     def __init__(self, drone_id, sysid, waypoints):
-        """
-        Args:
-            drone_id: GUI 用的 ID (如 's0_11')
-            sysid: MAVLink system ID (如 11)
-            waypoints: 航點序列 [(lat, lon, alt), ...]
-        """
         self.drone_id = drone_id
         self.sysid = sysid
         self.waypoints = waypoints
         self.wp_index = 0
         self.done = len(waypoints) == 0
+        self.sent_current_wp = False
+        self.fail_count = 0
+        self.status = TaskStatus.NORMAL
+        self.waiting_since = 0.0  # monotonic time 進入 WAITING_AT_BARRIER 的瞬間
 
     @property
     def current_target(self):
@@ -52,7 +65,7 @@ class DroneTask:
 
 class MissionExecutor(QObject):
     """
-    任務執行器
+    任務執行器 (事件驅動版 + rendezvous barrier)
 
     planned_waypoints 格式:
         {
@@ -60,31 +73,41 @@ class MissionExecutor(QObject):
             'waypoints': [
                 [(lat,lon,alt), ...],   # drone 0
                 [(lat,lon,alt), ...],   # drone 1
-            ]
+            ],
+            'rendezvous_indices': [3, 7, ...],   # 選填；缺省 = 全程不同步（各自跑）
         }
     """
 
-    # 信號
-    drone_waypoint_reached = pyqtSignal(str, int, int)  # (drone_id, wp_index, total)
+    MAX_RETRY = 3
+
+    drone_waypoint_reached = pyqtSignal(str, int, int)   # (drone_id, wp_index, total)
+    task_status_changed = pyqtSignal(str, str, str)       # (drone_id, status, message)
     mission_completed = pyqtSignal()
 
-    def __init__(self, sender, drone_gps,
-                 arrival_radius=2.0, send_rate_hz=2.0):
+    def __init__(self, sender, drone_gps, monitor=None,
+                 arrival_radius=2.0, tick_rate_hz=2.0,
+                 barrier_timeout_sec=20.0):
         super().__init__()
         self.sender = sender
         self.drone_gps = drone_gps
+        self.monitor = monitor  # 用於失敗 fallback 到 LOITER
         self.arrival_radius = arrival_radius
+        self.barrier_timeout_sec = barrier_timeout_sec
         self.state = MissionState.IDLE
         self.tasks = {}
+        self.rendezvous_indices = set()
 
-        self._interval_ms = int(1000 / send_rate_hz)
+        self._interval_ms = int(1000 / tick_rate_hz)
         self._timer = QTimer()
         self._timer.timeout.connect(self._tick)
 
+        # 只有 Ros2CommandSender 有 send_result signal；MavlinkSender 沒有
+        if hasattr(sender, 'send_result'):
+            sender.send_result.connect(self._on_send_result)
+
     # ------------------------------------------------------------------ 公開方法
 
     def start(self, planned_waypoints):
-        """啟動任務"""
         if self.state == MissionState.RUNNING:
             print("任務已在執行中")
             return
@@ -93,6 +116,9 @@ class MissionExecutor(QObject):
 
         drone_ids = planned_waypoints['drone_ids']
         waypoints_list = planned_waypoints['waypoints']
+        self.rendezvous_indices = set(
+            planned_waypoints.get('rendezvous_indices', []) or []
+        )
 
         for i, drone_id in enumerate(drone_ids):
             sysid = int(drone_id.split('_')[1])
@@ -102,87 +128,232 @@ class MissionExecutor(QObject):
         self._timer.start(self._interval_ms)
 
         total_wps = sum(t.total_waypoints for t in self.tasks.values())
+        rv_info = (
+            f"rendezvous WP={sorted(self.rendezvous_indices)}"
+            if self.rendezvous_indices else "無 rendezvous (各自跑)"
+        )
         print(f"任務啟動: {len(self.tasks)} 架無人機, "
               f"共 {total_wps} 個航點, "
               f"到達半徑={self.arrival_radius}m, "
-              f"發送週期={self._interval_ms}ms")
+              f"tick 週期={self._interval_ms}ms, "
+              f"barrier timeout={self.barrier_timeout_sec}s, "
+              f"{rv_info}")
 
     def pause(self):
-        """暫停任務"""
         if self.state == MissionState.RUNNING:
             self._timer.stop()
             self.state = MissionState.PAUSED
             print("任務暫停")
 
     def resume(self):
-        """恢復任務"""
         if self.state == MissionState.PAUSED:
             self._timer.start(self._interval_ms)
             self.state = MissionState.RUNNING
             print("任務恢復")
 
     def stop(self):
-        """停止任務"""
         self._timer.stop()
         self.tasks.clear()
+        self.rendezvous_indices = set()
         self.state = MissionState.IDLE
         print("任務停止")
 
     # ------------------------------------------------------------------ 控制迴圈
 
     def _tick(self):
-        """控制迴圈"""
-        all_done = True
+        """
+        控制 tick：
+            Phase 1: 每台機做到達判定；非 barrier 點直接推進，barrier 點改成 WAITING
+            Phase 2: 檢查 barrier 能不能釋放（全員到齊或 timeout）
+            Phase 3: 每台未送過當前航點的機送一次
+            Phase 4: 全部完成檢查
+        """
+        now = time.monotonic()
 
+        # ---- Phase 1: 到達判定 ----
         for task in self.tasks.values():
-            if task.done:
+            if task.done or task.status == TaskStatus.FALLBACK_LOITER:
+                continue
+            # 已在 barrier 等待的不再跑到達判定
+            if task.status == TaskStatus.WAITING_AT_BARRIER:
                 continue
 
-            all_done = False
             target = task.current_target
             if target is None:
                 continue
 
-            # 讀取當前 GPS
             gps = self.drone_gps.get(task.drone_id)
             if gps is None:
                 continue
 
-            tgt_lat, tgt_lon, tgt_alt = target
+            tgt_lat, tgt_lon, _ = target
             distance = _haversine(gps['lat'], gps['lon'], tgt_lat, tgt_lon)
+            if distance >= self.arrival_radius:
+                continue
 
-            # 到達判定
-            if distance < self.arrival_radius:
-                task.wp_index += 1
-                if task.wp_index >= task.total_waypoints:
-                    task.done = True
-                    self.drone_waypoint_reached.emit(
-                        task.drone_id, task.wp_index, task.total_waypoints
-                    )
-                    print(f"  {task.drone_id} → DONE "
-                          f"({task.total_waypoints}/{task.total_waypoints})")
-                    continue
-                else:
-                    self.drone_waypoint_reached.emit(
-                        task.drone_id, task.wp_index, task.total_waypoints
-                    )
-                    print(f"  {task.drone_id} → WP {task.wp_index}/{task.total_waypoints} "
-                          f"(距離: {distance:.1f}m)")
-                    # 更新目標
-                    tgt_lat, tgt_lon, tgt_alt = task.current_target
+            # 到達當前 wp_index
+            if (task.wp_index in self.rendezvous_indices
+                    and task.wp_index < task.total_waypoints - 1):
+                # rendezvous 點 → 不推進，進入 barrier 等待
+                task.status = TaskStatus.WAITING_AT_BARRIER
+                task.waiting_since = now
+                print(f"  {task.drone_id} → 抵達 barrier WP {task.wp_index}，等待同伴")
+                self.task_status_changed.emit(
+                    task.drone_id, task.status.value,
+                    f"waiting at WP {task.wp_index}"
+                )
+                continue
 
-            # 發送 setpoint
+            # 一般點 → 直接推進
+            self._advance_waypoint(task, distance)
+
+        # ---- Phase 2: barrier 釋放檢查 ----
+        self._check_barriers(now)
+
+        # ---- Phase 3: 發送未送過的目標 ----
+        for task in self.tasks.values():
+            if task.done or task.status in (
+                TaskStatus.FALLBACK_LOITER, TaskStatus.WAITING_AT_BARRIER
+            ):
+                continue
+            if task.sent_current_wp:
+                continue
+            target = task.current_target
+            if target is None:
+                continue
+            tgt_lat, tgt_lon, tgt_alt = target
+            task.sent_current_wp = True
             self.sender.send_position_global(
-                task.sysid, tgt_lat, tgt_lon, tgt_alt
+                task.drone_id, task.sysid, tgt_lat, tgt_lon, tgt_alt
             )
 
-        # 全部完成檢查
+        # ---- Phase 4: 完成檢查 ----
+        all_done = all(
+            t.done or t.status == TaskStatus.FALLBACK_LOITER
+            for t in self.tasks.values()
+        )
         if all_done and self.tasks:
             self._timer.stop()
             self.state = MissionState.IDLE
             self.mission_completed.emit()
             print("===== 任務全部完成 =====")
 
+    def _advance_waypoint(self, task, arrived_distance):
+        """把 task 推進一個航點，重置發送旗標。不處理 barrier 邏輯。"""
+        task.wp_index += 1
+        task.sent_current_wp = False
+        task.fail_count = 0
+        if task.wp_index >= task.total_waypoints:
+            task.done = True
+            self.drone_waypoint_reached.emit(
+                task.drone_id, task.wp_index, task.total_waypoints
+            )
+            print(f"  {task.drone_id} → DONE "
+                  f"({task.total_waypoints}/{task.total_waypoints})")
+            return
+        self.drone_waypoint_reached.emit(
+            task.drone_id, task.wp_index, task.total_waypoints
+        )
+        print(f"  {task.drone_id} → WP {task.wp_index}/{task.total_waypoints} "
+              f"(到達距離: {arrived_distance:.1f}m)")
+
+    def _check_barriers(self, now):
+        """檢查每個有人在等的 barrier 能不能釋放。"""
+        # 蒐集目前有哪些 barrier 有人卡著
+        waiting_groups = {}  # barrier_idx -> [task, ...]
+        for task in self.tasks.values():
+            if task.status == TaskStatus.WAITING_AT_BARRIER:
+                waiting_groups.setdefault(task.wp_index, []).append(task)
+
+        if not waiting_groups:
+            return
+
+        for barrier_idx, waiting_tasks in waiting_groups.items():
+            # 判斷是否「全員到齊」：所有仍活著的機都要麼在此 barrier 等，要麼早已越過此 idx
+            everyone_arrived = True
+            for task in self.tasks.values():
+                if task.done or task.status == TaskStatus.FALLBACK_LOITER:
+                    continue  # 失敗機/完成機不擋
+                if (task.status == TaskStatus.WAITING_AT_BARRIER
+                        and task.wp_index == barrier_idx):
+                    continue  # 在這裡等
+                if task.wp_index > barrier_idx:
+                    continue  # 早已越過（barrier 先前已對他釋放）
+                # 還在路上
+                everyone_arrived = False
+                break
+
+            force_reason = None
+            if not everyone_arrived:
+                # 檢查 timeout：以該 barrier 上最早進入等待的時間為準
+                oldest_wait = min(t.waiting_since for t in waiting_tasks)
+                if now - oldest_wait >= self.barrier_timeout_sec:
+                    force_reason = f"timeout {self.barrier_timeout_sec:.0f}s"
+                    print(f"⚠️  barrier WP {barrier_idx} {force_reason}，強制放行")
+                else:
+                    continue  # 還沒到齊、也還沒 timeout → 繼續等
+
+            # 釋放：把所有在此 barrier 等待的機一起推進
+            tag = "全員到齊" if force_reason is None else force_reason
+            print(f"✅ barrier WP {barrier_idx} 釋放 ({tag})，"
+                  f"推進 {len(waiting_tasks)} 架")
+            for task in waiting_tasks:
+                task.status = TaskStatus.NORMAL
+                msg = f"barrier WP {barrier_idx} released ({tag})"
+                self.task_status_changed.emit(
+                    task.drone_id, task.status.value, msg
+                )
+                self._advance_waypoint(task, self.arrival_radius)
+
+    # ------------------------------------------------------------------ 結果回呼
+
+    def _on_send_result(self, drone_id, sysid, success, message):
+        """Ros2CommandSender.send_result 的 slot"""
+        task = self.tasks.get(drone_id)
+        if task is None or task.done:
+            return
+        # 若已在 barrier 等待，舊指令的遲到回應不要再觸發重試/fallback
+        if task.status == TaskStatus.WAITING_AT_BARRIER:
+            return
+
+        if success:
+            if task.fail_count > 0 or task.status == TaskStatus.RETRYING:
+                task.status = TaskStatus.NORMAL
+                self.task_status_changed.emit(drone_id, task.status.value, "recovered")
+            task.fail_count = 0
+            return
+
+        task.fail_count += 1
+        print(f"⚠️  {drone_id} 發送失敗 {task.fail_count}/{self.MAX_RETRY}: {message}")
+
+        if task.fail_count < self.MAX_RETRY:
+            task.status = TaskStatus.RETRYING
+            task.sent_current_wp = False  # 下個 tick 會重送
+            self.task_status_changed.emit(
+                drone_id, task.status.value,
+                f"retry {task.fail_count}/{self.MAX_RETRY}: {message}"
+            )
+        else:
+            task.status = TaskStatus.FALLBACK_LOITER
+            self.task_status_changed.emit(
+                drone_id, task.status.value,
+                f"fallback LOITER after {self.MAX_RETRY} fails: {message}"
+            )
+            print(f"❌ {drone_id} 連續失敗 {self.MAX_RETRY} 次 → 切換 LOITER")
+            self._fallback_to_loiter(drone_id)
+
+    def _fallback_to_loiter(self, drone_id):
+        """用 monitor.set_mode 切 LOITER。set_mode 是 coroutine，透過 event loop 派送。"""
+        if self.monitor is None:
+            print(f"⚠️  無 monitor，無法將 {drone_id} 切到 LOITER")
+            return
+        try:
+            loop = asyncio.get_event_loop()
+        except RuntimeError:
+            loop = asyncio.new_event_loop()
+            asyncio.set_event_loop(loop)
+        loop.create_task(self.monitor.set_mode(drone_id, "LOITER"))
+
 
 # ------------------------------------------------------------------ 工具函式
 
@@ -196,4 +367,4 @@ def _haversine(lat1, lon1, lat2, lon2):
 
     a = (math.sin(dlat / 2) ** 2 +
          math.cos(lat1_r) * math.cos(lat2_r) * math.sin(dlon / 2) ** 2)
-    return R * 2 * math.atan2(math.sqrt(a), math.sqrt(1 - a))
+    return R * 2 * math.atan2(math.sqrt(a), math.sqrt(1 - a))

src/GUI/mission_group.py

@@ -28,13 +28,18 @@ 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         # 規劃原點
+        self.group_id = group_id             # 'A', 'B', 'C', ...
+        self.color = color                   # 顏色 hex
+        
+        # 唯一真實資料來源：該 group 選中的 drone ID
+        # 代表：group 擁有的 drone、被選中的 drone、可以操作的 drone
+        self.selected_drone_ids = set()
+        
+        self.mission_type = 'M_FORMATION'    # 預設任務類型
+        self.planned_waypoints = None        # 規劃結果 dict
+        self.executor = None                 # MissionExecutor 實例（延遲建立）
+        self.center_origin = None            # 規劃原點
+        self.leader_drone_id = None          # LEADER_FOLLOWER 專用：指定的領隊無人機 ID
 
     @property
     def state(self):
@@ -50,13 +55,13 @@ class MissionGroup:
 class DroneAssignDialog(QDialog):
     """無人機分配對話框"""
 
-    def __init__(self, parent, all_drone_ids, current_assigned, other_assigned):
+    def __init__(self, parent, all_drone_ids, current_assigned, other_assigned=None):
         """
         Args:
             parent: 父 widget
             all_drone_ids: 所有可用無人機 ID 列表
             current_assigned: 當前群組已分配的無人機 set
-            other_assigned: 其他群組已佔用的無人機 dict {drone_id: group_id}
+            other_assigned: (忽略 — 現在允許多 group 分配)
         """
         super().__init__(parent)
         self.setWindowTitle("分配無人機")
@@ -66,7 +71,6 @@ class DroneAssignDialog(QDialog):
             QLabel { color: #DDD; }
             QCheckBox { color: #DDD; spacing: 8px; padding: 4px; }
             QCheckBox::indicator { width: 16px; height: 16px; }
-            QCheckBox:disabled { color: #666; }
         """)
 
         layout = QVBoxLayout(self)
@@ -89,12 +93,9 @@ class DroneAssignDialog(QDialog):
         for drone_id in sorted_ids:
             cb = QCheckBox(drone_id)
 
+            # 所有 drone 都能被選摘（支持多 group 分配）
             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)
@@ -146,6 +147,8 @@ class GroupPanel(QWidget):
     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;
@@ -157,6 +160,7 @@ class GroupPanel(QWidget):
     def __init__(self, group: MissionGroup, parent=None):
         super().__init__(parent)
         self.group = group
+        self.all_btn_ref = None  # 保存全選按鈕的參考
         self._build_ui()
 
     def _make_sep(self):
@@ -296,7 +300,7 @@ class GroupPanel(QWidget):
         mid.addLayout(mid_body)
 
         # ============================
-        # 選取與分組（2x2 按鈕）
+        # 選取與分組（3x2 按鈕）
         # ============================
         right = QVBoxLayout()
         right.setSpacing(3)
@@ -310,9 +314,28 @@ class GroupPanel(QWidget):
         self.drone_list_label.setWordWrap(True)
         right.addWidget(self.drone_list_label)
 
-        # 2x2 按鈕網格
+        # 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(
@@ -321,22 +344,13 @@ class GroupPanel(QWidget):
         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))
-        grid_r1.addWidget(assign_btn)
-        grid_r1.addWidget(clear_btn)
-        right.addLayout(grid_r1)
-
-        grid_r2 = QHBoxLayout()
-        grid_r2.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(
-            lambda: self.select_all_requested.emit(self.group.group_id))
-        grid_r2.addWidget(box_btn)
-        grid_r2.addWidget(all_btn)
+        self.delete_group_btn = QPushButton("刪除群組")
+        self.delete_group_btn.setStyleSheet(BTN.format(bg='#EF5350', fg='white', hover='#E53935'))
+        self.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(self.delete_group_btn)
         right.addLayout(grid_r2)
 
         right.addStretch()
@@ -401,6 +415,21 @@ class GroupPanel(QWidget):
                 self._param_widgets[key] = (row_w, inp)
                 self._param_rows.append(row_w)
 
+        # LEADER_FOLLOWER 專用：領隊下拉選單
+        self._leader_row = QWidget()
+        leader_layout = QHBoxLayout(self._leader_row)
+        leader_layout.setContentsMargins(0, 0, 0, 0)
+        leader_layout.setSpacing(3)
+        leader_lbl = QLabel("領隊")
+        leader_lbl.setStyleSheet(LBL)
+        self._leader_combo = QComboBox()
+        self._leader_combo.setStyleSheet(COMBO)
+        self._leader_combo.setFixedWidth(70)
+        self._leader_combo.currentTextChanged.connect(self._on_leader_changed)
+        leader_layout.addWidget(leader_lbl, 1)
+        leader_layout.addWidget(self._leader_combo)
+        param_col.addWidget(self._leader_row)
+
         param_col.addStretch()
 
         # 初始顯示對應的參數
@@ -410,27 +439,50 @@ class GroupPanel(QWidget):
         self.type_combo.currentTextChanged.connect(self._update_param_visibility)
 
         # ── 組裝四欄：控制指令 > 任務規劃 > 任務參數 > 選取與分組 ──
-        cols.addLayout(left, 1)
+        # 使用伸展因子 0 讓列根據內容自動調整寬度，而不是均等分配
+        cols.addLayout(left, 0)
         cols.addWidget(self._make_sep())
-        cols.addLayout(mid, 1)
+        cols.addLayout(mid, 0)
         cols.addWidget(self._make_sep())
-        cols.addLayout(param_col, 1)
+        cols.addLayout(param_col, 0)
         cols.addWidget(self._make_sep())
-        cols.addLayout(right, 1)
+        cols.addLayout(right, 0)
+        cols.addStretch()  # 填充剩餘空間，使四列置左
 
         layout.addLayout(cols)
 
     def update_drone_list(self):
         """更新無人機列表顯示"""
-        if not self.group.drone_ids:
+        if not self.group.selected_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,
+            sorted_ids = sorted(self.group.selected_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;")
+        self._refresh_leader_options()
+
+    def _refresh_leader_options(self):
+        """依目前群組成員重新填充領隊下拉選單，保留目前選擇若仍有效"""
+        sorted_ids = sorted(self.group.selected_drone_ids,
+                            key=lambda x: (x.split('_')[0], int(x.split('_')[1])))
+        current = self.group.leader_drone_id
+        self._leader_combo.blockSignals(True)
+        self._leader_combo.clear()
+        self._leader_combo.addItems(sorted_ids)
+        if current and current in sorted_ids:
+            self._leader_combo.setCurrentText(current)
+        elif sorted_ids:
+            self._leader_combo.setCurrentText(sorted_ids[0])
+            self.group.leader_drone_id = sorted_ids[0]
+        else:
+            self.group.leader_drone_id = None
+        self._leader_combo.blockSignals(False)
+
+    def _on_leader_changed(self, drone_id):
+        self.group.leader_drone_id = drone_id if drone_id else None
 
     def update_status(self):
         """更新任務狀態顯示"""
@@ -445,18 +497,27 @@ class GroupPanel(QWidget):
             self.status_label.setText("⏸ 已暫停")
             self.status_label.setStyleSheet("color: #FFA000; font-size: 11px; font-weight: bold;")
         else:
-            n = len(self.group.drone_ids)
+            n = len(self.group.selected_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_delete_enabled(self, enabled):
+        """啟用或禁用刪除群組按鈕"""
+        self.delete_group_btn.setEnabled(enabled)
+
     def _update_param_visibility(self, _=None):
         """根據當前任務類型，顯示/隱藏對應的參數列"""
         mission_type = self.type_combo.currentText()
         visible_keys = {d[0] for d in self._param_defs.get(mission_type, [])}
         for key, (row_w, _inp) in self._param_widgets.items():
             row_w.setVisible(key in visible_keys)
+        self._leader_row.setVisible(mission_type == 'LEADER_FOLLOWER')
 
     def get_mission_params(self):
         """讀取當前顯示的參數值，回傳 dict"""

src/GUI/mission_planner.py

@@ -1,7 +1,7 @@
 #!/usr/bin/env python3
 """
 飛行任務規劃模組
-支援: M-Formation, Circle, Leader-Follower (Bezier 轉彎), Grid Sweep
+支援: M-Formation, Circle, Leader-Follower (arc-length virtual leader), Grid Sweep
 """
 import math
 from typing import List, Tuple, Optional, Dict, Any
@@ -72,7 +72,12 @@ class FormationPlanner:
                                mission_type: MissionType = MissionType.M_FORMATION,
                                params: Optional[Dict[str, Any]] = None
                                ) -> Tuple[List[List[Tuple[float, float, float]]],
-                                          Tuple[float, float, float]]:
+                                          Tuple[float, float, float],
+                                          List[int]]:
+        """
+        回傳 (waypoints_gps, origin, rendezvous_indices)。
+        rendezvous_indices：航點序列裡該群組需要等全員到齊才推進的 index 清單。
+        """
         if len(drone_gps_positions) == 0:
             raise ValueError("無人機位置列表不能為空")
 
@@ -89,10 +94,14 @@ class FormationPlanner:
         if mission_type == MissionType.M_FORMATION:
             s1, s2 = self._calculate_m_formation(drone_local, target_local, params)
             waypoints_local = [[s1[i], s2[i]] for i in range(len(drone_local))]
+            # 起點集合後一起出發到 stage2
+            rendezvous_indices = [0]
 
         elif mission_type == MissionType.CIRCLE_FORMATION:
             s1, s2 = self._calculate_circle_formation(drone_local, target_local, params)
             waypoints_local = [[s1[i], s2[i]] for i in range(len(drone_local))]
+            # 半程集合後一起進最終圓環位置
+            rendezvous_indices = [0]
 
         elif mission_type == MissionType.LEADER_FOLLOWER:
             params = params or {}
@@ -103,7 +112,9 @@ class FormationPlanner:
                 self.converter.gps_to_local(wp[0], wp[1], 0)[:2]
                 for wp in route_wps_gps
             ]
-            waypoints_local = self._calculate_leader_follower(drone_local, route_wps_local, params)
+            waypoints_local, rendezvous_indices = self._calculate_leader_follower(
+                drone_local, route_wps_local, params
+            )
 
         elif mission_type == MissionType.GRID_SWEEP:
             params = params or {}
@@ -114,7 +125,9 @@ class FormationPlanner:
                 self.converter.gps_to_local(c[0], c[1], 0)[:2]
                 for c in rect_corners_gps
             ]
-            waypoints_local = self._calculate_grid_sweep(drone_local, rect_corners_local, params)
+            waypoints_local, rendezvous_indices = self._calculate_grid_sweep(
+                drone_local, rect_corners_local, params
+            )
         else:
             raise ValueError(f"不支援的任務類型: {mission_type}")
 
@@ -123,7 +136,7 @@ class FormationPlanner:
             gps_wps = [self.converter.local_to_gps(*wp) for wp in drone_wps]
             waypoints_gps.append(gps_wps)
 
-        return waypoints_gps, self.current_origin
+        return waypoints_gps, self.current_origin, rendezvous_indices
 
     # ------------------------------------------------------------------ M-Formation
 
@@ -189,177 +202,321 @@ class FormationPlanner:
 
         return stage1_positions, stage2_positions
 
-    # ------------------------------------------------------------------ 路徑跟隨 (Bezier 轉彎)
+    # ------------------------------------------------------------------ 路徑跟隨 (虛擬領隊)
 
     def _calculate_leader_follower(self, drone_positions, route_wps_local, params):
         """
-        路徑跟隨編隊 — Bezier 曲線轉彎版
+        路徑跟隨編隊 — 虛擬領隊 / 弧長參數化版 (virtual leader / arc-length)
 
-        步驟:
-            1. _build_center_path: 在轉折 WP 處用二次 Bezier 曲線平滑轉彎
-            2. 固定排序，每架無人機沿中心路徑套用橫向+縱向偏移
+        核心想法：
+            - center_path 建好後計算每個 sample 的累積弧長 s
+            - 每架 drone 給 (lat_k, lon_k) 的隊形偏移
+            - 把 leader 想成沿 s 參數化的點，follower k 的目標 s = s_leader - lon_k
+            - follower k 第 i 個 waypoint = lookup(s_list[i] - lon_k) + lat_k × perp(tangent)
+            - 超過 path 起點/終點以 clip 處理，避免倒退或衝出
 
-        隊形 (俯視):
-            |     (前進方向)         |
-            |           D1          |  ← 左偏移, 後 0m
-            |                D2     |  ← 右偏移, 後 5m
-            |           D3          |  ← 左偏移, 後 10m
-            |                D4     |  ← 右偏移, 後 15m
+        解決前版「空間偏移」的三個 bug：
+            1. 起點暴衝：s<0 clip → follower 起點就是 start 加橫向偏移，不往後倒退
+            2. 弧段角度爆炸：lon 不再換算成弧度，直接沿 polyline lookup
+            3. straight/arc 切換不連續：統一走 polyline 線性內插與 segment 切線
+
+        Rank 定序規則：
+            **以 drone_positions 的輸入順序為準**（= GUI 選取順序）。
+            drone_positions[0] = rank 0 = leader (lon=0)，
+            drone_positions[1] = rank 1 (lon=5)，依此類推。
+            刻意「不」用位置投影自動排序，避免浮點噪音造成 run-to-run
+            leader 漂移，以確保整個 mission 像軍隊縱隊那樣順序固定。
+
+        隊形 (俯視, 以路徑行進方向為前):
+            D0 (lat=-L, lon=0)   ← front-right (leader)
+                D1 (lat=+L, lon=5)
+            D2 (lat=-L, lon=10)
+                D3 (lat=+L, lon=15)
         """
         N = len(drone_positions)
         lateral_offset = params.get('lateral_offset', 3.0)
         longitudinal_spacing = params.get('longitudinal_spacing', 5.0)
         altitude = params.get('altitude', self.base_altitude)
-        turn_margin = params.get('turn_margin', 0.35)      # 轉彎切入距離佔段長比例
-        curve_resolution = params.get('curve_resolution', 8)  # 每個彎道的插值點數
+        min_inner_radius = params.get('min_inner_radius', 3.0)
+        arc_resolution = params.get('arc_resolution', 12)
+        sharp_turn_cos = params.get('sharp_turn_cos_threshold', 0.0)  # cos(90°)=0
 
-        # Step 1: 建立帶 Bezier 轉彎的中心路徑
-        center_path = self._build_center_path(
-            route_wps_local, turn_margin, curve_resolution
+        # Step 1: 建立中心路徑（含圓弧、銳角單點；每點帶累積 s）
+        center_path, barrier_indices = self._build_center_path(
+            route_wps_local,
+            formation_max_lateral=lateral_offset,
+            min_inner_radius=min_inner_radius,
+            arc_resolution=arc_resolution,
+            sharp_turn_cos_threshold=sharp_turn_cos,
         )
 
-        # Step 2: 固定排序
-        first_dir = (center_path[0][2], center_path[0][3])
-        first_perp = (-first_dir[1], first_dir[0])
-        C_x = sum(p[0] for p in drone_positions) / N
-        C_y = sum(p[1] for p in drone_positions) / N
+        s_list = [pt['s'] for pt in center_path]
+        s_max = s_list[-1]
+        n_pts = len(center_path)
 
-        projections = [
-            ((pos[0] - C_x) * first_perp[0] + (pos[1] - C_y) * first_perp[1], i)
-            for i, pos in enumerate(drone_positions)
-        ]
-        projections.sort()
+        def lookup(s_target):
+            """
+            在 center path 上以弧長 s 回傳 (x, y, tangent_dir)。
+            s<0 → clip 到 start；s>s_max → clip 到 end。
+            tangent 方向取當前 polyline segment 的切線，避免對不連續的 dir 做插值。
+            """
+            if n_pts == 1:
+                pt = center_path[0]
+                return pt['x'], pt['y'], pt['dir']
 
-        # Step 3: 偏移
+            if s_target <= 0.0:
+                a = center_path[0]
+                b = center_path[1]
+                sdx, sdy = b['x'] - a['x'], b['y'] - a['y']
+                slen = math.hypot(sdx, sdy)
+                if slen > 1e-6:
+                    return a['x'], a['y'], (sdx / slen, sdy / slen)
+                return a['x'], a['y'], a['dir']
+
+            if s_target >= s_max:
+                a = center_path[-2]
+                b = center_path[-1]
+                sdx, sdy = b['x'] - a['x'], b['y'] - a['y']
+                slen = math.hypot(sdx, sdy)
+                if slen > 1e-6:
+                    return b['x'], b['y'], (sdx / slen, sdy / slen)
+                return b['x'], b['y'], b['dir']
+
+            # Binary search：s_list[lo] <= s_target < s_list[hi]
+            lo, hi = 0, n_pts - 1
+            while lo + 1 < hi:
+                mid = (lo + hi) // 2
+                if s_list[mid] <= s_target:
+                    lo = mid
+                else:
+                    hi = mid
+            a = center_path[lo]
+            b = center_path[hi]
+            ds = s_list[hi] - s_list[lo]
+            if ds < 1e-9:
+                return a['x'], a['y'], a['dir']
+            t = (s_target - s_list[lo]) / ds
+            x = a['x'] + t * (b['x'] - a['x'])
+            y = a['y'] + t * (b['y'] - a['y'])
+            sdx, sdy = b['x'] - a['x'], b['y'] - a['y']
+            slen = math.hypot(sdx, sdy)
+            if slen > 1e-6:
+                return x, y, (sdx / slen, sdy / slen)
+            return x, y, a['dir']
+
+        # Step 2: rank = 輸入順序（GUI 選取順序），不再做 projection sort
+        # 避免浮點噪音在投影值相近時翻轉 leader，保證 run-to-run 穩定
         all_waypoints = [None] * N
 
-        for rank, (_, original_idx) in enumerate(projections):
+        # 預先算好路徑終點的切線（給收尾點用）
+        end = center_path[-1]
+        end_dx, end_dy = end['dir']
+        if n_pts >= 2:
+            a = center_path[-2]
+            sdx, sdy = end['x'] - a['x'], end['y'] - a['y']
+            slen = math.hypot(sdx, sdy)
+            if slen > 1e-6:
+                end_dx, end_dy = sdx / slen, sdy / slen
+
+        for rank in range(N):
             lat_sign = -1 if rank % 2 == 0 else 1
             lat = lat_sign * lateral_offset
             lon = rank * longitudinal_spacing
 
             waypoints = []
-            for (cx, cy, dx, dy) in center_path:
+            for i in range(n_pts):
+                s_follower = s_list[i] - lon
+                x, y, (dx, dy) = lookup(s_follower)
                 perp_x, perp_y = -dy, dx
-                ox = cx + lat * perp_x - lon * dx
-                oy = cy + lat * perp_y - lon * dy
-                waypoints.append((ox, oy, altitude))
+                waypoints.append((
+                    x + lat * perp_x,
+                    y + lat * perp_y,
+                    altitude,
+                ))
 
-            all_waypoints[original_idx] = waypoints
+            # 收尾：全員到 path 終點的 rank 位置（lon 歸零）
+            waypoints.append((
+                end['x'] + lat * (-end_dy),
+                end['y'] + lat * end_dx,
+                altitude,
+            ))
 
-        return all_waypoints
+            all_waypoints[rank] = waypoints
 
-    def _build_center_path(self, waypoints, turn_margin, curve_resolution):
+        # barrier 索引仍指向 center_path 內 (range [0, n_pts-1])，
+        # 不觸及末尾收尾點，直接沿用即可
+        return all_waypoints, barrier_indices
+
+    def _build_center_path(self, waypoints,
+                           formation_max_lateral,
+                           min_inner_radius,
+                           arc_resolution,
+                           sharp_turn_cos_threshold):
         """
-        建立帶 Bezier 曲線轉彎的中心路徑
+        建立以「圓弧」連接直線段的中心路徑。
 
-        在每個轉折 WP 處:
-            1. 計算 pre_turn = WP - d_in × T
-            2. 計算 post_turn = WP + d_out × T
-            3. 用二次 Bezier 曲線: B(t) = (1-t)²·pre + 2t(1-t)·WP + t²·post
-            4. 方向從導數得到: B'(t) = 2(1-t)(WP-pre) + 2t(post-WP)
+        每個中間 waypoint 的處理：
+            1. 計算入向 u_in、出向 u_out 的夾角 β = acos(u_in·u_out)
+            2. 若 cos β < sharp_turn_cos_threshold → 銳角：只插單點 (hover + 原地轉向)，
+               barrier 放在該點，讓隊伍整體停下再繼續
+            3. 否則 → 平滑弧：
+                  R_base = formation_max_lateral + min_inner_radius
+                  d = R_base × tan(β/2)，並以相鄰段長的 45% 為上限
+                  R_actual = d / tan(β/2)
+                  tangent points T_in = WP - u_in·d, T_out = WP + u_out·d
+                  arc center = T_in + R_actual·sign·n_left，其中 sign=+1 左轉/CCW，-1 右轉/CW
+                  arc 由 theta_in 到 theta_out 以 arc_resolution 等分
+               path 內容: [T_in(straight,u_in), arc_samples..., T_out(straight,u_out)]
+               barrier 放在 T_out (轉完彎後的集合點)
 
-        Args:
-            waypoints: 路徑航點 [(x, y), ...]
-            turn_margin: 轉彎切入距離佔相鄰段長的比例 (0~0.5)
-            curve_resolution: 每個彎道的 Bezier 插值點數
+        path 是一個 list[dict]，每個 dict 至少含:
+            {'seg': 'straight' | 'arc',
+             'x': float, 'y': float,
+             'dir': (dx, dy)}
+        'arc' 類型額外含: 'arc_center', 'arc_R', 'arc_sign', 'theta'
 
         Returns:
-            [(x, y, dir_x, dir_y), ...] 中心路徑點 + 單位方向
+            (path, barrier_indices)
         """
         num_wps = len(waypoints)
 
         if num_wps < 2:
-            return [(waypoints[0][0], waypoints[0][1], 0.0, 1.0)]
+            return [{
+                'seg': 'straight',
+                'x': waypoints[0][0], 'y': waypoints[0][1],
+                'dir': (0.0, 1.0),
+            }], []
 
-        # 計算每段方向和長度
         segments = []
         for i in range(num_wps - 1):
             dx = waypoints[i + 1][0] - waypoints[i][0]
             dy = waypoints[i + 1][1] - waypoints[i][1]
-            length = math.sqrt(dx ** 2 + dy ** 2)
+            length = math.hypot(dx, dy)
             if length < 0.01:
-                segments.append((0.0, 1.0, length))
+                segments.append(((0.0, 1.0), length))
             else:
-                segments.append((dx / length, dy / length, length))
+                segments.append(((dx / length, dy / length), length))
 
+        R_base = formation_max_lateral + min_inner_radius
         path = []
+        barrier_indices = []
 
-        # 第一個航點
-        path.append((waypoints[0][0], waypoints[0][1],
-                      segments[0][0], segments[0][1]))
+        # 起點
+        path.append({
+            'seg': 'straight',
+            'x': waypoints[0][0], 'y': waypoints[0][1],
+            'dir': segments[0][0],
+        })
 
-        # 中間航點 (轉彎處)
         for i in range(1, num_wps - 1):
-            d_in_x, d_in_y, len_in = segments[i - 1]
-            d_out_x, d_out_y, len_out = segments[i]
+            u_in, len_in = segments[i - 1]
+            u_out, len_out = segments[i]
+            wp = waypoints[i]
 
-            # 切入距離 T: 取相鄰兩段中較短的 × turn_margin
-            T = min(len_in, len_out) * turn_margin
+            dot = u_in[0] * u_out[0] + u_in[1] * u_out[1]
+            dot = max(-1.0, min(1.0, dot))
 
-            if T < 0.5:
-                # 段太短，不插彎，直接加一個平均方向點
-                avg_dx = d_in_x + d_out_x
-                avg_dy = d_in_y + d_out_y
-                avg_len = math.sqrt(avg_dx ** 2 + avg_dy ** 2)
+            # 銳角：hover + 原地轉向
+            if dot < sharp_turn_cos_threshold:
+                avg_dx = u_in[0] + u_out[0]
+                avg_dy = u_in[1] + u_out[1]
+                avg_len = math.hypot(avg_dx, avg_dy)
                 if avg_len > 0.01:
-                    avg_dx /= avg_len
-                    avg_dy /= avg_len
+                    avg_dir = (avg_dx / avg_len, avg_dy / avg_len)
                 else:
-                    avg_dx, avg_dy = d_in_x, d_in_y
-                path.append((waypoints[i][0], waypoints[i][1], avg_dx, avg_dy))
+                    avg_dir = u_in
+                path.append({
+                    'seg': 'straight',
+                    'x': wp[0], 'y': wp[1],
+                    'dir': avg_dir,
+                })
+                barrier_indices.append(len(path) - 1)
                 continue
 
-            # P0 = pre_turn (弧線起始)
-            p0_x = waypoints[i][0] - d_in_x * T
-            p0_y = waypoints[i][1] - d_in_y * T
+            # 平滑弧
+            beta = math.acos(dot)
+            if beta < 1e-4:
+                # 幾乎直線，不插弧
+                path.append({
+                    'seg': 'straight',
+                    'x': wp[0], 'y': wp[1],
+                    'dir': u_in,
+                })
+                continue
 
-            # P1 = WP 本身 (控制點)
-            p1_x = waypoints[i][0]
-            p1_y = waypoints[i][1]
+            half_tan = math.tan(beta / 2.0)
+            d_ideal = R_base * half_tan
+            d_max = 0.45 * min(len_in, len_out)
+            d = min(d_ideal, d_max)
+            R_actual = d / half_tan
 
-            # P2 = post_turn (弧線結束)
-            p2_x = waypoints[i][0] + d_out_x * T
-            p2_y = waypoints[i][1] + d_out_y * T
+            T_in = (wp[0] - u_in[0] * d, wp[1] - u_in[1] * d)
+            T_out = (wp[0] + u_out[0] * d, wp[1] + u_out[1] * d)
 
-            # 加入 pre_turn 點 (方向 = incoming)
-            path.append((p0_x, p0_y, d_in_x, d_in_y))
+            # +1 CCW (左轉) / -1 CW (右轉)
+            cross = u_in[0] * u_out[1] - u_in[1] * u_out[0]
+            sign = 1 if cross > 0 else -1
 
-            # 加入 Bezier 插值點
-            for j in range(1, curve_resolution):
-                t = j / curve_resolution
+            # 入向左法線
+            n_left = (-u_in[1], u_in[0])
+            center = (
+                T_in[0] + R_actual * sign * n_left[0],
+                T_in[1] + R_actual * sign * n_left[1],
+            )
 
-                # B(t) = (1-t)²·P0 + 2t(1-t)·P1 + t²·P2
-                one_minus_t = 1.0 - t
-                bx = one_minus_t * one_minus_t * p0_x + \
-                     2 * t * one_minus_t * p1_x + \
-                     t * t * p2_x
-                by = one_minus_t * one_minus_t * p0_y + \
-                     2 * t * one_minus_t * p1_y + \
-                     t * t * p2_y
+            theta_in = math.atan2(T_in[1] - center[1], T_in[0] - center[0])
 
-                # B'(t) = 2(1-t)(P1-P0) + 2t(P2-P1) → 切線方向
-                tdx = 2 * one_minus_t * (p1_x - p0_x) + 2 * t * (p2_x - p1_x)
-                tdy = 2 * one_minus_t * (p1_y - p0_y) + 2 * t * (p2_y - p1_y)
+            # T_in (straight, 方向 u_in)
+            path.append({
+                'seg': 'straight',
+                'x': T_in[0], 'y': T_in[1],
+                'dir': u_in,
+            })
 
-                # 正規化
-                t_len = math.sqrt(tdx ** 2 + tdy ** 2)
-                if t_len > 0.01:
-                    tdx /= t_len
-                    tdy /= t_len
-                else:
-                    tdx, tdy = d_in_x, d_in_y
+            # 弧段採樣 k=1..arc_resolution-1（不含兩端）
+            for k in range(1, arc_resolution):
+                t = k / arc_resolution
+                theta = theta_in + sign * beta * t
+                px = center[0] + R_actual * math.cos(theta)
+                py = center[1] + R_actual * math.sin(theta)
+                # 切線: sign × (-sin θ, cos θ)
+                tx = sign * (-math.sin(theta))
+                ty = sign * math.cos(theta)
+                path.append({
+                    'seg': 'arc',
+                    'x': px, 'y': py,
+                    'dir': (tx, ty),
+                    'arc_center': center,
+                    'arc_R': R_actual,
+                    'arc_sign': sign,
+                    'theta': theta,
+                })
 
-                path.append((bx, by, tdx, tdy))
+            # T_out (straight, 方向 u_out)，barrier 放這
+            path.append({
+                'seg': 'straight',
+                'x': T_out[0], 'y': T_out[1],
+                'dir': u_out,
+            })
+            barrier_indices.append(len(path) - 1)
 
-            # 加入 post_turn 點 (方向 = outgoing)
-            path.append((p2_x, p2_y, d_out_x, d_out_y))
+        # 終點
+        path.append({
+            'seg': 'straight',
+            'x': waypoints[-1][0], 'y': waypoints[-1][1],
+            'dir': segments[-1][0],
+        })
 
-        # 最後一個航點
-        path.append((waypoints[-1][0], waypoints[-1][1],
-                      segments[-1][0], segments[-1][1]))
+        # 後處理：為每個 sample 加上累積 polyline 弧長 s
+        path[0]['s'] = 0.0
+        for i in range(1, len(path)):
+            prev = path[i - 1]
+            cur = path[i]
+            cur['s'] = prev['s'] + math.hypot(
+                cur['x'] - prev['x'], cur['y'] - prev['y']
+            )
 
-        return path
+        return path, barrier_indices
 
     # ------------------------------------------------------------------ 柵狀掃描
 
@@ -474,7 +631,15 @@ class FormationPlanner:
 
             all_waypoints[original_idx] = waypoints_list
 
-        return all_waypoints
+        # 每一條掃描線的「起點」都做一次同步：
+        #   wp 0 = gather, wp 1 = 第一條線起點, wp 2 = 第一條線終點,
+        #   wp 3 = 第二條線起點, wp 4 = 第二條線終點, ...
+        # 每條線的「起點 index」= 1, 3, 5, ... = 2*i + 1（i 從 0 開始）
+        # 所有 drone 的 waypoint 數量相同（num_lines 對所有 strip 都一致），
+        # 所以用同一份 rendezvous_indices
+        rendezvous_indices = [2 * i + 1 for i in range(num_lines)]
+
+        return all_waypoints, rendezvous_indices
 
 
 # ================================================================================
@@ -492,12 +657,12 @@ if __name__ == "__main__":
     target = (24.12400, 120.56795, 10.0)
 
     # M-Formation
-    wps, origin = planner.plan_formation_mission(drones, target, MissionType.M_FORMATION)
+    wps, origin, rv = planner.plan_formation_mission(drones, target, MissionType.M_FORMATION)
     print("M-Formation:")
     for i, wp_list in enumerate(wps):
         print(f"  Drone {i}: {len(wp_list)} waypoints")
 
-    # Leader-Follower (Bezier 轉彎)
+    # Leader-Follower (虛擬領隊 / 弧長參數化)
     route = [
         (24.12345, 120.56780),
         (24.12370, 120.56800),
@@ -505,18 +670,18 @@ if __name__ == "__main__":
         (24.12400, 120.56800),
         (24.12420, 120.56790),
     ]
-    wps_lf, origin_lf = planner.plan_formation_mission(
+    wps_lf, origin_lf, rv_lf = planner.plan_formation_mission(
         drones, target, MissionType.LEADER_FOLLOWER,
         params={
             'route_waypoints': route,
             'lateral_offset': 3.0,
             'longitudinal_spacing': 5.0,
-            'turn_margin': 0.35,
-            'curve_resolution': 8,
+            'min_inner_radius': 3.0,
+            'arc_resolution': 8,
             'altitude': 10.0
         }
     )
-    print(f"\nLeader-Follower (Bezier turns):")
+    print(f"\nLeader-Follower (arc-length virtual leader):")
     for i, wp_list in enumerate(wps_lf):
         print(f"  Drone {i}: {len(wp_list)} waypoints")
         for j, wp in enumerate(wp_list):
@@ -529,7 +694,7 @@ if __name__ == "__main__":
         (24.1240, 120.5683),
         (24.1240, 120.5679)
     ]
-    wps2, origin2 = planner.plan_formation_mission(
+    wps2, origin2, rv2 = planner.plan_formation_mission(
         drones, target, MissionType.GRID_SWEEP,
         params={'rect_corners': rect, 'line_spacing': 5.0, 'altitude': 10.0}
     )

src/GUI/overview_table.py

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

src/GUI/validation/test_mission.py

@@ -210,7 +210,7 @@ def main():
         target_gps = (target_lat, target_lon, BASE_ALTITUDE)
         print(f"  目標點: ({target_lat:.6f}, {target_lon:.6f})")
 
-        waypoints_per_drone, origin = planner.plan_formation_mission(
+        waypoints_per_drone, origin, _ = planner.plan_formation_mission(
             drone_gps_positions, target_gps, MissionType.M_FORMATION
         )
 
@@ -232,7 +232,7 @@ def main():
         target_gps = (rect_center_lat, rect_center_lon, BASE_ALTITUDE)
         print(f"  矩形中心: ({rect_center_lat:.6f}, {rect_center_lon:.6f})")
 
-        waypoints_per_drone, origin = planner.plan_formation_mission(
+        waypoints_per_drone, origin, _ = planner.plan_formation_mission(
             drone_gps_positions, target_gps, MissionType.GRID_SWEEP,
             params={
                 'rect_corners': rect_corners,

src/GUI/validation/verify_waypoints.py

@@ -365,7 +365,7 @@ def visualize_standalone():
     fig.suptitle('Mission Planner Verification (standalone)', fontsize=14, fontweight='bold')
 
     # M-Formation
-    wps_m, origin_m = planner.plan_formation_mission(drones, target, MissionType.M_FORMATION)
+    wps_m, origin_m, _ = planner.plan_formation_mission(drones, target, MissionType.M_FORMATION)
     conv_m = CoordinateConverter(origin_m[0], origin_m[1], 0)
     data_m = {
         'drone_ids': [f's0_{i + 1}' for i in range(len(drones))],
@@ -380,7 +380,7 @@ def visualize_standalone():
     # Grid Sweep 5m
     target_gs = (sum(c[0] for c in rect_corners) / 4,
                  sum(c[1] for c in rect_corners) / 4, 10.0)
-    wps_g, origin_g = planner.plan_formation_mission(
+    wps_g, origin_g, _ = planner.plan_formation_mission(
         drones, target_gs, MissionType.GRID_SWEEP,
         params={'rect_corners': rect_corners, 'line_spacing': 5.0, 'altitude': 10.0}
     )
@@ -403,7 +403,7 @@ def visualize_standalone():
         (24.12410, 120.56800),
         (24.12420, 120.56790),
     ]
-    wps_lf, origin_lf = planner.plan_formation_mission(
+    wps_lf, origin_lf, _ = planner.plan_formation_mission(
         drones, target, MissionType.LEADER_FOLLOWER,
         params={'route_waypoints': route, 'lateral_offset': 3.0,
                 'longitudinal_spacing': 5.0, 'altitude': 10.0}

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  應用開發介面
+5-2. 模組化 serial 連線機制 以利後期擴容其他模組

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)
@@ -231,6 +230,7 @@ class mavlink_bridge:
             'vx': msg.vx, 'vy': msg.vy, 'vz': msg.vz
         }
 
+
     def _handle_global_position(self, vehicle, component, msg, timestamp):
         """處理 GLOBAL_POSITION_INT 訊息 (msg_id: 33)"""
         component.status.position.latitude = msg.lat / 1e7  # 轉換為度
@@ -386,7 +386,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位置
-            'attitude': 0.5,       # 姿態
-            'velocity': 0.5,       # 速度
-            'battery': 1.0,        # 電池
-            'vfr_hud': 0.5,        # VFR HUD
-            'mode': 1.0,           # 飛行模式
-            'summary': 1.0,        # 載具摘要
+            'position': 0.0,       # GNSS位置
+            'position_ned': 0.0,   # LOCAL_POSITION_NED (位置+速度)
+            'attitude': 0.0,       # 姿態 (pitch yaw row 與其加速狀態)
+            'velocity': 0.0,       # 速度 (已經包含在 vfr_hud 未來移除)
+            'battery': 0.0,        # 電池
+            'vfr_hud': 0.0,        # VFR HUD (地速 空速 絕對高度 爬升率 航向 油門)
+            'mode': 0.0,           # 飛行模式 (已經在 summary 裡 未來移除)
+            'summary': 0.0,        # 載具摘要 (sysid 飛行模式 解鎖上鎖 gps狀態)
             # 在這裡新增更多 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 獲取所有載具
-        all_vehicles = mvv.vehicle_registry.get_all()
-        
-        for sysid, vehicle in all_vehicles.items():
+        # 高頻快路徑：讀取 registry 的 immutable 快照
+        for sysid, vehicle in mvv.vehicle_registry.read_all():
             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)
@@ -205,7 +210,9 @@ class VehicleStatusPublisher(Node):
         if pos.latitude is None or pos.longitude is None:
             return
         
-        publisher = self._get_or_create_publisher(sysid, 'position', sensor_msgs.msg.NavSatFix)
+        publisher = self._get_or_create_publisher(
+            sysid, 'position', sensor_msgs.msg.NavSatFix
+            )
         
         # 檢查是否有訂閱者
         if publisher.get_subscription_count() == 0:
@@ -222,6 +229,42 @@ class VehicleStatusPublisher(Node):
             msg.status.status = gps.fix_type - 1  # MAVLink fix_type 轉 NavSatStatus
         
         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）"""
@@ -231,7 +274,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 +513,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 +534,8 @@ class MavlinkCommandService(Node):
         # 狀態標記
         self.running = True
         
-        # mavlinkObject 的引用（將由外部設置）
-        self.mavlink_analyzer = None
+        # # mavlinkObject 的引用（將由外部設置） 用不到
+        # self.mavlink_analyzer = None
 
         # pending 旗標物件的儲存庫
         self._pending_by_sysid = {} # sysid(int) : PendingEntry
@@ -521,6 +568,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 +598,8 @@ class MavlinkCommandService(Node):
 
     def return_router(self):
         '''
-        這邊是給外部迴圈呼叫的 消耗 return_packet_ring 裡接收到的 mavlink 封包
+        在節點管理器哪邊被呼叫
+        消耗 return_packet_ring 裡接收到的 mavlink 封包
         分送到各自的 pending 中 
         藉由 event.set() 解開 service 中的 block 
         '''
@@ -568,6 +625,7 @@ class MavlinkCommandService(Node):
     def handle_mav_timesync_ping(self, request, response):
         '''
         用 timesync 封包驗證來回的時間
+        隸屬於 MavLink PING Protocol
         '''
         # 設定失效回應
         response.success = False
@@ -612,7 +670,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 +691,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 +745,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,7 +766,96 @@ 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: 參數請求
     # ═══════════════════════════════════════════════════════════════════════
@@ -790,7 +934,21 @@ class MavlinkCommandService(Node):
         response.success = True
         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 +995,8 @@ class fc_ros_manager:
         
         try:
             # 初始化 ROS2
-            rclpy.init()
+            if not rclpy.ok():
+                rclpy.init(args=None)
             
             # 創建節點 node
             self.status_publisher = VehicleStatusPublisher()
@@ -973,7 +1132,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
@@ -425,10 +433,22 @@ class VehicleViewRegistry:
     def get_all(self) -> Dict[int, VehicleView]:
         """取得所有載具視圖"""
         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

@@ -16,18 +16,22 @@ import threading
 import struct
 from enum import Enum, auto
 from abc import ABC, abstractmethod
+from dataclasses import dataclass
 
 # # XBee 模組
 # from xbee.frame import APIFrame
 
 # 自定義的 import
-from .utils import setup_logger
+from .utils import RingBuffer, setup_logger
 
 # ====================== 分割線 =====================
 
 logger = setup_logger(os.path.basename(__file__))
+MODULE_VER = "0.80"
 
-# ====================== 分割線 =====================
+rx_module_ack = RingBuffer(capacity=64, buffer_id=253)
+
+# ====================== State DEFINITION =====================
 
 # 定義 serial 連線的模式
 class SerialMode(Enum):
@@ -37,7 +41,29 @@ class SerialMode(Enum):
     NOT_USE = auto()       # 不使用
 
 
-# ====================== Frame Processor 基類與實現 =====================
+# ====================== AT Frame Data Classes =====================
+
+@dataclass
+class ATRequest:
+    """送往 dongle 的 AT 指令"""
+    command: bytes      # 例如 b'DB'
+    parameter: bytes    # 寫入用指令的參數，讀取型通常為空
+    frame_id: int       # XBee frame id；0x00 表示不要求回應
+
+
+@dataclass
+class ATResponse:
+    """dongle 回傳的 AT Response (0x88)"""
+    frame_id: int
+    command: bytes
+    status: int
+    data: bytes
+
+    @property
+    def is_ok(self) -> bool:
+        return self.status == 0x00
+
+# ====================== Frame Processor Base and Implementation =====================
 
 class FrameProcessor(ABC):
     """協議處理器基類"""
@@ -46,10 +72,10 @@ class FrameProcessor(ABC):
         self.buffer = bytearray()
     
     @abstractmethod
-    def process_incoming(self, data: bytes):
+    def process_incoming(self, data: bytes) -> bytes:
         """
         處理接收到的數據
-        返回：已完整解析的 payload 列表
+        返回：已完整解析的 payload 列表 後續要丟到 UDP 去
         """
         pass
     
@@ -65,7 +91,7 @@ class FrameProcessor(ABC):
 class RawFrameProcessor(FrameProcessor):
     """原始數據直通處理器"""
     
-    def process_incoming(self, data: bytes):
+    def process_incoming(self, data: bytes) -> bytes:
         """直接返回原始數據，不進行緩衝"""
         return [data] if data else []
     
@@ -75,110 +101,108 @@ class RawFrameProcessor(FrameProcessor):
 
 
 class XBeeFrameProcessor(FrameProcessor):
-    """XBee API 協議處理器"""
-    
-    def process_incoming(self, data: bytes):
+    """
+    XBee API 協議處理器
+
+    職責：
+    - XBee API frame 的拆幀 / 組幀
+    - 0x90 (RX Packet)   -> 解出 payload 回傳
+    - 0x88 (AT Response) -> 轉交 at_handler 處理（若有注入）
+    - 0x8B (TX Status)   -> 目前忽略
+    - 其他 frame type    -> 記 warning 忽略
+
+    若未來要做變化型 XBee（例如 API2 escape mode、不同 addressing），
+    繼承此類並覆寫 _encapsulate / _decapsulate / _try_extract_frame 即可。
+    """
+
+    # XBee API frame type
+    FRAME_TYPE_TX_REQUEST  = 0x10
+    FRAME_TYPE_AT_RESPONSE = 0x88
+    FRAME_TYPE_TX_STATUS   = 0x8B
+    FRAME_TYPE_RX_PACKET   = 0x90
+
+    def __init__(self, at_handler: "ATCommandHandler" = None):
+        super().__init__()
+        self.at_handler = at_handler
+
+    # ---- 對外契約 ----
+    def process_incoming(self, data: bytes) -> bytes:
         """處理 XBee API 幀並提取 payload"""
         self.buffer.extend(data)
         payloads = []
-        
+
+        while True:
+            frame = self._try_extract_frame()
+            if frame is None:
+                break
+            payload = self._dispatch_frame(frame)
+            if payload:
+                payloads.append(payload)
+
+        return payloads
+
+    def process_outgoing(self, data: bytes) -> bytes:
+        """將數據封裝為 XBee API 傳輸幀"""
+        return self._encapsulate(data)
+
+    # ---- 內部：拆幀與分派 ----
+    def _try_extract_frame(self) -> bytes:
+        """
+        從 buffer 嘗試抓一個完整 frame
+        - 對齊幀頭 0x7E
+        - 長度不足時等待下次進 buffer
+        - 成功則從 buffer 切掉並回傳整個 frame bytes
+        """
         while len(self.buffer) >= 3:
-            # 尋找幀頭
             if self.buffer[0] != 0x7E:
                 self.buffer.pop(0)
                 continue
-            
-            # 讀取 payload 長度
+
             length = (self.buffer[1] << 8) | self.buffer[2]
             full_length = 3 + length + 1  # 起始符(1) + 長度(2) + payload + 校驗和(1)
-            
-            # 等待完整幀
+
             if len(self.buffer) < full_length:
-                break
-            
-            # 提取完整 frame 並從緩衝區移除
+                return None
+
+            # TODO: 驗證 checksum：(sum(frame[3:]) & 0xFF) 應為 0xFF；不通過則丟棄此 frame 並從 buffer pop 1 byte 再重新對齊
+
             frame = bytes(self.buffer[:full_length])
             del self.buffer[:full_length]
-            
-            # 判斷 frame 類型並處理
-            frame_type = frame[3]
-            
-            if frame_type == 0x90:  # RX Packet
-                payload = XBeeFrameHandler.decapsulate_data(frame)
-                if payload:
-                    payloads.append(payload)
-            elif frame_type == 0x88:  # AT Response
-                # 可以在這裡處理 AT 指令回應
-                # response = XBeeFrameHandler.parse_at_command_response(frame)
-                # 目前忽略
-                pass
-            elif frame_type == 0x8B:  # Transmit Status
-                # 傳輸狀態，目前忽略
-                pass
-            else:
-                logger.warning(f"Unknown XBee frame type: 0x{frame_type:02X}")
-        
-        return payloads
-    
-    def process_outgoing(self, data: bytes) -> bytes:
-        """將數據封裝為 XBee API 傳輸幀"""
-        return XBeeFrameProcessor.encapsulate_data(data)
+            return frame
 
-
-# ====================== XBee Frame Handler =====================
-
-class XBeeFrameHandler:
-    """XBee API Frame 處理器"""
-    
-    @staticmethod
-    def parse_at_command_response(frame: bytes) -> dict:
-        """解析 AT Command Response (0x88)"""
-        if len(frame) < 8:
-            return None
-        
-        frame_type = frame[3]
-        if frame_type != 0x88:
-            return None
-        
-        frame_id = frame[4]
-        at_command = frame[5:7]
-        status = frame[7]
-        data = frame[8:] if len(frame) > 8 else b''
-        
-        return {
-            'frame_id': frame_id,
-            'command': at_command,
-            'status': status,
-            'data': data,
-            'is_ok': status == 0x00
-        }
-    
-    @staticmethod
-    def parse_receive_packet(frame: bytes) -> dict:
-        """解析 RX Packet (0x90) - 未來擴展用"""
-        # if len(frame) < 15 or frame[3] != 0x90:
-        #     return None
-        
-        # return {
-        #     'source_addr': frame[4:12],
-        #     'reserved': frame[12:14],
-        #     'options': frame[14],
-        #     'data': frame[15:-1]
-        # }
-        pass
         return None
 
+    def _dispatch_frame(self, frame: bytes) -> bytes:
+        """根據 frame type 分派；若是 RX payload 回傳 bytes，其餘回傳 None"""
+        frame_type = frame[3]
+
+        if frame_type == self.FRAME_TYPE_RX_PACKET:
+            return self._decapsulate(frame)
+
+        if frame_type == self.FRAME_TYPE_AT_RESPONSE:
+            if self.at_handler is not None:
+                self.at_handler.handle_frame(frame)
+            return None
+
+        if frame_type == self.FRAME_TYPE_TX_STATUS:
+            return None
+
+        logger.warning(f"Unknown XBee frame type: 0x{frame_type:02X}")
+        return None
+
+    # ---- 內部：編解碼 ----
     @staticmethod
-    def encapsulate_data(data: bytes, dest_addr64: bytes = b'\x00\x00\x00\x00\x00\x00\xFF\xFF', frame_id = 0x01) -> bytes:
+    def _encapsulate(
+        data: bytes,
+        dest_addr64: bytes = b'\x00\x00\x00\x00\x00\x00\xFF\xFF',
+        frame_id: int = 0x01,
+    ) -> bytes:
         """
-        將數據封裝為 XBee API 傳輸幀
-        
-        使用 XBee API 格式封裝數據:
-        - 傳輸請求幀 (0x10)
+        將 payload 包成 XBee TX Request (0x10)
         - 使用廣播地址
         - 添加適當的頭部和校驗和
         """
-        frame_type = 0x10
+        frame_type = XBeeFrameProcessor.FRAME_TYPE_TX_REQUEST
         dest_addr16 = b'\xFF\xFE'
         broadcast_radius = 0x00
         options = 0x00
@@ -190,89 +214,173 @@ class XBeeFrameHandler:
         return b'\x7E' + struct.pack(">H", len(frame)) + frame + struct.pack("B", checksum)
 
     @staticmethod
-    def decapsulate_data(data: bytes):
-        
-        # 獲取數據長度 (不包括校驗和)
-        length = (data[1] << 8) | data[2]
-
+    def _decapsulate(frame: bytes) -> bytes:
+        """從 RX Packet (0x90) 取出 payload"""
+        length = (frame[1] << 8) | frame[2]
         rf_data_start = 3 + 12
-        return data[rf_data_start:3 + length]
+        return frame[rf_data_start:3 + length]
 
 
+# ====================== Dongle Command Handler =====================
+
 class ATCommandHandler:
-    """AT 指令回應處理器"""
-    
+    """
+    XBee AT 指令處理器
+
+    職責：
+    - 組 AT Command Request frame (0x08) 並透過注入的 writer 送出
+    - 解析 AT Response frame (0x88)
+    - 依 AT 指令分派到對應的 handler
+
+    writer 由 SerialHandler 在連線建立後 (connection_made) 注入，
+    型別為 Callable[[bytes], None]（通常就是 serial transport.write）。
+    """
+
+    FRAME_TYPE_AT_COMMAND = 0x08
+
     def __init__(self, serial_port: str):
         self.serial_port = serial_port
+        self.writer = None
         self.handlers = {
             b'DB': self._handle_rssi,
             b'SH': self._handle_serial_high,
             b'SL': self._handle_serial_low,
             # 可擴展其他 AT 指令
         }
-    
-    def handle_response(self, response: dict):
-        """根據 AT 指令類型分派處理"""
-        if not response or not response['is_ok']:
-            if response:
-                logger.warning(f"[{self.serial_port}] AT {response['command'].decode()} 失敗，狀態碼: {response['status']}")
+
+    # ---- 發送端 ----
+    def set_writer(self, writer):
+        """由 SerialHandler 注入實際寫入 serial 的 callable"""
+        self.writer = writer
+
+    def send_command(self, request: ATRequest):
+        """發送一筆 AT 指令給 dongle"""
+        if self.writer is None:
+            logger.warning(
+                f"[{self.serial_port}] AT writer 尚未就緒，指令 {request.command!r} 丟棄"
+            )
             return
-        
-        command = response['command']
-        handler = self.handlers.get(command)
-        
+
+        self.writer(self._build_at_request(request))
+        # logger.debug(
+        #     f"[{self.serial_port}] send AT {request.command.decode(errors='replace')} "
+        #     f"(frame_id=0x{request.frame_id:02X})"
+        # ) # dev
+
+    @staticmethod
+    def _build_at_request(request: ATRequest) -> bytes:
+        """將 ATRequest 組成 XBee API AT Command Request frame (0x08) bytes"""
+        frame_type = ATCommandHandler.FRAME_TYPE_AT_COMMAND
+
+        frame = struct.pack(">B", frame_type) + struct.pack(">B", request.frame_id)
+        frame += request.command + request.parameter
+        checksum = 0xFF - (sum(frame) & 0xFF)
+        return b'\x7E' + struct.pack(">H", len(frame)) + frame + struct.pack("B", checksum)
+
+    # ---- 接收端 ----
+    def handle_frame(self, frame: bytes) -> None:
+        """
+        接收一整個 AT Response frame：
+        1. 解析成 ATResponse
+        2. 推進 rx_module_ack 供其他模組消費
+        3. 本地 dispatch 給對應的 _handle_xxx
+        """
+        parsed = self._parse(frame)
+        if parsed is None:
+            return
+
+        if not rx_module_ack.put(parsed):
+            logger.warning(
+                f"[{self.serial_port}] rx_module_ack overflow, drop {parsed.command!r}"
+            )
+
+        self._dispatch(parsed)
+
+    @staticmethod
+    def _parse(frame: bytes) -> ATResponse:
+        """解析 AT Command Response (0x88)；不符格式回傳 None"""
+        if len(frame) < 8:
+            return None
+
+        if frame[3] != 0x88:
+            return None
+
+        return ATResponse(
+            frame_id=frame[4],
+            command=frame[5:7],
+            status=frame[7],
+            data=frame[8:] if len(frame) > 8 else b'',
+        )
+
+    def _dispatch(self, response: ATResponse) -> None:
+        """根據 AT 指令類型分派處理"""
+        # print(f"[{self.serial_port}] AT Response: {response}")  # dev
+
+        if not response.is_ok:
+            logger.warning(
+                f"[{self.serial_port}] AT {response.command.decode()} "
+                f"失敗，狀態碼: {response.status}"
+            )
+            return
+
+        handler = self.handlers.get(response.command)
         if handler:
-            handler(response['data'])
+            handler(response.data)
         else:
-            logger.debug(f"[{self.serial_port}] 未處理的 AT 指令: {command.decode()}")
-    
+            logger.debug(
+                f"[{self.serial_port}] 未處理的 AT 指令: "
+                f"{response.command.decode()}"
+            )
+
     def _handle_rssi(self, data: bytes):
-        """處理 DB (RSSI) 回應"""
-        # 未來可實現 RSSI 處理邏輯
+        """處理 DB (RSSI) 回應：單 byte 無號值，單位 dBm"""
         pass
-    
+        # if data:
+            # print(f"[{self.serial_port}] RSSI = -{data[0]} dBm")  # dev
+            # logger.debug(f"[{self.serial_port}] RSSI = -{data[0]} dBm") # dev
+        
     def _handle_serial_high(self, data: bytes):
         """處理 SH (Serial Number High)"""
         pass
-    
+
     def _handle_serial_low(self, data: bytes):
         """處理 SL (Serial Number Low)"""
         pass
 
 
-# ====================== Serial Handler =====================
-
+# ================ Serial UDP Socket Object ==============
 class SerialHandler(asyncio.Protocol):
     """asyncio.Protocol 用於處理 Serial 收發"""
-    
+
     def __init__(self, udp_handler, serial_port_str, serial_mode: SerialMode):
         self.udp_handler = udp_handler  # UDP 的傳輸物件
         self.serial_port_str = serial_port_str
         self.serial_mode = serial_mode
         self.transport = None  # Serial 自己的傳輸物件
-        
-        # 根據模式創建對應的 processor
-        self.processor = self._create_processor(serial_mode)
-        
-        # AT 指令處理器（僅 XBee 模式使用）
-        if serial_mode == SerialMode.XBEEAPI2AT:
-            self.at_handler = ATCommandHandler(serial_port_str)
-        else:
-            self.at_handler = None
 
-    def _create_processor(self, serial_mode: SerialMode) -> FrameProcessor:
-        """工廠方法：根據模式創建處理器"""
-        if serial_mode == SerialMode.STRAIGHT:
-            return RawFrameProcessor()
-        elif serial_mode == SerialMode.XBEEAPI2AT:
-            return XBeeFrameProcessor()
-        else:
-            logger.warning(f"Unknown serial mode: {serial_mode}, using Raw")
+        # 根據模式建立 processor（需要 AT handler 時一併在工廠內建好注入）
+        self.processor = self._create_processor()
+
+    def _create_processor(self) -> FrameProcessor:
+        """工廠方法：根據 self.serial_mode 建立對應的 processor，必要時一併注入 AT handler"""
+        if self.serial_mode == SerialMode.STRAIGHT:
             return RawFrameProcessor()
 
+        if self.serial_mode == SerialMode.XBEEAPI2AT:
+            at_handler = ATCommandHandler(self.serial_port_str)
+            return XBeeFrameProcessor(at_handler=at_handler)
+
+        logger.warning(f"Unknown serial mode: {self.serial_mode}, using Raw")
+        return RawFrameProcessor()
+
     def connection_made(self, transport):
         """連接建立時的回調"""
         self.transport = transport
+
+        # XBee 模式下，將 transport.write 注入給 AT handler，讓它能送指令
+        if self.serial_mode == SerialMode.XBEEAPI2AT:
+            self.processor.at_handler.set_writer(self.transport.write)
+
         if hasattr(self.udp_handler, 'set_serial_handler'):
             self.udp_handler.set_serial_handler(self)
         logger.debug(f"Serial port {self.serial_port_str} connected")
@@ -290,8 +398,6 @@ class SerialHandler(asyncio.Protocol):
             )
 
 
-# ====================== UDP Handler =====================
-
 class UDPHandler(asyncio.DatagramProtocol):
     """asyncio.DatagramProtocol 用於處理 UDP 收發"""
     
@@ -560,6 +666,33 @@ class serial_manager:
             ret[key] = obj.serial_port
         return ret
 
+    def send_at_command(self, serial_id, request: ATRequest) -> bool:
+        """
+        對指定 serial_id 的 XBee dongle 發送一筆 AT 指令（thread-safe）
+        - serial_id: create_serial_link 取得的編號
+        - request: ATRequest 物件，攜帶 command / parameter / frame_id
+        回傳是否成功排進事件圈
+        """
+        if not self.running or not self.loop:
+            logger.error("Event loop not running, cannot send AT command")
+            return False
+
+        if serial_id not in self.serial_objects:
+            logger.error(f"Serial object {serial_id} not found")
+            return False
+
+        serial_obj = self.serial_objects[serial_id]
+        if serial_obj.serial_mode != SerialMode.XBEEAPI2AT:
+            logger.error(
+                f"Serial {serial_id} mode is {serial_obj.serial_mode.name}, "
+                f"AT command only supported in XBEEAPI2AT mode"
+            )
+            return False
+
+        at_handler = serial_obj.serial_handler.processor.at_handler
+        self.loop.call_soon_threadsafe(at_handler.send_command, request)
+        return True
+
     @staticmethod
     def check_serial_port(serial_port, baudrate):
         """檢查串口是否存在與可用"""
@@ -597,20 +730,42 @@ if __name__ == '__main__':
     sm = serial_manager()
     sm.start()
 
-    SERIAL_PORT = '/dev/ttyUSB0'  # 手動指定
-    SERIAL_BAUDRATE = 115200
-    UDP_REMOTE_PORT = 14571
-    sm.create_serial_link(SERIAL_PORT, SERIAL_BAUDRATE, UDP_REMOTE_PORT, SerialMode.XBEEAPI2AT)
-
     # SERIAL_PORT = '/dev/ttyACM0'  # 手動指定
     # SERIAL_BAUDRATE = 115200
     # UDP_REMOTE_PORT = 14571
     # sm.create_serial_link(SERIAL_PORT, SERIAL_BAUDRATE, UDP_REMOTE_PORT, SerialMode.STRAIGHT)
 
+    SERIAL_PORT = '/dev/ttyUSB0'  # 手動指定
+    SERIAL_BAUDRATE = 115200
+    UDP_REMOTE_PORT = 14561
+    sm.create_serial_link(SERIAL_PORT, SERIAL_BAUDRATE, UDP_REMOTE_PORT, SerialMode.XBEEAPI2AT)
+
     linked_serial = sm.get_serial_link()
     print(linked_serial)
-    time.sleep(60)
+
+    # 等 connection_made 完成 writer 注入，再發一筆 AT 指令測試
+    time.sleep(5)
+    rssi_request = ATRequest(command=b'DB', parameter=b'', frame_id=0x52)
+    for i in range(60):
+        sm.send_at_command(1, rssi_request)
+        time.sleep(1)
 
     sm.remove_serial_link(1)
     time.sleep(3)
     sm.shutdown()
+
+
+
+'''
+================= 改版記錄 ============================
+2026.4.20
+1. XBeeFrameHandler 結構移除
+2. XBeeFrameProcessor 新增 _encapsulate, _decapsulate 編碼解碼 xbee 封包的功能 (原來在 XBeeFrameHandler 中)
+3. XBeeFrameProcessor 新增 _try_extract_frame 處理被可能截斷的 UART 封包
+4. XBeeFrameProcessor 新增 _dispatch_frame 分配封包到 UDP 或者 Dongle Command Handler 
+5. ATCommandHandler 新增 _parse 去拆解 0x88 AT Command Response
+6. ATCommandHandler 新增 _dispatch 把拆解的結果 分配到 _handle_XXX
+7. ATCommandHandler 新增各項 _handle_XXX (未實作)
+
+'''
+

src/fc_network_apps/__init__.py

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

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,82 @@ 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:
-    """Return value for mode change requests."""
-
+class CommandLongResult:
     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()
-        super().__init__("fc_change_mode_client")
+    def __init__(self, service_name: str = DEFAULT_SERVICE_NAME, node_name: str = None) -> None:
+        if not rclpy.ok():
+            rclpy.init(args=None)
+        # 使用提供的 node_name，或使用帶時間戳的預設名稱以避免名稱衝突
+        if node_name is None:
+            import time
+            node_name = f"fc_command_long_client_{int(time.time() * 1000) % 100000}"
+        super().__init__(node_name)
         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 +96,286 @@ 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:
+        return self._send_command_long(
+            target_sysid=target_sysid,
+            target_compid=target_compid,
+            command=COMMAND_DO_SET_MODE,
+            confirmation=confirmation,
+            param1=float(base_mode),
+            param2=float(custom_mode),
+            param3=0.0,
+            param4=0.0,
+            param5=0.0,
+            param6=0.0,
+            param7=0.0,
+            timeout_sec=timeout_sec,
+        )
 
+    def takeoff(
+        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:
+        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 方法（不使用 ThreadPoolExecutor）
+    # 使用 asyncio 的 polling 機制，避免在線程中調用 spin_until_future_complete
+    # ============================================================================
+
+    async def _send_command_long_async(
+        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:
+        """非阻塞的 async 版本 - 使用 asyncio polling 而非 spin"""
         req = MavCommandLong.Request()
         req.target_sysid = target_sysid
         req.target_compid = target_compid
-        req.command = COMMAND_DO_SET_MODE
+        req.command = command
         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.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 ChangeModeResult(
-                success=False,
-                message="Service call timeout or no response.",
-                ack_result=-1,
-            )
+        
+        # 使用 asyncio.sleep 進行 polling，而非 spin_until_future_complete
+        # 使用 10ms 輪詢間隔以匹配 GUI 執行器的 spin_once() 頻率
+        timeout = timeout_sec + 1.0
+        elapsed = 0.0
+        poll_interval = 0.01  # 10ms - 與 GUI timer 頻率一致
+        
+        while elapsed < timeout:
+            if future.done():
+                try:
+                    response = future.result()
+                    if response is None:
+                        return CommandLongResult(
+                            success=False,
+                            message="Service returned None.",
+                            ack_result=-1,
+                        )
+                    return CommandLongResult(
+                        success=response.success,
+                        message=response.message,
+                        ack_result=response.ack_result,
+                    )
+                except Exception as e:
+                    return CommandLongResult(
+                        success=False,
+                        message=f"Error getting result: {e}",
+                        ack_result=-1,
+                    )
+            
+            # 讓出控制權給事件循環，允許 GUI executor 執行
+            await asyncio.sleep(poll_interval)
+            elapsed += poll_interval
+        
+        return CommandLongResult(
+            success=False,
+            message=f"Service timeout after {timeout}s.",
+            ack_result=-1,
+        )
 
-        response = future.result()
-        return ChangeModeResult(
-            success=response.success,
-            message=response.message,
-            ack_result=response.ack_result,
-        )
+    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"""
+        return await self._send_command_long_async(
+            target_sysid=target_sysid,
+            target_compid=target_compid,
+            command=COMMAND_DO_SET_MODE,
+            confirmation=confirmation,
+            param1=float(base_mode),
+            param2=float(custom_mode),
+            param3=0.0,
+            param4=0.0,
+            param5=0.0,
+            param6=0.0,
+            param7=0.0,
+            timeout_sec=timeout_sec,
+        )
+
+    async def arm_disarm_async(
+        self,
+        *,
+        target_sysid: int,
+        arm: bool,
+        target_compid: int = 0,
+        confirmation: int = 0,
+        param2: float = 0.0,
+        timeout_sec: float = DEFAULT_TIMEOUT_SEC,
+    ) -> CommandLongResult:
+        """非阻塞 async 版本的 arm_disarm"""
+        return await self._send_command_long_async(
+            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 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"""
+        return await self._send_command_long_async(
+            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,
+        )
+
+    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"""
+        return await self._send_command_long_async(
+            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,
+        )

src/fc_network_apps/navigation.py

@@ -0,0 +1,418 @@
+"""ROS2 client for fc_network MavPositionTargetGlobalInt service (SET_POSITION_TARGET_GLOBAL_INT / Offboard)."""
+
+from __future__ import annotations
+
+import asyncio
+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, node_name: str = None) -> None:
+        if not rclpy.ok():
+            rclpy.init(args=None)
+        if node_name is None:
+            import time
+            node_name = f"fc_position_target_global_int_client_{int(time.time() * 1000) % 100000}"
+        super().__init__(node_name)
+        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,
+        )
+
+    # ============================================================================
+    # 非阻塞 async 版本（asyncio polling，對齊 longCommand.py 的模板）
+    # 避免在 Qt 事件迴圈或主執行緒中呼叫 spin_until_future_complete
+    # ============================================================================
+
+    async def _send_position_target_global_int_async(
+        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:
+        """非阻塞 async 版本 - 使用 asyncio polling 而非 spin"""
+        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)
+
+        timeout = float(timeout_sec) + 1.0
+        elapsed = 0.0
+        poll_interval = 0.01  # 10ms - 與 GUI timer 頻率一致
+
+        while elapsed < timeout:
+            if future.done():
+                try:
+                    resp = future.result()
+                    if resp is None:
+                        return PositionTargetGlobalIntResult(
+                            success=False,
+                            message="Service returned None.",
+                            ack_result=-1,
+                        )
+                    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,
+                    )
+                    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),
+                    )
+                except Exception as e:
+                    return PositionTargetGlobalIntResult(
+                        success=False,
+                        message=f"Error getting result: {e}",
+                        ack_result=-1,
+                    )
+
+            await asyncio.sleep(poll_interval)
+            elapsed += poll_interval
+
+        return PositionTargetGlobalIntResult(
+            success=False,
+            message=f"Service timeout after {timeout}s.",
+            ack_result=-1,
+        )
+
+    async def goto_position_only_async(
+        self,
+        *,
+        target_sysid: int,
+        target_compid: int = 0,
+        latitude_deg: float,
+        longitude_deg: float,
+        alt: float = 0.0,
+        timeout_sec: float = DEFAULT_TIMEOUT_SEC,
+    ) -> PositionTargetGlobalIntResult:
+        """非阻塞 async 版本的 goto_position_only"""
+        lat_int = int(round(latitude_deg * 1e7))
+        lon_int = int(round(longitude_deg * 1e7))
+
+        return await self._send_position_target_global_int_async(
+            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/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
+sudo tcpdump -i lo -X udp port 14550
+
+幾個要討論的
+1. 專案文件中 .mat 檔案是幹嘛的?
+2. GUI 的 print [Panel/Map Update] 11 Hz 希望關閉
+3. readme 那串文字來源? 用途?
+4. GUI介面 加一下海拔高 跟 相對高 速度跟位置 變更為 XY速度 XY位置
+5. pitch yaw roll 出來了 弄一下
+6.

src/unitdev02/unitdev02/lifecycle_node.py

@@ -1,45 +0,0 @@
-
-
-
-import rclpy
-from rclpy.node import Node
-from std_msgs.msg import String
-import time
-
-# import mavros_msgs.srv
-
-class TalkerNode(Node):
-    def __init__(self):
-        start_time = time.time()
-        super().__init__('talker_node')
-        end_time = time.time()
-        print(f"Node initialization took {end_time - start_time:.2f} seconds")
-
-        self.publisher_ = self.create_publisher(String, 'hahatest/_1', 10)
-        self.timer = self.create_timer(1.0, self.timer_callback)  # 每秒執行一次
-        self.get_logger().info('TalkerNode has been started.')
-
-    def timer_callback(self):
-        msg = String()
-        msg.data = 'Hello, ROS 2!'
-        self.publisher_.publish(msg)
-        self.get_logger().info(f'Published: "{msg.data}"')
-
-def main(args=None):
-    rclpy.init(args=args)
-    node = TalkerNode()
-
-    print("Before sleep")
-    time.sleep(5)  # 等待 5 秒鐘
-    print("After sleep")
-    try:
-        start_time = time.time()
-        while time.time() - start_time < 10:  # 持續 10 秒鐘
-            rclpy.spin_once(node)
-            time.sleep(1)  # 每秒執行一次
-    finally:
-        node.destroy_node()
-        rclpy.shutdown()
-
-if __name__ == '__main__':
-    main()

src/unitdev02/unitdev02/ping_test.py

@@ -0,0 +1,188 @@
+import time
+from pymavlink import mavutil
+
+# # 強制 source_system=255
+# master = mavutil.mavlink_connection('udpout:localhost:14561', source_system=255)
+
+# print("Starting Force-Ping Test...")
+
+# while True:
+#     # 1. 發送 Heartbeat (證明連線活著)
+#     master.mav.heartbeat_send(mavutil.mavlink.MAV_TYPE_GCS, 
+#                              mavutil.mavlink.MAV_AUTOPILOT_INVALID, 0, 0, 0)
+    
+#     # 2. 強制發送 PING (target 1, 1)
+#     # 我們加一個計數器，方便在 tcpdump 裡辨識
+#     ping_seq = int(time.time()) % 100
+#     master.mav.ping_send(int(time.time() * 1e6), ping_seq, 1, 1)
+    
+#     print(f"Sent Heartbeat and PING (seq {ping_seq})")
+    
+#     # 3. 非阻塞讀取，避免卡死
+#     msg = master.recv_match(blocking=False)
+#     if msg:
+#         print(f"--> Received something: {msg.get_type()}")
+        
+#     time.sleep(1) # 每秒噴一次
+
+# -------------------------------------------------------------------
+
+# # 改成 udpin，這會讓你的程式守在 14561 等模擬器送資料進來
+# # 這樣你就能收到模擬器的 Heartbeat，建立連線後再發 PING
+# master = mavutil.mavlink_connection('udpin:localhost:14561', source_system=255)
+
+# print("Waiting for simulator heartbeat...")
+# # 這一行很重要：先收到 Heartbeat，pymavlink 才會設定好 target 地址
+# master.wait_heartbeat()
+# print(f"Heartbeat from system {master.target_system} component {master.target_component}")
+
+# while True:
+#     # 既然收到了，現在發 PING 就會順著原路回去
+#     master.mav.ping_send(int(time.time() * 1e6), 1, master.target_system, master.target_component)
+#     print("PING sent!")
+    
+#     msg = master.recv_match(type='PING', blocking=True, timeout=2)
+#     if msg:
+#         print(f"Got PING response: {msg}")
+#     time.sleep(1)
+
+
+# -------------------------------------------------------------------
+
+# import time
+# from pymavlink import mavutil
+
+# # 核心修改：因為 MAVProxy 正在往 14561 噴資料，我們必須「接住」它
+# # 使用 udpin 會讓你的程式在 14561 監聽
+# master = mavutil.mavlink_connection('udpin:localhost:14561', source_system=254)
+
+# print("正在等待 MAVProxy 轉發的 Heartbeat...")
+
+# # 只要這行跑過，代表你跟 MAVProxy 握手成功了
+# master.wait_heartbeat()
+# print(f"成功連線到系統 {master.target_system}！")
+
+# while True:
+#     # 1. 務必發送 Heartbeat，讓 MAVProxy 知道要把回應送回這個 Port
+#     master.mav.heartbeat_send(
+#         mavutil.mavlink.MAV_TYPE_GCS, 
+#         mavutil.mavlink.MAV_AUTOPILOT_INVALID, 
+#         0, 0, 0
+#     )
+
+#     # 2. 修改 target_component 為 1 (飛控核心)
+#     # 注意：這裡刻意將 seq 設為動態，方便你在 watch PING 裡觀察
+#     p_seq = int(time.time()) % 255
+#     master.mav.ping_send(
+#         int(time.time() * 1e6), 
+#         p_seq, 
+#         1, 
+#         1  # <--- 改成 1 試試看！
+#     )
+#     print(f"Sent PING seq {p_seq} to 1/1")
+
+#     # 3. 讀取所有訊息，看看有沒有 PING 回來
+#     # 有時候回應的 target_system 可能是 255 (你的 ID)
+#     msg = master.recv_match(type='PING', blocking=True, timeout=1.0)
+#     if msg:
+#         print(f"🎉 成功！收到回應: {msg}")
+    
+#     time.sleep(1)
+
+
+
+# -------------------------------------------------------------------
+
+
+# import time
+# from pymavlink import mavutil
+
+# # 建議一樣用 udpin 監聽 MAVProxy 的輸出
+# master = mavutil.mavlink_connection('udpin:localhost:14561', source_system=255)
+
+# def get_time_ns():
+#     return int(time.time() * 1e9) # 轉為奈秒
+
+# print("開始測試 TIMESYNC 響應...")
+
+# while True:
+#     now_ns = get_time_ns()
+    
+#     # 發送 TIMESYNC 請求
+#     # tc1=0 代表這是請求，ts1=當前時間
+#     master.mav.timesync_send(0, now_ns)
+    
+#     # 等待回應
+#     msg = master.recv_match(type='TIMESYNC', blocking=True, timeout=1.0)
+    
+#     if msg and msg.tc1 != 0:
+#         # 收到回應，msg.tc1 應該等於我們剛才送出的 now_ns
+#         rtt_ms = (get_time_ns() - msg.tc1) / 1e6
+#         print(f"🕒 TIMESYNC RTT: {rtt_ms:.3f} ms")
+#         break # 測試一次就好
+#     else:
+#         print("等待 TIMESYNC 回應中...")
+        
+#     time.sleep(1)
+
+# -------------------------------------------------------------------
+
+import time
+import statistics
+from pymavlink import mavutil
+
+# 連接到 MAVProxy 轉發的端口
+master = mavutil.mavlink_connection('udpin:localhost:14561', source_system=255)
+
+# 測試設定
+TARGET_SYS = 2  # 你想測試的載具 ID
+SAMPLES = 20    # 取 20 次樣本來算平均和抖動
+rtt_history = []
+
+print(f"開始測試對 System {TARGET_SYS} 的通道品質...")
+
+def get_time_ns():
+    return int(time.time() * 1e9)
+
+try:
+    while len(rtt_history) < SAMPLES:
+        ts1_send = get_time_ns()
+        
+        # 發送 TIMESYNC (V1/V2 格式相容)
+        # 針對特定系統發送
+        master.mav.timesync_send(0, ts1_send)
+        
+        # 等待回應 (過濾目標系統的訊息)
+        msg = master.recv_match(type='TIMESYNC', blocking=True, timeout=1.0)
+        
+        if msg and msg.get_srcSystem() == TARGET_SYS and msg.tc1 != 0:
+            ts1_recv = get_time_ns()
+            rtt_ns = ts1_recv - msg.tc1
+            rtt_ms = rtt_ns / 1e6
+            rtt_history.append(rtt_ms)
+            print(f"Sample {len(rtt_history)}: RTT = {rtt_ms:.3f} ms")
+        else:
+            print("Request timeout or mismatched ID...")
+            
+        time.sleep(0.1) # 快速採樣
+
+    # 4. 計算統計數據
+    avg_latency = statistics.mean(rtt_history)
+    std_dev = statistics.stdev(rtt_history) # 這就是 Jitter 的一種表現
+    max_rtt = max(rtt_history)
+    min_rtt = min(rtt_history)
+
+    print("\n--- 物理通道品質分析報告 ---")
+    print(f"目標載具  : System {TARGET_SYS}")
+    print(f"平均延遲 (Avg) : {avg_latency:.3f} ms")
+    print(f"最小延遲 (Min) : {min_rtt:.3f} ms")
+    print(f"最大延遲 (Max) : {max_rtt:.3f} ms")
+    print(f"抖動幅度 (Jitter): {std_dev:.3f} ms (標準差)")
+    
+    if std_dev > 10:
+        print("警告：網路抖動過高，可能影響控制穩定性！")
+    else:
+        print("通道品質：良好")
+
+except KeyboardInterrupt:
+    print("測試終止。")

src/unitdev02/unitdev02/pymav_nullbuffer.py

@@ -0,0 +1,46 @@
+from pymavlink.dialects.v20 import common as mavlink
+
+class NullBuffer:
+    def write(self, data): pass
+    def seek(self, pos): pass
+    def tell(self): return 0
+
+class CapturingBuffer:
+    def __init__(self):
+        self.last_data = b''
+    def write(self, data):
+        self.last_data = bytes(data)
+    def seek(self, pos): pass
+    def tell(self): return 0
+
+# 1. 初始化（僅作為編碼器）
+# file 參數可以是任何擁有 write() 方法的物件，這裡用 BytesIO 模擬
+# 初始化方法一：使用 BytesIO
+# import io
+# out_buf = io.BytesIO()
+# mav = mavlink.MAVLink(out_buf, srcSystem=1, srcComponent=191)
+
+# 初始化方法二：使用自定義的 NullBuffer
+mav = mavlink.MAVLink(NullBuffer(), srcSystem=1, srcComponent=191)
+mav.seq = 254
+
+# 2. 建立心跳包並取得二進制數據
+# 連續產出「不同」的訊息物件
+for i in range(3):
+    msg = mav.heartbeat_encode(mavlink.MAV_TYPE_GCS, 0, 0, 0, 0)
+    data = msg.pack(mav)
+    mav.seq = (mav.seq + 1) & 0xFF
+    # MAVLink 2 的序列號在第 3 個 Byte (Index 2)
+    print(f"第 {i} 次發送, Seq: {data[4]}, Raw: {data.hex()}")
+
+
+
+print("分隔線")
+
+buf = CapturingBuffer()
+mav_buf2 = mavlink.MAVLink(buf, srcSystem=1, srcComponent=191)
+
+for i in range(3):
+    mav_buf2.heartbeat_send(mavlink.MAV_TYPE_GCS, 0, 0, 0, 0)
+    data = buf.last_data
+    print(f"第 {i} 次發送, Seq: {data[4]}, Raw: {data.hex()}")

src/unitdev02/unitdev02/serial_udp_ad.py

@@ -0,0 +1,71 @@
+import asyncio
+import serial_asyncio
+
+SERIAL_PORT = '/dev/ttyUSB0'   # 修改為你的 serial port
+SERIAL_BAUDRATE = 115200       # 修改為你的 baudrate
+UDP_REMOTE_IP = '192.168.1.100' # 修改為目標 IP
+UDP_REMOTE_PORT = 5005         # 修改為目標 port
+UDP_LOCAL_PORT = 5006          # 本地 UDP 監聽 port
+
+class SerialToUDP(asyncio.Protocol):
+    def __init__(self, udp_transport):
+        self.udp_transport = udp_transport
+
+    def connection_made(self, transport):
+        self.transport = transport
+
+    def data_received(self, data):
+        # Serial 收到資料，轉發到 UDP
+        self.udp_transport.sendto(data, (UDP_REMOTE_IP, UDP_REMOTE_PORT))
+
+    def write_to_serial(self, data):
+        self.transport.write(data)
+
+class UDPToSerial(asyncio.DatagramProtocol):
+    def __init__(self, serial_proto):
+        self.serial_proto = serial_proto
+
+    def datagram_received(self, data, addr):
+        # UDP 收到資料，轉發到 Serial
+        self.serial_proto.write_to_serial(data)
+
+async def main():
+    loop = asyncio.get_running_loop()
+
+    # 定義協議工廠函數
+    def create_empty_protocol():
+        return asyncio.DatagramProtocol()
+
+    # 建立 UDP1 傳輸
+    udp_transport, _ = await loop.create_datagram_endpoint(
+        create_empty_protocol,
+        local_addr=('0.0.0.0', UDP_LOCAL_PORT)
+    )
+
+    # 建立 Serial 傳輸
+    serial_proto = SerialToUDP(udp_transport)
+    def get_serial_protocol():
+        return serial_proto
+
+    _, serial_transport = await serial_asyncio.create_serial_connection(
+        loop, get_serial_protocol, SERIAL_PORT, baudrate=SERIAL_BAUDRATE
+    )
+
+    # 建立 UDP2 監聽
+    udp_proto = UDPToSerial(serial_proto)
+    def get_udp_protocol():
+        return udp_proto
+
+    udp_listen_transport, _ = await loop.create_datagram_endpoint(
+        get_udp_protocol,
+        local_addr=('0.0.0.0', UDP_LOCAL_PORT)
+    )
+
+    # 保持運行
+    try:
+        await asyncio.Future()
+    except asyncio.CancelledError:
+        pass
+
+if __name__ == '__main__':
+    asyncio.run(main())

src/unitdev02/unitdev02/sslChech.sh

@@ -1,27 +0,0 @@
-#!/bin/bash
-
-# 網站清單
-DOMAINS=("google.com" "smarter.nchu.edu.tw")
-
-echo "網站 SSL 憑證剩餘天數："
-echo "---------------------------"
-
-for domain in "${DOMAINS[@]}"; do
-    end_date=$(echo | openssl s_client -servername "$domain" -connect "$domain:443" 2>/dev/null |
-        openssl x509 -noout -enddate | cut -d= -f2)
-
-    end_timestamp=$(date -d "$end_date" +%s)
-    now_timestamp=$(date +%s)
-
-    remaining_days=$(( (end_timestamp - now_timestamp) / 86400 ))
-
-    if [ $remaining_days -lt 0 ]; then
-        status="已過期 ❌"
-    elif [ $remaining_days -lt 15 ]; then
-        status="即將到期 ⚠️"
-    else
-        status="正常 ✅"
-    fi
-
-    printf "%-20s 到期日：%-25s 剩餘天數：%3d 天 %s\n" "$domain" "$end_date" "$remaining_days" "$status"
-done

src/unitdev02/unitdev02/udp_unix_ad.py

@@ -0,0 +1,40 @@
+import socket
+import sys
+import select
+
+# 設定來源 IP 與 Port
+SRC_IP = '127.0.0.1'    # 監聽所有介面
+SRC_PORT = 16661      # 請自行修改
+
+# 建立 UDP 監聽 socket
+src_sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
+src_sock.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1)
+src_sock.bind((SRC_IP, SRC_PORT))
+print(f"Listening for UDP on {SRC_IP}:{SRC_PORT}")
+
+# 設定目標 Unix socket 路徑
+UNIX_SOCKET_PATH = '/tmp/unix_socket_mavlink.sock'  # 請自行修改
+
+# 建立 Unix socket 連線
+unix_sock = socket.socket(socket.AF_UNIX, socket.SOCK_STREAM)
+try:
+    unix_sock.connect(UNIX_SOCKET_PATH)
+except Exception as e:
+    print(f"Failed to connect to unix socket: {e}")
+    sys.exit(1)
+
+while True:
+    # 使用 select 監聽兩個 socket
+    readable, _, _ = select.select([src_sock, unix_sock], [], [])
+    for sock in readable:
+        if sock is src_sock:
+            data, addr = src_sock.recvfrom(4096)
+            if data:
+                unix_sock.sendall(data)
+                # print(f"Received UDP data from {addr}: {data}") # debug
+                # break # debug
+        elif sock is unix_sock:
+            data = unix_sock.recv(4096)
+            if data:
+                # 回送到最近收到資料的 UDP client
+                src_sock.sendto(data, addr)

src/unitdev04/esp32.py

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

src/unitdev04/test01.py

@@ -0,0 +1,229 @@
+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
+
+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("markA ------")
+    print(msg.get_type())
+    print(msg.ordered_fieldnames)
+    print("markA ------ END")
+
+    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)
+            packet_count += 1
+        except socket.timeout:
+            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 # [0 0 0 0 0 0 0 0 0.....]
+    data_queue.put(servo_out)
+
+    def send_data_from_queue(sock, server_address, q, frame_rate_fdm=1, frame_count_fdm=1):
+        interval = 1.0 / frame_rate_fdm
+        while True:
+            start_time = time.time()
+            try:
+                data = q.get(timeout=0.1) # [0 0 0 0 0 0 0 0 0.....]
+                if data is None:
+                    break
+                last_data = data
+            except queue.Empty:
+                data = last_data
+            data_fdm = struct.pack('HHI16H', 0x481a, int(frame_rate_fdm * 0.1), frame_count_fdm, *servo_out) # [ FMD ]
+            sock.sendto(data_fdm, server_address)
+            frame_count_fdm += 3
+            # fdm_parser_example(data_fdm) # 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()
+    print('Start sending data to Gazebo')
+
+    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)
+
+from pymavlink.dialects.v20 import common  # 使用 MAVLink 2.0
+
+def mavlink_btye_generator_test():
+
+    # 創建一個 MAVLink 對象
+    mav = common.MAVLink(None)  # 不透過 connection，直接使用 None
+
+    # 創建一個 HEARTBEAT 訊息
+    msg = mav.heartbeat_encode(
+        type=mavutil.mavlink.MAV_TYPE_QUADROTOR,
+        autopilot=mavutil.mavlink.MAV_AUTOPILOT_GENERIC,
+        base_mode=0,
+        custom_mode=0,
+        system_status=mavutil.mavlink.MAV_STATE_ACTIVE
+    )
+
+    # 取得 MAVLink 訊息的 bytes
+    mavlink_bytes = msg.pack(mav)
+
+    # 回傳 bytes
+    print(mavlink_bytes)
+    print(type(mavlink_bytes))
+
+    mav2 = common.MAVLink(None)
+    # my_msg = mav2.decode(mavlink_bytes)
+    print("========")
+    print(dir(mav2))
+    # print(my_msg)
+
+
+
+print('Start')
+# fdm_switch_example_two()
+# fdm_parser_example()
+# mavlink_analyzer_simple(8)
+mavlink_btye_generator_test()

src/unitdev04/udptest4.py

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