Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-21 15:23:57 -04:00
Code Issues Packages Projects Releases Wiki Activity

autotest: add test for auto-disabling min alt fence breaches on disarming

Browse Source 
clean-up fence manipulation functions and add test for auto-enablement on copter
update tests to have some FENCE_ENABLE tests
add avoidance minimum and maximum altitude fence
add fence switch test while flying
add FenceAutoEnableDisableSwitch for auto mode 2
add more scenarios for plane fence auto-enable
validate fence rc switch behaviour
check fence autoenable by taking off in guided mode
more FENCE_AUTOENABLE tests
add FenceEnableDisableAux and FenceMinAltAutoEnableAbort
This commit is contained in:
Andy Piper 2023-12-22 09:36:34 +00:00 committed by Peter Barker
parent a4c7819117
commit cedccdb8fe
5 changed files with 492 additions and 14 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
Tools/autotest/Generic_Missions/CMAC-fence.txt Normal file
View File

@ -0,0 +1,6 @@
-35.363720 149.163651
-35.358738 149.165070
-35.359295 149.154434
-35.372292 149.157135
-35.368290 149.166809
-35.358738 149.165070

140
Tools/autotest/arducopter.py
View File

@ -1605,6 +1605,7 @@ class AutoTestCopter(vehicle_test_suite.TestSuite):
"FENCE_ENABLE": 1,
"AVOID_ENABLE": 0,
"FENCE_TYPE": 1,
"FENCE_ENABLE" : 1,
})
self.change_alt(10)
@ -1692,6 +1693,7 @@ class AutoTestCopter(vehicle_test_suite.TestSuite):
# enable fence, disable avoidance
self.set_parameters({
"AVOID_ENABLE": 0,
"FENCE_ENABLE" : 1,
"FENCE_TYPE": 8,
"FENCE_ALT_MIN": 20,
})
@ -1737,6 +1739,7 @@ class AutoTestCopter(vehicle_test_suite.TestSuite):
self.progress("Test Landing while fence floor enabled")
self.set_parameters({
"AVOID_ENABLE": 0,
"FENCE_ENABLE" : 1,
"FENCE_TYPE": 15,
"FENCE_ALT_MIN": 10,
"FENCE_ALT_MAX": 20,
@ -1757,16 +1760,100 @@ class AutoTestCopter(vehicle_test_suite.TestSuite):
self.set_rc(3, 1800)
self.wait_mode('RTL', timeout=120)
self.wait_landed_and_disarmed()
self.assert_fence_enabled()
# Assert fence is not healthy
# Assert fence is not healthy since it was enabled manually
self.assert_sensor_state(fence_bit, healthy=False)
# Disable the fence using mavlink command to ensure cleaned up SITL state
self.do_fence_disable()
self.assert_fence_disabled()
def FenceFloorAutoDisableLanding(self):
"""Ensures we can initiate and complete an RTL while the fence is enabled"""
fence_bit = mavutil.mavlink.MAV_SYS_STATUS_GEOFENCE
self.progress("Test Landing while fence floor enabled")
self.set_parameters({
"AVOID_ENABLE": 0,
"FENCE_TYPE": 11,
"FENCE_ALT_MIN": 10,
"FENCE_ALT_MAX": 20,
"FENCE_AUTOENABLE" : 1,
})
self.change_mode("GUIDED")
self.wait_ready_to_arm()
self.arm_vehicle()
self.takeoff(alt_min=15, mode="GUIDED")
# Check fence is enabled
self.assert_fence_enabled()
# Change to RC controlled mode
self.change_mode('LOITER')
self.set_rc(3, 1800)
self.wait_mode('RTL', timeout=120)
# Assert fence is not healthy now that we are in RTL
self.assert_sensor_state(fence_bit, healthy=False)
self.wait_landed_and_disarmed(0)
# the breach should have cleared since we auto-disable the
# fence on landing
self.assert_fence_disabled()
# Assert fences have gone now that we have landed and disarmed
self.assert_sensor_state(fence_bit, present=True, enabled=False)
def FenceFloorAutoEnableOnArming(self):
"""Ensures we can auto-enable fences on arming and still takeoff and land"""
fence_bit = mavutil.mavlink.MAV_SYS_STATUS_GEOFENCE
self.set_parameters({
"AVOID_ENABLE": 0,
"FENCE_TYPE": 11,
"FENCE_ALT_MIN": 10,
"FENCE_ALT_MAX": 20,
"FENCE_AUTOENABLE" : 3,
})
self.change_mode("GUIDED")
# Check fence is not enabled
self.assert_fence_disabled()
self.wait_ready_to_arm()
self.arm_vehicle()
self.takeoff(alt_min=15, mode="GUIDED")
# Check fence is enabled
self.assert_fence_enabled()
# Change to RC controlled mode
self.change_mode('LOITER')
self.set_rc(3, 1800)
self.wait_mode('RTL', timeout=120)
# Assert fence is not healthy now that we are in RTL
self.assert_sensor_state(fence_bit, healthy=False)
self.wait_landed_and_disarmed(0)
# the breach should have cleared since we auto-disable the
# fence on landing
self.assert_fence_disabled()
# Assert fences have gone now that we have landed and disarmed
self.assert_sensor_state(fence_bit, present=True, enabled=False)
# Disable the fence using mavlink command to ensure cleaned up SITL state
self.assert_fence_disabled()
def GPSGlitchLoiter(self, timeout=30, max_distance=20):
"""fly_gps_glitch_loiter_test. Fly south east in loiter and test
reaction to gps glitch."""
@ -7631,6 +7718,54 @@ class AutoTestCopter(vehicle_test_suite.TestSuite):
self.set_parameter("FENCE_ENABLE", 1)
self.check_avoidance_corners()
def AvoidanceAltFence(self):
'''Test fence avoidance at minimum and maximum altitude'''
ex = None
try:
self.set_parameters({
"FENCE_ENABLE": 1,
"FENCE_TYPE": 9, # min and max alt fence
"FENCE_ALT_MIN": 10,
"FENCE_ALT_MAX": 30,
})
self.change_mode('LOITER')
self.wait_ekf_happy()
tstart = self.get_sim_time()
self.takeoff(15, mode='LOITER')
self.progress("Increasing throttle, vehicle should stay below 30m")
self.set_rc(3, 1920)
tstart = self.get_sim_time()
while True:
if self.get_sim_time_cached() - tstart > 20:
break
alt = self.get_altitude(relative=True)
self.progress("Altitude %s" % alt)
if alt > 30:
raise NotAchievedException("Breached maximum altitude (%s)" % (str(alt),))
self.progress("Decreasing, vehicle should stay above 10m")
self.set_rc(3, 1080)
tstart = self.get_sim_time()
while True:
if self.get_sim_time_cached() - tstart > 20:
break
alt = self.get_altitude(relative=True)
self.progress("Altitude %s" % alt)
if alt < 10:
raise NotAchievedException("Breached minimum altitude (%s)" % (str(alt),))
except Exception as e:
self.progress("Caught exception: %s" %
self.get_exception_stacktrace(e))
ex = e
self.land_and_disarm()
self.disarm_vehicle(force=True)
if ex is not None:
raise ex
def ModeFollow(self):
'''Fly follow mode'''
foll_ofs_x = 30 # metres
@ -10509,6 +10644,7 @@ class AutoTestCopter(vehicle_test_suite.TestSuite):
self.AC_Avoidance_Proximity_AVOID_ALT_MIN,
self.AC_Avoidance_Fence,
self.AC_Avoidance_Beacon,
self.AvoidanceAltFence,
self.BaroWindCorrection,
self.SetpointGlobalPos,
self.ThrowDoubleDrop,
@ -10528,6 +10664,8 @@ class AutoTestCopter(vehicle_test_suite.TestSuite):
self.MaxAltFenceAvoid,
self.MinAltFence,
self.FenceFloorEnabledLanding,
self.FenceFloorAutoDisableLanding,
self.FenceFloorAutoEnableOnArming,
self.AutoTuneSwitch,
self.GPSGlitchLoiter,
self.GPSGlitchLoiter2,

331
Tools/autotest/arduplane.py
View File

@ -3698,6 +3698,332 @@ class AutoTestPlane(vehicle_test_suite.TestSuite):
self.fly_home_land_and_disarm(timeout=150)
def FenceMinAltAutoEnable(self):
'''Tests autoenablement of the alt min fence and fences on arming'''
self.set_parameters({
"FENCE_TYPE": 9, # Set fence type to min alt and max alt
"FENCE_ACTION": 1, # Set action to RTL
"FENCE_ALT_MIN": 25,
"FENCE_ALT_MAX": 100,
"FENCE_AUTOENABLE": 3,
"FENCE_ENABLE" : 0,
"RTL_AUTOLAND" : 2,
})
# check we can takeoff again
for i in [1, 2]:
# Grab Home Position
self.mav.recv_match(type='HOME_POSITION', blocking=True)
self.homeloc = self.mav.location()
self.wait_ready_to_arm()
self.arm_vehicle()
# max alt fence should now be enabled
if i == 1:
self.assert_fence_enabled()
self.takeoff(alt=50, mode='TAKEOFF')
self.change_mode("FBWA")
self.set_rc(3, 1100) # lower throttle
self.progress("Waiting for RTL")
tstart = self.get_sim_time()
mode = "RTL"
while not self.mode_is(mode, drain_mav=False):
self.mav.messages['HEARTBEAT'].custom_mode
self.progress("mav.flightmode=%s Want=%s Alt=%f" % (
self.mav.flightmode, mode, self.get_altitude(relative=True)))
if (self.get_sim_time_cached() > tstart + 120):
raise WaitModeTimeout("Did not change mode")
self.progress("Got mode %s" % mode)
self.fly_home_land_and_disarm()
self.change_mode("FBWA")
self.clear_mission(mavutil.mavlink.MAV_MISSION_TYPE_ALL)
self.set_current_waypoint(0, check_afterwards=False)
self.set_rc(3, 1000) # lower throttle
def FenceMinAltAutoEnableAbort(self):
'''Tests autoenablement of the alt min fence and fences on arming'''
self.set_parameters({
"FENCE_TYPE": 9, # Set fence type to min alt and max alt
"FENCE_ACTION": 1, # Set action to RTL
"FENCE_ALT_MIN": 25,
"FENCE_ALT_MAX": 100,
"FENCE_AUTOENABLE": 3,
"FENCE_ENABLE" : 0,
"RTL_AUTOLAND" : 2,
})
# Grab Home Position
self.mav.recv_match(type='HOME_POSITION', blocking=True)
self.homeloc = self.mav.location()
self.wait_ready_to_arm()
self.arm_vehicle()
# max alt fence should now be enabled
self.assert_fence_enabled()
self.takeoff(alt=50, mode='TAKEOFF')
self.change_mode("FBWA")
self.set_rc(3, 1100) # lower throttle
self.progress("Waiting for RTL")
tstart = self.get_sim_time()
mode = "RTL"
while not self.mode_is(mode, drain_mav=False):
self.mav.messages['HEARTBEAT'].custom_mode
self.progress("mav.flightmode=%s Want=%s Alt=%f" % (
self.mav.flightmode, mode, self.get_altitude(relative=True)))
if (self.get_sim_time_cached() > tstart + 120):
raise WaitModeTimeout("Did not change mode")
self.progress("Got mode %s" % mode)
self.load_generic_mission("flaps.txt")
self.change_mode("AUTO")
self.wait_distance_to_waypoint(8, 100, 10000000)
self.set_current_waypoint(8)
# abort the landing
self.wait_altitude(10, 20, timeout=200, relative=True)
self.change_mode("CRUISE")
self.set_rc(2, 1200)
# self.set_rc(3, 1600) # raise throttle
self.wait_altitude(30, 40, timeout=200, relative=True)
self.change_mode("AUTO")
self.clear_mission(mavutil.mavlink.MAV_MISSION_TYPE_ALL)
self.fly_home_land_and_disarm(timeout=150)
self.wait_disarmed()
def FenceAutoEnableDisableSwitch(self):
'''Tests autoenablement of regular fences and manual disablement'''
self.set_parameters({
"FENCE_TYPE": 11, # Set fence type to min alt
"FENCE_ACTION": 1, # Set action to RTL
"FENCE_ALT_MIN": 50,
"FENCE_ALT_MAX": 100,
"FENCE_AUTOENABLE": 2,
"FENCE_OPTIONS" : 1,
"FENCE_ENABLE" : 1,
"FENCE_RADIUS" : 300,
"FENCE_RET_ALT" : 0,
"FENCE_RET_RALLY" : 0,
"FENCE_TOTAL" : 0,
"TKOFF_ALT" : 75,
"RC7_OPTION" : 11, # AC_Fence uses Aux switch functionality
})
fence_bit = mavutil.mavlink.MAV_SYS_STATUS_GEOFENCE
# Grab Home Position
self.mav.recv_match(type='HOME_POSITION', blocking=True)
self.homeloc = self.mav.location()
self.set_rc_from_map({7: 1000}) # Turn fence off with aux function
self.wait_ready_to_arm()
cruise_alt = 75
self.takeoff(cruise_alt, mode='TAKEOFF')
self.progress("Fly above ceiling and check there is no breach")
self.set_rc(3, 2000)
self.change_altitude(self.homeloc.alt + cruise_alt + 80)
m = self.mav.recv_match(type='SYS_STATUS', blocking=True)
self.progress("Got (%s)" % str(m))
if (not (m.onboard_control_sensors_health & fence_bit)):
raise NotAchievedException("Fence Ceiling breached")
self.progress("Return to cruise alt")
self.set_rc(3, 1500)
self.change_altitude(self.homeloc.alt + cruise_alt)
self.progress("Fly below floor and check for no breach")
self.change_altitude(self.homeloc.alt + 25)
m = self.mav.recv_match(type='SYS_STATUS', blocking=True)
self.progress("Got (%s)" % str(m))
if (not (m.onboard_control_sensors_health & fence_bit)):
raise NotAchievedException("Fence Ceiling breached")
self.progress("Fly above floor and check fence is not re-enabled")
self.set_rc(3, 2000)
self.change_altitude(self.homeloc.alt + 75)
m = self.mav.recv_match(type='SYS_STATUS', blocking=True)
self.progress("Got (%s)" % str(m))
if (m.onboard_control_sensors_enabled & fence_bit):
raise NotAchievedException("Fence Ceiling re-enabled")
self.progress("Return to cruise alt")
self.set_rc(3, 1500)
self.change_altitude(self.homeloc.alt + cruise_alt)
self.fly_home_land_and_disarm(timeout=250)
def FenceEnableDisableSwitch(self):
'''Tests enablement and disablement of fences on a switch'''
fence_bit = mavutil.mavlink.MAV_SYS_STATUS_GEOFENCE
self.set_parameters({
"FENCE_TYPE": 4, # Set fence type to polyfence
"FENCE_ACTION": 6, # Set action to GUIDED
"FENCE_ALT_MIN": 10,
"FENCE_ENABLE" : 0,
"RC7_OPTION" : 11, # AC_Fence uses Aux switch functionality
})
self.progress("Checking fence is not present before being configured")
m = self.mav.recv_match(type='SYS_STATUS', blocking=True)
self.progress("Got (%s)" % str(m))
if (m.onboard_control_sensors_enabled & fence_bit):
raise NotAchievedException("Fence enabled before being configured")
self.wait_ready_to_arm()
# takeoff at a lower altitude to avoid immediately breaching polyfence
self.takeoff(alt=25)
self.change_mode("FBWA")
self.load_fence("CMAC-fence.txt")
self.set_rc_from_map({
3: 1500,
7: 2000,
}) # Turn fence on with aux function
m = self.mav.recv_match(type='FENCE_STATUS', blocking=True, timeout=2)
self.progress("Got (%s)" % str(m))
if m is None:
raise NotAchievedException("Got FENCE_STATUS unexpectedly")
self.progress("Checking fence is initially OK")
self.wait_sensor_state(mavutil.mavlink.MAV_SYS_STATUS_GEOFENCE,
present=True,
enabled=True,
healthy=True,
verbose=False,
timeout=30)
self.progress("Waiting for GUIDED")
tstart = self.get_sim_time()
mode = "GUIDED"
while not self.mode_is(mode, drain_mav=False):
self.mav.messages['HEARTBEAT'].custom_mode
self.progress("mav.flightmode=%s Want=%s Alt=%f" % (
self.mav.flightmode, mode, self.get_altitude(relative=True)))
if (self.get_sim_time_cached() > tstart + 120):
raise WaitModeTimeout("Did not change mode")
self.progress("Got mode %s" % mode)
# check we are in breach
self.assert_fence_enabled()
self.set_rc_from_map({
7: 1000,
}) # Turn fence off with aux function
# wait to no longer be in breach
self.delay_sim_time(5)
self.assert_fence_disabled()
self.fly_home_land_and_disarm(timeout=250)
self.do_fence_disable() # Ensure the fence is disabled after test
def FenceEnableDisableAux(self):
'''Tests enablement and disablement of fences via aux command'''
fence_bit = mavutil.mavlink.MAV_SYS_STATUS_GEOFENCE
enable = 0
self.set_parameters({
"FENCE_TYPE": 12, # Set fence type to polyfence + AltMin
"FENCE_ALT_MIN": 10,
"FENCE_ENABLE" : enable,
})
if not enable:
self.progress("Checking fence is not present before being configured")
m = self.mav.recv_match(type='SYS_STATUS', blocking=True)
self.progress("Got (%s)" % str(m))
if (m.onboard_control_sensors_enabled & fence_bit):
raise NotAchievedException("Fence enabled before being configured")
self.load_fence("CMAC-fence.txt")
self.wait_ready_to_arm()
# takeoff at a lower altitude to avoid immediately breaching polyfence
self.takeoff(alt=25)
self.change_mode("CRUISE")
self.wait_distance(150, accuracy=20)
self.run_auxfunc(
11,
2,
want_result=mavutil.mavlink.MAV_RESULT_ACCEPTED
)
m = self.mav.recv_match(type='FENCE_STATUS', blocking=True, timeout=2)
self.progress("Got (%s)" % str(m))
if m is None:
raise NotAchievedException("Got FENCE_STATUS unexpectedly")
self.progress("Checking fence is initially OK")
self.wait_sensor_state(mavutil.mavlink.MAV_SYS_STATUS_GEOFENCE,
present=True,
enabled=True,
healthy=True,
verbose=False,
timeout=30)
self.progress("Waiting for RTL")
tstart = self.get_sim_time()
mode = "RTL"
while not self.mode_is(mode, drain_mav=False):
self.mav.messages['HEARTBEAT'].custom_mode
self.progress("mav.flightmode=%s Want=%s Alt=%f" % (
self.mav.flightmode, mode, self.get_altitude(relative=True)))
if (self.get_sim_time_cached() > tstart + 120):
raise WaitModeTimeout("Did not change mode")
self.progress("Got mode %s" % mode)
# check we are in breach
self.assert_fence_enabled()
self.assert_fence_sys_status(True, True, False)
# wait until we get home
self.wait_distance_to_home(50, 100, timeout=200)
# now check we are now not in breach
self.assert_fence_sys_status(True, True, True)
# Turn fence off with aux function
self.run_auxfunc(
11,
0,
want_result=mavutil.mavlink.MAV_RESULT_ACCEPTED
)
# switch back to cruise
self.change_mode("CRUISE")
self.wait_distance(150, accuracy=20)
# re-enable the fences
self.run_auxfunc(
11,
2,
want_result=mavutil.mavlink.MAV_RESULT_ACCEPTED
)
m = self.mav.recv_match(type='FENCE_STATUS', blocking=True, timeout=2)
self.progress("Got (%s)" % str(m))
if m is None:
raise NotAchievedException("Got FENCE_STATUS unexpectedly")
self.progress("Waiting for RTL")
tstart = self.get_sim_time()
mode = "RTL"
while not self.mode_is(mode, drain_mav=False):
self.mav.messages['HEARTBEAT'].custom_mode
self.progress("mav.flightmode=%s Want=%s Alt=%f" % (
self.mav.flightmode, mode, self.get_altitude(relative=True)))
if (self.get_sim_time_cached() > tstart + 120):
raise WaitModeTimeout("Did not change mode")
self.progress("Got mode %s" % mode)
# wait to no longer be in breach
self.wait_distance_to_home(50, 100, timeout=200)
self.assert_fence_sys_status(True, True, True)
# fly home and land with fences still enabled
self.fly_home_land_and_disarm(timeout=250)
self.do_fence_disable() # Ensure the fence is disabled after test
def FenceBreachedChangeMode(self):
'''Tests manual mode change after fence breach, as set with FENCE_OPTIONS'''
""" Attempts to change mode while a fence is breached.
@ -5447,6 +5773,11 @@ class AutoTestPlane(vehicle_test_suite.TestSuite):
self.FenceRTLRally,
self.FenceRetRally,
self.FenceAltCeilFloor,
self.FenceMinAltAutoEnable,
self.FenceMinAltAutoEnableAbort,
self.FenceAutoEnableDisableSwitch,
self.FenceEnableDisableSwitch,
self.FenceEnableDisableAux,
self.FenceBreachedChangeMode,
self.FenceNoFenceReturnPoint,
self.FenceNoFenceReturnPointInclusion,

6
Tools/autotest/rover.py
View File

@ -4061,6 +4061,9 @@ Brakes have negligible effect (with=%0.2fm without=%0.2fm delta=%0.2fm)
self.clear_mission(mavutil.mavlink.MAV_MISSION_TYPE_FENCE,
target_system=target_system,
target_component=target_component)
self.set_parameters({
"FENCE_TYPE": 2, # circle only
})
self.delay_sim_time(5) # let breaches clear
# FIXME: should we allow this?
self.progress("Ensure we can arm with no poly in place")
@ -4068,6 +4071,9 @@ Brakes have negligible effect (with=%0.2fm without=%0.2fm delta=%0.2fm)
self.wait_ready_to_arm()
self.arm_vehicle()
self.disarm_vehicle()
self.set_parameters({
"FENCE_TYPE": 6, # polyfence + circle
})
self.test_poly_fence_noarms_exclusion_circle(target_system=target_system,
target_component=target_component)

23
Tools/autotest/vehicle_test_suite.py
View File

@ -2080,6 +2080,8 @@ class TestSuite(ABC):
def load_fence(self, filename):
filepath = os.path.join(testdir, self.current_test_name_directory, filename)
if not os.path.exists(filepath):
filepath = self.generic_mission_filepath_for_filename(filename)
self.progress("Loading fence from (%s)" % str(filepath))
locs = []
for line in open(filepath, 'rb'):
@ -6994,22 +6996,17 @@ class TestSuite(ABC):
self.mavproxy.expect("Loaded module relay")
self.mavproxy.send("relay set %d %d\n" % (relay_num, on_off))
def do_fence_en_or_dis_able(self, value, want_result=mavutil.mavlink.MAV_RESULT_ACCEPTED):
if value:
p1 = 1
else:
p1 = 0
self.run_cmd(
mavutil.mavlink.MAV_CMD_DO_FENCE_ENABLE,
p1=p1, # param1
want_result=want_result,
)
def do_fence_enable(self, want_result=mavutil.mavlink.MAV_RESULT_ACCEPTED):
self.do_fence_en_or_dis_able(True, want_result=want_result)
self.run_cmd(mavutil.mavlink.MAV_CMD_DO_FENCE_ENABLE, p1=1, want_result=want_result)
def do_fence_disable(self, want_result=mavutil.mavlink.MAV_RESULT_ACCEPTED):
self.do_fence_en_or_dis_able(False, want_result=want_result)
self.run_cmd(mavutil.mavlink.MAV_CMD_DO_FENCE_ENABLE, p1=0, want_result=want_result)
def do_fence_disable_floor(self, want_result=mavutil.mavlink.MAV_RESULT_ACCEPTED):
self.run_cmd(mavutil.mavlink.MAV_CMD_DO_FENCE_ENABLE, p1=0, p2=8, want_result=want_result)
def do_fence_enable_except_floor(self, want_result=mavutil.mavlink.MAV_RESULT_ACCEPTED):
self.run_cmd(mavutil.mavlink.MAV_CMD_DO_FENCE_ENABLE, p1=1, p2=7, want_result=want_result)
#################################################
# WAIT UTILITIES