From b8d4f0b637e3c2a483343f0b3bd4e4036b0cdcf7 Mon Sep 17 00:00:00 2001 From: Ramon Roche Date: Mon, 28 Sep 2026 11:36:16 -0700 Subject: [PATCH] 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 --- Tools/bench_test/bench/link_forwarding.py | 9 ++--- Tools/bench_test/bench/param_stress.py | 7 ++-- Tools/bench_test/px4bench/__init__.py | 41 ++++++++++++++++++++--- Tools/bench_test/px4bench/params.py | 29 +++++++++++++--- Tools/bench_test/sih/flight_mission.py | 6 ++-- 5 files changed, 74 insertions(+), 18 deletions(-) diff --git a/Tools/bench_test/bench/link_forwarding.py b/Tools/bench_test/bench/link_forwarding.py index 98cdf5162b0..d688cb2672b 100755 --- a/Tools/bench_test/bench/link_forwarding.py +++ b/Tools/bench_test/bench/link_forwarding.py @@ -31,7 +31,8 @@ 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) + send_heartbeat, wait_heartbeat) +from px4bench.params import recv_param_value HEARTBEAT_INTERVAL = 1.0 # GCS -> autopilot heartbeat cadence, seconds @@ -84,7 +85,7 @@ class ParamDownloader(threading.Thread): if now - self._last_progress > PARAM_STALL_TIMEOUT: self.error = 'no new param for {:.0f}s'.format(PARAM_STALL_TIMEOUT) return - m = self.mav.recv_match(type='PARAM_VALUE', blocking=True, timeout=0.2) + m = recv_param_value(self.mav, 0.2) if m is None: continue if self.expected == 0 and m.param_count > 0: @@ -151,7 +152,7 @@ def phase1_liveness(report, mav1, mav2, global_deadline): report.fail('phase1_budget', 'global deadline hit before checking {}'.format(label)) return False send_heartbeat(mav) - hb = mav.wait_heartbeat(timeout=5) + 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)) @@ -276,7 +277,7 @@ def phase4_post_liveness(report, mav1, mav2, global_deadline): report.fail('phase4_budget', 'global deadline hit before checking {}'.format(label)) return False send_heartbeat(mav) - hb = mav.wait_heartbeat(timeout=5) + 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)) diff --git a/Tools/bench_test/bench/param_stress.py b/Tools/bench_test/bench/param_stress.py index e48b5ee24cc..ee5878848e2 100755 --- a/Tools/bench_test/bench/param_stress.py +++ b/Tools/bench_test/bench/param_stress.py @@ -35,7 +35,8 @@ import px4bench from px4bench.params import (READ_TIMEOUT_S, SET_ECHO_TIMEOUT_S, drain_param_values, param_float_to_int32, param_id_str, param_is_saved, read_param, - read_until, set_param_int32, wait_param_echo) + read_until, recv_param_value, set_param_int32, + wait_param_echo) COMMIT_TIMEOUT_S = 8.0 @@ -80,7 +81,7 @@ def phase_full_download(report, mav): advertised if advertised is not None else '?')) break - m = mav.recv_match(type='PARAM_VALUE', blocking=True, timeout=1.0) + m = recv_param_value(mav, 1.0) if m is None: continue @@ -111,7 +112,7 @@ def phase_full_download(report, mav): mav.target_system, mav.target_component, b'', idx) deadline = time.monotonic() + 2.0 while time.monotonic() < deadline: - m = mav.recv_match(type='PARAM_VALUE', blocking=True, timeout=0.5) + m = recv_param_value(mav, 0.5) if m is None: continue r_idx = m.param_index diff --git a/Tools/bench_test/px4bench/__init__.py b/Tools/bench_test/px4bench/__init__.py index d366010d6cb..2700a382129 100644 --- a/Tools/bench_test/px4bench/__init__.py +++ b/Tools/bench_test/px4bench/__init__.py @@ -161,16 +161,49 @@ def connect(conn_str, baud=DEFAULT_BAUD, timeout: float = 20, source_system=254) assert isinstance(mav, mavutil.mavfile), 'unexpected connection type for {}'.format(conn_str) # announce ourselves so the autopilot streams to us send_heartbeat(mav) - hb = mav.wait_heartbeat(timeout=int(timeout)) + hb = wait_heartbeat(mav, timeout=timeout) if hb is None: mav.close() - raise TimeoutError('no HEARTBEAT on {} within {}s'.format(conn_str, timeout)) + 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.""" - return mav.wait_heartbeat(timeout=int(timeout)) + """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): diff --git a/Tools/bench_test/px4bench/params.py b/Tools/bench_test/px4bench/params.py index 1932182b700..4c088cb1483 100644 --- a/Tools/bench_test/px4bench/params.py +++ b/Tools/bench_test/px4bench/params.py @@ -11,6 +11,10 @@ per set (handler reply plus the changed-param announcement, times the number of mavlink instances), so callers must drain stale PARAM_VALUE messages before a set and then match the echo by expected value, never consume it positionally. + +Replies are matched on the pinned autopilot component (px4bench.connect): +another component on the same sysid can emit PARAM_VALUE too, and its +param_index/param_count must not be mixed into the autopilot's. """ import re @@ -19,6 +23,8 @@ import time from pymavlink import mavutil +from px4bench import is_from_target + MAV_PARAM_TYPE_INT32 = mavutil.mavlink.MAV_PARAM_TYPE_INT32 SET_ECHO_TIMEOUT_S = 5.0 @@ -43,6 +49,23 @@ def param_id_str(raw): return raw.rstrip('\x00') +def recv_param_value(mav, timeout): + """Return the next PARAM_VALUE from the pinned autopilot, or None. + + PARAM_VALUE from any other component is discarded. + """ + deadline = time.monotonic() + timeout + while True: + remaining = deadline - time.monotonic() + if remaining <= 0: + return None + m = mav.recv_match(type='PARAM_VALUE', blocking=True, timeout=remaining) + if m is None: + return None + if is_from_target(mav, m): + return m + + def request_param_read(mav, name): """Send PARAM_REQUEST_READ by name (param_index = -1).""" mav.mav.param_request_read_send( @@ -58,8 +81,7 @@ def read_param(mav, name, timeout=READ_TIMEOUT_S): request_param_read(mav, name) deadline = time.monotonic() + timeout while time.monotonic() < deadline: - m = mav.recv_match(type='PARAM_VALUE', blocking=True, - timeout=max(0.1, deadline - time.monotonic())) + m = recv_param_value(mav, max(0.1, deadline - time.monotonic())) if m is None: continue if param_id_str(m.param_id) == name: @@ -95,8 +117,7 @@ def wait_param_echo(mav, name, expected, timeout=SET_ECHO_TIMEOUT_S): seen = [] deadline = time.monotonic() + timeout while time.monotonic() < deadline: - m = mav.recv_match(type='PARAM_VALUE', blocking=True, - timeout=max(0.1, deadline - time.monotonic())) + m = recv_param_value(mav, max(0.1, deadline - time.monotonic())) if m is None: continue if param_id_str(m.param_id) != name: diff --git a/Tools/bench_test/sih/flight_mission.py b/Tools/bench_test/sih/flight_mission.py index bc62179e4fc..c4523f767ab 100755 --- a/Tools/bench_test/sih/flight_mission.py +++ b/Tools/bench_test/sih/flight_mission.py @@ -101,7 +101,7 @@ def wait_disarmed(mav, timeout): timeout=max(0.1, deadline - time.monotonic())) if m is None: continue - if m.get_srcSystem() != mav.target_system or m.get_srcComponent() != 1: + if not px4bench.is_from_target(mav, m): continue if not (m.base_mode & MAV_MODE_FLAG_SAFETY_ARMED): return time.monotonic() - start @@ -241,7 +241,7 @@ def fly(report, mav, shell, alt, report_dir): out = shell_cmd(report, shell, 'commander arm', 'arm_cmd') if out is None: return False - hb = mav.recv_match(type='HEARTBEAT', blocking=True, timeout=3) + hb = px4bench.wait_heartbeat(mav, timeout=3) if hb is not None and (hb.base_mode & MAV_MODE_FLAG_SAFETY_ARMED): armed = True else: @@ -312,7 +312,7 @@ def download_flight_log(report, mav, report_dir): from px4bench.ftp import mavftp try: ftp = mavftp.MAVFTP(mav, target_system=mav.target_system, - target_component=1) + target_component=mav.target_component) dirs = [e.name for e in bench_ftp.ftp_list(ftp, LOG_ROOT) if e.is_dir and not e.name.startswith('.')] if not dirs: