學長建議修改內容

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

README.md

@@ -1,7 +1,5 @@
 這是天雷系統的專案
 
-
-
 ===
 ## 功能簡介
 1. mavlink 多對多支援平台

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

@@ -13,10 +13,11 @@ 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 導入）
@@ -588,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, ...
@@ -636,6 +654,12 @@ 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
             )
         }
         
@@ -879,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, {
@@ -959,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/gui.py

@@ -33,7 +33,7 @@ from mission_group import (
 # ================================================================================
 
 class ControlStationUI(QMainWindow):
-    VERSION = '2.0.6'
+    VERSION = '2.0.8'
     
     def __init__(self):
         super().__init__()
@@ -450,11 +450,17 @@ class ControlStationUI(QMainWindow):
         loop.create_task(self.handle_service_response(future, f"起飛 {drone_id}"))
 
     def handle_setpoint_selected(self):
+        """發送位置命令到 active group 的所有選中無人機"""
+        group = self._get_active_group()
+        if not group:
+            self.statusBar().showMessage("⚠ 請先建立任務群組", 3000)
+            return
+        
         try:
             x = float(self.x_input.text() or '0')
             y = float(self.y_input.text() or '0')
             z = float(self.z_input.text() or '0')
-            for drone_id in self.monitor.selected_drones:
+            for drone_id in group.selected_drone_ids:
                 if self.monitor.send_setpoint(drone_id, x, y, z):
                     self.statusBar().showMessage(f"發送位置命令到 {drone_id}: ({x}, {y}, {z})", 3000)
                 else:
@@ -507,8 +513,18 @@ class ControlStationUI(QMainWindow):
             self.statusBar().showMessage(f"{action} 錯誤: {str(e)}", 3000)
 
     def handle_arm_selected(self):
+        """解鎖 active group 的所有選中無人機"""
+        group = self._get_active_group()
+        if not group:
+            self.statusBar().showMessage("⚠ 請先建立任務群組", 3000)
+            return
+        
+        selected = list(group.selected_drone_ids)
+        if not selected:
+            self.statusBar().showMessage("⚠ 尚未選擇任何無人機", 3000)
+            return
+        
         print(f"\n📢 [GUI] handle_arm_selected 被調用")
-        selected = list(self.monitor.selected_drones)
         print(f"   已選擇的無人機: {selected}")
         loop = asyncio.get_event_loop()
         for drone_id in selected:
@@ -523,8 +539,19 @@ class ControlStationUI(QMainWindow):
         print(f"   handle_arm_selected 完成\n")
 
     def handle_takeoff_selected(self):
+        """起飛 active group 的所有選中無人機"""
+        group = self._get_active_group()
+        if not group:
+            self.statusBar().showMessage("⚠ 請先建立任務群組", 3000)
+            return
+        
+        selected = list(group.selected_drone_ids)
+        if not selected:
+            self.statusBar().showMessage("⚠ 尚未選擇任何無人機", 3000)
+            return
+        
         loop = asyncio.get_event_loop()
-        for drone_id in self.monitor.selected_drones:
+        for drone_id in selected:
             future = self.monitor.takeoff_drone(drone_id, 10.0)
             loop.create_task(self.handle_service_response(future, f"批次起飛 {drone_id}"))
 
@@ -584,79 +611,175 @@ class ControlStationUI(QMainWindow):
         scroll.setWidgetResizable(True)
         idx = self.group_tab_widget.addTab(scroll, f"Group {gid}")
         self.group_tab_widget.tabBar().setTabTextColor(idx, QColor(color))
+
+        # 切換到新 group 的 tab
         self.group_tab_widget.setCurrentIndex(idx)
-        self.active_group_id = gid
+
         self.statusBar().showMessage(f"已新增 Group {gid}", 2000)
 
         # 更新刪除按鈕的啟用/禁用狀態
         self._update_delete_buttons_state()
 
     def _on_group_tab_changed(self, index):
-        """Tab 切換時更新 active group 並同步地圖模式"""
+        """切換 group tab — 只需切換 active group 並刷新 UI"""
         if index < 0:
             self.active_group_id = None
             return
-        # tab 標題是 "Group X"
+
         tab_text = self.group_tab_widget.tabText(index)
         gid = tab_text.replace("Group ", "")
-        if gid in self.mission_groups:
+        if gid not in self.mission_groups:
+            return
+
         self.active_group_id = gid
         group = self.mission_groups[gid]
+
+        # 同步地圖的任務模式
         self.drone_map.set_mission_mode(group.mission_type)
 
+        # 統一刷新所有 UI
+        self.refresh_selection_ui()
+
     def _get_active_group(self):
         """取得當前 active 的 MissionGroup"""
         if self.active_group_id and self.active_group_id in self.mission_groups:
             return self.mission_groups[self.active_group_id]
         return None
 
-    def _get_other_assigned(self, exclude_gid):
+    def refresh_selection_ui(self):
-        """取得其他群組已佔用的無人機 {drone_id: group_id}"""
+        """統一刷新所有 UI 元素：左側 drone 勾選、左側 socket 勾選、右側 group panel"""
-        assigned = {}
+        group = self._get_active_group()
-        for gid, group in self.mission_groups.items():
+        selected = group.selected_drone_ids if group else set()
-            if gid == exclude_gid:
+        current_group_id = group.group_id if group else None
-                continue
+
-            for did in group.drone_ids:
+        # 左側 drone checkbox
-                assigned[did] = gid
+        for drone_id, panel in self.drones.items():
-        return assigned
+            checkbox = panel.get_checkbox()
+            if checkbox:
+                blocked = (
+                    current_group_id is not None and
+                    self._is_drone_selected_by_other_group(drone_id, current_group_id)
+                )
+                checkbox.blockSignals(True)
+                checkbox.setStyleSheet(self._get_drone_checkbox_style(blocked))
+                checkbox.setChecked(drone_id in selected)
+                checkbox.setEnabled(not blocked or drone_id in selected)
+                checkbox.blockSignals(False)
+
+        # 左側 socket checkbox
+        for socket_id in self.socket_groups.keys():
+            self.refresh_socket_checkbox(socket_id, selected, current_group_id)
+
+        # 右側 group panel
+        if group:
+            panel = self.group_panels.get(group.group_id)
+            if panel:
+                panel.update_drone_list()
+                panel.update_status()
+
+    def _get_drone_checkbox_style(self, blocked=False):
+        """回傳 drone checkbox 樣式；被其他 group 佔用時顯示紅色填滿。"""
+        blocked_style = """
+            QCheckBox::indicator:disabled {
+                background-color: #D32F2F;
+                border: 2px solid #A32020;
+            }
+        """ if blocked else ""
+        return f"""
+            QCheckBox {{
+                color: #DDD;
+            }}
+            QCheckBox::indicator {{
+                width: 16px;
+                height: 16px;
+                border: 2px solid #888888;
+                border-radius: 3px;
+                background: transparent;
+            }}
+            QCheckBox::indicator:checked {{
+                background-color: #7FFFD4;
+                border: 2px solid #888888;
+            }}
+            {blocked_style}
+        """
+
+    def refresh_socket_checkbox(self, socket_id, selected_ids, current_group_id=None):
+        """根據 selected_ids 推導並更新 socket 的勾選狀態"""
+        drone_ids = [did for did in self.drones.keys() if self.get_socket_id(did) == socket_id]
+        if not drone_ids:
+            return
+
+        socket_widget = self.socket_groups.get(socket_id)
+        if not socket_widget:
+            return
+
+        checkbox = socket_widget.findChild(QCheckBox, f"socket_{socket_id}_checkbox")
+        if not checkbox:
+            return
+
+        effective_drone_ids = [
+            did for did in drone_ids
+            if current_group_id is None
+            or did in selected_ids
+            or not self._is_drone_selected_by_other_group(did, current_group_id)
+        ]
+        checked_count = sum(1 for did in effective_drone_ids if did in selected_ids)
+
+        checkbox.blockSignals(True)
+        if checked_count == 0:
+            checkbox.setCheckState(Qt.CheckState.Unchecked)
+        elif checked_count == len(effective_drone_ids):
+            checkbox.setCheckState(Qt.CheckState.Checked)
+        else:
+            checkbox.setCheckState(Qt.CheckState.PartiallyChecked)
+        checkbox.setEnabled(len(effective_drone_ids) > 0)
+        checkbox.blockSignals(False)
+
+
+    # 【已遷移至 refresh_socket_checkbox()】舊的 update_group_checkbox_state() 已廢棄
 
     def _handle_assign_drones(self, group_id):
-        """開啟無人機分配對話框（已勾選的 checkbox 會預先帶入）"""
+        """開啟無人機分配對話框 — 過濾被占用的drone"""
         group = self.mission_groups.get(group_id)
         if not group:
             return
         all_ids = list(self.drones.keys())
-        other_assigned = self._get_other_assigned(group_id)
 
-        # 將目前 checkbox 已勾選的無人機（且未被其他群組佔用）合併進 pre-selected
+        # 預先選中：目前 group 的 selected_drone_ids
-        currently_checked = self.get_selected_drones()
+        pre_selected = set(group.selected_drone_ids)
-        pre_selected = set(group.drone_ids)
-        for did in currently_checked:
-            if did not in other_assigned:
-                pre_selected.add(did)
 
-        dialog = DroneAssignDialog(self, all_ids, pre_selected, other_assigned)
+        dialog = DroneAssignDialog(self, all_ids, pre_selected, {})
         if dialog.exec() == QDialog.DialogCode.Accepted:
-            group.drone_ids = dialog.get_selected()
+            requested = dialog.get_selected()
+
+            allowed = set()
+            blocked = set()
+
+            for did in requested:
+                if self._is_drone_selected_by_other_group(did, group_id):
+                    blocked.add(did)
+                else:
+                    allowed.add(did)
+
+            group.selected_drone_ids = allowed
+
+            # 只有當操作目標組是 active 組時，才更新 UI
+            if group_id == self.active_group_id:
+                self.refresh_selection_ui()
+
             panel = self.group_panels.get(group_id)
             if panel:
                 panel.update_drone_list()
                 panel.update_status()
 
-            # 同步更新左側面板的 checkbox 狀態
+            if blocked:
-            self.monitor.selected_drones = group.drone_ids.copy()
-            for drone_id in all_ids:
-                if drone_id in self.drones:
-                    checkbox = self.drones[drone_id].get_checkbox()
-                    if checkbox:
-                        checkbox.blockSignals(True)
-                        checkbox.setChecked(drone_id in group.drone_ids)
-                        checkbox.blockSignals(False)
-                # 更新 socket 群組的 checkbox 狀態
-                self.update_group_checkbox_state(self.get_socket_id(drone_id))
-            
                 self.statusBar().showMessage(
-                f"Group {group_id}: 已分配 {len(group.drone_ids)} 台無人機", 3000)
+                    f"⚠ Group {group_id}: {len(blocked)} 台已被其他 group 勾選，未加入", 4000
+                )
+            else:
+                self.statusBar().showMessage(
+                    f"Group {group_id}: 已分配 {len(group.selected_drone_ids)} 台無人機", 3000
+                )
 
     def _handle_mission_type_changed(self, group_id, mission_type):
         """群組任務類型切換"""
@@ -738,19 +861,19 @@ class ControlStationUI(QMainWindow):
             print(f"❌ 找不到群組: {group_id}", flush=True)
             return
         
-        if not group.drone_ids:
+        if not group.selected_drone_ids:
             print(f"⚠️  群組中沒有無人機", flush=True)
             self.statusBar().showMessage(f"群組 {group_id} 中沒有無人機", 3000)
             return
         
-        print(f"   準備為 {len(group.drone_ids)} 台無人機切換模式...", flush=True)
+        print(f"   準備為 {len(group.selected_drone_ids)} 台無人機切換模式...", flush=True)
         self.statusBar().showMessage(f"正在切換模式...", 1000)
         
         # 使用 asyncio 執行（通過事件循環）
         async def do_mode_changes_async():
             print(f"\n   【異步任務】開始執行模式切換", flush=True)
             
-            for drone_id in group.drone_ids:
+            for drone_id in group.selected_drone_ids:
                 print(f"\n   【切換】{drone_id} → {mode}", flush=True)
                 try:
                     result = await self.monitor.set_mode(drone_id, mode)
@@ -789,11 +912,11 @@ class ControlStationUI(QMainWindow):
         if not group:
             print(f"   ⚠️ 群組不存在，返回\n")
             return
-        print(f"   群組內無人機: {group.drone_ids}")
+        print(f"   群組內無人機: {group.selected_drone_ids}")
         loop = asyncio.get_event_loop()
         print(f"   事件循環: {loop}")
         
-        for drone_id in group.drone_ids:
+        for drone_id in group.selected_drone_ids:
             print(f"\n   ┌─ 處理無人機: {drone_id}")
             print(f"   ├─ 準備調用 arm_drone(drone_id={drone_id}, arm=True)")
             coro = self.monitor.arm_drone(drone_id, True)
@@ -817,7 +940,7 @@ class ControlStationUI(QMainWindow):
         if not group:
             return
         loop = asyncio.get_event_loop()
-        for drone_id in group.drone_ids:
+        for drone_id in group.selected_drone_ids:
             future = self.monitor.takeoff_drone(drone_id, altitude)
             loop.create_task(self.handle_service_response(future, f"起飛 {drone_id} ({altitude}m)"))
 
@@ -829,102 +952,96 @@ class ControlStationUI(QMainWindow):
             f"請在地圖上框選要分配到 Group {group_id} 的無人機", 5000)
 
     def _handle_drone_box_selected(self, drone_ids_json):
-        """地圖框選完成 — 直接分配到指定群組"""
         group_id = self._pending_box_assign
         self._pending_box_assign = None
         if not group_id:
             return
+
         group = self.mission_groups.get(group_id)
         if not group:
             return
+
         drone_ids = json.loads(drone_ids_json)
-        other = self._get_other_assigned(group_id)
+
-        valid = {did for did in drone_ids if did not in other}
+        allowed = set()
-        group.drone_ids = valid
+        blocked = set()
+        for did in drone_ids:
+            if self._is_drone_selected_by_other_group(did, group_id):
+                blocked.add(did)
+            else:
+                allowed.add(did)
+
+        group.selected_drone_ids = allowed
+
+        if group_id == self.active_group_id:
+            self.refresh_selection_ui()
+
         panel = self.group_panels.get(group_id)
         if panel:
             panel.update_drone_list()
             panel.update_status()
 
-        # 同步更新左側面板的 checkbox 狀態
+        if blocked:
-        self.monitor.selected_drones = group.drone_ids.copy()
-        for drone_id in self.drones.keys():
-            checkbox = self.drones[drone_id].get_checkbox()
-            if checkbox:
-                checkbox.blockSignals(True)
-                checkbox.setChecked(drone_id in group.drone_ids)
-                checkbox.blockSignals(False)
-            self.update_group_checkbox_state(self.get_socket_id(drone_id))
-        
             self.statusBar().showMessage(
-            f"Group {group_id}: 框選分配 {len(valid)} 台無人機", 3000)
+                f"⚠ Group {group_id}: 部分 drone 已被其他 group 勾選，僅加入 {len(allowed)} 台", 4000
+            )
+        else:
+            self.statusBar().showMessage(
+                f"Group {group_id}: 框選分配 {len(allowed)} 台無人機", 3000
+            )
             
     def _handle_select_all_for_group(self, group_id):
-        """全選/取消全選 - Toggle 邏輯"""
+        """全選/取消全選 — Toggle 邏輯"""
         group = self.mission_groups.get(group_id)
         if not group:
             return
 
-        other = self._get_other_assigned(group_id)
+        selectable_ids = self._get_selectable_drones_for_group(group_id) | set(group.selected_drone_ids)
-        available = {did for did in self.drones.keys() if did not in other}
 
         # Toggle 邏輯：如果已全選，則清空；否則全選
-        if group.drone_ids == available:
+        if group.selected_drone_ids == selectable_ids:
             # 已全選 → 清空
-            group.drone_ids = set()
+            group.selected_drone_ids.clear()
-            self.monitor.selected_drones.clear()
             msg_status = "已取消全選"
         else:
             # 未全選 → 全選
-            group.drone_ids = available
+            group.selected_drone_ids = set(selectable_ids)
-            self.monitor.selected_drones = group.drone_ids.copy()
+            msg_status = f"全選可用無人機 {len(selectable_ids)} 台"
-            msg_status = f"全選分配 {len(available)} 台無人機"
+
+        # 只有當操作目標組是 active 組時，才更新 UI
+        if group_id == self.active_group_id:
+            self.refresh_selection_ui()
 
         panel = self.group_panels.get(group_id)
         if panel:
             panel.update_drone_list()
             panel.update_status()
-            # 更新按鈕文本
-            panel.set_all_select_state(group.drone_ids == available)
-        
-        # 同步更新左側面板的 checkbox 狀態
-        for drone_id in self.drones.keys():
-            checkbox = self.drones[drone_id].get_checkbox()
-            if checkbox:
-                checkbox.blockSignals(True)
-                checkbox.setChecked(drone_id in group.drone_ids)
-                checkbox.blockSignals(False)
-            self.update_group_checkbox_state(self.get_socket_id(drone_id))
 
         self.statusBar().showMessage(
             f"Group {group_id}: {msg_status}", 3000)
 
     def _handle_clear_group(self, group_id):
-        """清除群組的無人機分配"""
+        """清除群組的無人機選擇"""
         group = self.mission_groups.get(group_id)
         if not group:
             return
-        group.drone_ids = set()
+
+        group.selected_drone_ids.clear()
         group.planned_waypoints = None
         if group.executor:
             group.executor.stop()
         self.drone_map.clear_mission_plan_for_group(group_id)
+
+        # 只有當操作目標組是 active 組時，才更新 UI
+        if group_id == self.active_group_id:
+            self.refresh_selection_ui()
+
         panel = self.group_panels.get(group_id)
         if panel:
             panel.update_drone_list()
             panel.update_status()
             panel.clear_mission_info()
 
-        # 同步更新左側面板的 checkbox 狀態（全部取消勾選）
-        self.monitor.selected_drones.clear()
-        for drone_id in self.drones.keys():
-            checkbox = self.drones[drone_id].get_checkbox()
-            if checkbox:
-                checkbox.blockSignals(True)
-                checkbox.setChecked(False)
-                checkbox.blockSignals(False)
-            self.update_group_checkbox_state(self.get_socket_id(drone_id))
-        
         self.statusBar().showMessage(
             f"Group {group_id}: 已清除分組", 3000)
 
@@ -958,17 +1075,12 @@ class ControlStationUI(QMainWindow):
         if group_id in self.group_panels:
             del self.group_panels[group_id]
         
-        # 更新 active group
+        # 更新 active group — 讓 currentChanged signal 自動處理
         if self.active_group_id == group_id:
+            self.active_group_id = None
             if self.group_tab_widget.count() > 0:
                 self.group_tab_widget.setCurrentIndex(0)
-                # 更新 active_group_id 為當前 tab 的群組
+            # _on_group_tab_changed 會自動設定新的 active group
-                for gid, panel in self.group_panels.items():
-                    if panel == self.group_tab_widget.currentWidget().widget():
-                        self.active_group_id = gid
-                        break
-            else:
-                self.active_group_id = None
         
         self.statusBar().showMessage(f"已刪除 Group {group_id}", 3000)
         
@@ -1023,173 +1135,143 @@ class ControlStationUI(QMainWindow):
     # ================================================================================
 
     def handle_group_selection(self, socket_id, state):
-        """處理 socket 群組 checkbox 的勾選/取消勾選
+        """Socket 群組勾選 — 更新該 socket 下所有 drone 的選擇狀態"""
+        group = self._get_active_group()
+        if not group:
+            return
 
-        這個方法在用戶點擊 socket 群組的 checkbox 時被調用。
+        drone_ids = [did for did in self.drones.keys() if self.get_socket_id(did) == socket_id]
-        需要同時更新：
+        selectable = []
-        1. 該 socket 下所有無人機的 checkbox
+        blocked = []
-        2. self.monitor.selected_drones（用於控制面板同步）
+        for did in drone_ids:
-        3. 右侧活躍群組的無人機分配（新增）
+            if self._is_drone_selected_by_other_group(did, group.group_id):
+                blocked.append(did)
+            else:
+                selectable.append(did)
 
-        參數：
+        if not selectable:
-            socket_id: socket ID (str)
+            if blocked:
-            state: Qt.CheckState 的整數值 (0=Unchecked, 1=PartiallyChecked, 2=Checked)
+                self.statusBar().showMessage(
+                    f"⚠ Socket {socket_id} 的 drone 已被其他 group 勾選，無法操作", 4000
+                )
+            self.refresh_selection_ui()
+            return
+
+        all_selectable_selected = all(did in group.selected_drone_ids for did in selectable)
+
+        if all_selectable_selected:
+            group.selected_drone_ids.difference_update(selectable)
+        else:
+            group.selected_drone_ids.update(selectable)
+            if blocked:
+                self.statusBar().showMessage(
+                    f"⚠ 以下 drone 已被其他 group 勾選，略過: {', '.join(blocked)}", 4000
+                )
+
+        self.refresh_selection_ui()
+
+    def _is_drone_selected_by_other_group(self, drone_id, current_group_id):
         """
-        print(f"\n📢 [GUI] handle_group_selection 被調用", flush=True)
+        檢查該 drone 是否已被其他 group 勾選
-        print(f"   socket_id: {socket_id}, state: {state}", flush=True)
-        print(f"   state 類型: {type(state)}", flush=True)
         
-        # 獲取該 socket 下所有無人機
+        Args:
-        group_drones = [did for did in self.drones.keys() if self.get_socket_id(did) == socket_id]
+            drone_id: 無人機 ID
-        print(f"   該 socket 下的無人機: {group_drones}", flush=True)
+            current_group_id: 當前 group 的 ID
             
-        # 判斷是否勾選（只有 state == 2 時才是 Checked）
+        Returns:
-        is_checked = (state == 2)  # Qt.CheckState.Checked.value == 2
+            True 如果 drone 已被其他 group 選擇，否則 False
-        print(f"   is_checked: {is_checked}", flush=True)
+        """
+        for gid, group in self.mission_groups.items():
+            # 跳過當前 group
+            if gid == current_group_id:
+                continue
+            # 檢查該 drone 是否在其他 group 中被勾選
+            if drone_id in group.selected_drone_ids:
+                return True
+        return False
 
-        # 更新該 socket 下所有無人機的 checkbox 狀態
+    def _get_selectable_drones_for_group(self, group_id):
-        for drone_id in group_drones:
+        """
-            checkbox = self.drones[drone_id].get_checkbox()
+        取得指定 group 可以選擇的 drone 清單（不被其他 group 佔用的）
-            if checkbox:
-                print(f"   └─ 更新 {drone_id}: setChecked({is_checked})", flush=True)
-                checkbox.blockSignals(True)
-                checkbox.setChecked(is_checked)
-                checkbox.blockSignals(False)
         
-                # 同時更新 monitor.selected_drones 以同步控制面板
+        Args:
-                if is_checked:
+            group_id: group 的 ID
-                    self.monitor.selected_drones.add(drone_id)
-                else:
-                    self.monitor.selected_drones.discard(drone_id)
             
-        # 👇 新增：同步更新右侧活躍群組的無人機分配
+        Returns:
-        if self.active_group_id:
+            set: 可選擇的 drone ID 集合
-            group = self.mission_groups.get(self.active_group_id)
+        """
-            panel = self.group_panels.get(self.active_group_id)
+        selectable = set()
-            if group and panel:
+        for did in self.drones.keys():
-                print(f"   ├─ 同步右侧群組 {self.active_group_id}", flush=True)
+            if not self._is_drone_selected_by_other_group(did, group_id):
-                if is_checked:
+                selectable.add(did)
-                    # 勾選時：將該 socket 下的無人機添加到活躍群組
+        return selectable
-                    for drone_id in group_drones:
-                        group.drone_ids.add(drone_id)
-                    print(f"   │  └─ 添加到群組: {group_drones}", flush=True)
-                else:
-                    # 取消勾選時：從活躍群組移除該 socket 下的無人機
-                    for drone_id in group_drones:
-                        group.drone_ids.discard(drone_id)
-                    print(f"   │  └─ 從群組移除: {group_drones}", flush=True)
-                
-                # 更新右側群組面板的顯示
-                panel.update_drone_list()
-                panel.update_status()
-                print(f"   │  └─ 已更新右侧群組面板", flush=True)
-        
-        print(f"   最終 selected_drones: {self.monitor.selected_drones}", flush=True)
-        print(f"✓ handle_group_selection 完成\n", flush=True)
 
     def handle_drone_selection(self, drone_id, state):
-        is_checked = state == Qt.CheckState.Checked.value
+        """單台 drone 勾選 — 只修改 active group 的 selected_drone_ids"""
+        group = self._get_active_group()
+        if not group:
+            return
+
+        is_checked = (state == Qt.CheckState.Checked.value)
+
         if is_checked:
-            self.monitor.selected_drones.add(drone_id)
+            if self._is_drone_selected_by_other_group(drone_id, group.group_id):
-        else:
+                self.statusBar().showMessage(
-            self.monitor.selected_drones.discard(drone_id)
+                    f"⚠ {drone_id} 已被其他 group 勾選，無法加入 Group {group.group_id}", 3000
-        self.update_group_checkbox_state(self.get_socket_id(drone_id))
-        
-        # 同步更新任務群組的無人機分配狀態
-        # 遍歷所有任務群組，更新已分配的無人機列表顯示
-        if not is_checked:
-            # 取消勾選時：從所有包含該無人機的群組中移除
-            for group_id, group in self.mission_groups.items():
-                if drone_id in group.drone_ids:
-                    group.drone_ids.discard(drone_id)
-                    panel = self.group_panels.get(group_id)
-                    if panel:
-                        panel.update_drone_list()
-                        panel.update_status()
-                        # 更新全選按鈕狀態
-                        other = self._get_other_assigned(group_id)
-                        available = {did for did in self.drones.keys() if did not in other}
-                        panel.set_all_select_state(group.drone_ids == available)
-        else:
-            # 勾選時：檢查該無人機是否已分配給其他群組，若未分配則添加到當前活躍群組
-            is_already_assigned = any(
-                drone_id in group.drone_ids 
-                for group in self.mission_groups.values()
                 )
-            if not is_already_assigned and self.active_group_id:
+                self.refresh_selection_ui()
-                # 無人機未被分配給任何群組，可以添加到當前活躍群組
+                return
-                group = self.mission_groups.get(self.active_group_id)
+            group.selected_drone_ids.add(drone_id)
-                panel = self.group_panels.get(self.active_group_id)
+        else:
-                if group and panel:
+            group.selected_drone_ids.discard(drone_id)
-                    group.drone_ids.add(drone_id)
-                    panel.update_drone_list()
-                    panel.update_status()
-                    # 更新全選按鈕狀態
-                    other = self._get_other_assigned(self.active_group_id)
-                    available = {did for did in self.drones.keys() if did not in other}
-                    panel.set_all_select_state(group.drone_ids == available)
 
-    def update_group_checkbox_state(self, socket_id):
+        self.refresh_selection_ui()
-        group_drones = [did for did in self.drones.keys() if self.get_socket_id(did) == socket_id]
-        if not group_drones: return
-        checked_count = sum(1 for did in group_drones if self.drones[did].is_checked())
-        if socket_id in self.socket_groups:
-            group_checkbox = self.socket_groups[socket_id].findChild(QCheckBox, f"socket_{socket_id}_checkbox")
-            if group_checkbox:
-                group_checkbox.blockSignals(True)
-                if checked_count == 0: group_checkbox.setCheckState(Qt.CheckState.Unchecked)
-                elif checked_count == len(group_drones): group_checkbox.setCheckState(Qt.CheckState.Checked)
-                else: group_checkbox.setCheckState(Qt.CheckState.PartiallyChecked)
-                group_checkbox.blockSignals(False)
-
-    def handle_select_all(self):
-        all_selected = all(self.drones[did].is_checked() for did in self.drones)
-        new_state = not all_selected
-        for drone_id in self.drones:
-            self.drones[drone_id].set_checked(new_state)
-        for socket_id in self.socket_groups:
-            group_checkbox = self.socket_groups[socket_id].findChild(QCheckBox, f"socket_{socket_id}_checkbox")
-            if group_checkbox:
-                group_checkbox.blockSignals(True)
-                group_checkbox.setChecked(new_state)
-                group_checkbox.blockSignals(False)
 
     def handle_drone_clicked(self, drone_id):
-        if drone_id in self.drones:
+        """地圖上點擊 drone — 切換其選擇狀態"""
-            checkbox = self.drones[drone_id].get_checkbox()
+        group = self._get_active_group()
-            checkbox.setChecked(not checkbox.isChecked())
+        if not group:
+            return
 
-    def handle_clear_all_drone_selection(self):
+        if drone_id in group.selected_drone_ids:
-        for drone_id in self.drones.keys():
+            group.selected_drone_ids.remove(drone_id)
-            checkbox = self.drones[drone_id].get_checkbox()
+        else:
-            if checkbox:
+            # 嘗試勾選前先檢查是否被其他 group 使用
-                checkbox.blockSignals(True)
+            if self._is_drone_selected_by_other_group(drone_id, group.group_id):
-                checkbox.setChecked(False)
+                self.statusBar().showMessage(
-                checkbox.blockSignals(False)
+                    f"⚠ {drone_id} 已被其他 group 勾選，無法加入 Group {group.group_id}", 3000
-        self.monitor.selected_drones.clear()
+                )
-        for socket_id in self.socket_groups.keys():
+                self.refresh_selection_ui()
-            self.update_group_checkbox_state(socket_id)
+                return
+            group.selected_drone_ids.add(drone_id)
+
+        self.refresh_selection_ui()
     
     def handle_toggle_select_all_drones(self):
-        all_selected = all(self.drones[did].get_checkbox().isChecked() for did in self.drones.keys())
+        """全選 / 清空 — 切換 active group 的所有 drone"""
-        if all_selected:
+        group = self._get_active_group()
-            for drone_id in self.drones.keys():
+        if not group:
-                checkbox = self.drones[drone_id].get_checkbox()
+            return
-                if checkbox:
+
-                    checkbox.blockSignals(True)
+        selectable_ids = self._get_selectable_drones_for_group(group.group_id) | set(group.selected_drone_ids)
-                    checkbox.setChecked(False)
+
-                    checkbox.blockSignals(False)
+        if group.selected_drone_ids == selectable_ids:
-            self.monitor.selected_drones.clear()
+            # 已全選 → 清空
+            group.selected_drone_ids.clear()
         else:
-            for drone_id in self.drones.keys():
+            # 未全選 → 全選
-                checkbox = self.drones[drone_id].get_checkbox()
+            group.selected_drone_ids = set(selectable_ids)
-                if checkbox:
+
-                    checkbox.blockSignals(True)
+        self.refresh_selection_ui()
-                    checkbox.setChecked(True)
+
-                    checkbox.blockSignals(False)
+    def handle_clear_all_drone_selection(self):
-                    self.monitor.selected_drones.add(drone_id)
+        """清除 active group 的所有無人機選擇"""
-        for socket_id in self.socket_groups.keys():
+        group = self._get_active_group()
-            self.update_group_checkbox_state(socket_id)
+        if not group:
+            return
+        group.selected_drone_ids.clear()
+        self.refresh_selection_ui()
+        self.statusBar().showMessage("已清除所有選擇", 2000)
 
     # ================================================================================
     # 任務規劃 — 點擊地圖（路由到 active group）
@@ -1197,7 +1279,7 @@ class ControlStationUI(QMainWindow):
 
     def _get_group_drones(self, group):
         """取得群組的無人機 ID 列表（排序後）"""
-        return sorted(group.drone_ids, key=lambda x: (x.split('_')[0], int(x.split('_')[1])))
+        return sorted(group.selected_drone_ids, key=lambda x: (x.split('_')[0], int(x.split('_')[1])))
 
     def handle_map_click(self, lat, lon):
         """處理地圖點擊事件 — 根據 active group 的任務類型規劃"""
@@ -1518,9 +1600,6 @@ class ControlStationUI(QMainWindow):
                 print(f"  WP{j}: ({wp[0]:.6f}°, {wp[1]:.6f}°, {wp[2]:.1f}m)")
         print(f"\n{'=' * 60}")
     
-    def get_selected_drones(self):
-        return [did for did, panel in self.drones.items() if hasattr(panel, 'checkbox') and panel.checkbox.isChecked()]
-
     def update_field(self, panel, drone_id, field, text, color=None):
         if isinstance(panel, DronePanel):
             panel.update_field(field, text, color)
@@ -1544,8 +1623,9 @@ class ControlStationUI(QMainWindow):
             self.socket_groups[socket_id] = self.create_socket_group_panel(socket_id)
         self.socket_groups[socket_id].drones_layout.addWidget(panel)
         self.reorganize_socket_groups()
-        self.update_group_checkbox_state(socket_id)
         self.update_overview_table()
+        # 同步新 drone 到 UI
+        self.refresh_selection_ui()
 
     def reorganize_socket_groups(self):
         while self.drone_panel_layout.count():

src/GUI/mission_group.py

@@ -30,7 +30,11 @@ 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
+        
+        # 唯一真實資料來源：該 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 實例（延遲建立）
@@ -51,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("分配無人機")
@@ -67,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)
@@ -90,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)
@@ -160,7 +160,6 @@ class GroupPanel(QWidget):
     def __init__(self, group: MissionGroup, parent=None):
         super().__init__(parent)
         self.group = group
-        self._is_all_selected = False  # 追蹤全選狀態
         self.all_btn_ref = None  # 保存全選按鈕的參考
         self._build_ui()
 
@@ -454,11 +453,11 @@ class GroupPanel(QWidget):
 
     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(
@@ -467,7 +466,7 @@ class GroupPanel(QWidget):
 
     def _refresh_leader_options(self):
         """依目前群組成員重新填充領隊下拉選單，保留目前選擇若仍有效"""
-        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])))
         current = self.group.leader_drone_id
         self._leader_combo.blockSignals(True)
@@ -498,7 +497,7 @@ 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(
@@ -508,10 +507,6 @@ class GroupPanel(QWidget):
         """全選按鈕點擊 - 發送信號給 gui.py 處理 toggle 邏輯"""
         self.select_all_requested.emit(self.group.group_id)
     
-    def set_all_select_state(self, is_selected):
-        """外部設置全選狀態（按鈕文本保持「全選/取消」）"""
-        self._is_all_selected = is_selected
-    
     def set_delete_enabled(self, enabled):
         """啟用或禁用刪除群組按鈕"""
         self.delete_group_btn.setEnabled(enabled)

src/GUI/overview_table.py

@@ -7,7 +7,7 @@ class OverviewTable(QTableWidget):
     
     # 默認的資訊類型和映射
     DEFAULT_INFO_TYPES = ["模式", "ARM", "電壓", "經度", "緯度", "高度", "XY位置", "XY速度", "地速", "航向",
-                          "空速", "油門", "HUD ALT", "爬升率", "Roll", "Pitch", "Yaw", "丟包", "延遲"]
+                          "空速", "油門", "海拔高度", "爬升率", "Roll", "Pitch", "Yaw", "丟包", "延遲"]
     
     DEFAULT_INFO_TYPE_MAP = {
         "mode": 0,

src/fc_network_adapter/fc_network_adapter/mainOrchestrator.py

@@ -29,7 +29,7 @@ from .utils import acquireSerial, acquirePort
 from .utils.acquirePort import find_available_port
 
 logger = setup_logger(os.path.basename(__file__))
-PROJECT_VER = "v0.61"
+PROJECT_VER = "v0.70"
 
 class PanelState:
     def __init__(self):
@@ -45,7 +45,7 @@ class PanelState:
 
         # 關於創建通道時的暫存資訊
         self.udp_info_temp = {"IP": "127.0.0.1", "Port": "", "Direction": ""} # 暫存 UDP 設定資訊
-        self.serial_info_temp = {"Port": "", "Baud": 115200, "CommunicationType": "", "Go2Middleware": False} # 暫存 Serial 設定資訊
+        self.serial_info_temp = {"Port": "", "Baud": 115200, "CommunicationType": "", "Go2Middleware": True} # 暫存 Serial 設定資訊
 
         # 關於顯示通道資訊
         self.socket_info_single = {
@@ -426,7 +426,7 @@ class ControlPanel:
                         menu_stack.pop()
                         idx_stack.pop()
                     elif selected.action == "SET_SERIAL_COMM_TELEMETRY":
-                        state.serial_info_temp["CommunicationType"] = "XBee(AT-AT)"
+                        state.serial_info_temp["CommunicationType"] = "TELEMETRY"
                         menu_stack.pop()
                         idx_stack.pop()
 
@@ -767,7 +767,7 @@ class ControlPanel:
                 port_menu = MenuNode(f"{port}", children=[
                     MenuNode("Set Comm Type", "設定通訊形態", "SET_SERIAL_COMM", children=[
                         MenuNode("XBee(API-AT)", "XBee 模式", "SET_SERIAL_COMM_XBEE"),
-                        # MenuNode("Telemetry", "數傳模式", "SET_SERIAL_COMM_TELEMETRY"),
+                        MenuNode("Telemetry", "數傳模式", "SET_SERIAL_COMM_TELEMETRY"),
                     ]),
                     MenuNode("Set Baud", "設定 Baud", "TEXT_BAUD_SERIAL"),
                     MenuNode("Link to MW", "直接建立 Middleware UDP", "LINK_SERIAL_TO_MIDDLEWARE_UDP", children=[
@@ -1058,11 +1058,15 @@ class ControlPanel:
 
             # MAVLink 訊息名稱對應（縮寫版本）
             MSG_NAMES = {
-                0: "HB", 1: "SYS_ST", 24: "GPS_RAW", 27: "RAW_IMU",
+                0: "HB", 1: "S_STAT", 2: "S_TIME", 24: "GPS_RAW", 27: "RAW_IMU",
                 30: "ATT", 32: "LOC_POS", 33: "GLB_POS", 62: "NAV_CTL",
-                74: "VFR_HUD", 147: "BATT_ST"
+                74: "VFR_HUD", 147: "BATT_ST", 136: "TERRAIN", 241: "VIBRAT",
+                125: "POW_STA",
             }
 
+            # ardupilot mega 
+            # 178: "AHRS2", 163: "AHRS", 193: "EKF_STAT", 
+
             # 顯示前 12 個最常見的訊息類型（兩列各 6 個）
             msg_items = list(msg_counts.items())[:12]
             line = 13
@@ -1134,6 +1138,7 @@ class Orchestrator:
         self.bridge = mo.mavlink_bridge()
 
         # === 3) serial_manager 部分的準備 ===
+        sm.rx_module_ack.clear()
         self.plumber = sm.serial_manager()
 
         # === 4) ros 部分的準備 ===
@@ -1243,7 +1248,7 @@ class Orchestrator:
                             if serial_obj:
                                 self.panelState.serial_info_single["serial_port"] = serial_obj.serial_port
                                 self.panelState.serial_info_single["baudrate"] = serial_obj.baudrate
-                                self.panelState.serial_info_single["receiver_type"] = serial_obj.receiver_type.name
+                                self.panelState.serial_info_single["receiver_type"] = serial_obj.serial_mode.name
                                 self.panelState.serial_info_single["target_port"] = serial_obj.target_port
                                 self.panelState.serial_info_single["InfoReady"] = True # 標記資訊已準備好
                         elif action == "REMOVE_LINKED_SERIAL":
@@ -1516,8 +1521,8 @@ class Orchestrator:
 
         # 定義通訊類型映射表
         COMM_TYPE_MAP = {
-            "XBee(API-AT)": sm.SerialReceiverType.XBEEAPI2AT,
+            "XBee(API-AT)": sm.SerialMode.XBEEAPI2AT,
-            # "XBee(AT-AT)": sm.SerialReceiverType.TELEMETRY,  # TODO: 之後再弄
+            "TELEMETRY": sm.SerialMode.STRAIGHT,
             # 新增區
         }
 
@@ -1541,7 +1546,7 @@ class Orchestrator:
             serial_port=self.panelState.serial_info_temp['Port'],
             baudrate=self.panelState.serial_info_temp['Baud'],
             target_port=udp_port_tmp,
-            receiver_type=comm_type_tmp,
+            serial_mode=comm_type_tmp,
         )
 
         if not ret:
@@ -1572,7 +1577,7 @@ def main():
     if mvv.MODULE_VER != "1.00":
         print("Module Version Error! : mavlinkVehicleView")
         version_check = False
-    if sm.MODULE_VER != "0.60":
+    if sm.MODULE_VER != "0.80":
         print("Module Version Error! : serialManager")
         version_check = False
     if version_check == False:

src/fc_network_adapter/fc_network_adapter/mavlinkObject.py

@@ -230,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  # 轉換為度

src/fc_network_adapter/fc_network_adapter/mavlinkROS2Nodes.py

@@ -210,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:

src/fc_network_adapter/fc_network_adapter/serialManager.py

@@ -16,19 +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.60"
+MODULE_VER = "0.80"
 
-# ====================== 分割線 =====================
+rx_module_ack = RingBuffer(capacity=64, buffer_id=253)
+
+# ====================== State DEFINITION =====================
 
 # 定義 serial 連線的模式
 class SerialMode(Enum):
@@ -38,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):
     """協議處理器基類"""
@@ -47,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
     
@@ -66,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 []
     
@@ -76,110 +101,108 @@ class RawFrameProcessor(FrameProcessor):
 
 
 class XBeeFrameProcessor(FrameProcessor):
-    """XBee API 協議處理器"""
+    """
+    XBee API 協議處理器
 
-    def process_incoming(self, data: bytes):
+    職責：
+    - 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 len(self.buffer) >= 3:
+        while True:
-            # 尋找幀頭
+            frame = self._try_extract_frame()
-            if self.buffer[0] != 0x7E:
+            if frame is None:
-                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
-            
+            payload = self._dispatch_frame(frame)
-            # 提取完整 frame 並從緩衝區移除
-            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 self._encapsulate(data)
 
-
+    # ---- 內部：拆幀與分派 ----
-# ====================== XBee Frame Handler =====================
+    def _try_extract_frame(self) -> bytes:
-
-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
-
-    @staticmethod
-    def encapsulate_data(data: bytes, dest_addr64: bytes = b'\x00\x00\x00\x00\x00\x00\xFF\xFF', frame_id = 0x01) -> bytes:
         """
-        將數據封裝為 XBee API 傳輸幀
+        從 buffer 嘗試抓一個完整 frame
+        - 對齊幀頭 0x7E
+        - 長度不足時等待下次進 buffer
+        - 成功則從 buffer 切掉並回傳整個 frame bytes
+        """
+        while len(self.buffer) >= 3:
+            if self.buffer[0] != 0x7E:
+                self.buffer.pop(0)
+                continue
 
-        使用 XBee API 格式封裝數據:
+            length = (self.buffer[1] << 8) | self.buffer[2]
-        - 傳輸請求幀 (0x10)
+            full_length = 3 + length + 1  # 起始符(1) + 長度(2) + payload + 校驗和(1)
+
+            if len(self.buffer) < full_length:
+                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]
+            return frame
+
+        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: bytes,
+        dest_addr64: bytes = b'\x00\x00\x00\x00\x00\x00\xFF\xFF',
+        frame_id: int = 0x01,
+    ) -> bytes:
+        """
+        將 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
@@ -191,20 +214,33 @@ class XBeeFrameHandler:
         return b'\x7E' + struct.pack(">H", len(frame)) + frame + struct.pack("B", checksum)
 
     @staticmethod
-    def decapsulate_data(data: bytes):
+    def _decapsulate(frame: bytes) -> bytes:
-        
+        """從 RX Packet (0x90) 取出 payload"""
-        # 獲取數據長度 (不包括校驗和)
+        length = (frame[1] << 8) | frame[2]
-        length = (data[1] << 8) | data[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,
@@ -212,25 +248,96 @@ class ATCommandHandler:
             # 可擴展其他 AT 指令
         }
 
-    def handle_response(self, response: dict):
+    # ---- 發送端 ----
-        """根據 AT 指令類型分派處理"""
+    def set_writer(self, writer):
-        if not response or not response['is_ok']:
+        """由 SerialHandler 注入實際寫入 serial 的 callable"""
-            if response:
+        self.writer = writer
-                logger.warning(f"[{self.serial_port}] AT {response['command'].decode()} 失敗，狀態碼: {response['status']}")
+
+    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']
+        self.writer(self._build_at_request(request))
-        handler = self.handlers.get(command)
+        # 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) 回應"""
+        """處理 DB (RSSI) 回應：單 byte 無號值，單位 dBm"""
-        # 未來可實現 RSSI 處理邏輯
         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)"""
@@ -241,8 +348,7 @@ class ATCommandHandler:
         pass
 
 
-# ====================== Serial Handler =====================
+# ================ Serial UDP Socket Object ==============
-
 class SerialHandler(asyncio.Protocol):
     """asyncio.Protocol 用於處理 Serial 收發"""
 
@@ -252,28 +358,29 @@ class SerialHandler(asyncio.Protocol):
         self.serial_mode = serial_mode
         self.transport = None  # Serial 自己的傳輸物件
 
-        # 根據模式創建對應的 processor
+        # 根據模式建立 processor（需要 AT handler 時一併在工廠內建好注入）
-        self.processor = self._create_processor(serial_mode)
+        self.processor = self._create_processor()
 
-        # AT 指令處理器（僅 XBee 模式使用）
+    def _create_processor(self) -> FrameProcessor:
-        if serial_mode == SerialMode.XBEEAPI2AT:
+        """工廠方法：根據 self.serial_mode 建立對應的 processor，必要時一併注入 AT handler"""
-            self.at_handler = ATCommandHandler(serial_port_str)
+        if self.serial_mode == SerialMode.STRAIGHT:
-        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()
+        if self.serial_mode == SerialMode.XBEEAPI2AT:
-        else:
+            at_handler = ATCommandHandler(self.serial_port_str)
-            logger.warning(f"Unknown serial mode: {serial_mode}, using Raw")
+            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")
@@ -291,8 +398,6 @@ class SerialHandler(asyncio.Protocol):
             )
 
 
-# ====================== UDP Handler =====================
-
 class UDPHandler(asyncio.DatagramProtocol):
     """asyncio.DatagramProtocol 用於處理 UDP 收發"""
     
@@ -561,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):
         """檢查串口是否存在與可用"""
@@ -598,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/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()