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

Compare commits

base: chiyu1468:b720bb6ffdfc96d0b3ab4032ed6bf77dc853ea8a
Branches Tags
chiyu1468:master
chiyu1468:wenchun
chiyu1468:lunu
chiyu1468:chiyu
chiyu1468:ken
..
compare: chiyu1468:38d8d23514666be20428f20ec45885c646f85eae
Branches Tags
chiyu1468:master
chiyu1468:wenchun
chiyu1468:lunu
chiyu1468:chiyu
chiyu1468:ken

No commits in common. 'b720bb6ffdfc96d0b3ab4032ed6bf77dc853ea8a' and '38d8d23514666be20428f20ec45885c646f85eae' have entirely different histories.
b720bb6ffd ... 38d8d23514

4 changed files with 0 additions and 999 deletions
Show Stats

193
serial_udp_update.py
Unescape Escape View File

@ -1,193 +0,0 @@
import asyncio
import serial_asyncio
import struct
import os
import sys
import serial
import signal
import traceback
from pymavlink import mavutil
# === 設定區 ===
SERIAL_PORT = 'COM15' # 手動指定
SERIAL_BAUDRATE = 57600
UDP_REMOTE_IP = '127.0.0.1'
UDP_REMOTE_PORT = 14550
DEBUG_MODE = False
TARGET_ADDR64 = b'\x00\x00\x00\x00\x00\x00\xFF\xFF' # 廣播
# === 工具函數 ===
def check_serial_port():
try:
ser = serial.Serial(SERIAL_PORT, SERIAL_BAUDRATE)
ser.close()
return True
except serial.SerialException as e:
print(f"錯誤：串口設備 {SERIAL_PORT} 被占用或無法訪問：{str(e)}")
return False
except Exception as e:
print(f"錯誤：檢查串口時發生未知錯誤：{str(e)}")
return False
def build_api_tx_frame(data: bytes, dest_addr64: bytes, frame_id=0x01) -> bytes:
frame_type = 0x10
dest_addr16 = b'\xFF\xFE'
broadcast_radius = 0x00
options = 0x00
frame = struct.pack(">B", frame_type) + struct.pack(">B", frame_id)
frame += dest_addr64 + dest_addr16
frame += struct.pack(">BB", broadcast_radius, options) + data
checksum = 0xFF - (sum(frame) & 0xFF)
return b'\x7E' + struct.pack(">H", len(frame)) + frame + struct.pack("B", checksum)
# === Serial Protocol 實作 ===
class SerialToUDP(asyncio.Protocol):
def __init__(self, udp_protocol):
self.udp_protocol = udp_protocol
self.buffer = bytearray()
def connection_made(self, transport):
self.transport = transport
if hasattr(self.udp_protocol, 'set_serial_transport'):
self.udp_protocol.set_serial_transport(self)
print(f"Serial connection established on {SERIAL_PORT}")
def data_received(self, data):
self.buffer.extend(data)
while True:
if len(self.buffer) < 3:
return
if self.buffer[0] != 0x7E:
self.buffer.pop(0)
continue
length = (self.buffer[1] << 8) | self.buffer[2]
full_length = 3 + length + 1
if len(self.buffer) < full_length:
return
frame = self.buffer[:full_length]
del self.buffer[:full_length]
if hasattr(self.udp_protocol, 'send_udp'):
self.udp_protocol.send_udp(bytes(frame))
def write_to_serial(self, data):
try:
api_frame = build_api_tx_frame(data, TARGET_ADDR64)
pass
self.transport.write(api_frame)
except Exception as e:
print(f"[TX Error] 無法封裝或傳送資料: {e}")
# === UDP Protocol 實作 ===
class UDPHandler(asyncio.DatagramProtocol):
def __init__(self):
self.serial_transport = None
self.transport = None
self.mav_decoder = mavutil.mavlink.MAVLink(None)
def connection_made(self, transport):
self.transport = transport
print("UDP transport ready.")
def set_serial_transport(self, serial_transport):
self.serial_transport = serial_transport
def datagram_received(self, data, addr):
if self.serial_transport:
self.serial_transport.write_to_serial(data)
def send_udp(self, data):
decoded_data = self.decapsulate_data(data)
if decoded_data is None:
pass
return
self.decode_mavlink_data(decoded_data)
if self.transport:
self.transport.sendto(decoded_data, (UDP_REMOTE_IP, UDP_REMOTE_PORT))
def decapsulate_data(self, data):
try:
if not data or data[0] != 0x7E:
return None
length = (data[1] << 8) | data[2]
if len(data) < length + 4:
return None
frame_type = data[3]
if frame_type == 0x90:
rf_data_start = 3 + 12
return data[rf_data_start:3 + length]
else:
return None
except Exception as e:
print(f"[XBee 解封錯誤] {e}")
return None
def decode_mavlink_data(self, data):
try:
msg = self.mav_decoder.parse_char(data)
if msg:
if msg.get_type() == "HEARTBEAT":
pass # 不輸出任何訊息
else:
pass
except Exception as e:
print(f"[MAVLink Decode Error] {e}")
# === 主流程 ===
async def main():
if not check_serial_port():
print("程式終止：串口檢查失敗")
return
loop = asyncio.get_running_loop()
if os.name != 'nt': # Windows 不支援 add_signal_handler
for sig in (signal.SIGINT, signal.SIGTERM):
loop.add_signal_handler(sig, lambda: asyncio.create_task(shutdown(loop)))
udp_handler = UDPHandler()
try:
udp_transport, _ = await loop.create_datagram_endpoint(
lambda: udp_handler,
local_addr=('0.0.0.0', 0)
)
except Exception as e:
print(f"無法創建 UDP 端點：{str(e)}")
return
sock = udp_transport.get_extra_info('socket')
print(f"UDP listening on {sock.getsockname()}")
try:
serial_proto = SerialToUDP(udp_handler)
await serial_asyncio.create_serial_connection(
loop, lambda: serial_proto, SERIAL_PORT, baudrate=SERIAL_BAUDRATE
)
except Exception as e:
print(f"無法建立串口連接：{str(e)}")
traceback.print_exc()
udp_transport.close()
return
print("等待串口資料...")
try:
await asyncio.Future()
except asyncio.CancelledError:
pass
async def shutdown(loop):
print("Shutting down...")
tasks = [t for t in asyncio.all_tasks() if t is not asyncio.current_task()]
for task in tasks:
task.cancel()
await asyncio.gather(*tasks, return_exceptions=True)
loop.stop()
if __name__ == '__main__':
try:
asyncio.run(main())
except KeyboardInterrupt:
print("程式被使用者中斷")
except Exception as e:
print("程式執行錯誤：")
traceback.print_exc()

229
src/unitdev04/test01.py
Unescape Escape View File

@ -1,229 +0,0 @@
import rclpy
from pymavlink import mavutil
from time import sleep
import time
import socket
import struct
# used at fdm_switch_example_two
import threading
import queue
def mavlink_analyzer_simple(count = 500):
# rclpy.init()
# node = rclpy.create_node('mavlink_analyzer_simple')
print('Inintializing connection')
connection_string="udp:127.0.0.1:14550"
connection = mavutil.mavlink_connection(connection_string)
print('Catch messages')
msg_count = {}
start_time = time.time()
for _ in range(count):
msg = connection.recv_msg()
# msg = connection.recv_match(blocking=True)
if msg:
msg_type = msg.get_type()
if msg_type in msg_count:
msg_count[msg_type] += 1
else:
msg_count[msg_type] = 1
if msg.get_type() == 'SERVO_OUTPUT_RAW':
print(msg)
pass
else:
print('No message yet, 1 second for data to fill')
sleep(1)
print("markA ------")
print(msg.get_type())
print(msg.ordered_fieldnames)
print("markA ------ END")
print('Messages Type received:')
print(msg_count)
end_time = time.time()
print('Time elapsed: ', end_time - start_time)
print('Closing connection')
connection.close()
def fdm_parser_example(data=None):
if data is None:
data = bytes.fromhex('1a48e903b1340500e803e803e803e803000000000000000000000000000000000000000000000000')
# 1a48e903b1340500e803e803e803e803000000000000000000000000000000000000000000000000
parse_format = 'HHI16H'
if len(data) != struct.calcsize(parse_format):
print("got packet of len %u, expected %u" % (len(data), struct.calcsize(parse_format)))
exit(1)
decoded = struct.unpack(parse_format,data)
magic = decoded[0]
frame_rate_hz = decoded[1]
frame_count = decoded[2]
pwm = decoded[3:]
print("magic: %u, frame_rate_hz: %u, frame_count: %u, pwm: %s" % (magic, frame_rate_hz, frame_count, pwm))
def fdm_switch_example_one():
'''
這個範例在 SITL 與 Gazebo 之間 做一個簡單的UDP轉發器
沒有轉換數據格式
'''
# make a link get fdm from SITL, as a server
sock_s2g = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
sock_s2g.bind(('', 9002))
sock_s2g.settimeout(0.1)
# set a target link pass JSON to Gazebo, as a client
sock_g2s = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
server_address = ('127.0.0.1', 19002)
packet_count = 0
start_time = time.time()
while True:
try:
# 接到SITL的數據 並轉發給Gazebo
data_s2g, addr_s2g = sock_s2g.recvfrom(1024)
sock_g2s.sendto(data_s2g, server_address)
# 得到Gazebo的回應 並轉發給SITL
data_g2s, addr_g2s = sock_g2s.recvfrom(1024)
sock_s2g.sendto(data_g2s, addr_s2g)
packet_count += 1
except socket.timeout:
pass
current_time = time.time()
elapsed_time = current_time - start_time
if elapsed_time >= 1.0:
# print(f"Packets received in the last second: {packet_count}")
print(f'JSON Pack : {data_g2s}')
packet_count = 0
start_time = current_time
fdm_parser_example(data_s2g)
def fdm_switch_example_two():
'''
這個範例在 SITL 與 Gazebo 之間 做一個簡單的UDP轉發器
有轉換數據格式
time_usec
servo1_raw
servo16_raw
'''
# read info from SITL via MAVLink
connection_string="udp:127.0.0.1:14550" # uart mavlink # MATLAB
connection = mavutil.mavlink_connection(connection_string)
# set a target link pass JSON to Gazebo, as a client
sock_g2s = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
server_address = ('127.0.0.1', 19002)
data_queue = queue.Queue()
servo_out = [0] * 16 # [0 0 0 0 0 0 0 0 0.....]
data_queue.put(servo_out)
def send_data_from_queue(sock, server_address, q, frame_rate_fdm=1, frame_count_fdm=1):
interval = 1.0 / frame_rate_fdm
while True:
start_time = time.time()
try:
data = q.get(timeout=0.1) # [0 0 0 0 0 0 0 0 0.....]
if data is None:
break
last_data = data
except queue.Empty:
data = last_data
data_fdm = struct.pack('HHI16H', 0x481a, int(frame_rate_fdm * 0.1), frame_count_fdm, *servo_out) # [ FMD ]
sock.sendto(data_fdm, server_address)
frame_count_fdm += 3
# fdm_parser_example(data_fdm) # debug
elapsed_time = time.time() - start_time
sleep_time = interval - elapsed_time
if sleep_time > 0:
sleep(sleep_time)
Running = True
thread = threading.Thread(target=send_data_from_queue, args=(sock_g2s, server_address, data_queue))
thread.start()
print('Start sending data to Gazebo')
while Running:
msgb1 = None
msg = connection.recv_msg()
while msg :
if msg.get_type() == 'SERVO_OUTPUT_RAW':
msgb1 = msg
# break # 這裡不需要break，因為我要最後一個 servo_output_raw 的值
msg = connection.recv_msg()
if msgb1:
for i in range(16):
servo_out[i] = getattr(msgb1, f'servo{i+1}_raw')
data_queue.put(servo_out)
msgb1 = None
# Running = False # debug
else:
# print('No message yet, 10 ms for data to fill')
sleep(0.01)
# Example of putting data into the queue
# data_queue.put(data)
# To stop the thread, you can put None into the queue
# data_queue.put(None)
from pymavlink.dialects.v20 import common # 使用 MAVLink 2.0
def mavlink_btye_generator_test():
# 創建一個 MAVLink 對象
mav = common.MAVLink(None) # 不透過 connection，直接使用 None
# 創建一個 HEARTBEAT 訊息
msg = mav.heartbeat_encode(
type=mavutil.mavlink.MAV_TYPE_QUADROTOR,
autopilot=mavutil.mavlink.MAV_AUTOPILOT_GENERIC,
base_mode=0,
custom_mode=0,
system_status=mavutil.mavlink.MAV_STATE_ACTIVE
)
# 取得 MAVLink 訊息的 bytes
mavlink_bytes = msg.pack(mav)
# 回傳 bytes
print(mavlink_bytes)
print(type(mavlink_bytes))
mav2 = common.MAVLink(None)
# my_msg = mav2.decode(mavlink_bytes)
print("========")
print(dir(mav2))
# print(my_msg)
print('Start')
# fdm_switch_example_two()
# fdm_parser_example()
# mavlink_analyzer_simple(8)
mavlink_btye_generator_test()

193
src/unitdev04/udptest4.py
Unescape Escape View File

@ -1,193 +0,0 @@
import asyncio
import serial_asyncio
import struct
import os
import sys
import serial
import signal
import traceback
from pymavlink import mavutil
# === 設定區 ===
SERIAL_PORT = 'COM15' # 手動指定
SERIAL_BAUDRATE = 57600
UDP_REMOTE_IP = '127.0.0.1'
UDP_REMOTE_PORT = 14550
DEBUG_MODE = False
TARGET_ADDR64 = b'\x00\x00\x00\x00\x00\x00\xFF\xFF' # 廣播
# === 工具函數 ===
def check_serial_port():
try:
ser = serial.Serial(SERIAL_PORT, SERIAL_BAUDRATE)
ser.close()
return True
except serial.SerialException as e:
print(f"錯誤：串口設備 {SERIAL_PORT} 被占用或無法訪問：{str(e)}")
return False
except Exception as e:
print(f"錯誤：檢查串口時發生未知錯誤：{str(e)}")
return False
def build_api_tx_frame(data: bytes, dest_addr64: bytes, frame_id=0x01) -> bytes:
frame_type = 0x10
dest_addr16 = b'\xFF\xFE'
broadcast_radius = 0x00
options = 0x00
frame = struct.pack(">B", frame_type) + struct.pack(">B", frame_id)
frame += dest_addr64 + dest_addr16
frame += struct.pack(">BB", broadcast_radius, options) + data
checksum = 0xFF - (sum(frame) & 0xFF)
return b'\x7E' + struct.pack(">H", len(frame)) + frame + struct.pack("B", checksum)
# === Serial Protocol 實作 ===
class SerialToUDP(asyncio.Protocol):
def __init__(self, udp_protocol):
self.udp_protocol = udp_protocol
self.buffer = bytearray()
def connection_made(self, transport):
self.transport = transport
if hasattr(self.udp_protocol, 'set_serial_transport'):
self.udp_protocol.set_serial_transport(self)
print(f"Serial connection established on {SERIAL_PORT}")
def data_received(self, data):
self.buffer.extend(data)
while True:
if len(self.buffer) < 3:
return
if self.buffer[0] != 0x7E:
self.buffer.pop(0)
continue
length = (self.buffer[1] << 8) | self.buffer[2]
full_length = 3 + length + 1
if len(self.buffer) < full_length:
return
frame = self.buffer[:full_length]
del self.buffer[:full_length]
if hasattr(self.udp_protocol, 'send_udp'):
self.udp_protocol.send_udp(bytes(frame))
def write_to_serial(self, data):
try:
api_frame = build_api_tx_frame(data, TARGET_ADDR64)
pass
self.transport.write(api_frame)
except Exception as e:
print(f"[TX Error] 無法封裝或傳送資料: {e}")
# === UDP Protocol 實作 ===
class UDPHandler(asyncio.DatagramProtocol):
def __init__(self):
self.serial_transport = None
self.transport = None
self.mav_decoder = mavutil.mavlink.MAVLink(None)
def connection_made(self, transport):
self.transport = transport
print("UDP transport ready.")
def set_serial_transport(self, serial_transport):
self.serial_transport = serial_transport
def datagram_received(self, data, addr):
if self.serial_transport:
self.serial_transport.write_to_serial(data)
def send_udp(self, data):
decoded_data = self.decapsulate_data(data)
if decoded_data is None:
pass
return
self.decode_mavlink_data(decoded_data)
if self.transport:
self.transport.sendto(decoded_data, (UDP_REMOTE_IP, UDP_REMOTE_PORT))
def decapsulate_data(self, data):
try:
if not data or data[0] != 0x7E:
return None
length = (data[1] << 8) | data[2]
if len(data) < length + 4:
return None
frame_type = data[3]
if frame_type == 0x90:
rf_data_start = 3 + 12
return data[rf_data_start:3 + length]
else:
return None
except Exception as e:
print(f"[XBee 解封錯誤] {e}")
return None
def decode_mavlink_data(self, data):
try:
msg = self.mav_decoder.parse_char(data)
if msg:
if msg.get_type() == "HEARTBEAT":
pass # 不輸出任何訊息
else:
pass
except Exception as e:
print(f"[MAVLink Decode Error] {e}")
# === 主流程 ===
async def main():
if not check_serial_port():
print("程式終止：串口檢查失敗")
return
loop = asyncio.get_running_loop()
if os.name != 'nt': # Windows 不支援 add_signal_handler
for sig in (signal.SIGINT, signal.SIGTERM):
loop.add_signal_handler(sig, lambda: asyncio.create_task(shutdown(loop)))
udp_handler = UDPHandler()
try:
udp_transport, _ = await loop.create_datagram_endpoint(
lambda: udp_handler,
local_addr=('0.0.0.0', 0)
)
except Exception as e:
print(f"無法創建 UDP 端點：{str(e)}")
return
sock = udp_transport.get_extra_info('socket')
print(f"UDP listening on {sock.getsockname()}")
try:
serial_proto = SerialToUDP(udp_handler)
await serial_asyncio.create_serial_connection(
loop, lambda: serial_proto, SERIAL_PORT, baudrate=SERIAL_BAUDRATE
)
except Exception as e:
print(f"無法建立串口連接：{str(e)}")
traceback.print_exc()
udp_transport.close()
return
print("等待串口資料...")
try:
await asyncio.Future()
except asyncio.CancelledError:
pass
async def shutdown(loop):
print("Shutting down...")
tasks = [t for t in asyncio.all_tasks() if t is not asyncio.current_task()]
for task in tasks:
task.cancel()
await asyncio.gather(*tasks, return_exceptions=True)
loop.stop()
if __name__ == '__main__':
try:
asyncio.run(main())
except KeyboardInterrupt:
print("程式被使用者中斷")
except Exception as e:
print("程式執行錯誤：")
traceback.print_exc()

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

@ -1,384 +0,0 @@
# === import 保持不變 ===
import asyncio
import serial_asyncio
import struct
import serial
import traceback
import time
from collections import deque, defaultdict
from pymavlink import mavutil
import matplotlib.pyplot as plt
import matplotlib.animation as animation
import threading
# === 多組設備設定 ===
CONFIGS = [
{"serial_port": "/dev/ttyUSB2", "udp_port": 14551},
{"serial_port": "COM43", "udp_port": 14552},
{"serial_port": "COM15", "udp_port": 14553},
]
SERIAL_BAUDRATE = 115200
UDP_REMOTE_IP = '127.0.0.1'
TARGET_ADDR64 = b'\x00\x00\x00\x00\x00\x00\xFF\xFF'
# === 數據存儲 ===
rssi_history = defaultdict(lambda: deque(maxlen=5000))
time_history = defaultdict(lambda: deque(maxlen=5000))
# === 新增：丟包率追蹤 ===
packet_loss_history = defaultdict(lambda: deque(maxlen=1000))
packet_loss_time_history = defaultdict(lambda: deque(maxlen=1000))
mavlink_sequence_tracker = defaultdict(dict) # {sysid: {compid: {'last_seq': x, 'total_packets': y, 'lost_packets': z}}}
packet_loss_stats = defaultdict(lambda: {'loss_rate': 0.0, 'total_received': 0, 'total_lost': 0})
serial_to_sysid = {}
serial_last_mavlink_time = {} # 優化 1：追蹤各 serial port 最近的 MAVLink 時間
def calculate_packet_loss(sysid, compid, current_seq):
"""計算丟包率"""
global mavlink_sequence_tracker, packet_loss_stats
tracker = mavlink_sequence_tracker[sysid]
if compid not in tracker:
# 第一次收到這個component的消息
tracker[compid] = {
'last_seq': current_seq,
'total_packets': 1,
'lost_packets': 0,
'last_update': time.time()
}
return 0.0
comp_tracker = tracker[compid]
last_seq = comp_tracker['last_seq']
# 計算序列號差異（處理255溢出）
if current_seq > last_seq:
expected_packets = current_seq - last_seq
elif current_seq < last_seq:
# 序列號溢出（0-255循環）
expected_packets = (255 - last_seq) + current_seq + 1
else:
# 重複的序列號，忽略
return comp_tracker.get('loss_rate', 0.0)
# 更新統計
comp_tracker['total_packets'] += expected_packets
lost_packets = expected_packets - 1 # 實際收到1個，應該收到expected_packets個
comp_tracker['lost_packets'] += max(0, lost_packets)
comp_tracker['last_seq'] = current_seq
comp_tracker['last_update'] = time.time()
# 計算丟包率
total_expected = comp_tracker['total_packets']
total_lost = comp_tracker['lost_packets']
loss_rate = (total_lost / total_expected) * 100.0 if total_expected > 0 else 0.0
comp_tracker['loss_rate'] = loss_rate
# 更新全局統計（按sysid匯總所有component）
total_received = 0
total_lost_all = 0
total_expected_all = 0
for comp_id, comp_data in tracker.items():
total_expected_all += comp_data['total_packets']
total_lost_all += comp_data['lost_packets']
total_received += comp_data['total_packets'] - comp_data['lost_packets']
overall_loss_rate = (total_lost_all / total_expected_all) * 100.0 if total_expected_all > 0 else 0.0
packet_loss_stats[sysid] = {
'loss_rate': overall_loss_rate,
'total_received': total_received,
'total_lost': total_lost_all
}
# 記錄到歷史數據
now = time.time()
packet_loss_history[sysid].append(overall_loss_rate)
packet_loss_time_history[sysid].append(now)
return overall_loss_rate
def build_api_tx_frame(data: bytes, dest_addr64: bytes, frame_id=0x01) -> bytes:
frame_type = 0x10
dest_addr16 = b'\xFF\xFE'
broadcast_radius = 0x00
options = 0x00
frame = struct.pack(">B", frame_type) + struct.pack(">B", frame_id)
frame += dest_addr64 + dest_addr16
frame += struct.pack(">BB", broadcast_radius, options) + data
checksum = 0xFF - (sum(frame) & 0xFF)
return b'\x7E' + struct.pack(">H", len(frame)) + frame + struct.pack("B", checksum)
class SerialToUDP(asyncio.Protocol):
def __init__(self, udp_protocol, serial_port):
self.udp_protocol = udp_protocol
self.serial_port = serial_port
self.buffer = bytearray()
def connection_made(self, transport):
self.transport = transport
if hasattr(self.udp_protocol, 'set_serial_transport'):
self.udp_protocol.set_serial_transport(self)
print(f"[{self.serial_port}] Serial connection established.")
def data_received(self, data):
self.buffer.extend(data)
while True:
if len(self.buffer) < 3:
return
if self.buffer[0] != 0x7E:
self.buffer.pop(0)
continue
length = (self.buffer[1] << 8) | self.buffer[2]
full_length = 3 + length + 1
if len(self.buffer) < full_length:
return
frame = self.buffer[:full_length]
del self.buffer[:full_length]
if hasattr(self.udp_protocol, 'send_udp'):
self.udp_protocol.send_udp(bytes(frame))
if frame[3] == 0x88 and frame[5:7] == b'DB':
status = frame[7]
if status == 0x00 and len(frame) > 8:
rssi_value = frame[8]
now = time.time()
# === 優化 1：僅信任最近 0.5 秒內有接收 MAVLink 的 port
last_time = serial_last_mavlink_time.get(self.serial_port, 0)
if now - last_time <= 0.5:
sysid = serial_to_sysid.get(self.serial_port, None)
if sysid is not None:
rssi_history[sysid].append(-rssi_value)
time_history[sysid].append(now)
# print(f"[SYSID:{sysid}] RSSI = -{rssi_value} dBm")
else:
print(f"[{self.serial_port}] 找不到 sysid 對應，RSSI = -{rssi_value} dBm，已忽略")
else:
print(f"[{self.serial_port}] 超過 0.5 秒未接收 MAVLink，RSSI = -{rssi_value} dBm 已忽略")
else:
print(f"[{self.serial_port}] DB 指令失敗，狀態碼: {status}")
def write_to_serial(self, data):
try:
api_frame = build_api_tx_frame(data, TARGET_ADDR64)
self.transport.write(api_frame)
except Exception as e:
print(f"[{self.serial_port} TX Error] 無法封裝或傳送資料: {e}")
def send_at_command_db(self):
try:
frame_type = 0x08
frame_id = 0x52
at_command = b'DB'
parameter = b''
frame = struct.pack(">B", frame_type) + struct.pack(">B", frame_id) + at_command + parameter
checksum = 0xFF - (sum(frame) & 0xFF)
api_frame = b'\x7E' + struct.pack(">H", len(frame)) + frame + struct.pack("B", checksum)
self.transport.write(api_frame)
except Exception as e:
print(f"[{self.serial_port}] 發送 DB 指令失敗: {e}")
class UDPHandler(asyncio.DatagramProtocol):
def __init__(self, udp_port):
self.udp_port = udp_port
self.serial_transport = None
self.transport = None
self.mav_decoder = mavutil.mavlink.MAVLink(None)
def connection_made(self, transport):
self.transport = transport
print(f"[UDP:{self.udp_port}] transport ready.")
def set_serial_transport(self, serial_transport):
self.serial_transport = serial_transport
def datagram_received(self, data, addr):
if self.serial_transport:
self.serial_transport.write_to_serial(data)
def send_udp(self, data):
decoded_data = self.decapsulate_data(data)
if decoded_data is None:
return
self.decode_mavlink_data(decoded_data)
if self.transport:
self.transport.sendto(decoded_data, (UDP_REMOTE_IP, self.udp_port))
def decapsulate_data(self, data):
try:
if not data or data[0] != 0x7E:
return None
length = (data[1] << 8) | data[2]
if len(data) < length + 4:
return None
frame_type = data[3]
if frame_type == 0x90:
rf_data_start = 3 + 12
return data[rf_data_start:3 + length]
else:
return None
except Exception as e:
print(f"[UDP:{self.udp_port} 解封錯誤] {e}")
return None
def decode_mavlink_data(self, data):
try:
# 逐字節解析MAVLink
for byte in data:
msg = self.mav_decoder.parse_char(bytes([byte]))
if msg:
sysid = msg.get_srcSystem()
compid = msg.get_srcComponent()
seq = msg.get_seq()
if sysid == 0:
continue
if self.serial_transport:
serial_to_sysid[self.serial_transport.serial_port] = sysid
serial_last_mavlink_time[self.serial_transport.serial_port] = time.time()
self.serial_transport.send_at_command_db()
# === 新增：計算丟包率 ===
try:
loss_rate = calculate_packet_loss(sysid, compid, seq)
# print(f"[SYSID:{sysid}] 丟包率: {loss_rate:.1f}%, SEQ: {seq}")
except Exception as e:
print(f"[UDP:{self.udp_port}] 丟包率計算錯誤: {e}")
except Exception as e:
print(f"[UDP:{self.udp_port} MAVLink Decode Error] {e}")
def start_plotting():
# 創建子圖：上方RSSI，下方丟包率
fig, (ax1, ax2) = plt.subplots(2, 1, figsize=(12, 8))
def update(frame):
ax1.clear()
ax2.clear()
# === RSSI 圖 ===
ax1.set_title("RSSI", fontsize=14)
ax1.set_xlabel("Time (s ago)")
ax1.set_ylabel("RSSI (dBm)")
ax1.set_xlim(30, 0) # 顯示最近10秒
ax1.set_ylim(-100, -10)
ax1.grid(True, alpha=0.3)
# === 丟包率圖 ===
ax2.set_title("Packet Loss Rate", fontsize=14)
ax2.set_xlabel("Time (s ago)")
ax2.set_ylabel("Loss Rate (%)")
ax2.set_xlim(10, 0)
ax2.set_ylim(0, 100)
ax2.grid(True, alpha=0.3)
now = time.time()
# 顏色列表
colors = ['blue', 'red', 'green', 'orange', 'purple', 'brown', 'pink', 'gray']
for i, sysid in enumerate(sorted(rssi_history.keys())):
color = colors[i % len(colors)]
# === 繪製 RSSI ===
rssi_recent_indices = [
idx for idx, ts in enumerate(time_history[sysid]) if now - ts <= 30
]
if rssi_recent_indices:
rssi_t = [now - time_history[sysid][idx] for idx in rssi_recent_indices]
rssi_r = [rssi_history[sysid][idx] for idx in rssi_recent_indices]
if rssi_t and rssi_r:
ax1.plot(rssi_t, rssi_r, label=f"SYSID:{sysid}", color=color, linewidth=2)
# 在RSSI圖上顯示當前數值
if rssi_r:
latest_rssi = rssi_r[-1]
ax1.text(2, latest_rssi, f'{latest_rssi:.0f}dBm',
bbox=dict(boxstyle="round,pad=0.3", facecolor=color, alpha=0.7),
fontsize=10, fontweight='bold', color='white')
# === 繪製丟包率 ===
if sysid in packet_loss_history:
loss_recent_indices = [
idx for idx, ts in enumerate(packet_loss_time_history[sysid]) if now - ts <= 10
]
if loss_recent_indices:
loss_t = [now - packet_loss_time_history[sysid][idx] for idx in loss_recent_indices]
loss_r = [packet_loss_history[sysid][idx] for idx in loss_recent_indices]
if loss_t and loss_r:
ax2.plot(loss_t, loss_r, label=f"SYSID:{sysid}", color=color, linewidth=2, marker='o', markersize=3)
# 在丟包率圖上顯示當前數值和統計
if loss_r:
latest_loss = loss_r[-1]
stats = packet_loss_stats[sysid]
# 顯示丟包率百分比
ax2.text(2, latest_loss + (i * 8), f'{latest_loss:.1f}%',
bbox=dict(boxstyle="round,pad=0.3", facecolor=color, alpha=0.7),
fontsize=10, fontweight='bold', color='white')
# 在圖表左側顯示詳細統計
stats_text = f"SYSID {sysid}:\nRecieved: {stats['total_received']}\nLoss: {stats['total_lost']}\nLoss Rate: {latest_loss:.1f}%"
ax2.text(28, 85 - (i * 20), stats_text,
bbox=dict(boxstyle="round,pad=0.5", facecolor=color, alpha=0.8),
fontsize=9, color='white', verticalalignment='top')
# 設置圖例
ax1.legend(loc="upper right", fontsize=10)
ax2.legend(loc="upper right", fontsize=10)
ani = animation.FuncAnimation(fig, update, interval=1000)
plt.tight_layout()
plt.show()
async def setup_bridge(config):
serial_port = config["serial_port"]
udp_port = config["udp_port"]
try:
ser = serial.Serial(serial_port, SERIAL_BAUDRATE)
ser.close()
except Exception as e:
print(f"[{serial_port}] 串口打開失敗: {e}")
return
loop = asyncio.get_running_loop()
udp_handler = UDPHandler(udp_port)
try:
await loop.create_datagram_endpoint(
lambda: udp_handler,
local_addr=('0.0.0.0', 0)
)
except Exception as e:
print(f"[{serial_port}] 無法創建 UDP: {e}")
return
try:
serial_proto = SerialToUDP(udp_handler, serial_port)
await serial_asyncio.create_serial_connection(
loop, lambda: serial_proto, serial_port, baudrate=SERIAL_BAUDRATE
)
except Exception as e:
print(f"[{serial_port}] 無法建立串口連接：{e}")
traceback.print_exc()
return
print(f"[{serial_port}] Serial <--> UDP:{udp_port} bridge ready.")
async def main():
tasks = [setup_bridge(cfg) for cfg in CONFIGS]
await asyncio.gather(*tasks)
await asyncio.Future()
if __name__ == '__main__':
try:
threading.Thread(target=lambda: asyncio.run(main()), daemon=True).start()
start_plotting()
except KeyboardInterrupt:
print("使用者中斷程式")
except Exception as e:
print("程式執行錯誤：")
traceback.print_exc()
Write Preview
Loading…
Cancel
Save