mirror of
https://github.com/PX4/PX4-Autopilot.git
synced 2026-11-22 14:19:57 +08:00
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:
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
|
||||
@@ -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"
|
||||
Reference in new issue
Block a user