(modify) serialManager.py

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

src/fc_network_adapter/fc_network_adapter/mainOrchestrator.py

@@ -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()
@@ -812,6 +816,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"),
@@ -1595,6 +1600,7 @@ class Orchestrator:
         # 定義通訊類型映射表
         COMM_TYPE_MAP = {
             "XBee(API-AT)": sm.SerialMode.XBEEAPI2AT,
+            "XBee(API-AT)": sm.SerialMode.XBEEAPI_espv1,
             "TELEMETRY": sm.SerialMode.STRAIGHT,
             # 新增區
         }
@@ -1638,25 +1644,7 @@ class Orchestrator:
 
 def main():
 
-    # =========== 各項模組的版本先驗 ===========
+    logger.info(f"Each Module Running Version at mavlinkObkect:{mo.MODULE_VER}, mavlinkROS2Nodes:{mros.MODULE_VER}, mavlinkVehicleView:{mvv.MODULE_VER}, serialManager:{sm.MODULE_VER}")
-    # 除非你有在做這幾項模組的改版 不然動到這邊的版本號 代表執行環境有很大的問題!!!!!!
-    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 != "2.00":
-        print("Module Version Error! : serialManager")
-        version_check = False
-    if version_check == False:
-        print("Environment Obstacle! Check YOUR Execution System Path First!!")
-        return
-    # ========================================
 
     stop_evt = threading.Event()
     def signal_handler(signum, frame):

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,
+    vehicle_registry, # 儲存全部物件的地方
-    VehicleView,
+    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

@@ -187,6 +187,7 @@ class RFStatus:
 @dataclass
 class SocketInfo:
     """Socket連接資訊"""
+    src64_addr: Optional[bytes] = None # 模組的物理定址
     ip: Optional[str] = None  # IP位址
     port: Optional[int] = None  # 埠號
     local_ip: Optional[str] = None  # 本地IP
@@ -310,6 +311,7 @@ class RFModule:
 
 class VehicleView:
     """
+    最上層
     載具視圖 - 純狀態容器
     
     特點:

src/fc_network_adapter/fc_network_adapter/serialManager.py

@@ -19,11 +19,18 @@ from enum import Enum, auto
 from abc import ABC, abstractmethod
 from dataclasses import dataclass
 from typing import Callable, Optional
+from typing import List
 
 # # XBee 模組
 # from xbee.frame import APIFrame
 
 # 自定義的 import
+from .mavlinkVehicleView import (
+    vehicle_registry, # 儲存全部物件的地方
+    VehicleView,
+    RFModule,
+    RFModuleType,
+)
 from .utils import RingBuffer, setup_logger
 from .utils import pollStrategy
 
@@ -138,6 +145,8 @@ class XBeeFrameProcessor_Base(FrameProcessor):
     DEST_ADDR16_UNICAST = b'\xFF\xFE'
     DEST_ADDR16_BRAODCAST = b'\xFF\xFF'
 
+    DEST_ADDR64_BRAODCAST = b'\x00\x00\x00\x00\x00\x00\x00\x00'
+
     def __init__(self, at_handler: "ATCommandHandler" = None):
         super().__init__()
         self.at_handler = at_handler
@@ -221,7 +230,7 @@ class XBeeFrameProcessor_Base(FrameProcessor):
     @staticmethod
     def _encapsulate(
         data: bytes,
-        dest_addr64: bytes = b'\x00\x00\x00\x00\x00\x00\x00\x00',
+        dest_addr64: bytes = DEST_ADDR64_BRAODCAST,
         dest_addr16 = DEST_ADDR16_BRAODCAST,
         frame_id: int = 0x01,
     ) -> bytes:
@@ -251,14 +260,26 @@ class XBeeFrameProcessor_Base(FrameProcessor):
 
 
 class XBeeFrameProcessor_ESPv1(XBeeFrameProcessor_Base):
+    '''
+    ESP32 封包分類
+    - Mavlink 資料封包 : 
+        - Payload = mavlink [方向] GCS -> UAV (broadcast) 
+        - Payload = mavlink [方向] UAV -> GCS (with src64 Addr)
+    - Discovery 階段
+        - Payload = DISC_HEADER [方向] GCS -> UAV (broadcast)
+        - Payload = HELLO_HEADER + esp_sysid(1) [方向] UAV -> GCS (with src64 Addr)
+    - Poll 階段
+        - Payload = POLL_HEADER + esp_sysid(1) + grant_bytes(2) [方向] GCS -> UAV (with src64 Addr)
+        - Payload = DONE_HEADER + sysid(1) + sent_len(2) + remain_len(2) [方向] UAV -> GCS (with src64 Addr)
 
-    # GCS -> UAV:
+    這邊認定 esp_sysid 會等於 mavlink sysid
-    #   DISC
+
-    #   POLL + esp_sysid(1) + grant_bytes(2)
+    硬體產品系列 (Product Family) : XBP9B-DM
-    #
+    晶片世代: XBee PRO 900HP 200K
-    # UAV -> GCS:
+    運作頻段: 900 MHz RF。
-    #   HELO + esp_sysid(1)
+    運行協議 (Protocol / Function Set): DigiMesh
-    #   DONE + sysid(1) + sent_len(2) + remain_len(2)
+    韌體版本 (Firmware Version): 8075
+    '''
 
     DISC_HEADER = b'DISC'
     HELLO_HEADER = b'HELO'
@@ -269,6 +290,11 @@ class XBeeFrameProcessor_ESPv1(XBeeFrameProcessor_Base):
     MAX_PAYLOAD_PER_FRAME = 80
     CHUNK_SEND_INTERVAL_SEC = 0.01
 
+    # ADDR16 選項
+    DEST_ADDR16_BRAODCAST = b'\xFF\xFF'
+
+    DEST_ADDR64_BRAODCAST = b'\x00\x00\x00\x00\x00\x00\xFF\xFF'
+
     class Esp32DeviceInfo:
         def __init__(self, system_id, address_64, last_hello_time):
             self.system_id = system_id
@@ -276,6 +302,7 @@ class XBeeFrameProcessor_ESPv1(XBeeFrameProcessor_Base):
             self.last_hello_time = last_hello_time
 
             self.remain_bytes = 0 # 剩餘 buffer 量
+            self.last_poll_time = 0.0
             self.last_done_time = 0.0 # 最後送出Done的時間
             self.received_len = 0 # 收到封包累計
 
@@ -300,7 +327,9 @@ class XBeeFrameProcessor_ESPv1(XBeeFrameProcessor_Base):
         self.poll_done_event: Optional[asyncio.Event] = None
         self.current_poll_address_64: Optional[bytes] = None
 
-        self.last_discovery_time = 0.0
+        self.last_discovery_time = 0.0 # 這個是最後做廣播 discovery 的時間  
+        self.last_recieve_mavlink = 0.0 # 這個是最後收到 mavlink payload 時間 為了定義 poll-done 之間不要超時用的
+        self.MAX_mavPack_interval_timeout = 100  # ms，poll 期間 MAVLink/DONE 最大閒置間隔
         self.discovery_interval_seconds = 30.0 # 每次做 discovery 程序的間隔時間
         self.device_offline_timeout = self.discovery_interval_seconds * 2 # 遠端沒有回應會被踢出 超時時限
         self.operator_tick_interval_seconds = 0.03 # 
@@ -351,6 +380,8 @@ class XBeeFrameProcessor_ESPv1(XBeeFrameProcessor_Base):
             remote_device = self.esp32_address_mapping.get(sender_address_64)
             if remote_device is not None:
                 remote_device.received_len += len(payload)
+                if (self.operator_busy and self.current_poll_address_64 == sender_address_64):
+                    self.last_recieve_mavlink = time.time()
             return payload
 
         if frame_type == self.FRAME_TYPE_AT_RESPONSE:
@@ -375,6 +406,7 @@ class XBeeFrameProcessor_ESPv1(XBeeFrameProcessor_Base):
     def pack_discovery(self) -> bytes:
         return self._encapsulate(self.DISC_HEADER, frame_id=0x00)
 
+    # 處理每個裝置回傳的 Hello 訊息
     def handle_hello_report(self, payload: bytes, sender_address_64: bytes) -> None:   
         system_id = payload[4]
         remote_device = self.esp32_address_mapping.get(sender_address_64)
@@ -384,8 +416,7 @@ class XBeeFrameProcessor_ESPv1(XBeeFrameProcessor_Base):
                 system_id, sender_address_64, time.time()
             )
             logger.debug(
-                f"new HELO system_id={system_id}, address_64={sender_address_64.hex()}"
+                f"new HELO system_id={system_id}, address_64={sender_address_64.hex()}")
-            )
         elif remote_device.address_64 == sender_address_64:
             remote_device.last_hello_time = time.time()
         else:
@@ -394,6 +425,10 @@ class XBeeFrameProcessor_ESPv1(XBeeFrameProcessor_Base):
                 f"address_64={sender_address_64.hex()}"
             )
         
+        if vehicle:=vehicle_registry.get(system_id):
+            if not vehicle.rf_module:
+                vehicle.rf_module = RFModule(RFModuleType.XBEE)
+
     def pack_poll(self, target_address_64: bytes, grant_bytes: int = 0) -> Optional[bytes]:
         remote_device = self.esp32_address_mapping.get(target_address_64)
         if remote_device is None:
@@ -415,13 +450,16 @@ class XBeeFrameProcessor_ESPv1(XBeeFrameProcessor_Base):
         if remote_device is None:
             return
 
-        system_id, sent_length, remain_length = struct.unpack('>BHH', payload[4:9])
+        # 這段是有問題的 因為會有整數封包切割問題 以及載具端的 buffer 存量不足 故回傳的資訊量會與要求的不一致
-        if sent_length != remote_device.received_len:
+        # system_id, sent_length, remain_length = struct.unpack('>BHH', payload[4:9])
-            logger.info(
+        # if sent_length != remote_device.received_len:
-                f"POLL may be missing packets sent={sent_length} "
+        #     logger.info(
-                f"received={remote_device.received_len} system_id={system_id}"
+        #         f"POLL may be missing packets sent={sent_length} "
-            )
+        #         f"received={remote_device.received_len} system_id={system_id}"
+        #     )
 
+        # TODO 傳送速率
+        # TODO 累積速率預測
         remote_device.received_len = 0
         remote_device.remain_bytes = remain_length
         remote_device.last_done_time = time.time()
@@ -449,11 +487,11 @@ class XBeeFrameProcessor_ESPv1(XBeeFrameProcessor_Base):
         return len(self.gcs_transmit_queue)
 
     # 把 gcs_transmit_queue 的 mavlink 封包依照大小打包出來
-    def _pop_flush_batch(self, max_bytes: int) -> list[bytes]:
+    def _pop_flush_batch(self, max_bytes: int) -> List[bytes]:
         if not self.gcs_transmit_queue:
             return []
 
-        batch: list[bytes] = []
+        batch: List[bytes] = []
         total_bytes = 0
 
         while self.gcs_transmit_queue:
@@ -507,7 +545,7 @@ class XBeeFrameProcessor_ESPv1(XBeeFrameProcessor_Base):
     # ---- POLL 排程輔助 ----
 
     # 計算下次要 poll 的對象跟大小
-    def _pick_poll_target(self) -> tuple[Optional[bytes], int]:
+    def _pick_poll_target(self):
         poll_devices = [
             pollStrategy.PollDevice(
                 address_64=address_64,
@@ -715,6 +753,22 @@ class XBeeFrameProcessor_ESPv1(XBeeFrameProcessor_Base):
         self.is_discovery_phase = False
         self.pending_manual_discovery = False
 
+    async def _wait_poll_done_with_idle_timeout(self) -> bool:
+        idle_timeout_sec = self.MAX_mavPack_interval_timeout / 1000.0
+        poll_tick = min(0.02, idle_timeout_sec / 2)
+
+        while not self.poll_done_event.is_set():
+            if time.time() - self.last_recieve_mavlink >= idle_timeout_sec:
+                return False
+            try:
+                await asyncio.wait_for(
+                    self.poll_done_event.wait(),
+                    timeout=poll_tick,
+                )
+            except asyncio.TimeoutError:
+                continue
+        return True
+
     # poll 程序
     async def _run_one_poll(self, target_address_64: bytes, grant_bytes: int) -> None:
         if self.serial_writer is None:
@@ -733,18 +787,11 @@ class XBeeFrameProcessor_ESPv1(XBeeFrameProcessor_Base):
         self.current_poll_address_64 = target_address_64
         self.poll_done_event.clear()
         self.serial_writer(poll_frame)
+        self.last_recieve_mavlink = time.time()
 
-        timeout_seconds = pollStrategy.estimate_poll_timeout(
-            grant_bytes,
-            self.serial_baudrate,
-            self.guard_milliseconds,
-        )
         try:
-            await asyncio.wait_for(
+            completed = await self._wait_poll_done_with_idle_timeout()
-                self.poll_done_event.wait(),
+            if not completed:
-                timeout=timeout_seconds,
-            )
-        except asyncio.TimeoutError:
                 logger.warning(
                     f"POLL timeout address_64={target_address_64.hex()} "
                     f"grant_bytes={grant_bytes}"
@@ -819,21 +866,21 @@ class ATCommandHandler:
     # ---- 接收端 ----
     def handle_frame(self, frame: bytes) -> None:
         """
-        接收一整個 AT Response frame：
+        接收一整個 AT Response frame:
         1. 解析成 ATResponse
         2. 推進 rx_module_ack 供其他模組消費
         3. 本地 dispatch 給對應的 _handle_xxx
         """
-        parsed = self._parse(frame)
+        parsed_at_ack = self._parse(frame)
-        if parsed is None:
+        if parsed_at_ack is None:
             return
 
-        if not rx_module_ack.put(parsed):
+        if not rx_module_ack.put(parsed_at_ack):
             logger.warning(
-                f"[{self.serial_port}] rx_module_ack overflow, drop {parsed.command!r}"
+                f"[{self.serial_port}] rx_module_ack overflow, drop {parsed_at_ack.command!r}"
             )
 
-        self._dispatch(parsed)
+        self._dispatch(parsed_at_ack)
 
     @staticmethod
     def _parse(frame: bytes) -> ATResponse:
@@ -864,26 +911,27 @@ class ATCommandHandler:
 
         handler = self.handlers.get(response.command)
         if handler:
-            handler(response.data)
+            handler(response)
         else:
             logger.debug(
                 f"[{self.serial_port}] 未處理的 AT 指令: "
                 f"{response.command.decode()}"
             )
 
-    def _handle_rssi(self, data: bytes):
+    def _handle_rssi(self, response: ATResponse):
         """處理 DB (RSSI) 回應：單 byte 無號值，單位 dBm"""
-        pass
-        if data:
-            print(f"[{self.serial_port}] RSSI = -{data[0]} dBm")  # dev
-            # logger.debug(f"[{self.serial_port}] RSSI = -{data[0]} dBm") # dev
-            pass
 
-    def _handle_serial_high(self, data: bytes):
+        # print(f"[{self.serial_port}] RSSI = -{data[0]} dBm")  # dev
+        logger.debug(f"[{self.serial_port}] RSSI = -{response.data[0]} dBm") # dev
+
+
+
+
+    def _handle_serial_high(self, response: ATResponse):
         """處理 SH (Serial Number High)"""
         pass
 
-    def _handle_serial_low(self, data: bytes):
+    def _handle_serial_low(self, response: ATResponse):
         """處理 SL (Serial Number Low)"""
         pass
 
@@ -1245,7 +1293,7 @@ class serial_manager:
 
     def send_at_command(self, serial_id, request: ATRequest) -> bool:
         """
-        對指定 serial_id 的 XBee dongle 發送一筆 AT 指令（thread-safe）
+        對指定 serial_id 的 XBee dongle 發送一筆 AT 指令 (thread-safe)
         - serial_id: create_serial_link 取得的編號
         - request: ATRequest 物件，攜帶 command / parameter / frame_id
         回傳是否成功排進事件圈
@@ -1326,46 +1374,51 @@ if __name__ == '__main__':
     # UDP_REMOTE_PORT = 14571
     # sm.create_serial_link(SERIAL_PORT, SERIAL_BAUDRATE, UDP_REMOTE_PORT, SerialMode.STRAIGHT)
 
-    # # 測試項二
+    # 測試項二
-    # SERIAL_PORT = '/dev/ttyUSB0'  # 手動指定
+    print("運行 測試項二")
-    # SERIAL_BAUDRATE = 115200
+    SERIAL_PORT = '/dev/ttyUSB0'  # 手動指定
-    # 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)
-
-    # # 等 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()
-
-    # # 測試項三
-    SERIAL_PORT = '/dev/ttyUSB0'
     SERIAL_BAUDRATE = 115200
     UDP_REMOTE_PORT = 14561
-    sm.create_serial_link(SERIAL_PORT, SERIAL_BAUDRATE, UDP_REMOTE_PORT, SerialMode.XBEEAPI_espv1)
+    sm.create_serial_link(SERIAL_PORT, SERIAL_BAUDRATE, UDP_REMOTE_PORT, SerialMode.XBEEAPI2AT)
-
-    time.sleep(2)  # 等 serial 連線與 operator 啟動
-
     serial_id = 1
-    processor = sm.get_espv1_processor(serial_id)
+    device_sys_id = 10
-    if processor is not None:
-        processor.request_discovery()
-        processor.request_poll(target_system_id=1)
-        processor.request_poll(target_system_id=1, grant_bytes=200)
-        print(processor.get_status_snapshot())
-        print(processor.get_gcs_queue_byte_count())
 
-    sm.remove_serial_link(serial_id)
+    linked_serial = sm.get_serial_link()
-    time.sleep(3)
+    print(f"連結完成 : {linked_serial}. 等待兩秒")
+
+    # 等 connection_made 完成 writer 注入，再發一筆 AT 指令測試
+    time.sleep(2)
+    rssi_request = ATRequest(command=b'DB', parameter=b'', frame_id=device_sys_id)
+    print(f"手動送出 DB AT Command:")
+    for i in range(20):
+        sm.send_at_command(1, rssi_request)
+        time.sleep(1)
+
+    sm.remove_serial_link(1)
+    time.sleep(2)
     sm.shutdown()
+    print("結束運行")
+
+    # # 測試項三
+    # SERIAL_PORT = '/dev/ttyUSB0'
+    # SERIAL_BAUDRATE = 115200
+    # UDP_REMOTE_PORT = 14561
+    # sm.create_serial_link(SERIAL_PORT, SERIAL_BAUDRATE, UDP_REMOTE_PORT, SerialMode.XBEEAPI_espv1)
+
+    # time.sleep(2)  # 等 serial 連線與 operator 啟動
+
+    # serial_id = 1
+    # processor = sm.get_espv1_processor(serial_id)
+    # if processor is not None:
+    #     processor.request_discovery()
+    #     processor.request_poll(target_system_id=1)
+    #     processor.request_poll(target_system_id=1, grant_bytes=200)
+    #     print(processor.get_status_snapshot())
+    #     print(processor.get_gcs_queue_byte_count())
+
+    # sm.remove_serial_link(serial_id)
+    # time.sleep(30)
+    # sm.shutdown()
 
 '''
 ================= 改版記錄 ============================

src/fc_network_adapter/fc_network_adapter/utils/pollStrategy.py

@@ -5,6 +5,7 @@ POLL 輪詢策略，供 XBeeFrameProcessor_ESPv1 使用。
 """
 
 from dataclasses import dataclass
+from typing import List
 
 
 @dataclass(frozen=True)
@@ -23,10 +24,9 @@ class PollSchedulerState:
 
 
 def pick_next(
-    devices: list[PollDevice],
+    devices: List[PollDevice],
-    scheduler_state: PollSchedulerState,
+    scheduler_state: PollSchedulerState
-    # default_grant_bytes: int = 600,
+):
-) -> tuple[bytes | None, int]:
     """
     選下一個 POLL 目標。
 
@@ -46,26 +46,3 @@ def pick_next(
 
     return selected_device.address_64, grant_bytes
 
-
-def estimate_poll_timeout(
-    grant_bytes: int,
-    baudrate: int,
-    guard_milliseconds: int = 20,
-) -> float:
-    """
-    估算 POLL 後等待 DONE 的超時秒數。
-    移植自 udptest8 estimate_tdma_timeout。
-    """
-    grant_bytes = max(0, int(grant_bytes))
-    max_payload_per_chunk = 100
-    chunk_count = max(1, (grant_bytes + max_payload_per_chunk - 1) // max_payload_per_chunk)
-
-    uart_time = (grant_bytes + chunk_count * 18 + 32) * 10.0 / baudrate
-    timeout = (
-        uart_time
-        + chunk_count * 0.012
-        + (guard_milliseconds / 1000.0)
-        + 0.35
-    )
-    return timeout
-