test(mavsdk_tests): add contingency geofence flight tests

Fly the contingency geofence in SIH:
- a pilot flying a multicopter out is held at the geofence without a
  margin, and at GF_CONT_H_MARGIN with one;
- GF_PRE_MAX_HDIST holds a mission and GF_PRE_MAX_VDIST a climbing
  pilot before the geofence limits are reached;
- arming, a mission and a Go-to into the margin are refused;
- a fixed-wing mission held inside a margin narrower than its loiter
  radius crosses the geofence: Land takes over, and a geofence that
  only warns leaves the Hold in place.

Assisted-by: Claude:claude-fable-5-2
Signed-off-by: jonas <jonspero@me.com>
This commit is contained in:
jonas committed 2026-11-20 17:49:41 +01:00
1 parent a11e47e5ab
commit a33a52607b
4 files changed
+379

No files matched your search

@@ -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
+8
View File
@@ -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."""
@@ -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)
@@ -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"