Files
PX4-Autopilot/Tools/bench_test/px4bench/params.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

205 lines
7.4 KiB
Python

"""Parameter protocol helpers: PX4 int32 union encoding, read/set/echo.
PX4 transports an INT32 parameter as the raw bit pattern of the int placed
into the float param_value field. To send: pack the int as '<i', reinterpret
those 4 bytes as '<f'. To receive: pack the received float as '<f',
reinterpret as '<i'. This mirrors the union trick the firmware uses; the
param float is never a numeric conversion of the int.
Echo handling is hardware-learned: PX4 can emit more than one PARAM_VALUE
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
import struct
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
READ_TIMEOUT_S = 5.0
READ_UNTIL_TIMEOUT_S = 8.0
def int32_to_param_float(value):
"""Reinterpret an int32 bit pattern as the float carried in param_value."""
return struct.unpack('<f', struct.pack('<i', int(value)))[0]
def param_float_to_int32(value):
"""Reinterpret a param_value float's bit pattern as an int32."""
return struct.unpack('<i', struct.pack('<f', float(value)))[0]
def param_id_str(raw):
"""Normalize a PARAM_VALUE.param_id (bytes or str) to a plain string."""
if isinstance(raw, bytes):
raw = raw.decode('ascii', errors='replace')
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(
mav.target_system, mav.target_component,
name.encode('ascii'), -1)
def read_param(mav, name, timeout=READ_TIMEOUT_S):
"""PARAM_REQUEST_READ then wait for the matching PARAM_VALUE.
Returns (int32_value, raw_float) or (None, None) on timeout.
"""
request_param_read(mav, name)
deadline = time.monotonic() + timeout
while time.monotonic() < deadline:
m = recv_param_value(mav, max(0.1, deadline - time.monotonic()))
if m is None:
continue
if param_id_str(m.param_id) == name:
return param_float_to_int32(m.param_value), m.param_value
return None, None
def set_param_int32(mav, name, value):
"""PARAM_SET an int32 using the union encoding."""
mav.mav.param_set_send(
mav.target_system, mav.target_component,
name.encode('ascii'),
int32_to_param_float(value),
MAV_PARAM_TYPE_INT32)
def drain_param_values(mav):
"""Discard any queued PARAM_VALUE messages (stale echoes/broadcasts)."""
while mav.recv_match(type='PARAM_VALUE', blocking=False) is not None:
pass
def wait_param_echo(mav, name, expected, timeout=SET_ECHO_TIMEOUT_S):
"""Wait for a PARAM_VALUE for name carrying the expected int32 value.
PX4 can emit more than one PARAM_VALUE per set (handler reply plus the
changed-param announcement, times the number of mavlink instances), so
consuming a single message desyncs the harness by one echo forever.
Returns (matched, seen) where seen is every value observed for name; a
genuine wrong-echo firmware bug shows up as matched=False with the wrong
value(s) in seen.
"""
seen = []
deadline = time.monotonic() + timeout
while time.monotonic() < deadline:
m = recv_param_value(mav, max(0.1, deadline - time.monotonic()))
if m is None:
continue
if param_id_str(m.param_id) != name:
continue
value = param_float_to_int32(m.param_value)
seen.append(value)
if value == expected:
return True, seen
return False, seen
def read_until(mav, name, expected, timeout=READ_UNTIL_TIMEOUT_S):
"""Poll read_param until the board reports the expected int32 value.
A PARAM_SET propagates through the param system asynchronously, so an
echo (or even a single read) right after a set can still reflect the
prior value. Reading back in a loop until the board actually reports the
new value confirms it is committed to RAM, which closes the gap before a
save or a dependent check. This trusts the board's own read, not a queued
echo, so a stale PARAM_VALUE from an earlier set cannot satisfy it.
Returns (ok, last_seen): ok True once the board reports expected,
otherwise False with the last value read (None if nothing read back).
"""
deadline = time.monotonic() + timeout
last_seen = None
while True:
remaining = deadline - time.monotonic()
if remaining <= 0:
return False, last_seen
value, _ = read_param(mav, name, timeout=min(READ_TIMEOUT_S, remaining))
if value is not None:
last_seen = value
if value == expected:
return True, last_seen
else:
# brief pause so a non-responding read does not busy-spin
time.sleep(0.2)
# param show <name> prints one line:
# x + l SDLOG_UTC_OFFSET [used,idx] : 777
# three flag columns then the name; the SECOND flag is the save state:
# '*' unsaved, '+' saved, ' ' unmodified from default
# (src/systemcmds/param/param.cpp:822-823).
_PARAM_SHOW_RE = re.compile(
r'^\s*(?P<used>[x ])\s?(?P<saved>[*+ ])\s?(?P<ro>[l ])\s+'
r'(?P<name>[A-Z0-9_]+)\s+\[[^\]]*\]\s*:\s*(?P<value>-?\d+)')
def parse_param_show(output, name):
"""Parse `param show <name>` output for one parameter.
Returns (saved, value) where saved is True/False/None (None when the
line is not found or the save state is 'default'/unknown) and value is
the int32 value or None. Tolerant of ANSI escapes and the prompt.
"""
for raw in output.splitlines():
line = re.sub(r'\x1b\[[0-9;]*[A-Za-z]', '', raw).replace('nsh>', '')
m = _PARAM_SHOW_RE.search(line)
if not m or m.group('name') != name:
continue
flag = m.group('saved')
saved = True if flag == '+' else (False if flag == '*' else None)
try:
value = int(m.group('value'))
except ValueError:
value = None
return saved, value
return None, None
def param_is_saved(shell, name, timeout=10):
"""Run `param show <name>` over an open MavlinkShell and report save state.
Returns (saved, value, timed_out): saved True/False/None as parsed by
parse_param_show, value the int32 shown, timed_out True if the shell
command did not complete. Caller owns the shell (open and close it).
"""
out, timed_out = shell.run('param show {}'.format(name), timeout=timeout)
if timed_out:
return None, None, True
saved, value = parse_param_show(out, name)
return saved, value, False