chiyu1468
/
AirTrapMine
2
0
Fork 1
Code Issues Pull Requests Packages Projects Releases Wiki Activity

學長建議修改內容

Browse Source
lunu
lenting89 7 days ago
parent 3e6490d07f
commit 5075d99ad6
2 changed files with 93 additions and 45 deletions
Show Stats Download Patch File Download Diff File

29
src/unitdev04/esp32.py
Unescape Escape View File

@ -20,6 +20,7 @@ import gc
# GCS -> UAV:
# DISC
# POLL + target_sysid(1) + grant_bytes(2)
# COMM / RTK broadcast downlink bytes
#
# UAV -> GCS:
# HELO + sysid(1)
@ -29,7 +30,8 @@ import gc
# - HELO 不帶 remain。
# - DONE 保留 remain。
# - MY_SYSID 不寫死，由飛控 MAVLink 自動學。
# - DEST_64 不寫死，由 GCS 封包的 XBee 0x90 src64 自動學。
# - DEST_64 不寫死，由 DISC 的 XBee 0x90 src64 自動學。
# - POLL 只負責 TDMA 授權；COMM / RTK 直接 forward。
# =========================================================
@ -341,6 +343,10 @@ def learn_my_sysid_from_tx_buf():
global HELLO_SENT
global tx_buf
# 已經學到 SYSID 後，不再掃 tx_buf，減少 ESP32 運算量
if MY_SYSID is not None:
return
if len(tx_buf) == 0:
return
@ -361,9 +367,6 @@ def learn_my_sysid_from_tx_buf():
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:
@ -458,9 +461,7 @@ def trim_tx_buffer_if_needed():
def flush_tx_buffer(grant_bytes):
global tx_buf
if MY_SYSID is None:
return
# DEST_64 未學到時不能送資料，保留防護避免先 pop tx_buf 造成資料遺失
if DEST_64 is None:
return
@ -564,20 +565,14 @@ def process_xbee_buffer():
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()
# POLL 只負責 TDMA 授權；DISC 已負責 GCS address / SYSID 學習
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:
learn_dest64_from_src64(src64)
if DEST_64 is not None and src64 == DEST_64:
uart_fc.write(real_data)
# COMM / RTK / 一般 GCS broadcast 下行資料
# DISC 與 POLL 已經在前面處理掉，剩下資料直接轉給 FC / RTK module
uart_fc.write(real_data)
rx_buf = rx_buf[total_len:]

109
src/unitdev04/udptest8.py
Unescape Escape View File

@ -26,8 +26,9 @@ from matplotlib.backends.backend_tkagg import FigureCanvasTkAgg
#
# 封包格式：
# GCS -> UAV:
# DISC
# POLL + target_sysid(1) + grant_bytes(2)
# DISC broadcast
# POLL + target_sysid(1) + grant_bytes(2) unicast if src64 known
# COMM / RTK downlink bytes broadcast
#
# UAV -> GCS:
# HELO + sysid(1)
@ -37,19 +38,33 @@ from matplotlib.backends.backend_tkagg import FigureCanvasTkAgg
# ================= 多組設備設定 =================
CONFIGS = [
{"serial_port": "/dev/ttyUSB0", "udp_port": 14551},
{"serial_port": "COM15", "udp_port": 14590},
{"serial_port": "/dev/ttyUSB2", "udp_port": 14553},
{"serial_port": "/dev/ttyUSB3", "udp_port": 14554},
# udp_output_port：
# UAV MAVLink 上行資料會由 udptest 轉送到這個 UDP port 給 Mission Planner / MAVProxy。
# RTK / COMM 下行輸入 port 會自動使用 udp_output_port + COMM_LISTEN_PORT_OFFSET。
{"serial_port": "/dev/ttyUSB0", "udp_output_port": 14551},
{"serial_port": "COM15", "udp_output_port": 14590},
{"serial_port": "/dev/ttyUSB2", "udp_output_port": 14553},
{"serial_port": "/dev/ttyUSB3", "udp_output_port": 14554},
]
SERIAL_BAUDRATE = 115200
UDP_REMOTE_IP = '127.0.0.1'
UDP_LISTEN_FIXED_PORT = False
# COMM / RTK 下行使用固定 UDP port 接收，避免 Mission Planner 的 UDP output port 被佔用。
# 例如 COM15 預設：
# Mission Planner / MAVProxy listen : udp_output_port = 14590
# RTK / COMM input to udptest : 14590 + 1000 = 15590
COMM_LISTEN_PORT_OFFSET = 1000
TARGET_ADDR64 = b'\x00\x00\x00\x00\x00\x00\xFF\xFF'
# COMM / RTK 下行固定走 broadcast。
# POLL 仍由 get_poll_dest_addr64() 決定 unicast / fallback broadcast。
COMM_BROADCAST_ADDR64 = TARGET_ADDR64
COMM_QUEUE_FLUSH_BYTES = 150
COMM_MAX_PAYLOAD = 80
COMM_CHUNK_GAP_SEC = 0.01
INITIAL_ACTIVE_SYSIDS = []
ACTIVE_SYSIDS = list(INITIAL_ACTIVE_SYSIDS)
@ -127,6 +142,19 @@ def format_addr64(addr):
return ''.join(f'{b:02X}' for b in addr)
def get_udp_output_port(config):
"""UAV uplink MAVLink 轉送給 Mission Planner / MAVProxy 的 UDP output port。"""
return config.get('udp_output_port', config.get('udp_port'))
def get_comm_listen_port(config):
"""
RTK / COMM 下行資料進入 udptest 的固定 UDP input port。
由 udp_output_port 自動推算，避免每組 CONFIG 重複手寫。
"""
return get_udp_output_port(config) + COMM_LISTEN_PORT_OFFSET
def ensure_active_sysid(sysid):
if sysid is None:
return
@ -317,7 +345,7 @@ class SerialToUDP(asyncio.Protocol):
self.udp_protocol = udp_protocol
self.serial_port = serial_port
self.buffer = bytearray()
self.gcs_tx_queue = bytearray()
self.comm_tx_queue = bytearray()
self.transport = None
self.current_poll_sysid = None
@ -484,29 +512,46 @@ class SerialToUDP(asyncio.Protocol):
record_rssi(sysid, rssi_value, src64=src64)
def write_to_serial(self, data):
self.gcs_tx_queue.extend(data)
"""
UDPHandler 收到 RTK / COMM 下行資料後會呼叫這裡。
注意：這不是 TDMA POLL，不做 sysid 分流；一律排入 COMM broadcast queue。
"""
self.queue_comm_broadcast(data)
def queue_comm_broadcast(self, data):
"""COMM / RTK 下行資料佇列；後續用 XBee FFFF broadcast 送給所有 UAV。"""
self.comm_tx_queue.extend(data)
def flush_gcs_queue(self):
if not self.gcs_tx_queue:
"""相容舊呼叫名稱；實際執行 COMM broadcast queue flush。"""
self.flush_comm_broadcast_queue()
def flush_comm_broadcast_queue(self):
if not self.comm_tx_queue:
return
send_limit = min(len(self.gcs_tx_queue), 150)
data_to_send = self.gcs_tx_queue[:send_limit]
self.gcs_tx_queue = self.gcs_tx_queue[send_limit:]
asyncio.create_task(self._async_send_chunks(data_to_send))
send_limit = min(len(self.comm_tx_queue), COMM_QUEUE_FLUSH_BYTES)
data_to_send = self.comm_tx_queue[:send_limit]
self.comm_tx_queue = self.comm_tx_queue[send_limit:]
asyncio.create_task(self._async_send_comm_broadcast_chunks(data_to_send))
async def _async_send_chunks(self, data):
async def _async_send_comm_broadcast_chunks(self, data):
"""
COMM / RTK broadcast path：
- XBee dest64 固定為 FFFF broadcast
- 不使用 learned_sysid_to_addr64
- 不影響 send_poll() 的 unicast POLL path
"""
try:
max_payload = 80
sent_len = 0
while sent_len < len(data):
end_len = min(sent_len + max_payload, len(data))
end_len = min(sent_len + COMM_MAX_PAYLOAD, len(data))
chunk = data[sent_len:end_len]
sent_len = end_len
api_frame = build_api_tx_frame(chunk, TARGET_ADDR64, 0x00)
api_frame = build_api_tx_frame(chunk, COMM_BROADCAST_ADDR64, 0x00)
self.transport.write(api_frame)
await asyncio.sleep(0.01)
await asyncio.sleep(COMM_CHUNK_GAP_SEC)
except Exception:
pass
@ -516,8 +561,8 @@ class SerialToUDP(asyncio.Protocol):
# =========================================================
class UDPHandler(asyncio.DatagramProtocol):
def __init__(self, udp_port):
self.udp_port = udp_port
def __init__(self, udp_output_port):
self.udp_output_port = udp_output_port
self.serial_transport = None
self.transport = None
self.mav_decoder = mavutil.mavlink.MAVLink(None)
@ -529,6 +574,7 @@ class UDPHandler(asyncio.DatagramProtocol):
self.serial_transport = serial_transport
def datagram_received(self, data, addr):
# 固定 UDP input 收到 RTK / COMM，下行一律走 XBee broadcast。
if self.serial_transport:
self.serial_transport.write_to_serial(data)
@ -606,7 +652,7 @@ class UDPHandler(asyncio.DatagramProtocol):
pass
if self.transport:
self.transport.sendto(rf_data, (UDP_REMOTE_IP, self.udp_port))
self.transport.sendto(rf_data, (UDP_REMOTE_IP, self.udp_output_port))
return None
@ -616,7 +662,9 @@ class UDPHandler(asyncio.DatagramProtocol):
# =========================================================
async def setup_bridge(config):
port, udp = config['serial_port'], config['udp_port']
port = config['serial_port']
udp_output_port = get_udp_output_port(config)
comm_listen_port = get_comm_listen_port(config)
try:
ser = serial.Serial(port, SERIAL_BAUDRATE)
@ -626,10 +674,14 @@ async def setup_bridge(config):
return None
loop = asyncio.get_running_loop()
udp_handler = UDPHandler(udp)
udp_handler = UDPHandler(udp_output_port)
await loop.create_datagram_endpoint(
lambda: udp_handler,
local_addr=('0.0.0.0', comm_listen_port)
)
local_port = udp if UDP_LISTEN_FIXED_PORT else 0
await loop.create_datagram_endpoint(lambda: udp_handler, local_addr=('0.0.0.0', local_port))
print(f"[{port}] UDP output -> {UDP_REMOTE_IP}:{udp_output_port}, COMM input <- 0.0.0.0:{comm_listen_port}")
serial_proto = SerialToUDP(udp_handler, port)
await serial_asyncio.create_serial_connection(loop, lambda: serial_proto, port, baudrate=SERIAL_BAUDRATE)
@ -677,9 +729,10 @@ async def tdma_scheduler(serial_protocols):
tdma_done_events[sysid] = asyncio.Event()
while True:
# COMM / RTK 下行 broadcast queue；與 POLL unicast path 分開
for sp in serial_protocols:
if hasattr(sp, 'flush_gcs_queue'):
sp.flush_gcs_queue()
if hasattr(sp, 'flush_comm_broadcast_queue'):
sp.flush_comm_broadcast_queue()
await asyncio.sleep(current_guard_ms / 1000.0)

Write Preview
Loading…
Cancel
Save