test(mavsdk_tests): specify the VTOL pusher failure detection in SITL

The unit tests of the pusher failure detection pin its implementation.
These SIH flights state what the detection must achieve instead, so any
detector can be measured with the same flights and detection times.

Assisted-by: Claude:claude-opus-5-5
This commit is contained in:
jonas committed 2026-11-20 11:28:42 +01:00
1 parent 0d46711f17
commit 114c19cd31
5 files changed
+392 -2

No files matched your search

@@ -0,0 +1,236 @@
"""AutopilotTester for the pusher failure detection of the SIH standard VTOL with pusher assist.
The pusher is stopped with an undetected motor failure (`failure motor wrong`): its output is cut,
but it stays in the control allocation and no motor failure is reported, like a pusher without
ESC telemetry. The checks only use what the vehicle reports (quad-chute events, VTOL state,
outputs), not how a failure is detected, so any detector can be measured with the same flights.
"""
import threading
from dataclasses import dataclass
from typing import Any, List, Optional, Tuple
from mavsdk.plugins.action import ActionError # type: ignore
from mavsdk.plugins.failure import FailureType, FailureUnit # type: ignore
from mavsdk.plugins.telemetry import VtolState # type: ignore
from autopilot_tester import (MAV_CMD_NAV_LAND, MAV_CMD_NAV_WAYPOINT,
MAV_FRAME_GLOBAL_RELATIVE_ALT, AutopilotTester, log,
raw_mission_item)
# The pusher is motor 5 (1-based failure instance) on output channel 8.
PUSHER_MOTOR_INSTANCE = 5
PUSHER_OUTPUT_INDEX = 7
# Outputs are 1000 + 1000 * throttle, a stopped or unused pusher reads 900.
PUSHER_PUSHING_OUTPUT = 1100.0
PUSHER_STOPPED_OUTPUT = 901.0
QUADCHUTE_EVENT_PREFIX = "vtol_att_ctrl_quadchute"
PUSHER_QUADCHUTE_EVENT = "vtol_att_ctrl_quadchute_fwd_prop"
# The VTOL state is sampled, fast enough not to miss a short fixed-wing phase.
VTOL_STATE_RATE_HZ = 10.0
class FixedWingReached(AssertionError):
pass
@dataclass
class Wind:
"""Constant SIH wind. SIH_WIND_N and SIH_WIND_E are the speeds of the wind blowing from north
and from east, so a positive from_north_m_s is a headwind when flying north."""
from_north_m_s: float = 0.0
from_east_m_s: float = 0.0
@dataclass
class Quadchute:
time_s: float
event_name: str
class AutopilotTesterPusherFailure(AutopilotTester):
def __init__(self, speed_factor: Optional[float] = None) -> None:
super().__init__(speed_factor)
self._record_lock = threading.Lock()
self._quadchutes: List[Quadchute] = []
self._vtol_states: List[Tuple[float, Any]] = []
self._pusher_failure_time_s: Optional[float] = None
def connect(self, url: str) -> None:
super().connect(url)
self._events.subscribe_events(self._on_event)
self._telemetry.subscribe_vtol_state(self._on_vtol_state)
# ------------------------------------------------------------------
# Setup
# ------------------------------------------------------------------
def take_off_with_pusher_assist(self, altitude_m: float, use_airspeed: bool = True) -> None:
"""Take off with pusher assist and failure injection enabled, then hold."""
self.wait_until_ready()
self.set_param_int("VT_FWD_THRUST_EN", 1)
self.set_param_int("SYS_FAILURE_EN", 1)
# The control allocation only applies injected motor stops with a failure mode set.
self.set_param_int("CA_FAILURE_MODE", 1)
# Airspeed-less setup (vtol_without_airspeed_sensor.md), the sensor stays installed.
self.set_param_int("FW_USE_AIRSPD", 1 if use_airspeed else 0)
self.enable_actuator_output_status()
self._telemetry.set_rate_vtol_state(VTOL_STATE_RATE_HZ)
self.set_takeoff_altitude(altitude_m)
# Necessary for the takeoff altitude to be applied properly.
self.sleep_for(1)
self.arm()
self.takeoff()
self.wait_until_altitude(altitude_m, 60)
def set_wind(self, wind: Wind, settle_s: float) -> None:
# Wind on the parked SIH vehicle can fail its preflight checks, so set it in the air.
log("Wind from north {} m/s, from east {} m/s".format(
wind.from_north_m_s, wind.from_east_m_s))
self.set_param_float("SIH_WIND_N", wind.from_north_m_s)
self.set_param_float("SIH_WIND_E", wind.from_east_m_s)
self.sleep_for(settle_s)
def start_multicopter_mission(self, waypoints: List[Tuple[float, float]],
altitude_m: float) -> None:
"""Fly to each (north, east) waypoint relative to home in multicopter mode and land at
the last one."""
ct = self.get_coordinate_transformation()
def item(seq: int, command: int, north_m: float, east_m: float, z: float) -> Any:
latitude_deg, longitude_deg = ct.global_from_local(north_m, east_m)
return raw_mission_item(seq, command, MAV_FRAME_GLOBAL_RELATIVE_ALT,
x=round(latitude_deg * 1e7), y=round(longitude_deg * 1e7),
z=z, current=1 if seq == 0 else 0)
items = [item(seq, MAV_CMD_NAV_WAYPOINT, north_m, east_m, altitude_m)
for seq, (north_m, east_m) in enumerate(waypoints)]
items.append(item(len(items), MAV_CMD_NAV_LAND, *waypoints[-1], 0.0))
self.upload_mission_raw(items)
self._mission_raw.start_mission()
def wait_until_north_of(self, north_m: float, timeout_s: float) -> None:
self.wait_for_position_ned(lambda s: bool(s.position.north_m > north_m), timeout_s)
# ------------------------------------------------------------------
# Pusher
# ------------------------------------------------------------------
def _pusher_output(self) -> float:
return float(self._telemetry.actuator_output_status().actuator[PUSHER_OUTPUT_INDEX])
def check_pusher_pushing(self) -> None:
"""Precondition of the flights with a working pusher, otherwise they test nothing."""
output = self._pusher_output()
assert output >= PUSHER_PUSHING_OUTPUT, \
"Pusher output {:.0f}, expected pusher assist to push".format(output)
def fail_pusher(self) -> float:
"""Stop the pusher, return the time of the failure."""
self.inject_failure(FailureUnit.SYSTEM_MOTOR, FailureType.WRONG, PUSHER_MOTOR_INSTANCE)
self._pusher_failure_time_s = self.time_s()
log("Pusher stopped")
return self._pusher_failure_time_s
def check_pusher_stopped(self) -> None:
"""Check that the injected failure cut the pusher output while it should push."""
assert self.poll_condition_with_timeout(
lambda: self._pusher_output() <= PUSHER_STOPPED_OUTPUT, 1), \
"Pusher output {:.0f}, expected the pusher to be stopped".format(self._pusher_output())
# ------------------------------------------------------------------
# Quad-chute and VTOL state
# ------------------------------------------------------------------
def wait_for_quadchute(self, since_s: float, timeout_s: float,
expected_event: Optional[str] = None) -> float:
"""Wait for the first quad-chute after since_s, return its delay."""
assert self.poll_condition_with_timeout(
lambda: bool(self._quadchutes_since(since_s)), timeout_s), \
"No quad-chute within {} s".format(timeout_s)
quadchute = self._quadchutes_since(since_s)[0]
if expected_event is not None:
assert quadchute.event_name == expected_event, \
"Quad-chute {}, expected {}".format(quadchute.event_name, expected_event)
delay_s = quadchute.time_s - since_s
log("Quad-chute {} after {:.1f} s".format(quadchute.event_name, delay_s))
assert delay_s <= timeout_s, "Quad-chute after {:.1f} s, expected within {} s".format(
delay_s, timeout_s)
return delay_s
def wait_for_pusher_quadchute(self, timeout_s: float) -> float:
"""Wait for the pusher failure quad-chute after fail_pusher(), return the detection
time."""
assert self._pusher_failure_time_s is not None, "fail_pusher() was not called"
detection_time_s = self.wait_for_quadchute(
self._pusher_failure_time_s, timeout_s, PUSHER_QUADCHUTE_EVENT)
log("Detection time: {:.1f} s".format(detection_time_s))
return detection_time_s
def try_transition_to_fixedwing(self) -> None:
"""Command a front transition once, it may be denied."""
try:
self.transition_to_fixedwing()
except ActionError as e:
log("Front transition denied: {}".format(e))
def log_quadchutes_since(self, since_s: float) -> None:
quadchutes = self._quadchutes_since(since_s)
if not quadchutes:
log("No quad-chute")
for quadchute in quadchutes:
log("Quad-chute {} after {:.1f} s".format(
quadchute.event_name, quadchute.time_s - since_s))
def check_no_quadchute(self, allowed_event: Optional[str] = None) -> None:
quadchutes = [q for q in self._quadchutes_since(0.0) if q.event_name != allowed_event]
assert not quadchutes, "Quad-chute {} at {:.1f} s".format(
quadchutes[0].event_name, quadchutes[0].time_s)
def wait_until_transition_started(self, since_s: float, timeout_s: float) -> None:
assert self.poll_condition_with_timeout(
lambda: VtolState.TRANSITION_TO_FW in self._vtol_states_since(since_s), timeout_s), \
"No front transition started within {} s".format(timeout_s)
def check_never_fixed_wing_since(self, since_s: float, duration_s: float) -> None:
"""Watch the VTOL state for duration_s more."""
self.sleep_for(duration_s)
if VtolState.FW in self._vtol_states_since(since_s):
raise FixedWingReached("Fixed-wing mode reached")
def check_only_multicopter_since(self, since_s: float) -> None:
states = self._vtol_states_since(since_s)
assert states, "No VTOL state received"
other = [state for state in states if state != VtolState.MC]
assert not other, "VTOL state {}, expected multicopter only".format(other[0])
def time_s(self) -> float:
"""Autopilot time."""
return self._autopilot_time_us() / 1e6
# ------------------------------------------------------------------
# Internals
# ------------------------------------------------------------------
def _quadchutes_since(self, since_s: float) -> List[Quadchute]:
with self._record_lock:
return [q for q in self._quadchutes if q.time_s >= since_s]
def _vtol_states_since(self, since_s: float) -> List[Any]:
with self._record_lock:
return [state for time_s, state in self._vtol_states if time_s >= since_s]
def _on_event(self, event: Any, _user_data: Any) -> None:
if event.event_name.startswith(QUADCHUTE_EVENT_PREFIX):
with self._record_lock:
self._quadchutes.append(Quadchute(self.time_s(), str(event.event_name)))
def _on_vtol_state(self, state: Any, _user_data: Any) -> None:
with self._record_lock:
self._vtol_states.append((self.time_s(), state))
+5
View File
@@ -70,6 +70,11 @@
"test_filter": "vtol",
"timeout_min": 10
},
{
"model": "standard_vtol",
"test_filter": "vtol_pusher_failure",
"timeout_min": 10
},
{
"model": "xvert",
"test_filter": "sih_xvert_transition",
+9 -2
View File
@@ -5,14 +5,15 @@ import pytest
# Show the values in failed asserts inside the testers, not just in tests.
pytest.register_assert_rewrite("autopilot_tester", "autopilot_tester_calibration",
"autopilot_tester_flow", "autopilot_tester_follow_me",
"autopilot_tester_loiter", "autopilot_tester_rtl",
"autopilot_tester_vision_target")
"autopilot_tester_loiter", "autopilot_tester_pusher_failure",
"autopilot_tester_rtl", "autopilot_tester_vision_target")
from autopilot_tester import AutopilotTester # noqa: E402
from autopilot_tester_calibration import AutopilotTesterCalibration # noqa: E402
from autopilot_tester_flow import AutopilotTesterFlow # noqa: E402
from autopilot_tester_follow_me import AutopilotTesterFollowMe # noqa: E402
from autopilot_tester_loiter import AutopilotTesterLoiter # noqa: E402
from autopilot_tester_pusher_failure import AutopilotTesterPusherFailure # noqa: E402
from autopilot_tester_rtl import AutopilotTesterRtl # noqa: E402
from autopilot_tester_vision_target import AutopilotTesterVisionTarget # noqa: E402
@@ -66,6 +67,12 @@ def tester_loiter(request: pytest.FixtureRequest) -> Iterator[AutopilotTesterLoi
yield from connected_tester(request, AutopilotTesterLoiter)
@pytest.fixture
def tester_pusher_failure(request: pytest.FixtureRequest) -> Iterator[AutopilotTesterPusherFailure]:
"""A tester for the pusher failure detection of a standard VTOL, connected to the vehicle."""
yield from connected_tester(request, AutopilotTesterPusherFailure)
@pytest.fixture
def tester_flow(request: pytest.FixtureRequest) -> Iterator[AutopilotTesterFlow]:
"""A tester for optical flow aided navigation, connected to the vehicle."""
+1
View File
@@ -11,6 +11,7 @@ markers =
offboard_attitude: offboard attitude control
vtol_wind: VTOL tests that need a windy world
vtol_airspeed_fail: VTOL airspeed failure tests
vtol_pusher_failure: VTOL pusher failure detection tests
loiter: fixed-wing loiter tests
controlallocation: control allocation tests
motor_failure_injection: motor failure injection tests
@@ -0,0 +1,141 @@
"""Pusher failure detection of standard VTOLs with pusher assist (VT_FWD_THRUST_EN).
What the detection must achieve, whatever the detector:
1. A failed pusher never brings the vehicle into fixed-wing flight.
2. A pusher that fails while pusher assist flies the vehicle forward in multicopter mode triggers
the pusher failure quad-chute within DETECTION_TIME_S. The vehicle then stays in multicopter
mode and lands.
3. A working pusher triggers no quad-chute.
Accepted, not required: a headwind stronger than the pusher may be reported as a pusher failure,
the vehicle cannot make progress with pusher assist either way.
Known gaps of the current detector: a tailwind that keeps the vehicle moving hides the failure,
and a pusher that fails in hover is only noticed by the front transition checks.
The detection times are logged, so detectors can be compared with the same flights:
test/mavsdk_tests/mavsdk_test_runner.py --speed-factor 10 --model standard_vtol \\
--case '*pusher*' test/mavsdk_tests/configs/sih-sitl.json
SIH has constant wind only and a 0.2 kg vehicle, so the flights check behaviour, not tuning.
"""
import pytest
from autopilot_tester import log
from autopilot_tester_pusher_failure import (PUSHER_QUADCHUTE_EVENT,
AutopilotTesterPusherFailure, FixedWingReached,
Wind)
ALTITUDE_M = 20.0
SETTLE_S = 10.0
DETECTION_TIME_S = 10.0
# Backstop of the front transition, VT_TRANS_TIMEOUT
TRANSITION_TIMEOUT_S = 15.0
# SIH pusher assist makes progress against 7 m/s of headwind and drifts back at 8 m/s.
NO_WIND = Wind()
HEADWIND = Wind(from_north_m_s=3.0)
CROSSWIND = Wind(from_east_m_s=3.0)
STRONG_WIND = Wind(from_north_m_s=5.0)
TOO_STRONG_WIND = Wind(from_north_m_s=8.0)
@pytest.mark.vtol_pusher_failure
@pytest.mark.parametrize("wind", [NO_WIND, HEADWIND, CROSSWIND],
ids=["no_wind", "headwind", "crosswind"])
def test_pusher_failure_in_multicopter_cruise(
tester_pusher_failure: AutopilotTesterPusherFailure, wind: Wind) -> None:
"""Requirement 2: the pusher stops on a mission leg flown north."""
tester = tester_pusher_failure
tester.take_off_with_pusher_assist(ALTITUDE_M)
tester.set_wind(wind, SETTLE_S)
tester.start_multicopter_mission([(300.0, 0.0)], ALTITUDE_M)
tester.wait_until_north_of(40.0, 60)
tester.check_pusher_pushing()
failure_time_s = tester.fail_pusher()
tester.check_pusher_stopped()
tester.wait_for_pusher_quadchute(DETECTION_TIME_S)
tester.try_transition_to_fixedwing()
tester.wait_until_disarmed(240)
tester.check_only_multicopter_since(failure_time_s)
@pytest.mark.vtol_pusher_failure
@pytest.mark.parametrize("wind", [NO_WIND, STRONG_WIND], ids=["no_wind", "strong_wind"])
def test_working_pusher_in_multicopter_mission(
tester_pusher_failure: AutopilotTesterPusherFailure, wind: Wind) -> None:
"""Requirement 3: legs into, across and with the wind. The waypoints command a north
heading, so with the wind the pusher idles."""
tester = tester_pusher_failure
tester.request_ground_truth()
tester.take_off_with_pusher_assist(ALTITUDE_M)
tester.set_wind(wind, SETTLE_S)
tester.start_multicopter_mission([(100.0, 0.0), (100.0, 60.0), (0.0, 60.0)], ALTITUDE_M)
tester.wait_until_north_of(40.0, 60)
tester.check_pusher_pushing()
tester.wait_until_disarmed(450)
tester.check_no_quadchute()
tester.check_only_multicopter_since(0.0)
# A failsafe would have landed elsewhere.
tester.check_mission_land_within(5.0)
@pytest.mark.vtol_pusher_failure
def test_working_pusher_holding_position_in_strong_wind(
tester_pusher_failure: AutopilotTesterPusherFailure) -> None:
"""Requirement 3: the pusher pushes against the wind, but the vehicle is not asked to
move."""
tester = tester_pusher_failure
tester.take_off_with_pusher_assist(ALTITUDE_M)
tester.set_wind(STRONG_WIND, SETTLE_S)
tester.check_pusher_pushing()
tester.sleep_for(60)
tester.check_pusher_pushing()
tester.wait_until_speed_lower_than(0.5, 5)
tester.check_no_quadchute()
@pytest.mark.vtol_pusher_failure
def test_headwind_stronger_than_pusher(
tester_pusher_failure: AutopilotTesterPusherFailure) -> None:
"""Accepted, not required: the vehicle cannot hold its position with pusher assist, which
may be reported as a pusher failure."""
tester = tester_pusher_failure
tester.take_off_with_pusher_assist(ALTITUDE_M)
wind_time_s = tester.time_s()
tester.set_wind(TOO_STRONG_WIND, 60)
tester.log_quadchutes_since(wind_time_s)
tester.check_no_quadchute(allowed_event=PUSHER_QUADCHUTE_EVENT)
tester.check_only_multicopter_since(wind_time_s)
@pytest.mark.vtol_pusher_failure
@pytest.mark.parametrize("use_airspeed", [
True,
pytest.param(False, marks=pytest.mark.xfail(
strict=True, raises=FixedWingReached,
reason="no pusher check in front transitions yet, the open loop transition "
"(VT_F_TR_OL_TM) completes")),
], ids=["airspeed", "no_airspeed"])
def test_front_transition_with_failed_pusher(
tester_pusher_failure: AutopilotTesterPusherFailure, use_airspeed: bool) -> None:
"""Requirement 1: the pusher stops in hover, then a front transition is commanded."""
tester = tester_pusher_failure
tester.take_off_with_pusher_assist(ALTITUDE_M, use_airspeed)
tester.fail_pusher()
tester.sleep_for(2)
transition_time_s = tester.time_s()
tester.transition_to_fixedwing()
tester.wait_until_transition_started(transition_time_s, 5)
tester.sleep_for(1)
tester.check_pusher_stopped()
tester.check_never_fixed_wing_since(transition_time_s, TRANSITION_TIMEOUT_S + 5)
abort_time_s = tester.wait_for_quadchute(transition_time_s, TRANSITION_TIMEOUT_S + 5)
log("Front transition aborted after {:.1f} s".format(abort_time_s))
tester.wait_until_multicopter(10)