Files
PX4-Autopilot/Tools/bench_test/bench/link_forwarding.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

367 lines
15 KiB
Python
Executable File

#!/usr/bin/env python3
"""
Dual-link forwarding stress test for the mavlink nested-send lock path
(link_forwarding).
This is the flagship bench test for qualifying PX4 v1.18 on real NuttX
hardware after the mavlink locking rework. The rework changed how the
mavlink module takes its send lock, and nested-send paths (a send issued
from within a handler that already holds the lock) were silently
deadlockable on NuttX. A deadlock there does not crash: it hangs. Every
byte over both links just stops.
This test drives two links simultaneously (a USB/telemetry pair) and
hammers the nested-send path hardest in phase 3, where a full parameter
download runs over link2 while an nsh shell session and message pump run
over link1 at the same time. If the send lock can deadlock under nested
use, one of the phases will stop producing traffic and the corresponding
wait will time out. A timeout is a FAIL that names exactly what stalled.
Nothing here arms the vehicle. A hung phase must never hang the script:
every wait is bounded and there is a global wall-clock deadline.
"""
import argparse
import os
import sys
import threading
import time
sys.path.insert(0, os.path.dirname(os.path.dirname(os.path.abspath(__file__))))
from px4bench import (Reporter, MavlinkShell, SHELL_OPEN_TIMEOUT,
add_connection_args, connect, parse_mavlink_status,
send_heartbeat, wait_heartbeat)
from px4bench.params import recv_param_value
HEARTBEAT_INTERVAL = 1.0 # GCS -> autopilot heartbeat cadence, seconds
MAX_SILENT_GAP = 5.0 # a gap larger than this on either link is a FAIL
SHELL_STATUS_INTERVAL = 5.0 # run `mavlink status` over link1 this often in phase 3
SHELL_RUN_TIMEOUT = 10.0 # per shell command timeout in phase 3
PARAM_STALL_TIMEOUT = 10.0 # no new params for this long during download -> stalled
PARAM_HARD_CAP = 120.0 # absolute cap on the param download, seconds
GLOBAL_BUDGET_EXTRA = 300.0 # wall-clock budget = duration + this
class ParamDownloader(threading.Thread):
"""Full parameter download over one link, run in a daemon worker thread.
Exposes progress (received / expected) so a stalled download can be
reported by index. The thread stops itself if no new parameter arrives
within PARAM_STALL_TIMEOUT or the hard cap is hit.
"""
def __init__(self, mav):
super().__init__(daemon=True)
self.mav = mav
self.received = 0
self.expected = 0 # param_count reported by the autopilot, 0 until known
self.complete = False
self.error = None
self._seen = set()
self._last_progress = time.monotonic()
def run(self):
try:
self._download()
except Exception as e: # noqa: BLE001 - a worker crash must be reported, not lost
self.error = repr(e)
def _download(self):
self.mav.mav.param_request_list_send(self.mav.target_system,
self.mav.target_component)
start = time.monotonic()
self._last_progress = start
next_hb = 0.0
while True:
now = time.monotonic()
if now > next_hb:
send_heartbeat(self.mav)
next_hb = now + HEARTBEAT_INTERVAL
if now - start > PARAM_HARD_CAP:
self.error = 'hard cap {:.0f}s hit'.format(PARAM_HARD_CAP)
return
if now - self._last_progress > PARAM_STALL_TIMEOUT:
self.error = 'no new param for {:.0f}s'.format(PARAM_STALL_TIMEOUT)
return
m = recv_param_value(self.mav, 0.2)
if m is None:
continue
if self.expected == 0 and m.param_count > 0:
self.expected = m.param_count
if m.param_index not in self._seen:
self._seen.add(m.param_index)
self.received = len(self._seen)
self._last_progress = now
if self.expected > 0 and self.received >= self.expected:
self.complete = True
return
class LinkPump(threading.Thread):
"""Pump one link in a daemon thread: read everything, track gaps and rate,
send a heartbeat every HEARTBEAT_INTERVAL. Used for phase 2 where both
links must stay live at once.
"""
def __init__(self, mav, name, deadline):
super().__init__(daemon=True)
self.mav = mav
self.name = name
self.deadline = deadline # monotonic time to stop at
self.msgs = 0
self.bytes = 0
self.max_gap = 0.0
self._stop = threading.Event()
def stop(self):
self._stop.set()
def run(self):
last_msg = time.monotonic()
next_hb = 0.0
while not self._stop.is_set() and time.monotonic() < self.deadline:
now = time.monotonic()
if now > next_hb:
send_heartbeat(self.mav)
next_hb = now + HEARTBEAT_INTERVAL
m = self.mav.recv_match(blocking=True, timeout=0.1)
now = time.monotonic()
if m is None:
gap = now - last_msg
if gap > self.max_gap:
self.max_gap = gap
continue
gap = now - last_msg
if gap > self.max_gap:
self.max_gap = gap
last_msg = now
self.msgs += 1
try:
self.bytes += len(m.get_msgbuf())
except Exception: # noqa: BLE001 - length is best-effort accounting only
pass
def phase1_liveness(report, mav1, mav2, global_deadline):
"""Independent heartbeat check on each link. FAIL names the dead link."""
report.info('Phase 1: liveness on both links')
for label, mav in (('link1', mav1), ('link2', mav2)):
if time.monotonic() > global_deadline:
report.fail('phase1_budget', 'global deadline hit before checking {}'.format(label))
return False
send_heartbeat(mav)
hb = wait_heartbeat(mav, timeout=5)
if hb is None:
report.fail('phase1_heartbeat_{}'.format(label),
'no heartbeat on {} within 5s (link dead)'.format(label))
return False
report.ok('phase1_heartbeat_{}'.format(label), 'heartbeat received')
return True
def phase2_sustained(report, mav1, mav2, duration, global_deadline):
"""Sustained bidirectional traffic on both links for `duration` seconds."""
report.info('Phase 2: sustained bidirectional traffic for {}s'.format(duration))
deadline = min(time.monotonic() + duration, global_deadline)
pump1 = LinkPump(mav1, 'link1', deadline)
pump2 = LinkPump(mav2, 'link2', deadline)
pump1.start()
pump2.start()
# Wait for the pumps to finish their window, with a hard join deadline so a
# wedged pump thread cannot hang the script.
join_deadline = deadline + 10.0
for pump in (pump1, pump2):
remaining = max(0.1, join_deadline - time.monotonic())
pump.join(timeout=remaining)
for pump in (pump1, pump2):
if pump.is_alive():
pump.stop()
report.fail('phase2_pump_{}'.format(pump.name),
'pump thread on {} did not stop (stalled)'.format(pump.name))
return False
elapsed = duration
for pump in (pump1, pump2):
rate_msgs = pump.msgs / elapsed if elapsed > 0 else 0.0
rate_bytes = pump.bytes / elapsed if elapsed > 0 else 0.0
detail = '{} msgs ({:.1f} msg/s), {} B ({:.1f} B/s), max gap {:.2f}s'.format(
pump.msgs, rate_msgs, pump.bytes, rate_bytes, pump.max_gap)
if pump.msgs == 0:
report.fail('phase2_traffic_{}'.format(pump.name),
'{}: zero messages received'.format(pump.name))
return False
if pump.max_gap > MAX_SILENT_GAP:
report.fail('phase2_gap_{}'.format(pump.name),
'{}: silent gap {:.2f}s exceeds {:.0f}s: {}'.format(
pump.name, pump.max_gap, MAX_SILENT_GAP, detail))
return False
report.ok('phase2_traffic_{}'.format(pump.name), detail)
return True
def phase3_nested_hammer(report, mav1, mav2, global_deadline):
"""Param download over link2 in a worker thread WHILE link1 runs an nsh
shell (`mavlink status` every 5s) and keeps pumping. This is the nested
send lock stress: heavy simultaneous use of both links.
"""
report.info('Phase 3: nested-send hammer (param download on link2, shell on link1)')
shell = MavlinkShell(mav1)
if not shell.open():
report.fail('phase3_shell_open',
'nsh shell over link1 did not respond within {:.0f}s'.format(
SHELL_OPEN_TIMEOUT))
return False
downloader = ParamDownloader(mav2)
downloader.start()
hard_deadline = min(time.monotonic() + PARAM_HARD_CAP + 15.0, global_deadline)
next_status = 0.0
shell_runs = 0
try:
while downloader.is_alive():
now = time.monotonic()
if now > hard_deadline:
break
# keep link1 pumping so the autopilot keeps streaming to us
mav1.recv_match(blocking=True, timeout=0.1)
if now >= next_status:
_, timed_out = shell.run('mavlink status', timeout=SHELL_RUN_TIMEOUT)
if timed_out:
report.fail('phase3_shell_stall',
'shell over link1 stalled during param download on link2 '
'(param {}/{})'.format(downloader.received, downloader.expected))
return False
shell_runs += 1
next_status = now + SHELL_STATUS_INTERVAL
# Join the worker with a hard deadline. A thread stuck past the deadline
# is itself the finding; it is a daemon so the script can still exit.
downloader.join(timeout=max(0.1, hard_deadline - time.monotonic()) + 2.0)
finally:
shell.close()
if downloader.is_alive():
report.fail('phase3_param_stall',
'param download on link2 stalled at index {}/{}'.format(
downloader.received, downloader.expected))
return False
if downloader.error is not None:
report.fail('phase3_param_download',
'param download on link2 incomplete ({}) at {}/{}'.format(
downloader.error, downloader.received, downloader.expected))
return False
if not downloader.complete:
report.fail('phase3_param_download',
'param download on link2 incomplete at {}/{}'.format(
downloader.received, downloader.expected))
return False
report.ok('phase3_param_download',
'downloaded {}/{} params while running {} shell status calls on link1'.format(
downloader.received, downloader.expected, shell_runs))
report.ok('phase3_shell', '{} `mavlink status` calls completed over link1'.format(shell_runs))
return True
def phase4_post_liveness(report, mav1, mav2, global_deadline):
"""After the hammer, both links must still deliver a fresh heartbeat and
link1 must complete one more `mavlink status`. This proves both links
survived heavy simultaneous use.
"""
report.info('Phase 4: post-stress liveness')
for label, mav in (('link1', mav1), ('link2', mav2)):
if time.monotonic() > global_deadline:
report.fail('phase4_budget', 'global deadline hit before checking {}'.format(label))
return False
send_heartbeat(mav)
hb = wait_heartbeat(mav, timeout=5)
if hb is None:
report.fail('phase4_heartbeat_{}'.format(label),
'no fresh heartbeat on {} within 5s after stress'.format(label))
return False
report.ok('phase4_heartbeat_{}'.format(label), 'link alive after stress')
shell = MavlinkShell(mav1)
if not shell.open():
report.fail('phase4_shell_open', 'nsh shell over link1 did not respond after stress')
return False
try:
out, timed_out = shell.run('mavlink status', timeout=SHELL_RUN_TIMEOUT)
finally:
shell.close()
if timed_out:
report.fail('phase4_shell_stall', '`mavlink status` over link1 stalled after stress')
return False
instances = parse_mavlink_status(out)
if not instances:
report.fail('phase4_status_parse', 'could not parse any instance block from mavlink status')
return False
for inst in instances:
report.info(' {} | {} | {}'.format(
inst['header'],
inst['tx'] if inst['tx'] else 'tx: ?',
inst['rx'] if inst['rx'] else 'rx: ?'))
report.ok('phase4_status', 'parsed {} mavlink instance block(s)'.format(len(instances)))
return True
def main():
parser = argparse.ArgumentParser(
description='Dual-link forwarding stress of the mavlink nested-send lock path.')
add_connection_args(parser, dual_link=True)
parser.add_argument('--baudrate2', type=int, default=57600,
help='baud rate for the second connection (default: %(default)s)')
parser.add_argument('--duration', type=float, default=60,
help='sustained-traffic duration in seconds (default: %(default)s)')
args = parser.parse_args()
if args.connection2 is None:
print('error: link_forwarding requires a second connection (CONNECTION2)',
file=sys.stderr)
sys.exit(2)
report = Reporter('link_forwarding')
global_deadline = time.monotonic() + args.duration + GLOBAL_BUDGET_EXTRA
mav1 = None
mav2 = None
try:
report.info('Connecting link1: {} @ {}'.format(args.connection, args.baudrate))
try:
mav1 = connect(args.connection, baud=args.baudrate, timeout=args.connect_timeout)
except (TimeoutError, OSError) as e:
report.fail('connect_link1', str(e))
sys.exit(report.finish())
report.info('Connecting link2: {} @ {}'.format(args.connection2, args.baudrate2))
try:
mav2 = connect(args.connection2, baud=args.baudrate2, timeout=args.connect_timeout)
except (TimeoutError, OSError) as e:
report.fail('connect_link2', str(e))
sys.exit(report.finish())
if not phase1_liveness(report, mav1, mav2, global_deadline):
sys.exit(report.finish())
if not phase2_sustained(report, mav1, mav2, args.duration, global_deadline):
sys.exit(report.finish())
if not phase3_nested_hammer(report, mav1, mav2, global_deadline):
sys.exit(report.finish())
if not phase4_post_liveness(report, mav1, mav2, global_deadline):
sys.exit(report.finish())
finally:
for mav in (mav1, mav2):
if mav is not None:
try:
mav.close()
except Exception: # noqa: BLE001 - cleanup only
pass
sys.exit(report.finish())
if __name__ == '__main__':
main()