Files
PX4-Autopilot/Tools/bench_test/px4bench/__init__.py
T
Benjamin Cabé 8ace5a6253 fix(bench): reconnect to the board, not to another serial device (#29222)
After a reboot, the bench tests waited for the serial device of the board to come back, and, while it was gone, took the first other device of the same family, such as /dev/cu.usbmodem* on macOS. With a debug probe or another board connected, also a usbmodem device that stays while the board reboots, they connected to it, found no heartbeat, and reported that the board didn't come back. Only the devices of the family that weren't there before the reboot are now taken for the board, for a board that comes back under another name.

Assisted-by: Claude:opus-5.5

Signed-off-by: Benjamin Cabé <benjamin@zephyrproject.org>
2026-10-24 06:40:00 -08:00

591 lines
24 KiB
Python

"""px4bench: shared library for the PX4 bench-test suite (Tools/bench_test).
Core primitives every test builds on: MAVLink connection setup, PASS/FAIL
reporting, a non-interactive nsh shell over SERIAL_CONTROL (pattern from
Tools/mavlink_shell.py), reboot and USB re-detection helpers, and small
parsers for `mavlink status` output.
Protocol-area helpers live in submodules:
px4bench.params parameter read/set/echo with PX4's int32 union encoding
px4bench.missions mission item generation and the upload/download protocol
px4bench.ftp MAVFTP listing/download and ULog constants
Every operation takes a timeout: a hung board is a finding, not an excuse
for a hung test script.
"""
import glob
import os
import re
import sys
import time
from datetime import datetime, timezone
PYMAVLINK_INSTALL_HINT = """\
Failed to import pymavlink: {err}
Install it with:
pip3 install --user pymavlink
"""
try:
from pymavlink import mavutil
except ImportError as e:
print(PYMAVLINK_INSTALL_HINT.format(err=e))
sys.exit(2)
# pymavlink 2.4.49: add_message() crashes with TypeError if the first message
# of an instanced type (BATTERY_STATUS, ESC_STATUS, ...) arrives with its
# instance field unset -- it is then stored without the _instances dict and the
# next instanced arrival does None[key]. Timing-dependent, so it bites
# intermittently mid-suite. Wrap it; drop once fixed upstream.
_orig_add_message = mavutil.add_message
def _safe_add_message(messages, mtype, msg):
try:
_orig_add_message(messages, mtype, msg)
except TypeError:
messages[mtype] = msg
mavutil.add_message = _safe_add_message
SERIAL_CONTROL_DEV_SHELL = 10 # SERIAL_CONTROL_DEV_SHELL (see Tools/mavlink_shell.py)
DEFAULT_BAUD = 57600
# Default budget for MavlinkShell.open(). Opening spawns the nsh task on the
# firmware side and older releases (v1.17, #27840) failed a 5s single-write
# open right after a previous session closed, so the budget is generous and
# open() re-sends its wake write until the shell answers.
SHELL_OPEN_TIMEOUT = 10.0
USB_DEVICE_GLOB_DARWIN = '/dev/tty.usbmodem*'
USB_DEVICE_GLOB_LINUX = '/dev/serial/by-id/*PX4*'
# Exit code meaning "test skipped" (EX_TEMPFAIL): the orchestrator records it
# as SKIP, not FAIL. Used when a probe finds the firmware lacks a needed
# command, no SD card is mounted, or the operator declines the arming gate.
EXIT_SKIP = 75
# NuttX nsh prints exactly this for an unknown command
# (apps/nshlib/nsh_parse.c: "nsh: %s: command not found").
NSH_NOT_FOUND = 'command not found'
class Reporter:
"""Collects named PASS/FAIL checks and prints a summary.
Usage:
report = Reporter('boot_health')
report.check('heartbeat', ok, 'detail string')
...
sys.exit(report.finish())
"""
def __init__(self, test_name):
self.test_name = test_name
self.results = [] # (name, ok, detail)
self.start_time = time.monotonic()
def check(self, name, ok, detail=''):
tag = 'PASS' if ok else 'FAIL'
msg = '[{}] {}: {}'.format(tag, name, detail) if detail else '[{}] {}'.format(tag, name)
print(msg, flush=True)
self.results.append((name, bool(ok), detail))
return bool(ok)
def fail(self, name, detail=''):
return self.check(name, False, detail)
def ok(self, name, detail=''):
return self.check(name, True, detail)
def info(self, msg):
print('[INFO] {}'.format(msg), flush=True)
@property
def num_failed(self):
return sum(1 for _, ok, _ in self.results if not ok)
def finish(self):
"""Print summary, return process exit code (0 = all passed)."""
elapsed = time.monotonic() - self.start_time
total = len(self.results)
failed = self.num_failed
print('-' * 60)
if failed == 0:
print('{}: PASS ({} checks, {:.1f}s)'.format(self.test_name, total, elapsed))
else:
print('{}: FAIL ({}/{} checks failed, {:.1f}s)'.format(
self.test_name, failed, total, elapsed))
for name, ok, detail in self.results:
if not ok:
print(' FAILED: {}{}'.format(name, ' - ' + detail if detail else ''))
return 0 if failed == 0 else 1
FIRMWARE_INFO_ENV = 'PX4BENCH_FIRMWARE_INFO'
def make_report_dir(base='bench_reports', test_name=''):
"""Create and return a timestamped report directory.
When the suite preflight has established the firmware identity it
exports it via PX4BENCH_FIRMWARE_INFO (a JSON string); stamp it into
every report dir so each test result is traceable to a build.
"""
stamp = datetime.now(timezone.utc).strftime('%Y%m%dT%H%M%SZ')
path = os.path.join(base, '{}_{}'.format(stamp, test_name) if test_name else stamp)
os.makedirs(path, exist_ok=True)
info = os.environ.get(FIRMWARE_INFO_ENV)
if info:
try:
with open(os.path.join(path, 'firmware.json'), 'w') as f:
f.write(info.rstrip('\n') + '\n')
except OSError:
pass
return path
def is_serial_device(conn_str):
return conn_str.startswith('/dev/') or conn_str.upper().startswith('COM')
def connect(conn_str, baud=DEFAULT_BAUD, timeout: float = 20, source_system=254):
"""Open a MAVLink connection and wait for a heartbeat from the autopilot.
Returns the mavutil connection, or raises TimeoutError/OSError.
"""
mav = mavutil.mavlink_connection(conn_str, baud=baud, autoreconnect=False,
source_system=source_system,
source_component=mavutil.mavlink.MAV_COMP_ID_MISSIONPLANNER)
assert isinstance(mav, mavutil.mavfile), 'unexpected connection type for {}'.format(conn_str)
# announce ourselves so the autopilot streams to us
send_heartbeat(mav)
hb = wait_heartbeat(mav, timeout=timeout)
if hb is None:
mav.close()
raise TimeoutError('no autopilot HEARTBEAT on {} within {}s'.format(conn_str, timeout))
# Pin the target to the autopilot that sent this heartbeat. pymavlink
# (checked 2.4.42 through 2.4.49) only latches target_system once, from
# the first vehicle-looking heartbeat, and never sets target_component,
# which stays 0 (broadcast): param/mission requests then reach every
# component on the sysid and any of them may answer. Neither value is
# touched again by incoming traffic once set here.
mav.target_system = hb.get_srcSystem()
mav.target_component = hb.get_srcComponent()
return mav
def is_from_target(mav, msg):
"""True if msg was sent by the pinned autopilot (system and component)."""
return (msg.get_srcSystem() == mav.target_system and
msg.get_srcComponent() == mav.target_component)
def wait_heartbeat(mav, timeout: float = 10):
"""Wait for the next autopilot heartbeat. Returns the message or None.
A board can heartbeat from more than one component on the same sysid
(FMUv6X-RT on v1.17: compid 1 is PX4, compid 236 reports
MAV_AUTOPILOT_INVALID, #27852). Heartbeats with MAV_AUTOPILOT_INVALID
(GCS, companion, gimbal, other onboard components) are skipped, and once
connect() has pinned the target only that component's heartbeat counts.
"""
deadline = time.monotonic() + timeout
while True:
remaining = deadline - time.monotonic()
if remaining <= 0:
return None
m = mav.recv_match(type='HEARTBEAT', blocking=True, timeout=remaining)
if m is None:
return None
if m.autopilot == mavutil.mavlink.MAV_AUTOPILOT_INVALID:
continue
if mav.target_component != 0 and not is_from_target(mav, m):
continue
return m
def send_heartbeat(mav):
"""Send one GCS heartbeat so the autopilot keeps streaming to us."""
mav.mav.heartbeat_send(mavutil.mavlink.MAV_TYPE_GCS,
mavutil.mavlink.MAV_AUTOPILOT_INVALID, 0, 0, 0)
def drain(mav, duration=0.5):
"""Read and discard pending messages for a short window."""
deadline = time.monotonic() + duration
while time.monotonic() < deadline:
if mav.recv_match(blocking=True, timeout=0.05) is None:
pass
def parse_mavlink_status(text):
"""Parse `mavlink status` shell output into per-instance dicts.
Each instance block begins with `instance #N:` and contains rate lines
`tx: X B/s` / `rx: X B/s` (src/modules/mavlink/mavlink_main.cpp). Returns
a list of {'header', 'tx', 'rx'} dicts; its length is the instance count.
"""
instances = []
cur = None
for line in text.splitlines():
stripped = line.strip()
if 'instance #' in stripped:
if cur is not None:
instances.append(cur)
cur = {'header': stripped, 'tx': None, 'rx': None}
continue
if cur is None:
continue
if stripped.startswith('tx:') and cur['tx'] is None:
cur['tx'] = stripped
elif stripped.startswith('rx:') and cur['rx'] is None:
cur['rx'] = stripped
if cur is not None:
instances.append(cur)
return instances
def count_mavlink_instances(text):
"""Count MAVLink instances in `mavlink status` output."""
return len(re.findall(r'instance\s*#\s*\d+', text))
class MavlinkShell:
"""Non-interactive nsh shell over SERIAL_CONTROL (dev 10).
Write pattern follows Tools/mavlink_shell.py; completion detection uses
the sentinel-echo trick from Tools/HIL/run_nsh_cmd.py:
<cmd>; echo <sentinel>
so we know when output for <cmd> is complete without relying on prompt
parsing.
"""
def __init__(self, mav):
self.mav = mav
self.buf = ''
self._next_heartbeat = 0.0
self._seq = 0
def _write(self, text):
b = text
while len(b) > 0:
n = min(len(b), 70)
buf = [ord(c) for c in b[:n]]
buf.extend([0] * (70 - len(buf)))
self.mav.mav.serial_control_send(
SERIAL_CONTROL_DEV_SHELL,
mavutil.mavlink.SERIAL_CONTROL_FLAG_EXCLUSIVE |
mavutil.mavlink.SERIAL_CONTROL_FLAG_RESPOND,
0, 0, n, buf)
b = b[n:]
def _pump(self, window=0.03):
"""Read pending SERIAL_CONTROL data into self.buf, keep heartbeats going."""
now = time.monotonic()
if now > self._next_heartbeat:
send_heartbeat(self.mav)
self._next_heartbeat = now + 1.0
m = self.mav.recv_match(condition='SERIAL_CONTROL.count!=0',
type='SERIAL_CONTROL', blocking=True, timeout=window)
if m is not None:
self.buf += ''.join(chr(x) for x in m.data[:m.count])
def open(self, timeout: float = SHELL_OPEN_TIMEOUT):
"""Wake the shell and confirm a live prompt. Returns True on success.
The firmware spawns a fresh nsh task plus two pipes on the first
SERIAL_CONTROL write of a session, so opening is not free; we must
confirm the task is actually up and reading, not just that some byte
arrived. Leftover bytes from a prior session would pass a naive
len(buf) > 0 check and report a shell that is not there.
We require either the 'nsh>' prompt token, or a completed sentinel
round-trip driven through the shell: sending 'echo <sentinel>' and
seeing that sentinel echoed back proves nsh is up and processing our
input. The sentinel round-trip also covers builds whose prompt token
differs or is suppressed.
The wake write is re-sent every second until the deadline: the write
that spawns the shell task can be consumed before the nsh pipes are
reading (seen on v1.17 right after a prior session closed, #27840),
and a single write turns that dropped line into a bogus open failure.
"""
self._seq += 1
sentinel = 'BENCHOPEN{}'.format(self._seq)
self.buf = ''
deadline = time.monotonic() + timeout
next_wake = 0.0
while time.monotonic() < deadline:
if time.monotonic() >= next_wake:
self._write('\n')
self._write('echo {}\n'.format(sentinel))
next_wake = time.monotonic() + 1.0
self._pump()
if 'nsh>' in self.buf or re.search(
r'^{}\s*$'.format(re.escape(sentinel)), self.buf, re.MULTILINE):
self.buf = ''
return True
return False
def run(self, cmd, timeout: float = 10):
"""Run one command, return (output_str, timed_out_bool).
On timeout the partial output is returned and timed_out is True --
a command that never completes is exactly the hang we are hunting.
"""
self._seq += 1
sentinel = 'BENCHDONE{}'.format(self._seq)
self.buf = ''
# The sentinel echoes go on their OWN line: nsh aborts the rest of a
# ';' chain when a command fails (including 'command not found'), so
# chaining them onto the command line loses the sentinel for any
# non-zero exit and a failing command reads as a stall. nsh consumes
# input lines sequentially, so the sentinel line runs after the
# command finishes regardless of its exit status.
# Two echoes like run_nsh_cmd.py, in case the first line is garbled.
self._write('{}\n'.format(cmd))
self._write('echo {0}; echo {0}\n'.format(sentinel))
deadline = time.monotonic() + timeout
while time.monotonic() < deadline:
self._pump()
# sentinel on its own line (not the command echo containing "echo <sentinel>")
if re.search(r'^{}\s*$'.format(re.escape(sentinel)), self.buf, re.MULTILINE):
return self._strip(cmd, sentinel), False
return self._strip(cmd, sentinel), True
def _strip(self, cmd, sentinel):
"""Remove command echo and sentinel lines from captured output.
Strips ANY BENCHDONE sentinel, not just the current one: the second
safety echo of the previous command often arrives after run() already
returned and would otherwise leak into this command's output. Same
for BENCHOPEN: open() re-sends its wake echo until the shell answers,
so queued wake lines can execute (echo command and its output) after
open() already returned.
"""
lines = []
for line in self.buf.splitlines():
if sentinel in line or re.match(r'BENCHDONE\d+\s*$', line.strip()):
continue
if 'BENCHOPEN' in line:
continue
if cmd in line and 'echo' in line:
continue
# the terminal echo of the command itself (possibly prefixed by
# the prompt and ANSI erase sequences) is not command output
bare = re.sub(r'\x1b\[[0-9;]*[A-Za-z]', '', line).replace('nsh>', '').strip()
if bare == cmd:
continue
lines.append(line)
return '\n'.join(lines).strip()
def close(self, drain: float = 1.0):
"""Tear down the firmware shell session and confirm teardown.
A SERIAL_CONTROL message with the RESPOND flag cleared (flags=0) is
what triggers Mavlink::close_shell() and frees the nsh task and its
two pipes. Sending it and returning immediately lets a caller reopen
before the firmware has processed the teardown, which spawns a second
shell before the first is freed: repeat that and the board's task/fd
table is exhausted (a leak we drive, not a firmware hang). So after
sending flags=0 we briefly drain incoming SERIAL_CONTROL traffic to
give the firmware time to run close_shell before the caller can
reopen. Bounded wait, never a hang.
"""
try:
self.mav.mav.serial_control_send(SERIAL_CONTROL_DEV_SHELL, 0, 0, 0, 0, [0] * 70)
except Exception:
return
deadline = time.monotonic() + drain
while time.monotonic() < deadline:
m = self.mav.recv_match(type='SERIAL_CONTROL', blocking=True, timeout=0.1)
if m is None:
# a quiet window means the shell has stopped emitting; teardown
# has been processed
break
def shell_command_exists(shell, probe_cmd, timeout=10):
"""Probe the live firmware for a command via an open MavlinkShell.
probe_cmd should be a harmless invocation (e.g. 'simulator_sih status',
'sd_bench -h'): only the not-found reply matters, not the exit status.
Returns (present, output):
(True, out) the command exists in this firmware
(False, out) nsh replied 'command not found'
(None, out) the probe itself stalled, which is its own finding
"""
out, timed_out = shell.run(probe_cmd, timeout=timeout)
if timed_out:
return None, out
if NSH_NOT_FOUND in out:
return False, out
return True, out
def arming_gate(allow_arming, action='run the SIH flight test'):
"""Confirm that arming the (simulated) vehicle is acceptable.
The flight test arms the flight controller. Physics is simulated on the
FMU and pwm_out_sim replaces the real output drivers, but the board must
be bare: nothing may be connected to the output rails.
Returns True to proceed, False to skip:
--allow-arming given -> proceed without prompting (automation)
interactive TTY -> proceed only if the operator types 'arm'
non-TTY without the flag -> skip, naming --allow-arming
"""
if allow_arming:
return True
if not sys.stdin.isatty():
print('WARNING: this test arms the flight controller (simulated '
'flight). Non-interactive runs must pass --allow-arming to '
'confirm the board is bare; skipping.', flush=True)
return False
print('!' * 70)
print('WARNING: about to {}.'.format(action))
print('The flight controller WILL ARM. The flight is simulated on the')
print('FMU and pwm_out_sim replaces the real output drivers, but the')
print('board must be bare: NOTHING may be connected to the output rails')
print('(no ESCs, no motors, no servos).')
print('!' * 70)
answer = input("Type 'arm' to continue, anything else to skip: ").strip().lower()
if answer == 'arm':
return True
print('operator declined; skipping.', flush=True)
return False
def send_reboot(mav):
"""Request an autopilot reboot (MAV_CMD_PREFLIGHT_REBOOT_SHUTDOWN, param1=1)."""
mav.mav.command_long_send(
mav.target_system, mav.target_component,
mavutil.mavlink.MAV_CMD_PREFLIGHT_REBOOT_SHUTDOWN, 0,
1, 0, 0, 0, 0, 0, 0)
def list_usb_devices(pattern=None):
"""List candidate PX4 USB serial devices."""
if pattern:
return sorted(glob.glob(pattern))
devs = sorted(glob.glob(USB_DEVICE_GLOB_DARWIN))
if not devs:
devs = sorted(glob.glob(USB_DEVICE_GLOB_LINUX))
if not devs:
devs = sorted(glob.glob('/dev/ttyACM*'))
return devs
def device_family_glob(device):
"""Glob of the nodes of the same family as a serial device
(/dev/cu.usbmodem01 -> /dev/cu.usbmodem*), or None."""
base = re.sub(r'\d+$', '', device)
return base + '*' if base != device else None
def wait_device_gone(device, timeout=30):
"""Wait until the serial device node disappears. Returns elapsed or None."""
start = time.monotonic()
while time.monotonic() - start < timeout:
if not os.path.exists(device):
return time.monotonic() - start
time.sleep(0.2)
return None
def wait_device_back(device, timeout=45, pattern=None, others=None):
"""Wait until the device node (or a matching one) reappears.
USB enumeration can change the node name (usbmodem01 -> usbmodem101 on
macOS), so if the exact node does not come back we fall back to a glob
for the same device family, ignoring the nodes that were already there
(a debug probe or another board, also a usbmodem node, stays while the
board reboots): `others`, or the ones there when called. Returns
(device_path, elapsed) or (None, None).
"""
if pattern is None:
pattern = device_family_glob(device)
if others is None:
others = set(glob.glob(pattern)) - {device} if pattern else set()
start = time.monotonic()
while time.monotonic() - start < timeout:
if os.path.exists(device):
return device, time.monotonic() - start
if pattern:
found = sorted(set(glob.glob(pattern)) - others)
if found:
return found[0], time.monotonic() - start
time.sleep(0.2)
return None, None
def wait_reconnect(conn_str, baud=DEFAULT_BAUD, timeout=60, start=None):
"""Wait for a rebooted/re-enumerated board and reconnect.
For a serial device: waits for the node to vanish and return (the node
name may change across enumeration), lets the CDC ACM interface settle,
then retries connect() until a heartbeat arrives. Returns
(new_mav_connection, elapsed_seconds). Raises TimeoutError with a
description of what stalled.
"""
if start is None:
start = time.monotonic()
if is_serial_device(conn_str):
# the other devices of the family, before the board comes back under another name
pattern = device_family_glob(conn_str)
others = set(glob.glob(pattern)) - {conn_str} if pattern else set()
gone = wait_device_gone(conn_str, timeout=15)
# Some boards re-enumerate too fast to observe the node vanish; that is fine.
newdev, _ = wait_device_back(conn_str, timeout=timeout, pattern=pattern, others=others)
if newdev is None:
raise TimeoutError('serial device {} did not come back within {}s '
'(gone after {})'.format(
conn_str, timeout,
'{:.1f}s'.format(gone) if gone is not None else 'n/a'))
conn_str = newdev
time.sleep(2.0) # let the CDC ACM interface settle
else:
time.sleep(3.0)
remaining = max(5.0, timeout - (time.monotonic() - start))
last_err = None
while time.monotonic() - start < timeout:
try:
newmav = connect(conn_str, baud=baud, timeout=int(max(1, min(10, remaining))))
return newmav, time.monotonic() - start
except (TimeoutError, OSError) as e:
last_err = e
time.sleep(1.0)
raise TimeoutError('no heartbeat after reboot within {}s (last error: {})'.format(
timeout, last_err))
def reboot_and_reconnect(mav, conn_str, baud=DEFAULT_BAUD, timeout=60):
"""Reboot the autopilot and reconnect.
Returns (new_mav_connection, elapsed_seconds).
Raises TimeoutError with a description of what stalled.
"""
start = time.monotonic()
send_reboot(mav)
time.sleep(0.5)
mav.close()
return wait_reconnect(conn_str, baud=baud, timeout=timeout, start=start)
def add_connection_args(parser, dual_link=False):
"""Standard CLI surface shared by every test in the suite."""
parser.add_argument('connection',
help='MAVLink connection: serial device (/dev/tty.usbmodem01), '
'udp:IP:PORT, or tcp:IP:PORT')
if dual_link:
parser.add_argument('connection2', nargs='?', default=None,
help='optional second MAVLink connection (telemetry radio)')
parser.add_argument('--baudrate', '-b', type=int, default=DEFAULT_BAUD,
help='serial baud rate (default: %(default)s)')
parser.add_argument('--connect-timeout', type=float, default=20,
help='seconds to wait for the first heartbeat (default: %(default)s)')