#!/usr/bin/env python3
"""
SerialPingTest - 串口直连MAVLink延迟测试
直接读写 /dev/ttyAMA3，不走Copilot任何链路
测量纯串口RTT: 串口写COMMAND_LONG -> FMU -> 串口读COMMAND_ACK
用法: 先停Copilot释放串口，再运行:
python3 SerialPingTest.py
python3 SerialPingTest.py -p /dev/ttyAMA3 -b 115200 -r 50
python3 SerialPingTest.py -r 100 # 100Hz
python3 SerialPingTest.py -d # debug模式, 打印所有收到的消息
"""
import sys
import os
import time
import struct
import argparse
import math
from collections import OrderedDict
============================================================
MAVLink 常量
============================================================
MAVLINK_STX_M1 = 0xFE # MAVLink 1.0
MAVLINK_CORE_HEADER_LEN = 5 # length + seq + sysid + compid + msgid
MAVLINK_MSG_ID_COMMAND_LONG = 76
MAVLINK_MSG_ID_COMMAND_ACK = 77
MAV_CMD_PLANE_CONNECTION_STATE_REPORT = 12010
SYS_ID = 0x02
COMP_ID = 0xFE
CRC_EXTRA (from ArduPilot ardupilotmega.h MAVLINK_MESSAGE_CRCS)
CRC_EXTRA_TABLE = {
0: 50, # HEARTBEAT
76: 152, # COMMAND_LONG
77: 143, # COMMAND_ACK
}
============================================================
MAVLink CRC - 与 ArduPilot checksum.h 完全一致的逐字节算法
============================================================
def crc_accumulate_byte(data, crc):
"""单字节CRC累加, 等价于C: crc_accumulate(data, &crcAccum)"""
tmp = (data ^ (crc & 0xFF)) & 0xFF
tmp ^= (tmp << 4) & 0xFF
crc = ((crc >> 8) & 0xFFFF) ^ ((tmp << 8) & 0xFFFF) ^ ((tmp << 3) & 0xFFFF) ^ ((tmp >> 4) & 0xFFFF)
return crc & 0xFFFF
def crc_accumulate_buffer(data, crc):
"""多字节CRC累加, 等价于C: crc_accumulate_buffer(&crcAccum, buf, len)"""
for b in data:
crc = crc_accumulate_byte(b, crc)
return crc
def crc_calculate(data):
"""计算CRC, 等价于C: crc_calculate(buf, len), init=0xFFFF"""
crc = 0xFFFF
for b in data:
crc = crc_accumulate_byte(b, crc)
return crc
============================================================
MAVLink 消息打包 - 与 _mav_finalize_message_chan_send 完全一致
============================================================
def pack_mavlink_v1(msgid, sysid, compid, seq, payload):
"""打包 MAVLink 1.0 消息帧"""
length = len(payload)
buf0=STX, buf1=length, buf2=seq, buf3=sysid, buf4=compid, buf5=msgid
header = struct.pack('<BBBBBB', MAVLINK_STX_M1, length, seq, sysid, compid, msgid)
CRC: crc_calculate(&buf1, MAVLINK_CORE_HEADER_LEN) 即 length+seq+sysid+compid+msgid
+ crc_accumulate_buffer(payload) + crc_accumulate(crc_extra)
crc_extra = CRC_EXTRA_TABLE.get(msgid, 0)
crc = crc_calculate(header1:1+MAVLINK_CORE_HEADER_LEN) # length+seq+sysid+compid+msgid
crc = crc_accumulate_buffer(payload, crc)
crc = crc_accumulate_byte(crc_extra & 0xFF, crc)
return header + payload + struct.pack('<H', crc)
def pack_command_long(sysid, compid, seq, target_system, target_component, command,
confirmation, param1, param2, param3, param4, param5, param6, param7):
"""打包 MAVLink 1.0 COMMAND_LONG 消息"""
payload = struct.pack('<fffffffHBBB',
param1, param2, param3, param4, param5, param6, param7,
command, target_system, target_component, confirmation)
return pack_mavlink_v1(MAVLINK_MSG_ID_COMMAND_LONG, sysid, compid, seq, payload)
============================================================
MAVLink 消息解析
============================================================
class MAVLinkParser:
"""MAVLink 1.0 消息解析器"""
def init(self, debug=False):
self.debug = debug
self.state = 0
self.length = 0
self.seq = 0
self.sysid = 0
self.compid = 0
self.msgid = 0
self.payload = bytearray()
self.n_read = 0
self.msg_count = 0
def parse(self, data):
results = \[\]
for b in data:
msg = self._parse_byte(b)
if msg is not None:
results.append(msg)
return results
def _parse_byte(self, b):
b = b & 0xFF
if self.state == 0:
if b == MAVLINK_STX_M1:
self.state = 1
self.n_read = 0
return None
if self.state == 1:
self.length = b
self.state = 2
self.n_read = 0
self.hdr_buf = bytearray()
return None
if self.state == 2:
self.hdr_buf.append(b)
self.n_read += 1
if self.n_read == 4:
self.seq = self.hdr_buf0
self.sysid = self.hdr_buf1
self.compid = self.hdr_buf2
self.msgid = self.hdr_buf3
self.state = 3
self.payload = bytearray()
self.n_read = 0
return None
if self.state == 3:
self.payload.append(b)
self.n_read += 1
if self.n_read == self.length:
self.state = 4
self.crc_buf = bytearray()
self.n_read = 0
return None
if self.state == 4:
self.crc_buf.append(b)
self.n_read += 1
if self.n_read == 2:
self.state = 0
self.msg_count += 1
if self.debug:
print(f"Parser msg#{self.msg_count} id={self.msgid} sys={self.sysid} comp={self.compid} len={self.length}")
return (self.msgid, self.sysid, self.compid, bytes(self.payload))
return None
self.state = 0
return None
def parse_command_ack(payload):
"""解析 COMMAND_ACK payload"""
if len(payload) < 3:
return None
command = struct.unpack_from('<H', payload, 0)0
result = payload2
return {'command': command, 'result': result}
============================================================
SerialPingTest 主类
============================================================
class SerialPingTest:
def init(self, port, baudrate, rate, debug=False):
self.port = port
self.baudrate = baudrate
self.rate = rate
self.interval_s = 1.0 / rate
self.timeout_s = 0.5
self.debug = debug
self.serial_fd = -1
self.parser = MAVLinkParser(debug=debug)
self.seq = 0
self.pending = OrderedDict()
self.max_pending = 100
self.count = 0
self.lost = 0
self.min_rtt = 999999.0
self.max_rtt = 0.0
self.sum_rtt = 0.0
self.sum_rtt2 = 0.0
self.stats_interval = max(rate, 1)
self.total_bytes_read = 0
self.running = False
def open_serial(self):
try:
self.serial_fd = os.open(self.port, os.O_RDWR | os.O_NOCTTY | os.O_NONBLOCK)
except OSError as e:
print(f"SerialPingTest open {self.port} failed: {e}")
return False
import termios
try:
old = termios.tcgetattr(self.serial_fd)
except termios.error as e:
print(f"SerialPingTest tcgetattr failed: {e}")
os.close(self.serial_fd)
self.serial_fd = -1
return False
iflag = old0
oflag = old1
cflag = old2
lflag = old3
cc = list(old6)
baud_map = {
9600: termios.B9600, 19200: termios.B19200, 38400: termios.B38400,
57600: termios.B57600, 115200: termios.B115200, 230400: termios.B230400,
460800: termios.B460800, 921600: termios.B921600,
1000000: termios.B1000000, 1500000: termios.B1500000,
}
baud_const = baud_map.get(self.baudrate, termios.B115200)
cflag &= ~(termios.PARENB | termios.CSTOPB | termios.CSIZE | termios.CRTSCTS)
cflag |= termios.CS8 | termios.CREAD | termios.CLOCAL
iflag &= ~(termios.IXON | termios.IXOFF | termios.IXANY |
termios.IGNBRK | termios.BRKINT | termios.PARMRK |
termios.ISTRIP | termios.INLCR | termios.IGNCR | termios.ICRNL)
lflag &= ~(termios.ECHO | termios.ECHONL | termios.ICANON | termios.ISIG | termios.IEXTEN)
oflag &= ~(termios.OPOST | termios.ONLCR)
cctermios.VMIN = 0
cctermios.VTIME = 0
new = iflag, oflag, cflag, lflag, baud_const, baud_const, cc
try:
termios.tcsetattr(self.serial_fd, termios.TCSANOW, new)
except termios.error as e:
print(f"SerialPingTest tcsetattr failed: {e}")
os.close(self.serial_fd)
self.serial_fd = -1
return False
termios.tcflush(self.serial_fd, termios.TCIOFLUSH)
print(f"SerialPingTest serial {self.port} opened, fd={self.serial_fd}")
return True
def close_serial(self):
if self.serial_fd >= 0:
os.close(self.serial_fd)
self.serial_fd = -1
def send_ping(self):
now_us = self._time_us()
confirmation = self.seq & 0xFF
data = pack_command_long(
sysid=SYS_ID, compid=COMP_ID, seq=self.seq & 0xFF,
target_system=1, target_component=1,
command=MAV_CMD_PLANE_CONNECTION_STATE_REPORT,
confirmation=confirmation,
param1=1.0, param2=3.0, param3=0, param4=0,
param5=0, param6=0, param7=0
)
self.seq += 1
if self.debug:
print(f"SerialPingTest TX {len(data)} bytes, confirm={confirmation}, hex={data.hex()}")
try:
os.write(self.serial_fd, data)
except OSError as e:
print(f"SerialPingTest write failed: {e}")
return
self.pendingconfirmation = now_us
if len(self.pending) > self.max_pending:
self.pending.popitem(last=False)
def read_serial(self):
try:
data = os.read(self.serial_fd, 4096)
except (OSError, BlockingIOError):
return
if not data:
return
self.total_bytes_read += len(data)
messages = self.parser.parse(data)
for msgid, sysid, compid, payload in messages:
if msgid == MAVLINK_MSG_ID_COMMAND_ACK:
ack = parse_command_ack(payload)
if ack and ack'command' == MAV_CMD_PLANE_CONNECTION_STATE_REPORT:
self._handle_ack(ack)
elif self.debug:
print(f"SerialPingTest ACK cmd={ack'command' if ack else '?'} (not 12010)")
elif self.debug:
print(f"SerialPingTest RX msgid={msgid} sys={sysid} comp={compid} len={len(payload)}")
def _handle_ack(self, ack):
now_us = self._time_us()
if not self.pending:
return
confirm, send_time = next(iter(self.pending.items()))
self.pending.popitem(last=False)
rtt_ms = (now_us - send_time) / 1000.0
self.count += 1
if rtt_ms < self.min_rtt:
self.min_rtt = rtt_ms
if rtt_ms > self.max_rtt:
self.max_rtt = rtt_ms
self.sum_rtt += rtt_ms
self.sum_rtt2 += rtt_ms * rtt_ms
print(f"SerialPingTest ACK result={ack'result'} confirm={confirm} RTT={rtt_ms:.3f} ms")
if self.count % self.stats_interval == 0:
self.print_stats()
def check_timeout(self):
now_us = self._time_us()
timeout_us = self.timeout_s * 1e6
while self.pending:
confirm, send_time = next(iter(self.pending.items()))
if now_us - send_time > timeout_us:
self.pending.popitem(last=False)
self.lost += 1
print(f"SerialPingTest confirm={confirm} TIMEOUT")
else:
break
def print_stats(self):
if self.count == 0:
print(f"SerialPingTest No ACK received (lost={self.lost}, bytes_read={self.total_bytes_read}, msgs_parsed={self.parser.msg_count})")
return
avg = self.sum_rtt / self.count
variance = (self.sum_rtt2 / self.count) - (avg * avg)
stddev = math.sqrt(variance) if variance > 0 else 0.0
total = self.count + self.lost
loss_rate = (self.lost / total * 100.0) if total > 0 else 0.0
print(f"SerialPingTest ============================================================")
print(f"SerialPingTest Stats (count={self.count}, lost={self.lost}, loss={loss_rate:.1f}%)")
print(f"SerialPingTest Min: {self.min_rtt:.3f} ms")
print(f"SerialPingTest Max: {self.max_rtt:.3f} ms")
print(f"SerialPingTest Avg: {avg:.3f} ms")
print(f"SerialPingTest Std: {stddev:.3f} ms")
print(f"SerialPingTest ============================================================")
def run(self):
print(f"SerialPingTest ============================================================")
print(f"SerialPingTest Serial Direct MAVLink Latency Test")
print(f"SerialPingTest Port: {self.port} @ {self.baudrate}")
print(f"SerialPingTest Send: serial write COMMAND_LONG(12010)")
print(f"SerialPingTest Recv: serial read COMMAND_ACK")
print(f"SerialPingTest Rate: {self.rate}Hz (interval {self.interval_s*1000:.0f}ms)")
print(f"SerialPingTest Timeout: {self.timeout_s*1000:.0f}ms")
print(f"SerialPingTest Debug: {self.debug}")
print(f"SerialPingTest Ctrl+C to stop")
print(f"SerialPingTest ============================================================")
if not self.open_serial():
print("SerialPingTest cannot open serial, exit")
return -1
self.running = True
try:
while self.running:
self.send_ping()
deadline = self._time_us() + int(self.interval_s * 1e6)
while self._time_us() < deadline:
self.read_serial()
time.sleep(0.001)
self.check_timeout()
except KeyboardInterrupt:
print("\nSerialPingTest interrupted")
finally:
print("\nSerialPingTest Final stats:")
self.print_stats()
self.close_serial()
print("SerialPingTest stopped")
return 0
@staticmethod
def _time_us():
return int(time.time() * 1e6)
def main():
parser = argparse.ArgumentParser(description='Serial Direct MAVLink Latency Test')
parser.add_argument('-p', '--port', default='/dev/ttyAMA3', help='Serial port')
parser.add_argument('-b', '--baud', type=int, default=115200, help='Baud rate')
parser.add_argument('-r', '--rate', type=int, default=50, help='Ping rate in Hz')
parser.add_argument('-d', '--debug', action='store_true', help='Debug mode')
args = parser.parse_args()
test = SerialPingTest(port=args.port, baudrate=args.baud, rate=args.rate, debug=args.debug)
return test.run()
if name == 'main':
sys.exit(main())