tmpCommit1

- 修正 POLL-DONE 程序的 timeout 判定方式
- 修正為 py3.8 語法
- 不在判定要求量跟實收量一致性

README.md

@@ -67,6 +67,8 @@ cd ~/AirTrapMine/src/ # 這是範例!!!
 python -m fc_network_adapter.fc_network_adapter.mainOrchestrator
 python -m fc_network_adapter.tests.demo_integration
 python -m someotherpkg.src.example_wholeMoving
+
+python fc_network_apps/vehicleDiagShow.py
 ```
 
 2. 

serial_udp_update.py

@@ -1,193 +0,0 @@
-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/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 = "v1.20"
+PROJECT_VER = "v1.25"
 
 class PanelState:
     def __init__(self):
@@ -435,6 +435,10 @@ class ControlPanel:
                         state.serial_info_temp["CommunicationType"] = "XBee(API-AT)"
                         menu_stack.pop()
                         idx_stack.pop()
+                    elif selected.action == "SET_SERIAL_COMM_XBEE_ESP":
+                        state.serial_info_temp["CommunicationType"] = "XBee(API-API)espv1"
+                        menu_stack.pop()
+                        idx_stack.pop()
                     elif selected.action == "SET_SERIAL_COMM_TELEMETRY":
                         state.serial_info_temp["CommunicationType"] = "TELEMETRY"
                         menu_stack.pop()
@@ -486,6 +490,10 @@ class ControlPanel:
                             # 一樣退兩層
                                 menu_stack.pop()
                                 idx_stack.pop()
+                    
+                    elif selected.action == "DO_DISCOVERY_PANEL":
+                        if hasattr(selected, 'serial_id'):
+                            cmd_q.put(("DO_DISCOVERY", selected.serial_id))
 
                     # related with -> 列表選單
                     elif selected.action in ("PREV_PAGE", "NEXT_PAGE"):
@@ -812,6 +820,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("XBee(API-API)", "XBee 模式(with ESP)", "SET_SERIAL_COMM_XBEE_ESP"),
                         MenuNode("Telemetry", "數傳模式", "SET_SERIAL_COMM_TELEMETRY"),
                     ]),
                     MenuNode("Set Baud", "設定 Baud", "TEXT_BAUD_SERIAL"),
@@ -863,6 +872,7 @@ class ControlPanel:
                 obj_menu = MenuNode(f"Serial #{serial_id}", f"連結口 {serial_id}", None, children=[
                     MenuNode("Info", "查看詳細資訊", "INSPECT_LINKED_SERIAL"),
                     MenuNode("Remove", "移除此連結口", "REMOVE_LINKED_SERIAL"),
+                    MenuNode("Discovery", "搜尋遠端RF(Only XBEE-ESP32)", "DO_DISCOVERY_PANEL"),
                     # MenuNode("Change UDP Target", "變更目標 UDP (工程)", "CHANGE_LINKED_SERIAL_TARGET"),
                     MenuNode("GoUp", "回到列表", "BACK"),
                 ])
@@ -1314,6 +1324,11 @@ class Orchestrator:
                         elif action == "REMOVE_LINKED_SERIAL":
                             serial_id = cmd[1]
                             self.plumber.remove_serial_link(serial_id)
+                        elif action == "DO_DISCOVERY":
+                            serial_id = cmd[1]
+                            processor = self.plumber.get_espv1_processor(serial_id)
+                            if processor:
+                                processor.request_discovery()
 
                         elif action == "INSPECT_VEHICLE":
                             sysid, compid = cmd[1], cmd[2]
@@ -1595,6 +1610,7 @@ class Orchestrator:
         # 定義通訊類型映射表
         COMM_TYPE_MAP = {
             "XBee(API-AT)": sm.SerialMode.XBEEAPI2AT,
+            "XBee(API-API)espv1": sm.SerialMode.XBEEAPI_espv1,
             "TELEMETRY": sm.SerialMode.STRAIGHT,
             # 新增區
         }
@@ -1638,25 +1654,7 @@ class Orchestrator:
 
 def main():
 
-    # =========== 各項模組的版本先驗 ===========
-    # 除非你有在做這幾項模組的改版 不然動到這邊的版本號 代表執行環境有很大的問題!!!!!!
-    version_check = True
-    if mo.MODULE_VER != "1.50":
-        print("Module Version Error! : mavlinkObkect")
-        version_check = False
-    if mros.MODULE_VER != "2.50":
-        print("Module Version Error! : mavlinkROS2Nodes")
-        version_check = False
-    if mvv.MODULE_VER != "1.10":
-        print("Module Version Error! : mavlinkVehicleView")
-        version_check = False
-    if sm.MODULE_VER != "0.80":
-        print("Module Version Error! : serialManager")
-        version_check = False
-    if version_check == False:
-        print("Environment Obstacle! Check YOUR Execution System Path First!!")
-        return
-    # ========================================
+    logger.info(f"Each Module Running Version at mavlinkObkect:{mo.MODULE_VER}, mavlinkROS2Nodes:{mros.MODULE_VER}, mavlinkVehicleView:{mvv.MODULE_VER}, serialManager:{sm.MODULE_VER}")
 
     stop_evt = threading.Event()
     def signal_handler(signum, frame):
@@ -1698,4 +1696,8 @@ if __name__ == "__main__":
 2025.05.28
 1. 工程模式 新增 Node 重啟的功能
 
+2026.08.05 (ver 1.25)
+1. 增加對於 esp32 連線模式的支援
+
+
 '''

src/fc_network_adapter/fc_network_adapter/mavlinkObject.py

@@ -47,8 +47,8 @@ from pymavlink.dialects.v20 import ardupilotmega as mav_ardupilot
 
 # 自定義的 import
 from .mavlinkVehicleView import (
-    vehicle_registry,
-    VehicleView,
+    vehicle_registry, # 儲存全部物件的地方
+    VehicleView, # 代表每台載具的最外層
     VehicleComponent,
     ComponentType,
     StatusTextEntry,

src/fc_network_adapter/fc_network_adapter/mavlinkROS2Nodes.py

@@ -214,7 +214,7 @@ class VehicleStatusPublisher(Node):
             topic_name = f'{self.topicString_prefix}/sys{sysid}/{topic}'
             publisher = self.create_publisher(msg_type, topic_name, qos)
             self.fc_publishers[key] = publisher
-            logger.info(f"Created publisher: {topic_name}")
+            logger.debug(f"Created publisher: {topic_name}")
         return self.fc_publishers[key]
     
     def _publish_position_gnss(self, sysid: int, status: mvv.ComponentStatus):

src/fc_network_adapter/fc_network_adapter/mavlinkVehicleView.py

@@ -37,6 +37,7 @@ class ComponentType(Enum):
 class RFModuleType(Enum):
     """RF模組類型"""
     XBEE = "xbee"
+    XBEE_ESP = "xbee_esp"
     UDP = "udp"
     TCP = "tcp"
     OTHER = "other"
@@ -181,12 +182,14 @@ class RFStatus:
     at_response: Optional[str] = None  # AT 命令回應
     link_quality: Optional[int] = None  # 連接品質
     timestamp: Optional[float] = None  # 時間戳記
+    avg_bandwidth: Optional[float] = None # 量測到的傳輸速度 bytes/ms
     custom_status: Dict[str, Any] = field(default_factory=dict)  # 其他自定義狀態
 
 
 @dataclass
 class SocketInfo:
     """Socket連接資訊"""
+    src64_addr: Optional[bytes] = None # 模組的物理定址
     ip: Optional[str] = None  # IP位址
     port: Optional[int] = None  # 埠號
     local_ip: Optional[str] = None  # 本地IP
@@ -273,6 +276,18 @@ class RFModule:
         self.type = rf_type
         self.status = RFStatus()
         self.socket_info = SocketInfo()
+
+        # # only for ESP32
+        # self.poll_data_times: deque[int] = None
+        # self.poll_data_sizes: deque[int] = None
+        # self.poll_total_time = None
+        # self.poll_total_size = None
+        # self.remain_bytes = None
+
+    def update_avg_bandwidth(self, bandwidth: float, timestamp: Optional[float] = None) -> None:
+        self.status.avg_bandwidth = bandwidth
+        if timestamp:
+            self.status.timestamp = timestamp
     
     def update_rssi(self, rssi: int, timestamp: Optional[float] = None) -> None:
         """更新RSSI"""
@@ -310,6 +325,7 @@ class RFModule:
 
 class VehicleView:
     """
+    最上層
     載具視圖 - 純狀態容器
     
     特點:

src/fc_network_adapter/fc_network_adapter/utils/pollStrategy.py

@@ -0,0 +1,60 @@
+"""
+POLL 輪詢策略，供 XBeeFrameProcessor_ESPv1 使用。
+
+不依賴 serialManager，僅接受裝置快照與排程狀態。
+"""
+
+from dataclasses import dataclass
+from typing import List
+
+MIN_TRIGGER_INTERVAL_MS = 5
+
+@dataclass(frozen=True)
+class PollDevice:
+    """從 esp32AddrMapping 抽出的唯讀快照。"""
+    address_64: bytes
+    remain_bytes: int
+    last_poll_time: float
+
+
+def pick_next(devices: List[PollDevice], now_time):
+    """
+    選下一個 POLL 目標。
+
+    回傳 (target_address_64, grant_bytes)；devices 為空時回傳 (None, 0)。
+
+    poll time = 0.2305 * grant_bytes + 58.094 (R平方為0.979 高線性相關)  @-40db 
+    """
+    if not devices:
+        return None, 0
+
+    grant_bytes = 0
+    device_count = len(devices)
+    weights = [0] * device_count
+
+    for i in range(device_count):
+        gap = int((now_time - devices[i].last_poll_time)*1000)
+        # # 時間大於觸發間隔 才會計算
+        # if gap >= MIN_TRIGGER_INTERVAL_MS:
+        #     weights[i] += devices[i].remain_bytes
+        #     weights[i] += gap
+
+        weights[i] += devices[i].remain_bytes
+        weights[i] += gap
+
+    maxarg = weights.index(max(weights))
+
+    if weights[maxarg] == 0:
+        return None, 0
+    
+    selected_device = devices[maxarg]
+
+    if selected_device.remain_bytes < 500:
+        grant_bytes = 170  # 這個大概是 100 ms 總傳輸時間
+    elif selected_device.remain_bytes < 1200:
+        grant_bytes = 610  # 這個大概是 200 ms 總傳輸時間
+    else:
+        grant_bytes = 1050 # 這個大概是 300 ms 總傳輸時間
+
+    return selected_device.address_64, grant_bytes
+

src/fc_network_apps/vehicleDiagShow.py

@@ -0,0 +1,544 @@
+#!/usr/bin/env python3
+"""Standalone curses TUI for fc_network vehicle diagnostics (sys_diags, status_text, summary)."""
+
+from __future__ import annotations
+
+import curses
+import json
+import re
+import sys
+import threading
+import time
+from dataclasses import dataclass, field
+from typing import Dict, List, Optional, Tuple
+
+import rclpy
+from rclpy.node import Node
+from std_msgs.msg import String
+
+from fc_interfaces.msg import SystemDiagnosticsRaw
+
+VEHICLE_TOPIC_RE = re.compile(
+    r'^/fc_network/vehicle/(sys\d+)/(sys_diags|status_text|summary)$'
+)
+STATUS_TEXT_RE = re.compile(r'^\[([\d.]+)\]\s*\[(-?\d+)\]\s*(.*)$')
+
+STATUS_TEXT_TTL_SEC = 30.0
+STALE_SEC = 3.0
+RESCAN_SEC = 2.0
+OFFLINE_SEC = 10.0
+DRAW_HZ = 5.0
+
+# MAV_SYS_STATUS_SENSOR bit labels (MAVLink common)
+SENSOR_BITS: Tuple[Tuple[int, str], ...] = (
+    (268435456, 'PreArm'),
+    (1, 'Gyro'),
+    (2, 'Accel'),
+    (4, 'Mag'),
+    (8, 'Baro'),
+    (16, 'DiffPress'),
+    (32, 'GPS'),
+    (64, 'OptFlow'),
+    (128, 'Vision'),
+    (256, 'Laser'),
+    (512, 'ExtGT'),
+    (1024, 'AngRate'),
+    (2048, 'AttStab'),
+    (4096, 'YawPos'),
+    (8192, 'AltCtrl'),
+    (16384, 'XYCtrl'),
+    (32768, 'Motor'),
+    (65536, 'RC'),
+    (131072, 'Gyro2'),
+    (262144, 'Accel2'),
+    (524288, 'Mag2'),
+    (1048576, 'Geofence'),
+    (2097152, 'AHRS'),
+    (4194304, 'Terrain'),
+    (8388608, 'RevMotor'),
+    (16777216, 'Log'),
+    (33554432, 'Batt'),
+    (67108864, 'Prox'),
+    (134217728, 'Satcom'),
+    (536870912, 'ObsAvoid'),
+    (1073741824, 'Propulsion'),
+)
+
+MAV_STATE_LABELS = {
+    0: 'UNINIT',
+    1: 'BOOT',
+    2: 'CAL',
+    3: 'STANDBY',
+    4: 'ACTIVE',
+    5: 'CRITICAL',
+    6: 'EMERGENCY',
+    7: 'OFF',
+    8: 'TERM',
+}
+
+SEVERITY_LABELS = {
+    0: 'EMERG',
+    1: 'ALERT',
+    2: 'CRIT',
+    3: 'ERROR',
+    4: 'WARN',
+    5: 'NOTICE',
+    6: 'INFO',
+    7: 'DEBUG',
+}
+
+
+@dataclass
+class StatusTextEntry:
+    received_at: float
+    severity: int
+    text: str
+    raw: str
+    sys_key: str
+
+
+@dataclass
+class VehicleDiagState:
+    sys_key: str
+    sysid: int
+    summary: Optional[dict] = None
+    diag: Optional[SystemDiagnosticsRaw] = None
+    status_entries: List[StatusTextEntry] = field(default_factory=list)
+    last_summary_ts: float = 0.0
+    last_diag_ts: float = 0.0
+    last_topic_seen_ts: float = 0.0
+    offline: bool = False
+
+
+def sys_key_to_id(sys_key: str) -> int:
+    return int(sys_key.replace('sys', '', 1))
+
+
+def purge_status_entries(entries: List[StatusTextEntry], now: float, ttl: float = STATUS_TEXT_TTL_SEC) -> None:
+    cutoff = now - ttl
+    entries[:] = [e for e in entries if e.received_at >= cutoff]
+
+
+def parse_status_text(raw: str, sys_key: str, received_at: float) -> StatusTextEntry:
+    match = STATUS_TEXT_RE.match(raw.strip())
+    if match:
+        severity = int(match.group(2))
+        text = match.group(3)
+    else:
+        severity = -1
+        text = raw
+    return StatusTextEntry(
+        received_at=received_at,
+        severity=severity,
+        text=text,
+        raw=raw,
+        sys_key=sys_key,
+    )
+
+
+def decode_sensors(install: int, enabled: int, health: int) -> List[Tuple[str, str]]:
+    """Return list of (label, status) where status is OK / FAIL / OFF."""
+    items: List[Tuple[str, str]] = []
+    for bit, label in SENSOR_BITS:
+        if not (install & bit):
+            continue
+        if not (enabled & bit):
+            items.append((label, 'OFF'))
+        elif health & bit:
+            items.append((label, 'OK'))
+        else:
+            items.append((label, 'FAIL'))
+    return items
+
+
+def format_age(seconds: float) -> str:
+    if seconds < 0:
+        return 'never'
+    if seconds < 1.0:
+        return f'{seconds * 1000:.0f}ms'
+    if seconds < 60.0:
+        return f'{seconds:.1f}s'
+    return f'{seconds / 60:.1f}m'
+
+
+def truncate(text: str, width: int) -> str:
+    if width <= 0:
+        return ''
+    if len(text) <= width:
+        return text
+    if width <= 3:
+        return text[:width]
+    return text[: width - 3] + '...'
+
+
+class VehicleDiagNode(Node):
+    """ROS2 node: discover vehicles and subscribe to diagnostic topics."""
+
+    def __init__(self) -> None:
+        super().__init__('vehicle_diag_show')
+        self._lock = threading.Lock()
+        self._vehicles: Dict[str, VehicleDiagState] = {}
+        self._subs: Dict[str, Dict[str, object]] = {}
+        self._scan_timer = self.create_timer(RESCAN_SEC, self._rescan_topics)
+        self._rescan_topics()
+
+    def snapshot(self) -> Dict[str, VehicleDiagState]:
+        now = time.monotonic()
+        with self._lock:
+            for state in self._vehicles.values():
+                purge_status_entries(state.status_entries, now)
+                if state.last_topic_seen_ts > 0:
+                    state.offline = (now - state.last_topic_seen_ts) > OFFLINE_SEC
+            return {k: state for k, state in self._vehicles.items()}
+
+    def _get_or_create_state(self, sys_key: str) -> VehicleDiagState:
+        if sys_key not in self._vehicles:
+            self._vehicles[sys_key] = VehicleDiagState(
+                sys_key=sys_key,
+                sysid=sys_key_to_id(sys_key),
+            )
+        return self._vehicles[sys_key]
+
+    def _rescan_topics(self) -> None:
+        now = time.monotonic()
+        found: set = set()
+        for topic_name, _ in self.get_topic_names_and_types():
+            match = VEHICLE_TOPIC_RE.match(topic_name)
+            if not match:
+                continue
+            sys_key = match.group(1)
+            found.add(sys_key)
+            with self._lock:
+                state = self._get_or_create_state(sys_key)
+                state.last_topic_seen_ts = now
+                state.offline = False
+            self._ensure_subscriptions(sys_key)
+
+        with self._lock:
+            for sys_key, state in self._vehicles.items():
+                if sys_key not in found and state.last_topic_seen_ts > 0:
+                    state.offline = (now - state.last_topic_seen_ts) > OFFLINE_SEC
+
+    def _ensure_subscriptions(self, sys_key: str) -> None:
+        if sys_key in self._subs:
+            return
+        base = f'/fc_network/vehicle/{sys_key}'
+        subs = {
+            'summary': self.create_subscription(
+                String,
+                f'{base}/summary',
+                lambda msg, sk=sys_key: self._on_summary(sk, msg),
+                10,
+            ),
+            'sys_diags': self.create_subscription(
+                SystemDiagnosticsRaw,
+                f'{base}/sys_diags',
+                lambda msg, sk=sys_key: self._on_sys_diags(sk, msg),
+                10,
+            ),
+            'status_text': self.create_subscription(
+                String,
+                f'{base}/status_text',
+                lambda msg, sk=sys_key: self._on_status_text(sk, msg),
+                10,
+            ),
+        }
+        with self._lock:
+            self._subs[sys_key] = subs
+
+    def _on_summary(self, sys_key: str, msg: String) -> None:
+        now = time.monotonic()
+        try:
+            summary = json.loads(msg.data)
+        except json.JSONDecodeError:
+            summary = {'raw': msg.data}
+        with self._lock:
+            state = self._get_or_create_state(sys_key)
+            state.summary = summary
+            state.last_summary_ts = now
+            state.last_topic_seen_ts = now
+
+    def _on_sys_diags(self, sys_key: str, msg: SystemDiagnosticsRaw) -> None:
+        now = time.monotonic()
+        with self._lock:
+            state = self._get_or_create_state(sys_key)
+            state.diag = msg
+            state.last_diag_ts = now
+            state.last_topic_seen_ts = now
+
+    def _on_status_text(self, sys_key: str, msg: String) -> None:
+        now = time.monotonic()
+        entry = parse_status_text(msg.data, sys_key, now)
+        with self._lock:
+            state = self._get_or_create_state(sys_key)
+            state.status_entries.append(entry)
+            purge_status_entries(state.status_entries, now)
+            state.last_topic_seen_ts = now
+
+
+class DiagCursesApp:
+    """Curses front-end for vehicle diagnostics."""
+
+    def __init__(self, node: VehicleDiagNode) -> None:
+        self._node = node
+        self._running = True
+
+    def run(self) -> None:
+        curses.wrapper(self._main)
+
+    def _init_colors(self) -> None:
+        curses.start_color()
+        curses.use_default_colors()
+        curses.init_pair(1, curses.COLOR_GREEN, -1)    # OK / DISARMED / INFO
+        curses.init_pair(2, curses.COLOR_RED, -1)        # FAIL / ARMED / ERROR+
+        curses.init_pair(3, curses.COLOR_YELLOW, -1)   # WARN / OFF
+        curses.init_pair(4, curses.COLOR_CYAN, -1)     # header / INFO
+        curses.init_pair(5, curses.COLOR_WHITE, -1)    # dim / offline
+        curses.init_pair(6, curses.COLOR_MAGENTA, -1)  # NOTICE
+
+    def _main(self, stdscr) -> None:
+        curses.curs_set(0)
+        stdscr.nodelay(True)
+        stdscr.keypad(True)
+        self._init_colors()
+
+        while self._running and rclpy.ok():
+            try:
+                self._draw(stdscr)
+            except curses.error:
+                pass
+            key = stdscr.getch()
+            if key in (ord('q'), ord('Q'), 27):
+                self._running = False
+            time.sleep(1.0 / DRAW_HZ)
+
+    def _draw(self, stdscr) -> None:
+        stdscr.erase()
+        height, width = stdscr.getmaxyx()
+        now = time.monotonic()
+        vehicles = self._node.snapshot()
+        sorted_keys = sorted(vehicles.keys(), key=sys_key_to_id)
+
+        header = f' Vehicle Diagnostics Monitor  vehicles:{len(sorted_keys)}  Q:quit '
+        stdscr.attron(curses.color_pair(4) | curses.A_BOLD)
+        stdscr.addstr(0, 0, truncate(header.ljust(width - 1), width - 1))
+        stdscr.attroff(curses.color_pair(4) | curses.A_BOLD)
+
+        row = 1
+        if not sorted_keys:
+            msg = ' Waiting for vehicles... (need /fc_network/vehicle/sysN/* topics) '
+            if row < height - 1:
+                stdscr.addstr(row, max(0, (width - len(msg)) // 2), truncate(msg, width - 1))
+            row += 2
+
+        vehicle_rows_end = max(row, height - 8)
+        for sys_key in sorted_keys:
+            if row >= vehicle_rows_end:
+                break
+            state = vehicles[sys_key]
+            row = self._draw_vehicle_block(stdscr, row, width, state, now)
+            if row < height - 1:
+                try:
+                    stdscr.addstr(row, 0, truncate('-' * (width - 1), width - 1))
+                except curses.error:
+                    pass
+                row += 1
+
+        log_title = ' Status Text (last 30s) '
+        if row < height - 1:
+            stdscr.attron(curses.color_pair(4))
+            stdscr.addstr(row, 0, truncate(log_title.ljust(width - 1), width - 1))
+            stdscr.attroff(curses.color_pair(4))
+            row += 1
+
+        all_entries: List[StatusTextEntry] = []
+        for sys_key in sorted_keys:
+            all_entries.extend(vehicles[sys_key].status_entries)
+        all_entries.sort(key=lambda e: e.received_at)
+
+        log_rows = height - row - 1
+        if log_rows <= 0:
+            stdscr.refresh()
+            return
+
+        if not all_entries:
+            empty = ' (no messages in last 30s) '
+            if row < height - 1:
+                stdscr.addstr(row, 0, truncate(empty, width - 1))
+        else:
+            visible = all_entries[-log_rows:]
+            for entry in visible:
+                if row >= height - 1:
+                    break
+                self._draw_status_line(stdscr, row, width, entry)
+                row += 1
+
+        stdscr.refresh()
+
+    def _draw_vehicle_block(
+        self,
+        stdscr,
+        row: int,
+        width: int,
+        state: VehicleDiagState,
+        now: float,
+    ) -> int:
+        summary = state.summary or {}
+        mode = summary.get('mode_name', 'UNKNOWN')
+        armed = summary.get('armed', False)
+        conn = summary.get('connection_type', '?')
+        socket_id = summary.get('socket_id', -1)
+        mav_status = summary.get('mav_status', 0)
+        mav_label = MAV_STATE_LABELS.get(int(mav_status), str(mav_status))
+
+        last_update = max(state.last_summary_ts, state.last_diag_ts)
+        age_sec = (now - last_update) if last_update > 0 else -1.0
+        stale = age_sec >= 0 and age_sec > STALE_SEC
+
+        armed_str = 'ARMED' if armed else 'DISARMED'
+        flags = []
+        if state.offline:
+            flags.append('OFFLINE')
+        if stale and not state.offline:
+            flags.append('STALE')
+        flag_str = f"  [{' '.join(flags)}]" if flags else ''
+
+        line1 = (
+            f' {state.sys_key} (MAV {state.sysid})  {mode}  {armed_str}  '
+            f'{conn}  socket:{socket_id}  {mav_label}  '
+            f'updated {format_age(age_sec)}{flag_str}'
+        )
+        if row < stdscr.getmaxyx()[0] - 1:
+            if state.offline:
+                stdscr.attron(curses.color_pair(5))
+                stdscr.addstr(row, 0, truncate(line1, width - 1))
+                stdscr.attroff(curses.color_pair(5))
+            else:
+                stdscr.addstr(row, 0, truncate(line1, width - 1))
+                armed_pos = line1.find(armed_str)
+                if armed_pos >= 0:
+                    pair = curses.color_pair(2) if armed else curses.color_pair(1)
+                    stdscr.attron(pair | curses.A_BOLD)
+                    stdscr.addstr(row, armed_pos, armed_str)
+                    stdscr.attroff(pair | curses.A_BOLD)
+        row += 1
+
+        if state.diag is not None and row < stdscr.getmaxyx()[0] - 1:
+            load_pct = state.diag.mcu_load / 10.0
+            drop_pct = state.diag.bus_error_rate / 10.0
+            line2 = f' MCU {load_pct:.1f}%  CommDrop {drop_pct:.1f}%'
+            stdscr.addstr(row, 0, truncate(line2, width - 1))
+            row += 1
+
+            sensors = decode_sensors(
+                state.diag.sensors_install_mask,
+                state.diag.sensors_enabled_mask,
+                state.diag.sensors_health_mask,
+            )
+            row = self._draw_sensor_line(stdscr, row, width, sensors)
+        elif row < stdscr.getmaxyx()[0] - 1:
+            stdscr.addstr(row, 0, truncate(' (no sys_diags yet)', width - 1))
+            row += 1
+
+        return row
+
+    def _draw_sensor_line(
+        self,
+        stdscr,
+        row: int,
+        width: int,
+        sensors: List[Tuple[str, str]],
+    ) -> int:
+        max_row, _ = stdscr.getmaxyx()
+        if not sensors:
+            if row < max_row - 1:
+                stdscr.addstr(row, 0, truncate(' Sensors: (none installed)', width - 1))
+            return row + 1
+
+        lines: List[List[Tuple[str, str]]] = [[]]
+        line_len = len(' Sensors: ')
+        for label, status in sensors:
+            token = f'{label}:{status}  '
+            if lines[-1] and line_len + len(token) >= width - 1:
+                lines.append([])
+                line_len = 2
+            lines[-1].append((label, status))
+            line_len += len(token)
+
+        for idx, line_items in enumerate(lines):
+            if row >= max_row - 1:
+                break
+            prefix = ' Sensors: ' if idx == 0 else '  '
+            col = 0
+            stdscr.addstr(row, col, prefix)
+            col += len(prefix)
+            for label, status in line_items:
+                token = f'{label}:{status}  '
+                if col + len(token) >= width - 1:
+                    stdscr.addstr(row, col, truncate('...', width - col - 1))
+                    return row + 1
+                pair = curses.A_NORMAL
+                if status == 'OK':
+                    pair = curses.color_pair(1)
+                elif status == 'FAIL':
+                    pair = curses.color_pair(2) | curses.A_BOLD
+                elif status == 'OFF':
+                    pair = curses.color_pair(3)
+                stdscr.attron(pair)
+                stdscr.addstr(row, col, token)
+                stdscr.attroff(pair)
+                col += len(token)
+            row += 1
+        return row
+
+    def _draw_status_line(
+        self,
+        stdscr,
+        row: int,
+        width: int,
+        entry: StatusTextEntry,
+    ) -> None:
+        sev_label = SEVERITY_LABELS.get(entry.severity, str(entry.severity))
+        prefix = f'[{entry.sys_key:5}] [{entry.severity} {sev_label:5}] '
+        body = truncate(entry.text, max(0, width - len(prefix) - 2))
+        line = prefix + body
+
+        pair = curses.A_NORMAL
+        if entry.severity <= 3:
+            pair = curses.color_pair(2)
+        elif entry.severity == 4:
+            pair = curses.color_pair(3)
+        elif entry.severity == 5:
+            pair = curses.color_pair(6)
+        elif entry.severity == 6:
+            pair = curses.color_pair(1)
+        elif entry.severity == 7:
+            pair = curses.color_pair(5)
+
+        stdscr.attron(pair)
+        stdscr.addstr(row, 0, truncate(line, width - 1))
+        stdscr.attroff(pair)
+
+
+def main() -> int:
+    if not sys.stdout.isatty():
+        print('vehicleDiagShow requires an interactive terminal (TTY).', file=sys.stderr)
+        return 1
+
+    rclpy.init()
+    node = VehicleDiagNode()
+    spin_thread = threading.Thread(target=rclpy.spin, args=(node,), daemon=True)
+    spin_thread.start()
+
+    try:
+        DiagCursesApp(node).run()
+    finally:
+        node.destroy_node()
+        rclpy.shutdown()
+        spin_thread.join(timeout=2.0)
+
+    return 0
+
+
+if __name__ == '__main__':
+    sys.exit(main())

src/unitdev02/unitdev02/devnote.txt

@@ -27,6 +27,7 @@
 python -m fc_network_adapter.tests.test_vehicleStatusPublisher
 python -m fc_network_adapter.tests.test_ringBuffer
 python -m fc_network_adapter.fc_network_adapter.mainOrchestrator
+python -m fc_network_adapter.fc_network_adapter.serialManager
 
 python -m someotherpkg.src.example_takeoff_land
 python -m someotherpkg.src.example_change_mode

src/unitdev02/unitdev02/esp32.py

@@ -0,0 +1,641 @@
+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)
+#
+# UAV -> GCS:
+#   HELO + sysid(1)
+#   DONE + sysid(1) + sent_len(2) + remain_len(2)
+#
+# 注意：
+# - HELO 不帶 remain。
+# - DONE 保留 remain。
+# - MY_SYSID 不寫死，由飛控 MAVLink 自動學。
+# - DEST_64 不寫死，由 GCS 封包的 XBee 0x90 src64 自動學。
+# =========================================================
+
+
+# ================= 設定區 =================
+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
+
+    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 MY_SYSID is not None:
+                return
+
+            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
+
+    if MY_SYSID is None: # TODO 這個重複判斷了 應該刪 不要浪費效率
+        return
+
+    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() # TODO 加一個 if MY_SYSID is None
+
+                        if MY_SYSID is not None and DEST_64 is not None:
+                            delay_ms = 20 + ((MY_SYSID * 37) + (time.ticks_ms() & 0xFF)) % 180 # TODO 這邊會有多少秒延遲? 為啥是37? 不能21或者17? 20 ~ 199  210 / 30000
+                            time.sleep_ms(delay_ms)
+                            send_hello_report(force=True)
+                            DISC_PENDING = False
+                        else:
+                            DISC_PENDING = True
+
+                    # -------------------------------------------------
+                    # 判斷是不是 POLL
+                    # -------------------------------------------------
+                    else: # TODO 這邊要用 elif 去判斷 POLL
+                        target_sysid, grant_bytes = parse_poll_payload(real_data)
+
+                        if target_sysid is not None:
+                            learn_dest64_from_src64(src64)
+                            learn_my_sysid_from_tx_buf()
+                            try_send_pending_hello() # TODO 這邊三行怪怪的 GCS 下 poll 然後會回應 HELO 這個很怪 然後為何要每次重做上面兩行 拖效率
+
+                            if MY_SYSID is not None and target_sysid == MY_SYSID:
+                                flush_tx_buffer(grant_bytes)
+
+                        else: 
+                            # 一般 GCS -> FC MAVLink 下行資料
+                            if DEST_64 is None: # TODO 這個概念不對 隨便一個 poll 就把本來有做過 DISC 的權限搶走了 危險!!!
+                                learn_dest64_from_src64(src64)
+
+                            if DEST_64 is not None and src64 == DEST_64: # TODO 從 GCS 到 FC 的 mavlink 封包 切開不要跟 POLL 放在同一個判斷
+                                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/unitdev02/unitdev02/ntrip_dev.py

@@ -0,0 +1,209 @@
+# import socket
+
+# def get_rtk2go_source_table():
+#     host = "rtk2go.com"
+#     port = 2101
+#
+#     print(f"正在連線至 {host}:{port} 獲取掛載點列表...\n")
+#     ...
+
+from __future__ import annotations
+
+import socket
+import base64
+import time
+
+
+# ── NMEA GGA（告訴 Caster「我在哪」，觸發 RTCM 推流）────────────────
+
+def nmea_checksum(body: str) -> str:
+    """body 不含 '$' 與 '*checksum'，例如 'GPGGA,123519,...'"""
+    value = 0
+    for ch in body:
+        value ^= ord(ch)
+    return f"{value:02X}"
+
+
+def decimal_to_nmea_dm(deg: float, *, is_latitude: bool) -> tuple[str, str]:
+    """十進位度 → NMEA 的 (d)dmm.mmmm 與半球字元。"""
+    if is_latitude:
+        hemi = "N" if deg >= 0 else "S"
+        deg_width = 2
+    else:
+        hemi = "E" if deg >= 0 else "W"
+        deg_width = 3
+    deg = abs(deg)
+    d = int(deg)
+    m = (deg - d) * 60.0
+    return f"{d:0{deg_width}d}{m:07.4f}", hemi
+
+
+def build_gga_sentence(lat_deg: float, lon_deg: float, alt_m: float = 100.0) -> bytes:
+    """
+    組一筆 $GPGGA 句子（含 checksum），回傳 bytes 可直接 sock.sendall。
+
+    lat_deg / lon_deg：十進位經緯度（北、東為正）。
+    測試時請改成 mount 服務範圍內的近似位置；正式使用應來自 GNSS 真實定位。
+    """
+    utc = time.gmtime()
+    t_str = f"{utc.tm_hour:02d}{utc.tm_min:02d}{utc.tm_sec:02d}.00"
+
+    lat_dm, ns = decimal_to_nmea_dm(lat_deg, is_latitude=True)
+    lon_dm, ew = decimal_to_nmea_dm(lon_deg, is_latitude=False)
+
+    # quality=1（GPS fix）、8 顆星、HDOP=1.0 僅供測試示意
+    body = (
+        f"GPGGA,{t_str},{lat_dm},{ns},{lon_dm},{ew},"
+        f"1,08,1.0,{alt_m:.1f},M,0.0,M,,"
+    )
+    sentence = f"${body}*{nmea_checksum(body)}\r\n"
+    return sentence.encode("ascii")
+
+
+def send_gga(sock: socket.socket, lat_deg: float, lon_deg: float, alt_m: float = 100.0) -> str:
+    """送出 GGA，回傳可讀句子供列印。"""
+    payload = build_gga_sentence(lat_deg, lon_deg, alt_m)
+    sock.sendall(payload)
+    return payload.decode("ascii").strip()
+
+
+# ── NTRIP 連線 + RTCM 接收 ─────────────────────────────────────────
+
+def get_rtcm_payload(
+    host,
+    port,
+    mountpoint,
+    user="",
+    password="",
+    target_ids=None,
+    *,
+    gga_lat=None,
+    gga_lon=None,
+    gga_alt_m=100.0,
+    gga_interval_sec=5.0,
+    filter_ids=True,
+):
+    """
+  連線 NTRIP caster 並解析 RTCM3。
+
+  gga_lat / gga_lon：若兩者皆有設定，握手成功後會週期送出 GGA（許多 Caster 如 SNIP 至少需一次）。
+  gga_interval_sec：GGA 重送間隔（秒）。
+  filter_ids：True 時只印 target_ids 內的 message；False 印全部（方便學習觀察）。
+    """
+    if target_ids is None:
+        target_ids = []
+    need_gga = gga_lat is not None and gga_lon is not None
+
+    sock = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
+    sock.settimeout(10)
+
+    try:
+        sock.connect((host, port))
+
+        auth = base64.b64encode(f"{user}:{password}".encode()).decode()
+        request = f"GET /{mountpoint} HTTP/1.0\r\n"
+        request += "User-Agent: NTRIP PythonClient\r\n"
+        request += f"Authorization: Basic {auth}\r\n"
+        request += "Connection: close\r\n\r\n"
+        sock.sendall(request.encode())
+
+        response = sock.recv(4096)
+        if b"ICY 200 OK" not in response:
+            print(f"連線失敗: {response.decode(errors='ignore')}")
+            return
+
+        print(f"已成功連接至 {mountpoint}")
+        if need_gga:
+            print(
+                f"GGA 模式：每 {gga_interval_sec}s 回報位置 "
+                f"({gga_lat:.6f}, {gga_lon:.6f})，觸發 Caster 推流"
+            )
+        else:
+            print("未設定 GGA（部分 Caster 如 RTK2GO 仍可推流；SNIP/區域站常需 GGA）")
+        print(f"RTCM 過濾目標: {target_ids if filter_ids else '(關閉，顯示全部)'}\n")
+
+        last_gga_time = 0.0
+        buffer = b""
+
+        while True:
+            now = time.time()
+            if need_gga and (now - last_gga_time >= gga_interval_sec):
+                line = send_gga(sock, gga_lat, gga_lon, gga_alt_m)
+                print(f"[GGA] {line}")
+                last_gga_time = now
+
+            try:
+                chunk = sock.recv(4096)
+            except socket.timeout:
+                continue
+
+            if not chunk:
+                print("Caster 關閉連線")
+                break
+
+            buffer += chunk
+
+            while len(buffer) >= 3:
+                if buffer[0] != 0xD3:
+                    buffer = buffer[1:]
+                    continue
+
+                length = ((buffer[1] & 0x03) << 8) | buffer[2]
+                total_len = length + 6
+
+                if len(buffer) < total_len:
+                    break
+
+                packet = buffer[:total_len]
+                buffer = buffer[total_len:]
+                msg_id = (packet[3] << 4) | (packet[4] >> 4)
+
+                if not filter_ids or str(msg_id) in target_ids:
+                    print(f"Found ID: {msg_id} | Length: {total_len} bytes")
+
+    except KeyboardInterrupt:
+        print("\n使用者中斷")
+    except Exception as e:
+        print(f"發生錯誤: {e}")
+    finally:
+        sock.close()
+
+
+if __name__ == "__main__":
+    TARGET_LIST = ["1007", "1230", "1074"]
+
+    # ── Case 1：RTK2GO（通常可不送 GGA）────────────────────────────
+    # USER = "chiyu1468@hotmail.com"
+    # HOST = "rtk2go.com"
+    # PORT = 2101
+    # MOUNT = "No1bio_02"
+    # get_rtcm_payload(
+    #     HOST, PORT, MOUNT,
+    #     user=USER,
+    #     target_ids=TARGET_LIST,
+    #     filter_ids=False,
+    # )
+
+    # ── Case 2：區域 Caster（需 GGA，座標請改成服務區內近似點）──────
+    USER = "uavlab6061"
+    PW = "iamsupersmart"
+    HOST = "210.241.63.193"
+    PORT = 81
+    MOUNT = "2020_GNSS"
+    # 十進位度；請依 SNIP / 基準站服務範圍調
+    GGA_LAT = 24.155792000
+    GGA_LON = 120.630679000
+
+    get_rtcm_payload(
+        HOST,
+        PORT,
+        MOUNT,
+        user=USER,
+        password=PW,
+        target_ids=TARGET_LIST,
+        gga_lat=GGA_LAT,
+        gga_lon=GGA_LON,
+        gga_alt_m=100.0,
+        gga_interval_sec=30.0,
+        filter_ids=False,
+    )

src/unitdev02/unitdev02/tryByteDict.py

@@ -0,0 +1,33 @@
+import timeit
+
+# 建立 1000 個元素的字典，避免單一 Key 的特殊雜湊值干擾
+d_int = {i: True for i in range(1000)}
+d_bytes = {f"k_{i:06d}".encode(): True for i in range(1000)}
+
+# 正確命中中間的值
+target_int = 500
+target_bytes_same = f"k_{500:06d}".encode()
+
+# 將變數傳入 timeit 的全域環境
+setup_env = lambda: None
+globals_dict = {
+    'd_int': d_int,
+    'd_bytes': d_bytes,
+    'target_int': target_int,
+    'target_bytes_same': target_bytes_same
+}
+
+# 1. int 查詢
+t_int = timeit.timeit("d_int.get(target_int)", globals=globals_dict, number=50000000)
+
+# 2. bytes 查詢 (同物件：直接傳入現成變數)
+t_bytes_same = timeit.timeit("d_bytes.get(target_bytes_same)", globals=globals_dict, number=50000000)
+
+# 3. bytes 查詢 (真・新物件：每次都在查詢時動態切片生成新物件，強迫重新計算 Hash)
+globals_dict['dynamic_source'] = f"k_{500:06d}".encode() + b"dummy"
+t_bytes_new = timeit.timeit("d_bytes.get(dynamic_source[:8])", globals=globals_dict, number=50000000)
+
+print(f"修正後測試結果 (50,000,000 次)：")
+print(f"int 查詢          : {t_int:.4f} 秒 (1.0x)")
+print(f"bytes 查詢 (同物件): {t_bytes_same:.4f} 秒 (約 {t_bytes_same/t_int:.2f}x)")
+print(f"bytes 查詢 (新物件): {t_bytes_new:.4f} 秒 (約 {t_bytes_new/t_int:.2f}x)")