@ -19,11 +19,18 @@ from enum import Enum, auto
from abc import ABC , abstractmethod from abc import ABC , abstractmethod
from dataclasses import dataclass from dataclasses import dataclass
from typing import Callable , Optional from typing import Callable , Optional
from typing import List
# # XBee 模組 # # XBee 模組
# from xbee.frame import APIFrame # from xbee.frame import APIFrame
# 自定義的 import # 自定義的 import
from . mavlinkVehicleView import (
vehicle_registry , # 儲存全部物件的地方
VehicleView ,
RFModule ,
RFModuleType ,
)
from . utils import RingBuffer , setup_logger from . utils import RingBuffer , setup_logger
from . utils import pollStrategy from . utils import pollStrategy
@ -138,6 +145,8 @@ class XBeeFrameProcessor_Base(FrameProcessor):
DEST_ADDR16_UNICAST = b ' \xFF \xFE '
DEST_ADDR16_UNICAST = b ' \xFF \xFE '
DEST_ADDR16_BRAODCAST = b ' \xFF \xFF '
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 ) :
def __init__ ( self , at_handler : " ATCommandHandler " = None ) :
super ( ) . __init__ ( )
super ( ) . __init__ ( )
self . at_handler = at_handler
self . at_handler = at_handler
@ -221,7 +230,7 @@ class XBeeFrameProcessor_Base(FrameProcessor):
@staticmethod
@staticmethod
def _encapsulate (
def _encapsulate (
data : bytes ,
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 ,
dest_addr16 = DEST_ADDR16_BRAODCAST ,
frame_id : int = 0x01 ,
frame_id : int = 0x01 ,
) - > bytes :
) - > bytes :
@ -251,14 +260,26 @@ class XBeeFrameProcessor_Base(FrameProcessor):
class XBeeFrameProcessor_ESPv1 ( XBeeFrameProcessor_Base ) : class XBeeFrameProcessor_ESPv1 ( XBeeFrameProcessor_Base ) :
'''
# GCS -> UAV:
ESP32 封包分類
# DISC
- Mavlink 資料封包 :
# POLL + esp_sysid(1) + grant_bytes(2)
- Payload = mavlink [ 方向 ] GCS - > UAV ( broadcast )
#
- Payload = mavlink [ 方向 ] UAV - > GCS ( with src64 Addr )
# UAV -> GCS:
- Discovery 階段
# HELO + esp_sysid(1)
- Payload = DISC_HEADER [ 方向 ] GCS - > UAV ( broadcast )
# DONE + sysid(1) + sent_len(2) + remain_len(2)
- 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
'''
DISC_HEADER = b ' DISC '
DISC_HEADER = b ' DISC '
HELLO_HEADER = b ' HELO '
HELLO_HEADER = b ' HELO '
@ -269,6 +290,11 @@ class XBeeFrameProcessor_ESPv1(XBeeFrameProcessor_Base):
MAX_PAYLOAD_PER_FRAME = 80
MAX_PAYLOAD_PER_FRAME = 80
CHUNK_SEND_INTERVAL_SEC = 0.01
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 :
class Esp32DeviceInfo :
def __init__ ( self , system_id , address_64 , last_hello_time ) :
def __init__ ( self , system_id , address_64 , last_hello_time ) :
self . system_id = system_id
self . system_id = system_id
@ -276,6 +302,7 @@ class XBeeFrameProcessor_ESPv1(XBeeFrameProcessor_Base):
self . last_hello_time = last_hello_time
self . last_hello_time = last_hello_time
self . remain_bytes = 0 # 剩餘 buffer 量
self . remain_bytes = 0 # 剩餘 buffer 量
self . last_poll_time = 0.0
self . last_done_time = 0.0 # 最後送出Done的時間
self . last_done_time = 0.0 # 最後送出Done的時間
self . received_len = 0 # 收到封包累計
self . received_len = 0 # 收到封包累計
@ -300,7 +327,9 @@ class XBeeFrameProcessor_ESPv1(XBeeFrameProcessor_Base):
self . poll_done_event : Optional [ asyncio . Event ] = None
self . poll_done_event : Optional [ asyncio . Event ] = None
self . current_poll_address_64 : Optional [ bytes ] = 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，
self . discovery_interval_seconds = 30.0 # 每次做 discovery 程序的間隔時間
self . discovery_interval_seconds = 30.0 # 每次做 discovery 程序的間隔時間
self . device_offline_timeout = self . discovery_interval_seconds * 2 # 遠端沒有回應會被踢出 超時時限
self . device_offline_timeout = self . discovery_interval_seconds * 2 # 遠端沒有回應會被踢出 超時時限
self . operator_tick_interval_seconds = 0.03 #
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 )
remote_device = self . esp32_address_mapping . get ( sender_address_64 )
if remote_device is not None :
if remote_device is not None :
remote_device . received_len + = len ( payload )
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
return payload
if frame_type == self . FRAME_TYPE_AT_RESPONSE :
if frame_type == self . FRAME_TYPE_AT_RESPONSE :
@ -375,6 +406,7 @@ class XBeeFrameProcessor_ESPv1(XBeeFrameProcessor_Base):
def pack_discovery ( self ) - > bytes :
def pack_discovery ( self ) - > bytes :
return self . _encapsulate ( self . DISC_HEADER , frame_id = 0x00 )
return self . _encapsulate ( self . DISC_HEADER , frame_id = 0x00 )
# 處理每個裝置回傳的 Hello 訊息
def handle_hello_report ( self , payload : bytes , sender_address_64 : bytes ) - > None :
def handle_hello_report ( self , payload : bytes , sender_address_64 : bytes ) - > None :
system_id = payload [ 4 ]
system_id = payload [ 4 ]
remote_device = self . esp32_address_mapping . get ( sender_address_64 )
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 ( )
system_id , sender_address_64 , time . time ( )
)
)
logger . debug (
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 :
elif remote_device . address_64 == sender_address_64 :
remote_device . last_hello_time = time . time ( )
remote_device . last_hello_time = time . time ( )
else :
else :
@ -394,6 +425,10 @@ class XBeeFrameProcessor_ESPv1(XBeeFrameProcessor_Base):
f " address_64= { sender_address_64 . hex ( ) } "
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 ] :
def pack_poll ( self , target_address_64 : bytes , grant_bytes : int = 0 ) - > Optional [ bytes ] :
remote_device = self . esp32_address_mapping . get ( target_address_64 )
remote_device = self . esp32_address_mapping . get ( target_address_64 )
if remote_device is None :
if remote_device is None :
@ -415,13 +450,16 @@ class XBeeFrameProcessor_ESPv1(XBeeFrameProcessor_Base):
if remote_device is None :
if remote_device is None :
return
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 . received_len = 0
remote_device . remain_bytes = remain_length
remote_device . remain_bytes = remain_length
remote_device . last_done_time = time . time ( )
remote_device . last_done_time = time . time ( )
@ -449,11 +487,11 @@ class XBeeFrameProcessor_ESPv1(XBeeFrameProcessor_Base):
return len ( self . gcs_transmit_queue )
return len ( self . gcs_transmit_queue )
# 把 gcs_transmit_queue 的 mavlink 封包依照大小打包出來
# 把 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 :
if not self . gcs_transmit_queue :
return [ ]
return [ ]
batch : l [ bytes ] = [ ]
batch : L [ bytes ] = [ ]
total_bytes = 0
total_bytes = 0
while self . gcs_transmit_queue :
while self . gcs_transmit_queue :
@ -507,7 +545,7 @@ class XBeeFrameProcessor_ESPv1(XBeeFrameProcessor_Base):
# ---- POLL 排程輔助 ----
# ---- POLL 排程輔助 ----
# 計算下次要 poll 的對象跟大小
# 計算下次要 poll 的對象跟大小
def _pick_poll_target ( self ) - > tuple [ Optional [ bytes ] , int ] :
def _pick_poll_target ( self ) :
poll_devices = [
poll_devices = [
pollStrategy . PollDevice (
pollStrategy . PollDevice (
address_64 = address_64 ,
address_64 = address_64 ,
@ -715,6 +753,22 @@ class XBeeFrameProcessor_ESPv1(XBeeFrameProcessor_Base):
self . is_discovery_phase = False
self . is_discovery_phase = False
self . pending_manual_discovery = 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 程序
# poll 程序
async def _run_one_poll ( self , target_address_64 : bytes , grant_bytes : int ) - > None :
async def _run_one_poll ( self , target_address_64 : bytes , grant_bytes : int ) - > None :
if self . serial_writer is None :
if self . serial_writer is None :
@ -733,22 +787,15 @@ class XBeeFrameProcessor_ESPv1(XBeeFrameProcessor_Base):
self . current_poll_address_64 = target_address_64
self . current_poll_address_64 = target_address_64
self . poll_done_event . clear ( )
self . poll_done_event . clear ( )
self . serial_writer ( poll_frame )
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 :
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 ,
logger . warning (
)
f " POLL timeout address_64= { target_address_64 . hex ( ) } "
except asyncio . TimeoutError :
f " grant_bytes= { grant_bytes } "
logger . warning (
)
f " POLL timeout address_64= { target_address_64 . hex ( ) } "
f " grant_bytes= { grant_bytes } "
)
finally :
finally :
self . operator_busy = False
self . operator_busy = False
self . current_poll_address_64 = None
self . current_poll_address_64 = None
@ -819,21 +866,21 @@ class ATCommandHandler:
# ---- 接收端 ----
# ---- 接收端 ----
def handle_frame ( self , frame : bytes ) - > None :
def handle_frame ( self , frame : bytes ) - > None :
"""
"""
接收一整個 AT Response frame ：
接收一整個 AT Response frame :
1. 解析成 ATResponse
1. 解析成 ATResponse
2. 推進 rx_module_ack 供其他模組消費
2. 推進 rx_module_ack 供其他模組消費
3. 本地 dispatch 給對應的 _handle_xxx
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
return
if not rx_module_ack . put ( parsed ) :
if not rx_module_ack . put ( parsed _at_ack ) :
logger . warning (
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
@staticmethod
def _parse ( frame : bytes ) - > ATResponse :
def _parse ( frame : bytes ) - > ATResponse :
@ -864,26 +911,27 @@ class ATCommandHandler:
handler = self . handlers . get ( response . command )
handler = self . handlers . get ( response . command )
if handler :
if handler :
handler ( response . data )
handler ( response )
else :
else :
logger . debug (
logger . debug (
f " [ { self . serial_port } ] 未處理的 AT 指令: "
f " [ { self . serial_port } ] 未處理的 AT 指令: "
f " { response . command . decode ( ) } "
f " { response . command . decode ( ) } "
)
)
def _handle_rssi ( self , data: bytes ) :
def _handle_rssi ( self , response: ATResponse ) :
""" 處理 DB (RSSI) 回應：單 byte 無號值，單位 dBm """
""" 處理 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) """
""" 處理 SH (Serial Number High) """
pass
pass
def _handle_serial_low ( self , data : bytes ) :
def _handle_serial_low ( self , response: ATResponse ) :
""" 處理 SL (Serial Number Low) """
""" 處理 SL (Serial Number Low) """
pass
pass
@ -1245,7 +1293,7 @@ class serial_manager:
def send_at_command ( self , serial_id , request : ATRequest ) - > bool :
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 取得的編號
- serial_id : create_serial_link 取得的編號
- request : ATRequest 物件 ， 攜帶 command / parameter / frame_id
- request : ATRequest 物件 ， 攜帶 command / parameter / frame_id
回傳是否成功排進事件圈
回傳是否成功排進事件圈
@ -1326,46 +1374,51 @@ if __name__ == '__main__':
# UDP_REMOTE_PORT = 14571
# UDP_REMOTE_PORT = 14571
# sm.create_serial_link(SERIAL_PORT, SERIAL_BAUDRATE, UDP_REMOTE_PORT, SerialMode.STRAIGHT)
# 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
SERIAL_BAUDRATE = 115200
# sm.create_serial_link(SERIAL_PORT, SERIAL_BAUDRATE, UDP_REMOTE_PORT, SerialMode.XBEEAPI2AT)
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()
linked_serial = sm . get_serial_link (
# print(linked_serial)
print ( f " 連結完成 : { linked_serial } . 等待兩秒 "
# # 等 connection_made 完成 writer 注入，再發一筆 AT 指令測試
# 等 connection_made 完成 writer 注入，再發一筆 AT 指令測試
# time.sleep(5)
time . sleep ( 2 )
# rssi_request = ATRequest(command=b'DB', parameter=b'', frame_id=0x52)
rssi_request = ATRequest ( command = b ' DB ' , parameter = b ' ' , frame_id = device_sys_id )
# for i in range(60):
print ( f " 手動送出 DB AT Command: " )
# sm.send_at_command(1, rssi_request)
for i in range ( 20 ) :
# time.sleep(1)
sm . send_at_command ( 1 , rssi_request )
time . sleep ( 1 )
# sm.remove_serial_link(1)
sm . remove_serial_link ( 1 )
# time.sleep(3)
time . sleep ( 2 )
# sm.shutdown()
sm . shutdown ( )
print ( " 結束運行 " )
# # 測試項三
# # 測試項三
SERIAL_PORT = ' /dev/ttyUSB0
# SERIAL_PORT = '/dev/ttyUSB0
SERIAL_BAUDRATE =
# SERIAL_BAUDRATE =
UDP_REMOTE_PORT =
# UDP_REMOTE_PORT =
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.XBEEAPI_espv1
time . sleep ( 2 )
# time.sleep(2)
serial_id =
# serial_id =
processor = sm . get_espv1_processor ( serial_id
# processor = sm.get_espv1_processor(serial_id
if processor is not None
# if processor is not None
#
#
#
#
#
sm . remove_serial_link ( serial_id
# sm.remove_serial_link(serial_id
time . sleep ( 3
# time.sleep(30
sm . shutdown (
# sm.shutdown(
''' '''
== == == == == == == == = 改版記錄 == == == == == == == == == == == == == == == == == == == == == == = 改版記錄 == == == == == == == == == == == == == ==
Chiyu Chen