Files
PX4-Autopilot/Tools/bench_test/px4bench/__init__.py
T
Ramon Roche b8d4f0b637 fix(bench): pin the autopilot component instead of the first heartbeat
Some boards heartbeat from more than one component on the same sysid. An
FMUv6X-RT on v1.17 sends HEARTBEAT from compid 1 (PX4) and from compid 236
with autopilot=MAV_AUTOPILOT_INVALID. connect() accepted the first
heartbeat from anyone and left target_component at pymavlink's default of
0, so param and mission requests went out as broadcast, the other
component could answer, and param_stress mixed its PARAM_VALUE stream
(index, count, values) into the autopilot's download.

connect() now waits for a heartbeat whose autopilot is not
MAV_AUTOPILOT_INVALID and pins target_system/target_component to its
source. pymavlink only latches target_system once and never sets the
component, so the pin holds for the life of the connection. The
wait_heartbeat() helper shares that filter and, once pinned, only counts
the pinned component; wait_reconnect() goes through connect() and gets
the same behavior after a reboot.

PARAM_VALUE replies in px4bench.params, param_stress and link_forwarding
are filtered to the pinned component, and flight_mission uses the pinned
component for MAVFTP and the armed/disarmed heartbeat checks instead of a
hardcoded 1.

Diagnosed by @farhangnaderi in #27852.

Assisted-by: Claude:claude-opus-5-5
Signed-off-by: Ramon Roche <mrpollo@gmail.com>
2026-09-28 11:47:52 -07:00

577 lines
23 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 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):
"""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. Returns (device_path, elapsed) or (None, None).
"""
if pattern is None:
base = re.sub(r'\d+$', '', device)
pattern = base + '*' if base != device else None
start = time.monotonic()
while time.monotonic() - start < timeout:
if os.path.exists(device):
return device, time.monotonic() - start
if pattern:
found = sorted(glob.glob(pattern))
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):
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)
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)')