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>

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))

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

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):

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:

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: