From 114c19cd3103da54e8081e9c3cab09b8af742300 Mon Sep 17 00:00:00 2001 From: jonas Date: Mon, 16 Nov 2026 18:04:47 +0100 Subject: [PATCH] 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 --- .../autopilot_tester_pusher_failure.py | 236 ++++++++++++++++++ test/mavsdk_tests/configs/sih-sitl.json | 5 + test/mavsdk_tests/conftest.py | 11 +- test/mavsdk_tests/pytest.ini | 1 + test/mavsdk_tests/test_vtol_pusher_failure.py | 141 +++++++++++ 5 files changed, 392 insertions(+), 2 deletions(-) create mode 100644 test/mavsdk_tests/autopilot_tester_pusher_failure.py create mode 100644 test/mavsdk_tests/test_vtol_pusher_failure.py diff --git a/test/mavsdk_tests/autopilot_tester_pusher_failure.py b/test/mavsdk_tests/autopilot_tester_pusher_failure.py new file mode 100644 index 00000000000..9adc864ee33 --- /dev/null +++ b/test/mavsdk_tests/autopilot_tester_pusher_failure.py @@ -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)) diff --git a/test/mavsdk_tests/configs/sih-sitl.json b/test/mavsdk_tests/configs/sih-sitl.json index f4a2980d7a8..1bd51e78a13 100644 --- a/test/mavsdk_tests/configs/sih-sitl.json +++ b/test/mavsdk_tests/configs/sih-sitl.json @@ -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", diff --git a/test/mavsdk_tests/conftest.py b/test/mavsdk_tests/conftest.py index 68b82f1e290..222cca13a4a 100644 --- a/test/mavsdk_tests/conftest.py +++ b/test/mavsdk_tests/conftest.py @@ -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.""" diff --git a/test/mavsdk_tests/pytest.ini b/test/mavsdk_tests/pytest.ini index 211bd962d58..5949f8a3055 100644 --- a/test/mavsdk_tests/pytest.ini +++ b/test/mavsdk_tests/pytest.ini @@ -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 diff --git a/test/mavsdk_tests/test_vtol_pusher_failure.py b/test/mavsdk_tests/test_vtol_pusher_failure.py new file mode 100644 index 00000000000..cd4a3f8b265 --- /dev/null +++ b/test/mavsdk_tests/test_vtol_pusher_failure.py @@ -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)