mirror of https://github.com/ArduPilot/ardupilot
Autotest: Correct the logic for fence based autotest functions
Adds corrections to enabling fence using aux function. Correctly test fences statically. Only uploaded fences can be checked using a fence file, so we check those first. Then we add steps to check tin can, max and minm all set the fence as present, as expected. Plane will support MAV_PROTOCOL_CAPABILITY_MISSION_FENCE, so we assert that it does support it. To test ceiling and floor, leverage some existing functions for takeoff, change altitude and land. Check for respective breach. Add a floor breach check to copter.
This commit is contained in:
James O'Shannessy
Peter Barker
parent
783a4b7965
commit
215901be30
|
|
@ -1,6 +0,0 @@
|
|||
-35.363720 149.163651
|
||||
-35.358738 149.165070
|
||||
-35.359295 149.154434
|
||||
-35.372292 149.157135
|
||||
-35.368290 149.166809
|
||||
-35.358738 149.165070
|
||||
|
|
@ -1,10 +0,0 @@
|
|||
QGC WPL 110
|
||||
0 0 0 16 0.000000 0.000000 0.000000 0.000000 -35.362938 149.165086 585.090027 1
|
||||
1 0 3 22 15.000000 0.000000 0.000000 0.000000 0.000000 0.000000 100.000000 1
|
||||
2 0 3 16 0.000000 0.000000 0.000000 0.000000 -35.359628 149.164405 100.000000 1
|
||||
3 0 3 16 0.000000 0.000000 0.000000 0.000000 -35.360380 149.157558 60.000000 1
|
||||
4 0 3 16 0.000000 0.000000 0.000000 0.000000 -35.364289 149.156969 100.000000 1
|
||||
5 0 3 16 0.000000 0.000000 0.000000 0.000000 -35.369329 149.158092 140.000000 1
|
||||
6 0 0 189 0.000000 0.000000 0.000000 0.000000 -35.369329 149.158092 0.000000 1
|
||||
7 0 3 31 0.000000 -75.000000 0.000000 1.000000 -35.367154 149.164864 100.000000 1
|
||||
8 0 3 21 0.000000 0.000000 0.000000 1.000000 -35.363200 149.165233 0.000000 1
|
||||
|
|
@ -0,0 +1,13 @@
|
|||
QGC WPL 110
|
||||
0 0 0 16 0.000000 0.000000 0.000000 0.000000 -35.363262 149.165237 584.090027 1
|
||||
1 0 3 22 15.000000 0.000000 0.000000 0.000000 -35.361279 149.164230 30.000000 1
|
||||
2 0 3 16 0.000000 0.000000 0.000000 0.000000 -35.361229 149.163025 80.000000 1
|
||||
3 0 3 16 0.000000 0.000000 0.000000 0.000000 -35.364563 149.163773 80.000000 1
|
||||
4 0 3 16 0.000000 0.000000 0.000000 0.000000 -35.364384 149.164795 80.000000 1
|
||||
5 0 3 16 0.000000 0.000000 0.000000 0.000000 -35.358878 149.163760 80.000000 1
|
||||
6 0 0 177 2.000000 3.000000 0.000000 0.000000 0.000000 0.000000 0.000000 1
|
||||
7 0 3 189 0.000000 0.000000 0.000000 0.000000 -35.362915 149.162613 60.000000 1
|
||||
8 0 3 16 0.000000 0.000000 0.000000 0.000000 -35.358867 149.164409 60.000000 1
|
||||
9 0 3 16 0.000000 0.000000 0.000000 0.000000 -35.359862 149.164697 55.000000 1
|
||||
10 0 3 16 0.000000 0.000000 0.000000 0.000000 -35.360670 149.164857 39.889999 1
|
||||
11 0 3 21 0.000000 0.000000 0.000000 1.000000 -35.363041 149.165222 0.000000 1
|
||||
|
|
@ -1102,7 +1102,7 @@ class AutoTestCopter(AutoTest):
|
|||
def fly_fence_avoid_test_radius_check(self, timeout=180, avoid_behave=avoid_behave_slide):
|
||||
using_mode = "LOITER" # must be something which adjusts velocity!
|
||||
self.change_mode(using_mode)
|
||||
self.set_parameter("FENCE_ENABLE", 1) # fence
|
||||
self.set_parameter("FENCE_AUTOENABLE", 1) # fence
|
||||
self.set_parameter("FENCE_TYPE", 2) # circle
|
||||
fence_radius = 15
|
||||
self.set_parameter("FENCE_RADIUS", fence_radius)
|
||||
|
|
@ -1306,7 +1306,7 @@ class AutoTestCopter(AutoTest):
|
|||
"""Hold loiter position."""
|
||||
|
||||
# enable fence, disable avoidance
|
||||
self.set_parameter("FENCE_ENABLE", 1)
|
||||
self.set_parameter("FENCE_AUTOENABLE", 1)
|
||||
self.set_parameter("AVOID_ENABLE", 0)
|
||||
self.set_parameter("FENCE_TYPE", 1)
|
||||
|
||||
|
|
@ -1335,6 +1335,42 @@ class AutoTestCopter(AutoTest):
|
|||
|
||||
self.zero_throttle()
|
||||
|
||||
# fly_alt_fence_test - fly up until you hit the fence
|
||||
def fly_alt_min_fence_test(self):
|
||||
self.takeoff(50, mode="LOITER", timeout=120)
|
||||
"""Hold loiter position."""
|
||||
self.mavproxy.send('switch 5\n') # loiter mode
|
||||
self.wait_mode('LOITER')
|
||||
|
||||
# enable fence, disable avoidance
|
||||
self.set_parameter("FENCE_AUTOENABLE", 1)
|
||||
self.set_parameter("AVOID_ENABLE", 0)
|
||||
self.set_parameter("FENCE_TYPE", 8)
|
||||
|
||||
self.change_alt(10)
|
||||
|
||||
# first east
|
||||
self.progress("turn east")
|
||||
self.set_rc(4, 1580)
|
||||
self.wait_heading(160)
|
||||
self.set_rc(4, 1500)
|
||||
|
||||
# fly forward (east) at least 20m
|
||||
self.set_rc(2, 1100)
|
||||
self.wait_distance(20)
|
||||
|
||||
# stop flying forward and start flying based on input:
|
||||
self.set_rc(2, 1500)
|
||||
self.set_rc(3, 1200)
|
||||
|
||||
# wait for fence to trigger
|
||||
self.wait_mode('RTL', timeout=120)
|
||||
|
||||
self.wait_rtl_complete()
|
||||
|
||||
self.zero_throttle()
|
||||
|
||||
|
||||
def fly_gps_glitch_loiter_test(self, timeout=30, max_distance=20):
|
||||
"""fly_gps_glitch_loiter_test. Fly south east in loiter and test
|
||||
reaction to gps glitch."""
|
||||
|
|
@ -5032,7 +5068,7 @@ class AutoTestCopter(AutoTest):
|
|||
ex = None
|
||||
try:
|
||||
self.load_fence("copter-avoidance-fence.txt")
|
||||
self.set_parameter("FENCE_ENABLE", 0)
|
||||
self.set_parameter("FENCE_AUTOENABLE", 1)
|
||||
self.set_parameter("PRX_TYPE", 10)
|
||||
self.set_parameter("RC10_OPTION", 40) # proximity-enable
|
||||
self.reboot_sitl()
|
||||
|
|
@ -5135,7 +5171,7 @@ class AutoTestCopter(AutoTest):
|
|||
ex = None
|
||||
try:
|
||||
self.load_fence("copter-avoidance-fence.txt")
|
||||
self.set_parameter("FENCE_ENABLE", 1)
|
||||
self.set_parameter("FENCE_AUTOENABLE", 1)
|
||||
self.check_avoidance_corners()
|
||||
except Exception as e:
|
||||
self.print_exception_caught(e)
|
||||
|
|
@ -6479,6 +6515,10 @@ class AutoTestCopter(AutoTest):
|
|||
"Test Max Alt Fence",
|
||||
self.fly_alt_max_fence_test), # 26s
|
||||
|
||||
("MinAltFence",
|
||||
"Test Max Alt Fence",
|
||||
self.fly_alt_min_fence_test), #26s
|
||||
|
||||
("AutoTuneSwitch",
|
||||
"Fly AUTOTUNE on a switch",
|
||||
self.fly_autotune_switch), # 105s
|
||||
|
|
|
|||
|
|
@ -969,16 +969,14 @@ class AutoTestPlane(AutoTest):
|
|||
|
||||
self.set_parameter("RC7_OPTION", 11) # AC_Fence uses Aux switch functionality
|
||||
self.set_parameter("FENCE_ACTION", 4) # Fence action Brake
|
||||
self.set_parameter("FENCE_ENABLE", 1)
|
||||
|
||||
self.wait_sensor_state(mavutil.mavlink.MAV_SYS_STATUS_GEOFENCE,
|
||||
present=True,
|
||||
enabled=False,
|
||||
healthy=True)
|
||||
self.set_rc_from_map({
|
||||
3: 1000,
|
||||
7: 2000,
|
||||
})
|
||||
}) # Turn fence on with aux function
|
||||
|
||||
|
||||
self.mavproxy.expect("fence enabled")
|
||||
self.delay_sim_time(1) # This is needed else the SYS_STATUS may not have updated
|
||||
|
||||
self.progress("Checking fence is initially OK")
|
||||
self.wait_sensor_state(mavutil.mavlink.MAV_SYS_STATUS_GEOFENCE,
|
||||
|
|
@ -1108,6 +1106,7 @@ class AutoTestPlane(AutoTest):
|
|||
ex = None
|
||||
try:
|
||||
self.progress("Checking for bizarre healthy-when-not-present-or-enabled")
|
||||
self.set_parameter("FENCE_TYPE", 4) # Start by only setting polygon fences, otherwise fence will report present
|
||||
self.assert_fence_sys_status(False, False, True)
|
||||
self.load_fence("CMAC-fence.txt")
|
||||
m = self.mav.recv_match(type='FENCE_STATUS', blocking=True, timeout=2)
|
||||
|
|
@ -1149,6 +1148,19 @@ class AutoTestPlane(AutoTest):
|
|||
self.wait_sensor_state(mavutil.mavlink.MAV_SYS_STATUS_GEOFENCE, False, False, True)
|
||||
if self.get_parameter("FENCE_TOTAL") != 0:
|
||||
raise NotAchievedException("Expected zero points remaining")
|
||||
self.assert_fence_sys_status(False, False, True)
|
||||
self.do_fence_disable()
|
||||
|
||||
# ensure that a fence is present if it is tin can, min alt or max alt
|
||||
self.progress("Test other fence types (tin-can, min alt, max alt")
|
||||
self.set_parameter("FENCE_TYPE", 1) # max alt
|
||||
self.assert_fence_sys_status(True, False, True)
|
||||
self.set_parameter("FENCE_TYPE", 8) # min alt
|
||||
self.assert_fence_sys_status(True, False, True)
|
||||
self.set_parameter("FENCE_TYPE", 2) # tin can
|
||||
self.assert_fence_sys_status(True, False, True)
|
||||
|
||||
|
||||
|
||||
except Exception as e:
|
||||
self.print_exception_caught(e)
|
||||
|
|
@ -1334,8 +1346,8 @@ class AutoTestPlane(AutoTest):
|
|||
|
||||
self.change_mode('MANUAL')
|
||||
|
||||
self.progress("Asserting we don't support transfer of fence via mission item protocol")
|
||||
self.assert_no_capability(mavutil.mavlink.MAV_PROTOCOL_CAPABILITY_MISSION_FENCE)
|
||||
self.progress("Asserting we do support transfer of fence via mission item protocol")
|
||||
self.assert_capability(mavutil.mavlink.MAV_PROTOCOL_CAPABILITY_MISSION_FENCE)
|
||||
|
||||
# grab home position:
|
||||
self.mav.recv_match(type='HOME_POSITION', blocking=True)
|
||||
|
|
@ -2333,76 +2345,51 @@ class AutoTestPlane(AutoTest):
|
|||
raise ex
|
||||
|
||||
def test_fence_alt_ceil_floor(self):
|
||||
ex = None
|
||||
target_system = 1
|
||||
target_component = 1
|
||||
try:
|
||||
self.progress("Testing Fence Enable")
|
||||
fence_bit = mavutil.mavlink.MAV_SYS_STATUS_GEOFENCE
|
||||
self.set_parameter("FENCE_TYPE", 9) # Set fence type to max and min alt
|
||||
self.set_parameter("FENCE_ACTION", 0) # Set action to report
|
||||
self.set_parameter("FENCE_ALT_MAX", 200)
|
||||
self.set_parameter("FENCE_ALT_MIN", 100)
|
||||
|
||||
# Load Fence
|
||||
self.load_fence("CMAC-fence.txt")
|
||||
if self.get_parameter("FENCE_TOTAL") == 0:
|
||||
raise NotAchievedException("Expected fence to be present")
|
||||
|
||||
# Load Mission
|
||||
self.load_mission("CMAC-mission.txt")
|
||||
|
||||
# Set up fence parameters
|
||||
self.set_parameter("FENCE_AUTOENABLE", 1) # Enable/Disable on Takeoff/Landing
|
||||
self.set_parameter("FENCE_ACTION", 1) # RTL
|
||||
self.set_parameter("FENCE_TYPE", 13) # Set Fence Type to Alt Max/Min and Polygon
|
||||
self.set_parameter("FENCE_ALT_MIN", 80)
|
||||
self.set_parameter("FENCE_ALT_MAX", 120)
|
||||
# Grab Home Position
|
||||
self.mav.recv_match(type='HOME_POSITION', blocking=True)
|
||||
self.homeloc = self.mav.location()
|
||||
|
||||
# Now we can arm the vehicle and kick off the flight
|
||||
self.wait_ready_to_arm()
|
||||
self.arm_vehicle()
|
||||
self.change_mode("AUTO")
|
||||
cruise_alt = 150
|
||||
self.takeoff(cruise_alt)
|
||||
|
||||
# On takeoff complete, check fence is enabled
|
||||
self.progress("Looking for takeoff complete")
|
||||
self.mavproxy.expect("APM: Takeoff complete at [0-9]+\.[0-9]+m")
|
||||
#self.progress("Looking for fence enable")
|
||||
#self.mavproxy.expect("fence enabled")
|
||||
self.do_fence_enable()
|
||||
|
||||
# Fly until we breach fence floor where we enter RTL
|
||||
self.progress("Looking for fence breach")
|
||||
self.mavproxy.expect("fence breach")
|
||||
|
||||
# As we RTL, we should re-enter fence zone. Wait until we're inside
|
||||
self.progress("Waiting for return to altitude")
|
||||
self.wait_altitude(altitude_min=100, altitude_max=110, timeout=30, relative=True)
|
||||
self.progress("Fly above ceiling and check for breach")
|
||||
self.change_altitude(self.homeloc.alt + cruise_alt + 80)
|
||||
m = self.mav.recv_match(type='SYS_STATUS', blocking=True)
|
||||
print("%s" % str(m))
|
||||
if ((m.onboard_control_sensors_health & fence_bit)):
|
||||
raise NotAchievedException("Fence Ceiling did not breach")
|
||||
|
||||
# Skip the waypoint outside fence, continue mission
|
||||
self.progress("Skipping to next waypoint")
|
||||
self.mav.waypoint_set_current_send(4)
|
||||
self.change_mode("AUTO")
|
||||
self.drain_mav()
|
||||
self.progress("Return to cruise alt and check for breach clear")
|
||||
self.change_altitude(self.homeloc.alt + cruise_alt)
|
||||
|
||||
# Fly until we breach fence ceiling where we enter RTL
|
||||
self.mavproxy.expect("fence breach")
|
||||
m = self.mav.recv_match(type='SYS_STATUS', blocking=True)
|
||||
print("%s" % str(m))
|
||||
if (not (m.onboard_control_sensors_health & fence_bit)):
|
||||
raise NotAchievedException("Fence breach did not clear")
|
||||
|
||||
# As we RTL, we should re-enter fence zone. Wait until we're inside
|
||||
self.wait_altitude(altitude_min=100, altitude_max=110, timeout=60, relative=True)
|
||||
|
||||
# Skip the waypoint outside fence, continue mission
|
||||
self.progress("Skipping to next waypoint")
|
||||
self.mav.waypoint_set_current_send(6)
|
||||
self.change_mode("AUTO")
|
||||
self.drain_mav()
|
||||
self.progress("Fly below floor and check for breach")
|
||||
self.change_altitude(self.homeloc.alt + cruise_alt - 80)
|
||||
|
||||
# Continue through to landing sequence, which disables the fence
|
||||
self.progress("Continuing mission to land")
|
||||
self.mavproxy.expect("APM: Fence disabled \(auto disable\)")
|
||||
m = self.mav.recv_match(type='SYS_STATUS', blocking=True)
|
||||
print("%s" % str(m))
|
||||
if ((m.onboard_control_sensors_health & fence_bit)):
|
||||
raise NotAchievedException("Fence Floor did not breach")
|
||||
|
||||
# Once landed, wait till disarmed
|
||||
self.wait_disarmed(timeout=120)
|
||||
except Exception as e:
|
||||
self.progress("Exception caught:")
|
||||
self.progress(self.get_exception_stacktrace(e))
|
||||
ex = e
|
||||
if ex is not None:
|
||||
raise ex
|
||||
self.do_fence_disable()
|
||||
|
||||
self.fly_home_land_and_disarm(timeout=150)
|
||||
|
||||
|
||||
|
||||
|
||||
def tests(self):
|
||||
'''return list of all tests'''
|
||||
|
|
|
|||
Loading…
Reference in New Issue