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)