diff --git a/test/mavsdk_tests/autopilot_tester_geofence.py b/test/mavsdk_tests/autopilot_tester_geofence.py new file mode 100644 index 00000000000..a906657fb0a --- /dev/null +++ b/test/mavsdk_tests/autopilot_tester_geofence.py @@ -0,0 +1,179 @@ +"""AutopilotTester for the emergency and the contingency geofence. + +The plan fence is a rectangle in home-relative local (north, east) coordinates. How far the +vehicle went is taken from the ground truth, so a breach shows even if the estimate hides it. +""" + +import math +import threading +from typing import Any, List, Optional, Tuple + +from mavsdk.plugins.action import ActionError # type: ignore +from mavsdk.plugins.telemetry import FlightMode # type: ignore + +from autopilot_tester import ( + MANUAL_CONTROL_RATE_HZ, MAV_CMD_NAV_WAYPOINT, MAV_FRAME_GLOBAL_INT, + MAV_FRAME_GLOBAL_RELATIVE_ALT, MissionOptions, log, raw_mission_item) +from autopilot_tester_loiter import AutopilotTesterLoiter +from geometry import CoordinateTransformation + +MAV_CMD_NAV_VTOL_TAKEOFF = 84 +MAV_CMD_NAV_FENCE_POLYGON_VERTEX_INCLUSION = 5001 +MAV_MISSION_TYPE_FENCE = 1 + +# Values of GF_ACTION and GF_CONT_ACTION. +ACTION_WARNING = 1 +ACTION_HOLD = 2 +ACTION_LAND = 5 + +Local2 = Tuple[float, float] + + +class AutopilotTesterGeofence(AutopilotTesterLoiter): + + def __init__(self, speed_factor: Optional[float] = None) -> None: + super().__init__(speed_factor) + # north_min, north_max, east_min, east_max + self._fence: Optional[Tuple[float, float, float, float]] = None + self._track_handle: Any = None + self._track_lock = threading.Lock() + self._min_fence_distance_m = math.inf + self._max_home_distance_m = 0.0 + self._max_relative_altitude_m = -math.inf + + # ------------------------------------------------------------------ + # Fence, missions and commands + # ------------------------------------------------------------------ + + def upload_inclusion_rectangle(self, north_min_m: float, north_max_m: float, + east_min_m: float, east_max_m: float) -> None: + """Upload one inclusion polygon as the geofence plan.""" + ct = self.get_coordinate_transformation() + corners: List[Local2] = [(north_min_m, east_min_m), (north_max_m, east_min_m), + (north_max_m, east_max_m), (north_min_m, east_max_m)] + items = [] + for seq, corner in enumerate(corners): + latitude_deg, longitude_deg = ct.global_from_local(*corner) + items.append(raw_mission_item( + seq, MAV_CMD_NAV_FENCE_POLYGON_VERTEX_INCLUSION, MAV_FRAME_GLOBAL_INT, + x=round(latitude_deg * 1e7), y=round(longitude_deg * 1e7), + param1=float(len(corners)), + # MAVSDK requires a current item, even though it makes little sense for a fence. + current=1 if seq == 0 else 0, + autocontinue=0, mission_type=MAV_MISSION_TYPE_FENCE)) + + self._mission_raw.upload_geofence(items) + self._fence = (north_min_m, north_max_m, east_min_m, east_max_m) + # PX4 needs time to load the fence. + self.sleep_for(1) + + def upload_mission_to(self, north_m: float, options: MissionOptions) -> None: + """Upload a mission from above Home to a point north of it.""" + self._upload_mission([(0.0, 0.0), (north_m, 0.0)], options) + + def upload_vtol_mission(self, waypoints: List[Local2], rel_alt_m: float) -> None: + """Upload a VTOL takeoff followed by the waypoints, without a landing.""" + ct = self.get_coordinate_transformation() + items = [] + for seq, waypoint in enumerate(waypoints): + latitude_deg, longitude_deg = ct.global_from_local(*waypoint) + items.append(raw_mission_item( + seq, MAV_CMD_NAV_VTOL_TAKEOFF if seq == 0 else MAV_CMD_NAV_WAYPOINT, + MAV_FRAME_GLOBAL_RELATIVE_ALT, + x=round(latitude_deg * 1e7), y=round(longitude_deg * 1e7), z=rel_alt_m, + current=1 if seq == 0 else 0)) + + self.upload_mission_raw(items) + + def select_mission_mode(self) -> None: + """Switch to Mission mode. A disarmed vehicle accepts that even with a rejected mission.""" + assert self._try_start_mission(), "Could not switch to Mission mode" + + def arming_is_denied(self) -> bool: + """One arming attempt, without the retries of arm().""" + try: + self._action.arm() + except ActionError as e: + log("Arming denied: {}".format(e)) + return True + return False + + def fly_in_position_mode_until_hold(self, forward: float, throttle: float, + timeout_s: float) -> None: + """Hold the sticks in Position mode until a failsafe takes over with Hold. + A throttle of 0.5 keeps the altitude.""" + self._send_manual_control(1, 0.0, 0.0, 0.5, 0.0) + self._manual_control.start_position_control() + self._send_manual_control(1, 0.0, 0.0, 0.5, 0.0) + assert self._telemetry.flight_mode() == FlightMode.POSCTL, "Not in Position mode" + + log("Flying with forward {} and throttle {}".format(forward, throttle)) + start_us = self._autopilot_time_us() + while self._telemetry.flight_mode() != FlightMode.HOLD: + assert (self._autopilot_time_us() - start_us) / 1e6 < timeout_s, \ + "No Hold within {} s".format(timeout_s) + self._manual_control.set_manual_control_input(forward, 0.0, throttle, 0.0) + self.sleep_for(1.0 / MANUAL_CONTROL_RATE_HZ) + + def keep_sticks_centered_for(self, duration_s: float) -> None: + """Keep the manual control link alive without asking for anything.""" + self._send_manual_control(duration_s, 0.0, 0.0, 0.5, 0.0) + + def check_flight_mode_for(self, flight_mode: FlightMode, duration_s: float) -> None: + start_us = self._autopilot_time_us() + while (self._autopilot_time_us() - start_us) / 1e6 < duration_s: + current = self._telemetry.flight_mode() + assert current == flight_mode, \ + "Flight mode {}, expected {}".format(current, flight_mode) + self.sleep_for(0.2) + + # ------------------------------------------------------------------ + # How far the vehicle went + # ------------------------------------------------------------------ + + def start_tracking(self) -> None: + """Record the extremes of the ground truth position from now on. Needs store_home().""" + assert self._track_handle is None, "Already tracking" + ct = CoordinateTransformation(*self._home_or_fail()) + home_altitude_m = self.home_absolute_altitude_m() + + with self._track_lock: + self._min_fence_distance_m = math.inf + self._max_home_distance_m = 0.0 + self._max_relative_altitude_m = -math.inf + + def on_ground_truth(ground_truth: Any, _user_data: Any) -> None: + if math.isnan(ground_truth.latitude_deg) or math.isnan(ground_truth.longitude_deg): + return + north_m, east_m = ct.local_from_global(ground_truth.latitude_deg, + ground_truth.longitude_deg) + with self._track_lock: + self._max_home_distance_m = max(self._max_home_distance_m, + math.hypot(north_m, east_m)) + self._max_relative_altitude_m = max( + self._max_relative_altitude_m, + ground_truth.absolute_altitude_m - home_altitude_m) + if self._fence is not None: + north_min_m, north_max_m, east_min_m, east_max_m = self._fence + self._min_fence_distance_m = min( + self._min_fence_distance_m, north_m - north_min_m, north_max_m - north_m, + east_m - east_min_m, east_max_m - east_m) + + self._track_handle = self._telemetry.subscribe_ground_truth(on_ground_truth) + + def min_fence_distance_m(self) -> float: + """Closest the vehicle came to the fence from inside; negative once it was outside.""" + with self._track_lock: + return self._min_fence_distance_m + + def home_distance_m(self) -> float: + """Current horizontal distance from Home.""" + return self._ground_truth_horizontal_distance_to(*self._home_or_fail()) + + def max_home_distance_m(self) -> float: + with self._track_lock: + return self._max_home_distance_m + + def max_relative_altitude_m(self) -> float: + with self._track_lock: + return self._max_relative_altitude_m diff --git a/test/mavsdk_tests/conftest.py b/test/mavsdk_tests/conftest.py index 68b82f1e290..9592219ff74 100644 --- a/test/mavsdk_tests/conftest.py +++ b/test/mavsdk_tests/conftest.py @@ -5,6 +5,7 @@ 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_geofence", "autopilot_tester_loiter", "autopilot_tester_rtl", "autopilot_tester_vision_target") @@ -12,6 +13,7 @@ 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_geofence import AutopilotTesterGeofence # noqa: E402 from autopilot_tester_loiter import AutopilotTesterLoiter # noqa: E402 from autopilot_tester_rtl import AutopilotTesterRtl # noqa: E402 from autopilot_tester_vision_target import AutopilotTesterVisionTarget # noqa: E402 @@ -66,6 +68,12 @@ def tester_loiter(request: pytest.FixtureRequest) -> Iterator[AutopilotTesterLoi yield from connected_tester(request, AutopilotTesterLoiter) +@pytest.fixture +def tester_geofence(request: pytest.FixtureRequest) -> Iterator[AutopilotTesterGeofence]: + """A tester for the emergency and the contingency geofence, connected to the vehicle.""" + yield from connected_tester(request, AutopilotTesterGeofence) + + @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/test_multicopter_contingency_geofence.py b/test/mavsdk_tests/test_multicopter_contingency_geofence.py new file mode 100644 index 00000000000..d32cab46238 --- /dev/null +++ b/test/mavsdk_tests/test_multicopter_contingency_geofence.py @@ -0,0 +1,131 @@ +import pytest +from mavsdk.plugins.telemetry import FlightMode # type: ignore + +from autopilot_tester import MissionOptions, log +from autopilot_tester_geofence import AutopilotTesterGeofence +from autopilot_tester_loiter import horizontal_distance + +TAKEOFF_ALTITUDE_M = 10.0 + +# At 5 m/s the vehicle is past a limit by the time the 5 Hz check sees it, and then it has to brake. +OVERSHOOT_M = 20.0 +# Between the estimate that triggers and the ground truth that is measured. +POSITION_TOLERANCE_M = 2.0 + + +def take_off(tester: AutopilotTesterGeofence) -> None: + tester.set_takeoff_altitude(TAKEOFF_ALTITUDE_M) + tester.arm() + tester.takeoff() + tester.wait_until_hovering() + tester.wait_until_altitude(TAKEOFF_ALTITUDE_M, 30) + + +@pytest.mark.multicopter +@pytest.mark.parametrize("margin_m", [0.0, 40.0]) +def test_pilot_is_held_at_geofence_or_margin(tester_geofence: AutopilotTesterGeofence, + margin_m: float) -> None: + """A pilot flies towards the fence in Position mode. + + Without a margin the vehicle is only held once it has crossed the emergency geofence. With a + margin it is held when it comes that close, and never reaches it.""" + tester = tester_geofence + tester.wait_until_ready() + tester.store_home() + tester.upload_inclusion_rectangle(-80.0, 80.0, -80.0, 80.0) + tester.set_param_float("GF_CONT_H_MARGIN", margin_m) + take_off(tester) + + tester.start_tracking() + tester.fly_in_position_mode_until_hold(forward=0.5, throttle=0.5, timeout_s=60) + tester.keep_sticks_centered_for(8) + tester.wait_for_flight_mode(FlightMode.HOLD, 1) + + closest_m = tester.min_fence_distance_m() + log("Closest to the fence: {:.1f} m".format(closest_m)) + assert margin_m - OVERSHOOT_M < closest_m < margin_m + POSITION_TOLERANCE_M, \ + "Came within {:.1f} m of the fence with a margin of {} m".format(closest_m, margin_m) + + +@pytest.mark.multicopter +def test_mission_is_held_at_contingency_geofence_home_distance( + tester_geofence: AutopilotTesterGeofence) -> None: + """GF_PRE_MAX_HDIST holds a mission before it reaches GF_MAX_HOR_DIST.""" + tester = tester_geofence + tester.wait_until_ready() + tester.store_home() + tester.set_param_float("GF_MAX_HOR_DIST", 300.0) + # Straight north, to 240 m. + tester.prepare_straight_mission(MissionOptions(leg_length_m=60.0, relative_altitude_m=15.0)) + tester.arm() + tester.start_and_wait_for_mission_sequence(1) + + # A mission that is being flown keeps its verdict, so the limit can be set behind it. + tester.set_param_float("GF_PRE_MAX_HDIST", 150.0) + assert tester.home_distance_m() < 120.0, "The limit was set too late" + tester.start_tracking() + tester.wait_for_flight_mode(FlightMode.HOLD, 60) + # The mission does not resume. + tester.check_flight_mode_for(FlightMode.HOLD, 10) + + furthest_m = tester.max_home_distance_m() + log("Furthest from Home: {:.1f} m".format(furthest_m)) + assert 150.0 - POSITION_TOLERANCE_M < furthest_m < 150.0 + OVERSHOOT_M, \ + "Went {:.1f} m from Home".format(furthest_m) + + +@pytest.mark.multicopter +def test_pilot_is_held_at_contingency_geofence_altitude(tester_geofence: AutopilotTesterGeofence) -> None: + """GF_PRE_MAX_VDIST holds a climbing pilot before GF_MAX_VER_DIST is reached.""" + tester = tester_geofence + tester.wait_until_ready() + tester.store_home() + tester.set_param_float("GF_MAX_VER_DIST", 45.0) + tester.set_param_float("GF_PRE_MAX_VDIST", 25.0) + take_off(tester) + + tester.start_tracking() + tester.fly_in_position_mode_until_hold(forward=0.0, throttle=1.0, timeout_s=60) + tester.keep_sticks_centered_for(8) + tester.wait_for_flight_mode(FlightMode.HOLD, 1) + + highest_m = tester.max_relative_altitude_m() + log("Highest above Home: {:.1f} m".format(highest_m)) + assert 25.0 - POSITION_TOLERANCE_M < highest_m < 25.0 + OVERSHOOT_M, \ + "Climbed to {:.1f} m".format(highest_m) + + +@pytest.mark.multicopter +def test_contingency_geofence_refuses_arming_missions_and_targets( + tester_geofence: AutopilotTesterGeofence) -> None: + """Within the margin arming is denied, and a mission or a Go-to that leads there is refused.""" + tester = tester_geofence + tester.wait_until_ready() + tester.store_home() + # Home is 30 m from each edge. + tester.upload_inclusion_rectangle(-30.0, 30.0, -30.0, 30.0) + + tester.set_param_float("GF_CONT_H_MARGIN", 40.0) + tester.sleep_for(2) + assert tester.arming_is_denied(), "Armed within the contingency geofence margin" + + tester.set_param_float("GF_CONT_H_MARGIN", 10.0) + tester.sleep_for(2) + options = MissionOptions(relative_altitude_m=TAKEOFF_ALTITUDE_M) + + # 5 m from the northern edge: the mission is rejected, so Mission mode cannot be armed. + tester.upload_mission_to(25.0, options) + tester.select_mission_mode() + assert tester.arming_is_denied(), "Armed with a mission into the margin" + + # 15 m from the northern edge. + tester.upload_mission_to(15.0, options) + tester.arm() + tester.execute_mission() + + # The vehicle now holds at the last waypoint. A target within the margin leaves it there. + tester.command_reposition((25.0, 0.0), TAKEOFF_ALTITUDE_M) + tester.check_stays_within((15.0, 0.0), 3.0, 8) + + tester.command_reposition((-15.0, 0.0), TAKEOFF_ALTITUDE_M) + tester.wait_for_position_ned(lambda s: horizontal_distance(s, (-15.0, 0.0)) < 2.0, 30) diff --git a/test/mavsdk_tests/test_vtol_contingency_geofence.py b/test/mavsdk_tests/test_vtol_contingency_geofence.py new file mode 100644 index 00000000000..9adb29ee410 --- /dev/null +++ b/test/mavsdk_tests/test_vtol_contingency_geofence.py @@ -0,0 +1,61 @@ +import pytest +from mavsdk.plugins.telemetry import FlightMode # type: ignore + +from autopilot_tester import log +from autopilot_tester_geofence import ACTION_LAND, ACTION_WARNING, AutopilotTesterGeofence + +MISSION_ALTITUDE_M = 40.0 + +# Home is 60 m inside the northern edge of the fence, and the vehicle first flies east along it. +FENCE_NORTH_M = 60.0 +# The mission itself keeps the margin: its waypoints are at least 100 m from the fence. +WAYPOINTS = [(-40.0, 1500.0), (-300.0, 1500.0)] +# Wider than the distance of the vehicle on its way to the first waypoint, and narrower than the +# default loiter radius of 80 m. +MARGIN_M = 75.0 + + +@pytest.mark.vtol +@pytest.mark.parametrize("geofence_action, final_flight_mode", [ + (ACTION_LAND, FlightMode.LAND), + (ACTION_WARNING, FlightMode.HOLD), +], ids=["land", "warning"]) +def test_more_severe_action_wins_after_contingency_geofence_hold( + tester_geofence: AutopilotTesterGeofence, geofence_action: int, + final_flight_mode: FlightMode) -> None: + """A fixed-wing mission is held within the margin, and the loiter then crosses the emergency + geofence. + + The margin is narrower than the loiter radius, so the Hold cannot stay inside the fence. + With GF_ACTION Land, Land takes over. An emergency geofence that only warns leaves the Hold + in place: the mission, which is still valid, must not resume.""" + tester = tester_geofence + tester.wait_until_ready() + tester.store_home() + tester.set_param_int("MIS_TKO_LAND_REQ", 0) + tester.set_param_int("GF_ACTION", geofence_action) + tester.upload_inclusion_rectangle(-600.0, FENCE_NORTH_M, -300.0, 1900.0) + tester.upload_vtol_mission(WAYPOINTS, MISSION_ALTITUDE_M) + tester.arm() + tester.start_mission_raw_and_wait_for_sequence(0) + tester.wait_until_fixedwing(60) + tester.wait_for_flight_mode(FlightMode.MISSION, 1) + + # The vehicle is closer to the fence than the margin that is set now. + tester.start_tracking() + tester.set_param_float("GF_CONT_H_MARGIN", MARGIN_M) + tester.wait_for_flight_mode(FlightMode.HOLD, 10) + held_at_m = tester.min_fence_distance_m() + log("Held {:.1f} m from the fence".format(held_at_m)) + assert 0.0 < held_at_m < MARGIN_M, "Not held inside the margin" + + if final_flight_mode == FlightMode.HOLD: + # More than one orbit. + tester.check_flight_mode_for(FlightMode.HOLD, 60) + + else: + tester.wait_for_flight_mode(final_flight_mode, 120) + + closest_m = tester.min_fence_distance_m() + log("Closest to the fence: {:.1f} m".format(closest_m)) + assert closest_m < 0.0, "The loiter did not cross the geofence"