mirror of
https://github.com/PX4/PX4-Autopilot.git
synced 2026-11-22 14:19:57 +08:00
test(mavsdk_tests): follow the margin-only contingency geofence
The Home distance case now sets GF_CONT_H_MARGIN, which is kept from GF_MAX_HOR_DIST. The altitude case is removed with GF_PRE_MAX_VDIST. Assisted-by: Claude:claude-fable-5-2 Signed-off-by: jonas <jonspero@me.com>
This commit is contained in:
1 parent
a7c85d305c
commit
f43fdb7cf2
2 files changed
+4
-35
No files matched your search
@@ -39,7 +39,6 @@ class AutopilotTesterGeofence(AutopilotTesterLoiter):
|
||||
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
|
||||
@@ -132,12 +131,10 @@ class AutopilotTesterGeofence(AutopilotTesterLoiter):
|
||||
"""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):
|
||||
@@ -147,9 +144,6 @@ class AutopilotTesterGeofence(AutopilotTesterLoiter):
|
||||
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(
|
||||
@@ -170,7 +164,3 @@ class AutopilotTesterGeofence(AutopilotTesterLoiter):
|
||||
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
|
||||
@@ -52,7 +52,7 @@ def test_pilot_is_held_at_geofence_or_margin(tester_geofence: AutopilotTesterGeo
|
||||
@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."""
|
||||
"""GF_CONT_H_MARGIN holds a mission that distance before it reaches GF_MAX_HOR_DIST."""
|
||||
tester = tester_geofence
|
||||
tester.wait_until_ready()
|
||||
tester.store_home()
|
||||
@@ -62,9 +62,9 @@ def test_mission_is_held_at_contingency_geofence_home_distance(
|
||||
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"
|
||||
# A mission that is being flown keeps its verdict, so the margin can be set behind it.
|
||||
tester.set_param_float("GF_CONT_H_MARGIN", 150.0)
|
||||
assert tester.home_distance_m() < 120.0, "The margin was set too late"
|
||||
tester.start_tracking()
|
||||
tester.wait_for_flight_mode(FlightMode.HOLD, 60)
|
||||
# The mission does not resume.
|
||||
@@ -76,27 +76,6 @@ def test_mission_is_held_at_contingency_geofence_home_distance(
|
||||
"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:
|
||||
|
||||
Reference in new issue
Block a user