@@ -19,18 +19,25 @@ 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
# ====================== 分割線 =====================
logger = setup_logger ( os . path . basename ( __file__ ) )
MODULE_VER = " 2.00 "
MODULE_VER = " 2.02 "
rx_module_ack = RingBuffer ( capacity = 64 , buffer_id = 253 )
@@ -138,10 +145,15 @@ 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
# ---- 對外契約 ----
def process_incoming ( self , data : bytes ) - > bytes :
""" 處理 XBee API 幀並提取 payload """
@@ -221,8 +233,8 @@ class XBeeFrameProcessor_Base(FrameProcessor):
@staticmethod
def _encapsulate (
data : bytes ,
dest_addr64 : bytes = b ' \x00 \x00 \x00 \x00 \x00 \x00 \x00 \x00 ' ,
dest_addr16 = DEST_ADDR16_BRAODCAST ,
dest_addr64 : bytes = DEST_ADDR64_BRAODCAST ,
dest_addr16 : bytes = DEST_ADDR16_BRAODCAST ,
frame_id : int = 0x01 ,
) - > bytes :
"""
@@ -238,6 +250,8 @@ class XBeeFrameProcessor_Base(FrameProcessor):
frame + = dest_addr64 + dest_addr16
frame + = struct . pack ( " >BB " , broadcast_radius , options ) + data
checksum = 0xFF - ( sum ( frame ) & 0xFF )
# ret = b'\x7E' + struct.pack(">H", len(frame)) + frame + struct.pack("B", checksum)
# logger.debug(ret.hex())
return b ' \x7E ' + struct . pack ( " >H " , len ( frame ) ) + frame + struct . pack ( " B " , checksum )
@staticmethod
@@ -251,14 +265,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:
# DISC
# POLL + esp_sysid(1) + grant_bytes(2)
#
# UAV -> GCS:
# HELO + esp_sysid(1)
# DONE + sysid(1) + sent_len(2) + remain_len(2)
這邊認定 esp_sysid 會等於 mavlink sysid
硬體產品系列 ( Product Family ) : XBP9B - DM
晶片世代 : XBee PRO 900 HP 200 K
運作頻段 : 900 MHz RF 。
運行協議 ( Protocol / Function Set ) : DigiMesh
韌體版本 ( Firmware Version ) : 8075
'''
DISC_HEADER = b ' DISC '
HELLO_HEADER = b ' HELO '
@@ -269,6 +295,8 @@ class XBeeFrameProcessor_ESPv1(XBeeFrameProcessor_Base):
MAX_PAYLOAD_PER_FRAME = 80
CHUNK_SEND_INTERVAL_SEC = 0.01
class Esp32DeviceInfo :
def __init__ ( self , system_id , address_64 , last_hello_time ) :
self . system_id = system_id
@@ -276,9 +304,12 @@ 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 # 收到封包累計
def __init__ ( self , at_handler : " ATCommandHandler " = None ) :
super ( ) . __init__ ( at_handler )
@@ -289,6 +320,10 @@ class XBeeFrameProcessor_ESPv1(XBeeFrameProcessor_Base):
self . operator_busy = False
self . operator_running = False
# ADDR16 選項
self . DEST_ADDR16_BRAODCAST = b ' \xFF \xFE '
self . DEST_ADDR64_BRAODCAST = b ' \x00 \x00 \x00 \x00 \x00 \x00 \xFF \xFF '
self . serial_writer : Optional [ Callable [ [ bytes ] , None ] ] = None
self . event_loop : Optional [ asyncio . AbstractEventLoop ] = None
self . serial_baudrate = 115200
@@ -300,8 +335,10 @@ 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 . discovery_interval_seconds = 30.0 # 每次做 discovery 程序的間隔時間
self . last_discovery_time = 0.0 # 這個是最後做廣播 discovery 的時間
self . last_recieve_mavlink = 0.0 # 這個是最後收到 mavlink payload 時間 為了定義 poll-done 之間不要超時用的
self . mavPack_interval_timeout = 150 # ms，
self . discovery_interval_seconds = 200.0 # 每次做 discovery 程序的間隔時間
self . device_offline_timeout = self . discovery_interval_seconds * 2 # 遠端沒有回應會被踢出 超時時限
self . operator_tick_interval_seconds = 0.03 #
self . guard_milliseconds = 50 # POLL DONE 的保底時間間隔
@@ -351,6 +388,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 :
@@ -359,12 +398,12 @@ class XBeeFrameProcessor_ESPv1(XBeeFrameProcessor_Base):
return None
if frame_type == self . FRAME_TYPE_TX_STATUS :
length = ( frame [ 1 ] << 8 ) | frame [ 2 ]
logger . debug (
f " TX Status raw= { frame . hex ( ) } , api_len= { length } , "
f " fid=0x { frame [ 4 ] : 02X } , dest16=0x { ( frame [ 5 ] << 8 ) | frame [ 6 ] : 04X } , "
f " retry= { frame [ 7 ] } , delivery= { frame [ 8 ] } , discovery= { frame [ 9 ] } "
)
# length = (frame[1] << 8) | frame[2]
# logger.debug(
# f" TX Status raw={frame.hex()}, api_len={length}, "
# f" fid=0x{frame[4]:02X}, dest16=0x{(frame[5]<<8)|frame[6]:04X}, "
# f" retry={frame[7]}, delivery={frame[8]}, discovery={frame[9]}"
# )
return None
logger . warning ( f " Unknown XBee frame type: 0x { frame_type : 02X } " )
@@ -373,9 +412,11 @@ class XBeeFrameProcessor_ESPv1(XBeeFrameProcessor_Base):
# ---- DISC / POLL 封裝 ----
def pack_discovery ( self ) - > bytes :
return self . _encapsulate ( self . DISC_HEADER , frame_id = 0x00 )
# logger.debug(f"pack discovery")
return self . _encapsulate ( self . DISC_HEADER , dest_addr16 = self . DEST_ADDR16_BRAODCAST , dest_addr64 = self . DEST_ADDR64_BRAODCAST , frame_id = 0x00 )
def handle_hello_report ( self , payload : bytes , sender_address_64 : bytes ) - > None :
# 處理每個裝置回傳的 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 +425,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 :
@@ -393,6 +433,10 @@ class XBeeFrameProcessor_ESPv1(XBeeFrameProcessor_Base):
f " SYSID duplicated system_id= { system_id } , "
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 )
@@ -415,15 +459,20 @@ class XBeeFrameProcessor_ESPv1(XBeeFrameProcessor_Base):
if remote_device is None :
return
system_id , sent_length , remain_length = struct . unpack ( ' >BHH ' , payload [ 4 : 9 ] )
if sent_length != remote_device . received_len :
logger . info (
f " POLL may be missing packets sent= { sent_length } "
f " received= { remote_device . received_len } system_id= { system_id } "
)
remote_device . received_len = 0
system_id , sent_length , remain_length = struct . unpack ( ' >BHH ' , payload [ 4 : 9 ] )
remote_device . remain_bytes = remain_length
# 這段是有問題的 因為會有整數封包切割問題 以及載具端的 buffer 存量不足 故回傳的資訊量會與要求的不一致
# if sent_length != remote_device.received_len:
# logger.info(
# 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 . last_done_time = time . time ( )
if (
@@ -449,11 +498,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 +556,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 ,
@@ -529,7 +578,7 @@ class XBeeFrameProcessor_ESPv1(XBeeFrameProcessor_Base):
stale_addresses = [
address_64
for address_64 , device in self . esp32_address_mapping . items ( )
if ( now - device . last_hello_time > self . device_offline_timeout ) and \
if ( now - device . last_hello_time > self . device_offline_timeout ) and \
( now - device . last_done_time > self . device_offline_timeout )
]
for address_64 in stale_addresses :
@@ -544,12 +593,9 @@ class XBeeFrameProcessor_ESPv1(XBeeFrameProcessor_Base):
def _should_run_discovery ( self ) - > bool :
# 條件1. 目前沒有任何遠端ESP裝置被紀錄 或者 手動啟動
if ( not self . esp32_address_mapping ) or ( self . pending_manual_discovery ) :
return True
return ( time . time ( ) - self . last_discovery_time ) > 2
# 條件2. 每個固定週期 會做一次
return (
time . time ( ) - self . last_discovery_time
> = self . discovery_interval_seconds
)
return ( time . time ( ) - self . last_discovery_time ) > self . discovery_interval_seconds
# ---- 手動請求（thread-safe 對外 API）
@@ -715,6 +761,23 @@ 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 . mavPack_interval_timeout / 1000.0
poll_tick = min ( 0.02 , idle_timeout_sec )
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,22 +796,15 @@ 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 (
self . poll_done_event . wait ( ) ,
timeout = timeout_seconds ,
)
except asyncio . TimeoutError :
logger . warning (
f " POLL timeout address_64= { target_address_64 . hex ( ) } "
f " grant_bytes= { grant_bytes } "
)
completed = await self . _wait_poll_done_with_idle_timeout ( )
if not completed :
logger . warning (
f " POLL timeout address_64= { target_address_64 . hex ( ) } "
f " grant_bytes= { grant_bytes } "
)
finally :
self . operator_busy = False
self . current_poll_address_64 = None
@@ -819,21 +875,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 )
if parsed is None :
parsed_at_ack = self . _parse ( frame )
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 +920,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 +1302,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
回傳是否成功排進事件圈
@@ -1327,26 +1384,31 @@ if __name__ == '__main__':
# sm.create_serial_link(SERIAL_PORT, SERIAL_BAUDRATE, UDP_REMOTE_PORT, SerialMode.STRAIGHT)
# # 測試項二
# print("運行 測試項二")
# SERIAL_PORT = '/dev/ttyUSB0' # 手動指定
# SERIAL_BAUDRATE = 115200
# UDP_REMOTE_PORT = 14561
# sm.create_serial_link(SERIAL_PORT, SERIAL_BAUDRATE, UDP_REMOTE_PORT, SerialMode.XBEEAPI2AT)
# serial_id = 1
# device_sys_id = 10
# linked_serial = sm.get_serial_link()
# print(linked_serial)
# print(f"連結完成 : { 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):
# 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(3 )
# time.sleep(2 )
# sm.shutdown()
# print("結束運行")
# # 測試項三
# 測試項三
SERIAL_PORT = ' /dev/ttyUSB0 '
SERIAL_BAUDRATE = 115200
UDP_REMOTE_PORT = 14561
@@ -1363,8 +1425,8 @@ if __name__ == '__main__':
print ( processor . get_status_snapshot ( ) )
print ( processor . get_gcs_queue_byte_count ( ) )
time . sleep ( 120 )
sm . remove_serial_link ( serial_id )
time . sleep ( 3 )
sm . shutdown ( )
'''
ken910606
chiyu1468