@ -19,18 +19,11 @@ 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
@ -145,8 +138,6 @@ 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
@ -230,7 +221,7 @@ class XBeeFrameProcessor_Base(FrameProcessor):
@staticmethod
def _encapsulate (
data : bytes ,
dest_addr64 : bytes = DEST_ADDR64_BRAODCAST ,
dest_addr64 : bytes = b ' \x00 \x00 \x00 \x00 \x00 \x00 \x00 \x00 ' ,
dest_addr16 = DEST_ADDR16_BRAODCAST ,
frame_id : int = 0x01 ,
) - > bytes :
@ -260,26 +251,14 @@ 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 )
這邊認定 esp_sysid 會等於 mavlink sysid
硬體產品系列 ( Product Family ) : XBP9B - DM
晶片世代 : XBee PRO 900 HP 200 K
運作頻段 : 900 MHz RF 。
運行協議 ( Protocol / Function Set ) : DigiMesh
韌體版本 ( Firmware Version ) : 8075
'''
# 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)
DISC_HEADER = b ' DISC '
HELLO_HEADER = b ' HELO '
@ -290,11 +269,6 @@ 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
@ -302,7 +276,6 @@ 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 # 收到封包累計
@ -327,9 +300,7 @@ 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 # 這個是最後做廣播 discovery 的時間
self . last_recieve_mavlink = 0.0 # 這個是最後收到 mavlink payload 時間 為了定義 poll-done 之間不要超時用的
self . MAX_mavPack_interval_timeout = 100 # ms，
self . last_discovery_time = 0.0
self . discovery_interval_seconds = 30.0 # 每次做 discovery 程序的間隔時間
self . device_offline_timeout = self . discovery_interval_seconds * 2 # 遠端沒有回應會被踢出 超時時限
self . operator_tick_interval_seconds = 0.03 #
@ -380,8 +351,6 @@ 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 :
@ -406,7 +375,6 @@ 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 )
@ -416,7 +384,8 @@ 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 :
@ -425,10 +394,6 @@ 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 :
@ -450,16 +415,13 @@ class XBeeFrameProcessor_ESPv1(XBeeFrameProcessor_Base):
if remote_device is None :
return
# 這段是有問題的 因為會有整數封包切割問題 以及載具端的 buffer 存量不足 故回傳的資訊量會與要求的不一致
# 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}"
# )
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 } "
)
# TODO 傳送速率
# TODO 累積速率預測
remote_device . received_len = 0
remote_device . remain_bytes = remain_length
remote_device . last_done_time = time . time ( )
@ -487,11 +449,11 @@ class XBeeFrameProcessor_ESPv1(XBeeFrameProcessor_Base):
return len ( self . gcs_transmit_queue )
# 把 gcs_transmit_queue 的 mavlink 封包依照大小打包出來
def _pop_flush_batch ( self , max_bytes : int ) - > L [ bytes ] :
def _pop_flush_batch ( self , max_bytes : int ) - > l [ bytes ] :
if not self . gcs_transmit_queue :
return [ ]
batch : L [ bytes ] = [ ]
batch : l [ bytes ] = [ ]
total_bytes = 0
while self . gcs_transmit_queue :
@ -545,7 +507,7 @@ class XBeeFrameProcessor_ESPv1(XBeeFrameProcessor_Base):
# ---- POLL 排程輔助 ----
# 計算下次要 poll 的對象跟大小
def _pick_poll_target ( self ) :
def _pick_poll_target ( self ) - > tuple [ Optional [ bytes ] , int ] :
poll_devices = [
pollStrategy . PollDevice (
address_64 = address_64 ,
@ -753,22 +715,6 @@ 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 :
@ -787,15 +733,22 @@ 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 :
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 } "
)
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 } "
)
finally :
self . operator_busy = False
self . current_poll_address_64 = None
@ -866,21 +819,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 _at_ack = self . _parse ( frame )
if parsed _at_ack is None :
parsed = self . _parse ( frame )
if parsed is None :
return
if not rx_module_ack . put ( parsed _at_ack ) :
if not rx_module_ack . put ( parsed ) :
logger . warning (
f " [ { self . serial_port } ] rx_module_ack overflow, drop { parsed _at_ack . command !r} "
f " [ { self . serial_port } ] rx_module_ack overflow, drop { parsed . command !r} "
)
self . _dispatch ( parsed _at_ack )
self . _dispatch ( parsed )
@staticmethod
def _parse ( frame : bytes ) - > ATResponse :
@ -911,27 +864,26 @@ class ATCommandHandler:
handler = self . handlers . get ( response . command )
if handler :
handler ( response )
handler ( response . data )
else :
logger . debug (
f " [ { self . serial_port } ] 未處理的 AT 指令: "
f " { response . command . decode ( ) } "
)
def _handle_rssi ( self , response: ATResponse ) :
def _handle_rssi ( self , data: bytes ) :
""" 處理 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
# 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 ) :
def _handle_serial_high ( self , data : bytes ) :
""" 處理 SH (Serial Number High) """
pass
def _handle_serial_low ( self , response: ATResponse ) :
def _handle_serial_low ( self , data : bytes ) :
""" 處理 SL (Serial Number Low) """
pass
@ -1293,7 +1245,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
回傳是否成功排進事件圈
@ -1374,51 +1326,46 @@ if __name__ == '__main__':
# UDP_REMOTE_PORT = 14571
# 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
# # 測試項二
# SERIAL_PORT = '/dev/ttyUSB0' # 手動指定
# SERIAL_BAUDRATE = 115200
# UDP_REMOTE_PORT = 14561
# sm.create_serial_link(SERIAL_PORT, SERIAL_BAUDRATE, UDP_REMOTE_PORT, SerialMode.XBEEAPI2AT)
linked_serial = sm . get_serial_link (
print ( f " 連結完成 : { linked_serial } . 等待兩秒 "
# linked_serial = sm.get_serial_link()
# print(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 )
# # 等 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 ( 2 )
sm . shutdown ( )
print ( " 結束運行 " )
# 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)
# time.sleep(2) # 等 serial 連線與 operator 啟動
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 )
# 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())
time . sleep ( 2 ) # 等 serial 連線與 operator 啟動
# sm.remove_serial_link(serial_id)
# time.sleep(30)
# sm.shutdown()
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 ( 3 )
sm . shutdown ( )
'''
== == == == == == == == = 改版記錄 == == == == == == == == == == == == == ==