Files
PX4-Autopilot/Tools/bench_test/px4bench/missions.py
T
Ramon Roche 9526efa4f6 refactor(bench): restructure suite into px4bench package with bench/ and sih/ split
Turn the flat script pile into a standalone project so contributors can
navigate and extend it:

- px4bench/ shared library package: core primitives in __init__ (Reporter,
  connect, MavlinkShell, reboot/replug, viewer tee, mavlink status parsers,
  pymavlink add_message workaround) plus protocol modules params.py,
  missions.py, and ftp.py extracted from the tests. Zero helper duplication
  remains across scripts.
- bench/ holds the real-firmware tests (boot_health, reboot_loop,
  usb_replug, link_forwarding, param_stress, mission_stress, log_transfer);
  sih/ holds the simulated flight (flight_mission), making the
  simulation/no-simulation boundary explicit.
- pyproject.toml (px4bench 0.1.0, BSD-3-Clause, pymavlink/pyserial deps,
  pyulog extra) so pip install -e Tools/bench_test works; every script
  remains directly runnable without installation via a parent-dir path shim.
- README rewritten contributor-first: architecture, per-test justification
  tied to the v1.18 risk areas, why pymavlink over MAVSDK, how to add a
  test, baseline workflow.
- Consistent CLI surface (shared connection args; --report-dir replaces
  log_transfer's --outdir); decorative section banners removed.

All hardware-learned behavior is preserved exactly: param echo drain and
match-by-value, shell sentinel strip-all, the add_message workaround,
explicit param save before reboot, mission clear before upload, RTL in
MAV_FRAME_MISSION, and the post-flight ULog download.

Signed-off-by: Ramon Roche <mrpollo@gmail.com>
2026-07-07 21:33:15 -07:00

258 lines
10 KiB
Python

"""Mission protocol helpers: item generation, upload/download/compare/clear.
Mission protocol (verified in src/modules/mavlink/mavlink_mission.cpp):
upload: MISSION_COUNT -> per-item MISSION_REQUEST_INT / MISSION_REQUEST
-> we reply MISSION_ITEM_INT -> final MISSION_ACK(ACCEPTED)
download: MISSION_REQUEST_LIST -> MISSION_COUNT -> per-item
MISSION_REQUEST_INT -> we reply, then send MISSION_ACK(ACCEPTED)
clear: MISSION_CLEAR_ALL -> MISSION_ACK(ACCEPTED)
The autopilot may request items with either MISSION_REQUEST_INT or the
deprecated MISSION_REQUEST; both are answered with MISSION_ITEM_INT, and
duplicate or out-of-order requests are answered as asked.
"""
import math
import time
from pymavlink import mavutil
MAV_CMD_NAV_WAYPOINT = mavutil.mavlink.MAV_CMD_NAV_WAYPOINT
MAV_FRAME = mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT_INT
MAV_MISSION_TYPE_MISSION = mavutil.mavlink.MAV_MISSION_TYPE_MISSION
MAV_MISSION_ACCEPTED = mavutil.mavlink.MAV_MISSION_ACCEPTED
BASE_LAT = 47.397742
BASE_LON = 8.545594
STEP_TIMEOUT_S = 5.0
TRANSACTION_DEADLINE_S = 60.0
COUNT_RETRANSMITS = 3
UPLOAD_TYPES = ['MISSION_REQUEST_INT', 'MISSION_REQUEST', 'MISSION_ACK']
DOWNLOAD_COUNT_TYPES = ['MISSION_COUNT', 'MISSION_ACK']
DOWNLOAD_ITEM_TYPES = ['MISSION_ITEM_INT', 'MISSION_ITEM', 'MISSION_ACK']
def transaction_deadline(num_items):
"""Transaction budget scaled for large missions on slow links.
220 items over a 57600 baud radio shared with telemetry streams can
legitimately take more than a minute; 0.5s per item is still tight
enough that a genuine stall (per-step 5s silence) fails much earlier.
"""
return max(TRANSACTION_DEADLINE_S, 0.5 * num_items)
class Item:
"""A single generated waypoint; comparison uses the fields we round-trip."""
def __init__(self, seq, lat_int, lon_int, alt, current):
self.seq = seq
self.command = MAV_CMD_NAV_WAYPOINT
self.frame = MAV_FRAME
self.current = current
self.autocontinue = 1
self.param1 = 0.0
self.param2 = 2.0 # acceptance radius
self.param3 = 0.0
self.param4 = 0.0
self.x = lat_int # int31, 1e-7 deg
self.y = lon_int
self.z = alt
def generate_mission(iteration, k):
"""Deterministic mission of k items, keyed on (iteration, k)."""
items = []
for seq in range(k):
lat_int = int((BASE_LAT + 0.0001 * seq) * 1e7) + iteration
lon_int = int((BASE_LON + 0.0001 * seq) * 1e7) + iteration
alt = 20.0 + (seq % 50)
items.append(Item(seq, lat_int, lon_int, alt, 1 if seq == 0 else 0))
return items
def send_item(mav, item):
mav.mav.mission_item_int_send(
mav.target_system, mav.target_component,
item.seq, item.frame, item.command,
item.current, item.autocontinue,
item.param1, item.param2, item.param3, item.param4,
item.x, item.y, item.z,
MAV_MISSION_TYPE_MISSION)
def upload_mission(report, mav, items, iteration):
"""Run the upload handshake. Returns (ok, duration, detail)."""
start = time.monotonic()
deadline = start + transaction_deadline(len(items))
mav.mav.mission_count_send(
mav.target_system, mav.target_component,
len(items), MAV_MISSION_TYPE_MISSION)
count_retransmits = 0
seq_reached = -1
saw_any_request = False
while time.monotonic() < deadline:
m = mav.recv_match(type=UPLOAD_TYPES, blocking=True, timeout=STEP_TIMEOUT_S)
if m is None:
# Initial silence: our MISSION_COUNT may have been dropped; retry.
if not saw_any_request and count_retransmits < COUNT_RETRANSMITS:
count_retransmits += 1
report.info('iter {}: no request, retransmitting MISSION_COUNT '
'({}/{})'.format(iteration, count_retransmits, COUNT_RETRANSMITS))
mav.mav.mission_count_send(
mav.target_system, mav.target_component,
len(items), MAV_MISSION_TYPE_MISSION)
continue
return (False, time.monotonic() - start,
'upload stalled waiting for request/ack at seq {}'.format(seq_reached))
t = m.get_type()
if t in ('MISSION_REQUEST_INT', 'MISSION_REQUEST'):
saw_any_request = True
seq = m.seq
# Respond to whatever seq is requested (handles dup / out-of-order).
if 0 <= seq < len(items):
send_item(mav, items[seq])
seq_reached = max(seq_reached, seq)
else:
return (False, time.monotonic() - start,
'autopilot requested out-of-range seq {} (have {})'.format(
seq, len(items)))
elif t == 'MISSION_ACK':
ok = (m.type == MAV_MISSION_ACCEPTED)
detail = 'ACK type {} at seq {}'.format(m.type, seq_reached)
return ok, time.monotonic() - start, detail
return (False, time.monotonic() - start,
'upload transaction deadline {:.0f}s hit at seq {}'.format(
transaction_deadline(len(items)), seq_reached))
def download_mission(mav):
"""Request and collect the mission. Returns (items_or_None, duration, detail)."""
start = time.monotonic()
deadline = start + TRANSACTION_DEADLINE_S
mav.mav.mission_request_list_send(
mav.target_system, mav.target_component, MAV_MISSION_TYPE_MISSION)
count = None
while time.monotonic() < deadline:
m = mav.recv_match(type=DOWNLOAD_COUNT_TYPES, blocking=True, timeout=STEP_TIMEOUT_S)
if m is None:
return None, time.monotonic() - start, 'download stalled waiting for MISSION_COUNT'
t = m.get_type()
if t == 'MISSION_COUNT':
count = m.count
# extend the budget now that the item count is known
deadline = start + transaction_deadline(count)
break
if t == 'MISSION_ACK':
# An error ACK here means the request was rejected.
return None, time.monotonic() - start, 'got MISSION_ACK type {} instead of COUNT'.format(m.type)
if count is None:
return None, time.monotonic() - start, 'download stalled before MISSION_COUNT'
items = {}
next_seq = 0
while next_seq < count and time.monotonic() < deadline:
mav.mav.mission_request_int_send(
mav.target_system, mav.target_component, next_seq, MAV_MISSION_TYPE_MISSION)
m = mav.recv_match(type=DOWNLOAD_ITEM_TYPES, blocking=True, timeout=STEP_TIMEOUT_S)
if m is None:
return None, time.monotonic() - start, \
'download stalled waiting for item seq {} of {}'.format(next_seq, count)
t = m.get_type()
if t in ('MISSION_ITEM_INT', 'MISSION_ITEM'):
items[m.seq] = m
if m.seq == next_seq:
next_seq += 1
elif t == 'MISSION_ACK':
return None, time.monotonic() - start, \
'unexpected MISSION_ACK type {} at seq {}'.format(m.type, next_seq)
if len(items) < count:
return None, time.monotonic() - start, \
'download incomplete: got {}/{} items'.format(len(items), count)
# Terminate the download transaction with our own ACK.
mav.mav.mission_ack_send(
mav.target_system, mav.target_component,
MAV_MISSION_ACCEPTED, MAV_MISSION_TYPE_MISSION)
ordered = [items[s] for s in range(count)]
return ordered, time.monotonic() - start, 'downloaded {} items'.format(count)
def compare_mission(expected, downloaded):
"""Compare item-by-item. Returns (ok, detail with first differing seq)."""
if len(downloaded) != len(expected):
return False, 'count mismatch: expected {} got {}'.format(
len(expected), len(downloaded))
for exp, got in zip(expected, downloaded):
if got.seq != exp.seq:
return False, 'seq {}: seq field mismatch (got {})'.format(exp.seq, got.seq)
if got.command != exp.command:
return False, 'seq {}: command {} != {}'.format(exp.seq, got.command, exp.command)
if got.frame != exp.frame:
return False, 'seq {}: frame {} != {}'.format(exp.seq, got.frame, exp.frame)
if got.x != exp.x:
return False, 'seq {}: x {} != {}'.format(exp.seq, got.x, exp.x)
if got.y != exp.y:
return False, 'seq {}: y {} != {}'.format(exp.seq, got.y, exp.y)
if not math.isclose(got.z, exp.z, abs_tol=1e-3):
return False, 'seq {}: z {} != {} (tol 1e-3)'.format(exp.seq, got.z, exp.z)
return True, 'all {} items match'.format(len(expected))
def clear_mission(mav):
"""MISSION_CLEAR_ALL -> MISSION_ACK(ACCEPTED). Returns (ok, detail)."""
deadline = time.monotonic() + TRANSACTION_DEADLINE_S
mav.mav.mission_clear_all_send(
mav.target_system, mav.target_component, MAV_MISSION_TYPE_MISSION)
while time.monotonic() < deadline:
m = mav.recv_match(type='MISSION_ACK', blocking=True, timeout=STEP_TIMEOUT_S)
if m is None:
return False, 'clear stalled waiting for MISSION_ACK'
if m.type == MAV_MISSION_ACCEPTED:
return True, 'cleared'
return False, 'clear rejected: MISSION_ACK type {}'.format(m.type)
return False, 'clear transaction deadline hit'
def verify_cleared(mav):
"""After clear, MISSION_REQUEST_LIST should yield MISSION_COUNT == 0."""
deadline = time.monotonic() + TRANSACTION_DEADLINE_S
mav.mav.mission_request_list_send(
mav.target_system, mav.target_component, MAV_MISSION_TYPE_MISSION)
while time.monotonic() < deadline:
m = mav.recv_match(type=DOWNLOAD_COUNT_TYPES, blocking=True, timeout=STEP_TIMEOUT_S)
if m is None:
return False, 'verify-clear stalled waiting for MISSION_COUNT'
t = m.get_type()
if t == 'MISSION_COUNT':
# Terminate the (empty) list transaction.
mav.mav.mission_ack_send(
mav.target_system, mav.target_component,
MAV_MISSION_ACCEPTED, MAV_MISSION_TYPE_MISSION)
if m.count == 0:
return True, 'count 0 after clear'
return False, 'count {} after clear (expected 0)'.format(m.count)
if t == 'MISSION_ACK':
# Empty mission may be reported via ACK; treat non-error as cleared.
if m.type == MAV_MISSION_ACCEPTED:
return True, 'empty mission (ACK) after clear'
return False, 'verify-clear got MISSION_ACK type {}'.format(m.type)
return False, 'verify-clear transaction deadline hit'